diff --git a/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt b/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
index 5580e9c..57b423f 100644
--- a/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
+++ b/Documentation/devicetree/bindings/arm/vexpress-sysreg.txt
@@ -27,24 +27,45 @@ Example:
 This block also can also act a bridge to the platform's configuration
 bus via "system control" interface, addressing devices with site number,
 position in the board stack, config controller, function and device
-numbers - see motherboard's TRM for more details.
-
-The node describing a config device must refer to the sysreg node via
-"arm,vexpress,config-bridge" phandle (can be also defined in the node's
-parent) and relies on the board topology properties - see main vexpress
-node documentation for more details. It must also define the following
-property:
-- arm,vexpress-sysreg,func : must contain two cells:
-  - first cell defines function number (eg. 1 for clock generator,
-    2 for voltage regulators etc.)
-  - device number (eg. osc 0, osc 1 etc.)
+numbers - see motherboard's TRM for more details. All configuration
+controller accessible via this interface must reference the sysreg
+node via "arm,vexpress,config-bridge" phandle and define appropriate
+topology properties - see main vexpress node documentation for more
+details. Each child of such node describes one function and must
+define the following properties:
+- compatible value : must be one of (corresponding to the TRM):
+	"arm,vexpress-amp"
+	"arm,vexpress-dvimode"
+	"arm,vexpress-energy"
+	"arm,vexpress-muxfpga"
+	"arm,vexpress-osc"
+	"arm,vexpress-power"
+	"arm,vexpress-reboot"
+	"arm,vexpress-reset"
+	"arm,vexpress-scc"
+	"arm,vexpress-shutdown"
+	"arm,vexpress-temp"
+	"arm,vexpress-volt"
+- arm,vexpress-sysreg,func : must contain a set of two cells long groups:
+  - first cell of each group defines the function number
+    (eg. 1 for clock generator, 2 for voltage regulators etc.)
+  - second cell of each group defines device number (eg. osc 0,
+    osc 1 etc.)
+  - some functions (eg. energy meter, with its 64 bit long counter)
+    are using more than one function/device number pair
 
 Example:
 	mcc {
+		compatible = "arm,vexpress,config-bus";
 		arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 		osc@0 {
 			compatible = "arm,vexpress-osc";
 			arm,vexpress-sysreg,func = <1 0>;
 		};
+
+		energy@0 {
+			compatible = "arm,vexpress-energy";
+			arm,vexpress-sysreg,func = <13 0>, <13 1>;
+		};
 	};
diff --git a/Documentation/devicetree/bindings/arm/vexpress.txt b/Documentation/devicetree/bindings/arm/vexpress.txt
index ae49161..39844cd 100644
--- a/Documentation/devicetree/bindings/arm/vexpress.txt
+++ b/Documentation/devicetree/bindings/arm/vexpress.txt
@@ -80,12 +80,17 @@ but also control clock generators, voltage regulators, gather
 environmental data like temperature, power consumption etc. Even
 the video output switch (FPGA) is controlled that way.
 
-Nodes describing devices controlled by this infrastructure should
-point at the bridge device node:
+The controllers are not mapped into normal memory address space
+and must be accessed through bridges - other devices capable
+of generating transactions on the configuration bus.
+
+The nodes describing configuration controllers must define
+the following properties:
+- compatible value:
+	compatible = "arm,vexpress,config-bus";
 - bridge phandle:
 	arm,vexpress,config-bridge = <phandle>;
-This property can be also defined in a parent node (eg. for a DCC)
-and is effective for all children.
+and children describing available functions.
 
 
 Platform topology
@@ -197,7 +202,7 @@ Example of a VE tile description (simplified)
 	};
 
 	dcc {
-		compatible = "simple-bus";
+		compatible = "arm,vexpress,config-bus";
 		arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 		osc@0 {
diff --git a/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts b/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
index 15f98cb..a25c262 100644
--- a/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
+++ b/arch/arm/boot/dts/vexpress-v2p-ca15_a7.dts
@@ -312,6 +312,7 @@
 			arm,vexpress-sysreg,func = <12 0>;
 			label = "A15 Pcore";
 		};
+
 		power@1 {
 			/* Total power for the three A7 cores */
 			compatible = "arm,vexpress-power";
@@ -322,14 +323,14 @@
 		energy@0 {
 			/* Total energy for the two A15 cores */
 			compatible = "arm,vexpress-energy";
-			arm,vexpress-sysreg,func = <13 0>;
+			arm,vexpress-sysreg,func = <13 0>, <13 1>;
 			label = "A15 Jcore";
 		};
 
 		energy@2 {
 			/* Total energy for the three A7 cores */
 			compatible = "arm,vexpress-energy";
-			arm,vexpress-sysreg,func = <13 2>;
+			arm,vexpress-sysreg,func = <13 2>, <13 3>;
 			label = "A7 Jcore";
 		};
 	};
diff --git a/arch/arm/mach-vexpress/ct-ca9x4.c b/arch/arm/mach-vexpress/ct-ca9x4.c
index 6f34497..35e394a 100644
--- a/arch/arm/mach-vexpress/ct-ca9x4.c
+++ b/arch/arm/mach-vexpress/ct-ca9x4.c
@@ -128,6 +128,10 @@ static struct platform_device pmu_device = {
 	.resource	= pmu_resources,
 };
 
+static struct clk_lookup osc1_lookup = {
+	.dev_id		= "ct:clcd",
+};
+
 static struct platform_device osc1_device = {
 	.name		= "vexpress-osc",
 	.id		= 1,
@@ -135,6 +139,7 @@ static struct platform_device osc1_device = {
 	.resource	= (struct resource []) {
 		VEXPRESS_RES_FUNC(0xf, 1),
 	},
+	.dev.platform_data = &osc1_lookup,
 };
 
 static void __init ct_ca9x4_init(void)
@@ -155,10 +160,7 @@ static void __init ct_ca9x4_init(void)
 		amba_device_register(ct_ca9x4_amba_devs[i], &iomem_resource);
 
 	platform_device_register(&pmu_device);
-	platform_device_register(&osc1_device);
-
-	WARN_ON(clk_register_clkdev(vexpress_osc_setup(&osc1_device.dev),
-			NULL, "ct:clcd"));
+	vexpress_sysreg_config_device_register(&osc1_device);
 }
 
 #ifdef CONFIG_SMP
diff --git a/arch/arm/mach-vexpress/v2m.c b/arch/arm/mach-vexpress/v2m.c
index 4f8b8cb..ac95220 100644
--- a/arch/arm/mach-vexpress/v2m.c
+++ b/arch/arm/mach-vexpress/v2m.c
@@ -340,11 +340,6 @@ static void __init v2m_init(void)
 	regulator_register_fixed(0, v2m_eth_supplies,
 			ARRAY_SIZE(v2m_eth_supplies));
 
-	platform_device_register(&v2m_muxfpga_device);
-	platform_device_register(&v2m_shutdown_device);
-	platform_device_register(&v2m_reboot_device);
-	platform_device_register(&v2m_dvimode_device);
-
 	platform_device_register(&v2m_sysreg_device);
 	platform_device_register(&v2m_pcie_i2c_device);
 	platform_device_register(&v2m_ddc_i2c_device);
@@ -356,6 +351,11 @@ static void __init v2m_init(void)
 	for (i = 0; i < ARRAY_SIZE(v2m_amba_devs); i++)
 		amba_device_register(v2m_amba_devs[i], &iomem_resource);
 
+	vexpress_sysreg_config_device_register(&v2m_muxfpga_device);
+	vexpress_sysreg_config_device_register(&v2m_shutdown_device);
+	vexpress_sysreg_config_device_register(&v2m_reboot_device);
+	vexpress_sysreg_config_device_register(&v2m_dvimode_device);
+
 	ct_desc->init_tile();
 }
 
@@ -423,17 +423,11 @@ void __init v2m_dt_init_early(void)
 	versatile_sched_clock_init(vexpress_get_24mhz_clock_base(), 24000000);
 }
 
-static const struct of_device_id v2m_dt_bus_match[] __initconst = {
-	{ .compatible = "simple-bus", },
-	{ .compatible = "arm,amba-bus", },
-	{ .compatible = "arm,vexpress,config-bus", },
-	{}
-};
 
 static void __init v2m_dt_init(void)
 {
 	l2x0_of_init(0x00400000, 0xfe0fffff);
-	of_platform_populate(NULL, v2m_dt_bus_match, NULL, NULL);
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
 }
 
 static const char * const v2m_dt_match[] __initconst = {
diff --git a/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
index 2f2ecd2..ac2cb24 100644
--- a/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
+++ b/arch/arm64/boot/dts/rtsm_ve-motherboard.dtsi
@@ -200,7 +200,7 @@
 		};
 
 		mcc {
-			compatible = "arm,vexpress,config-bus", "simple-bus";
+			compatible = "arm,vexpress,config-bus";
 			arm,vexpress,config-bridge = <&v2m_sysreg>;
 
 			v2m_oscclk1: osc@1 {
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 552373c..f24e79d 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -41,4 +41,13 @@ config ARM_CCI
 	help
 	  Driver supporting the CCI cache coherent interconnect for ARM
 	  platforms.
+
+config VEXPRESS_CONFIG
+	bool "Versatile Express configuration bus"
+	default y if ARCH_VEXPRESS
+	depends on ARM || ARM64
+	select REGMAP
+	help
+	  Platform configuration infrastructure for the ARM Ltd.
+	  Versatile Express.
 endmenu
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index 8947bdd..f095aa7 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -10,3 +10,5 @@ obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
 obj-$(CONFIG_OMAP_INTERCONNECT)	+= omap_l3_smx.o omap_l3_noc.o
 # CCI cache coherent interconnect for ARM platforms
 obj-$(CONFIG_ARM_CCI)		+= arm-cci.o
+
+obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o
diff --git a/drivers/bus/vexpress-config.c b/drivers/bus/vexpress-config.c
new file mode 100644
index 0000000..27a07df
--- /dev/null
+++ b/drivers/bus/vexpress-config.c
@@ -0,0 +1,202 @@
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Copyright (C) 2014 ARM Limited
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/vexpress.h>
+
+
+struct vexpress_config_bridge {
+	struct vexpress_config_bridge_ops *ops;
+	void *context;
+};
+
+
+static DEFINE_MUTEX(vexpress_config_mutex);
+static struct class *vexpress_config_class;
+static u32 vexpress_config_site_master = VEXPRESS_SITE_MASTER;
+
+
+void vexpress_config_set_master(u32 site)
+{
+	vexpress_config_site_master = site;
+}
+
+u32 vexpress_config_get_master(void)
+{
+	return vexpress_config_site_master;
+}
+
+void vexpress_config_lock(void *arg)
+{
+	mutex_lock(&vexpress_config_mutex);
+}
+
+void vexpress_config_unlock(void *arg)
+{
+	mutex_unlock(&vexpress_config_mutex);
+}
+
+
+static void vexpress_config_find_prop(struct device_node *node,
+		const char *name, u32 *val)
+{
+	/* Default value */
+	*val = 0;
+
+	of_node_get(node);
+	while (node) {
+		if (of_property_read_u32(node, name, val) == 0) {
+			of_node_put(node);
+			return;
+		}
+		node = of_get_next_parent(node);
+	}
+}
+
+int vexpress_config_get_topo(struct device_node *node, u32 *site,
+		u32 *position, u32 *dcc)
+{
+	vexpress_config_find_prop(node, "arm,vexpress,site", site);
+	if (*site == VEXPRESS_SITE_MASTER)
+		*site = vexpress_config_site_master;
+	if (WARN_ON(vexpress_config_site_master == VEXPRESS_SITE_MASTER))
+		return -EINVAL;
+	vexpress_config_find_prop(node, "arm,vexpress,position", position);
+	vexpress_config_find_prop(node, "arm,vexpress,dcc", dcc);
+
+	return 0;
+}
+
+
+static void vexpress_config_devres_release(struct device *dev, void *res)
+{
+	struct vexpress_config_bridge *bridge = dev_get_drvdata(dev->parent);
+	struct regmap *regmap = res;
+
+	bridge->ops->regmap_exit(regmap, bridge->context);
+}
+
+struct regmap *devm_regmap_init_vexpress_config(struct device *dev)
+{
+	struct vexpress_config_bridge *bridge;
+	struct regmap *regmap;
+	struct regmap **res;
+
+	if (WARN_ON(dev->parent->class != vexpress_config_class))
+		return ERR_PTR(-ENODEV);
+
+	bridge = dev_get_drvdata(dev->parent);
+	if (WARN_ON(!bridge))
+		return ERR_PTR(-EINVAL);
+
+	res = devres_alloc(vexpress_config_devres_release, sizeof(*res),
+			GFP_KERNEL);
+	if (!res)
+		return ERR_PTR(-ENOMEM);
+
+	regmap = bridge->ops->regmap_init(dev, bridge->context);
+	if (IS_ERR(regmap)) {
+		devres_free(res);
+		return regmap;
+	}
+
+	*res = regmap;
+	devres_add(dev, res);
+
+	return regmap;
+}
+
+
+struct device *vexpress_config_bridge_register(struct device *parent,
+		struct vexpress_config_bridge_ops *ops, void *context)
+{
+	struct device *dev;
+	struct vexpress_config_bridge *bridge;
+
+	if (!vexpress_config_class) {
+		vexpress_config_class = class_create(THIS_MODULE,
+				"vexpress-config");
+		if (IS_ERR(vexpress_config_class))
+			return (void *)vexpress_config_class;
+	}
+
+	dev = device_create(vexpress_config_class, parent, 0,
+			NULL, "%s.bridge", dev_name(parent));
+
+	if (IS_ERR(dev))
+		return dev;
+
+	bridge = devm_kmalloc(dev, sizeof(*bridge), GFP_KERNEL);
+	if (!bridge) {
+		put_device(dev);
+		device_unregister(dev);
+		return ERR_PTR(-ENOMEM);
+	}
+	bridge->ops = ops;
+	bridge->context = context;
+
+	dev_set_drvdata(dev, bridge);
+
+	dev_dbg(parent, "Registered bridge '%s', parent node %p\n",
+			dev_name(dev), parent->of_node);
+
+	return dev;
+}
+
+
+static int vexpress_config_node_match(struct device *dev, const void *data)
+{
+	const struct device_node *node = data;
+
+	dev_dbg(dev, "Parent node %p, looking for %p\n",
+			dev->parent->of_node, node);
+
+	return dev->parent->of_node == node;
+}
+
+static int vexpress_config_populate(struct device_node *node)
+{
+	struct device_node *bridge;
+	struct device *parent;
+
+	bridge = of_parse_phandle(node, "arm,vexpress,config-bridge", 0);
+	if (!bridge)
+		return -EINVAL;
+
+	parent = class_find_device(vexpress_config_class, NULL, bridge,
+			vexpress_config_node_match);
+	if (WARN_ON(!parent))
+		return -ENODEV;
+
+	return of_platform_populate(node, NULL, NULL, parent);
+}
+
+static int __init vexpress_config_init(void)
+{
+	int err = 0;
+	struct device_node *node;
+
+	/* Need the config devices early, before the "normal" devices... */
+	for_each_compatible_node(node, NULL, "arm,vexpress,config-bus") {
+		err = vexpress_config_populate(node);
+		if (err)
+			break;
+	}
+
+	return err;
+}
+postcore_initcall(vexpress_config_init);
+
diff --git a/drivers/clk/versatile/clk-vexpress-osc.c b/drivers/clk/versatile/clk-vexpress-osc.c
index 2dc8b41..529a59c 100644
--- a/drivers/clk/versatile/clk-vexpress-osc.c
+++ b/drivers/clk/versatile/clk-vexpress-osc.c
@@ -11,8 +11,6 @@
  * Copyright (C) 2012 ARM Limited
  */
 
-#define pr_fmt(fmt) "vexpress-osc: " fmt
-
 #include <linux/clkdev.h>
 #include <linux/clk-provider.h>
 #include <linux/err.h>
@@ -22,7 +20,7 @@
 #include <linux/vexpress.h>
 
 struct vexpress_osc {
-	struct vexpress_config_func *func;
+	struct regmap *reg;
 	struct clk_hw hw;
 	unsigned long rate_min;
 	unsigned long rate_max;
@@ -36,7 +34,7 @@ static unsigned long vexpress_osc_recalc_rate(struct clk_hw *hw,
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 	u32 rate;
 
-	vexpress_config_read(osc->func, 0, &rate);
+	regmap_read(osc->reg, 0, &rate);
 
 	return rate;
 }
@@ -60,7 +58,7 @@ static int vexpress_osc_set_rate(struct clk_hw *hw, unsigned long rate,
 {
 	struct vexpress_osc *osc = to_vexpress_osc(hw);
 
-	return vexpress_config_write(osc->func, 0, rate);
+	return regmap_write(osc->reg, 0, rate);
 }
 
 static struct clk_ops vexpress_osc_ops = {
@@ -70,56 +68,31 @@ static struct clk_ops vexpress_osc_ops = {
 };
 
 
-struct clk * __init vexpress_osc_setup(struct device *dev)
-{
-	struct clk_init_data init;
-	struct vexpress_osc *osc = kzalloc(sizeof(*osc), GFP_KERNEL);
-
-	if (!osc)
-		return NULL;
-
-	osc->func = vexpress_config_func_get_by_dev(dev);
-	if (!osc->func) {
-		kfree(osc);
-		return NULL;
-	}
-
-	init.name = dev_name(dev);
-	init.ops = &vexpress_osc_ops;
-	init.flags = CLK_IS_ROOT;
-	init.num_parents = 0;
-	osc->hw.init = &init;
-
-	return clk_register(NULL, &osc->hw);
-}
-
-void __init vexpress_osc_of_setup(struct device_node *node)
+static int vexpress_osc_probe(struct platform_device *pdev)
 {
+	struct clk_lookup *cl = pdev->dev.platform_data; /* Non-DT lookup */
 	struct clk_init_data init;
 	struct vexpress_osc *osc;
 	struct clk *clk;
 	u32 range[2];
 
-	osc = kzalloc(sizeof(*osc), GFP_KERNEL);
+	osc = devm_kzalloc(&pdev->dev, sizeof(*osc), GFP_KERNEL);
 	if (!osc)
-		goto error;
+		return -ENOMEM;
 
-	osc->func = vexpress_config_func_get_by_node(node);
-	if (!osc->func) {
-		pr_err("Failed to obtain config func for node '%s'!\n",
-				node->full_name);
-		goto error;
-	}
+	osc->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(osc->reg))
+		return PTR_ERR(osc->reg);
 
-	if (of_property_read_u32_array(node, "freq-range", range,
+	if (of_property_read_u32_array(pdev->dev.of_node, "freq-range", range,
 			ARRAY_SIZE(range)) == 0) {
 		osc->rate_min = range[0];
 		osc->rate_max = range[1];
 	}
 
-	of_property_read_string(node, "clock-output-names", &init.name);
-	if (!init.name)
-		init.name = node->full_name;
+	if (of_property_read_string(pdev->dev.of_node, "clock-output-names",
+			&init.name) != 0)
+		init.name = dev_name(&pdev->dev);
 
 	init.ops = &vexpress_osc_ops;
 	init.flags = CLK_IS_ROOT;
@@ -128,20 +101,37 @@ void __init vexpress_osc_of_setup(struct device_node *node)
 	osc->hw.init = &init;
 
 	clk = clk_register(NULL, &osc->hw);
-	if (IS_ERR(clk)) {
-		pr_err("Failed to register clock '%s'!\n", init.name);
-		goto error;
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	of_clk_add_provider(pdev->dev.of_node, of_clk_src_simple_get, clk);
+
+	/* Only happens for non-DT cases */
+	if (cl) {
+		cl->clk = clk;
+		clkdev_add(cl);
 	}
 
-	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	dev_dbg(&pdev->dev, "Registered clock '%s'\n", init.name);
 
-	pr_debug("Registered clock '%s'\n", init.name);
+	return 0;
+}
 
-	return;
+static struct of_device_id vexpress_osc_of_match[] = {
+	{ .compatible = "arm,vexpress-osc", },
+	{}
+};
 
-error:
-	if (osc->func)
-		vexpress_config_func_put(osc->func);
-	kfree(osc);
+static struct platform_driver vexpress_osc_driver = {
+	.driver	= {
+		.name = "vexpress-osc",
+		.of_match_table = vexpress_osc_of_match,
+	},
+	.probe = vexpress_osc_probe,
+};
+
+static int __init vexpress_osc_init(void)
+{
+	return platform_driver_register(&vexpress_osc_driver);
 }
-CLK_OF_DECLARE(vexpress_soc, "arm,vexpress-osc", vexpress_osc_of_setup);
+core_initcall(vexpress_osc_init);
diff --git a/drivers/hwmon/vexpress.c b/drivers/hwmon/vexpress.c
index d867e6b..d8a6113 100644
--- a/drivers/hwmon/vexpress.c
+++ b/drivers/hwmon/vexpress.c
@@ -26,7 +26,7 @@
 
 struct vexpress_hwmon_data {
 	struct device *hwmon_dev;
-	struct vexpress_config_func *func;
+	struct regmap *reg;
 };
 
 static ssize_t vexpress_hwmon_name_show(struct device *dev,
@@ -56,7 +56,7 @@ static ssize_t vexpress_hwmon_u32_show(struct device *dev,
 	int err;
 	u32 value;
 
-	err = vexpress_config_read(data->func, 0, &value);
+	err = regmap_read(data->reg, 0, &value);
 	if (err)
 		return err;
 
@@ -71,11 +71,11 @@ static ssize_t vexpress_hwmon_u64_show(struct device *dev,
 	int err;
 	u32 value_hi, value_lo;
 
-	err = vexpress_config_read(data->func, 0, &value_lo);
+	err = regmap_read(data->reg, 0, &value_lo);
 	if (err)
 		return err;
 
-	err = vexpress_config_read(data->func, 1, &value_hi);
+	err = regmap_read(data->reg, 1, &value_hi);
 	if (err)
 		return err;
 
@@ -175,8 +175,8 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 	if (!match)
 		return -ENODEV;
 
-	data->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!data->func)
+	data->reg = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (!data->reg)
 		return -ENODEV;
 
 	err = sysfs_create_group(&pdev->dev.kobj, match->data);
@@ -193,7 +193,6 @@ static int vexpress_hwmon_probe(struct platform_device *pdev)
 
 error:
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
-	vexpress_config_func_put(data->func);
 	return err;
 }
 
@@ -207,8 +206,6 @@ static int vexpress_hwmon_remove(struct platform_device *pdev)
 	match = of_match_device(vexpress_hwmon_of_match, &pdev->dev);
 	sysfs_remove_group(&pdev->dev.kobj, match->data);
 
-	vexpress_config_func_put(data->func);
-
 	return 0;
 }
 
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 49bb445..08657fa 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -1219,10 +1219,3 @@ config MCP_UCB1200_TS
 	depends on MCP_UCB1200 && INPUT
 
 endmenu
-
-config VEXPRESS_CONFIG
-	bool "ARM Versatile Express platform infrastructure"
-	depends on ARM || ARM64
-	help
-	  Platform configuration infrastructure for the ARM Ltd.
-	  Versatile Express.
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index 5aea5ef..816b258 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -161,7 +161,7 @@ obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
 obj-$(CONFIG_MFD_SEC_CORE)	+= sec-core.o sec-irq.o
 obj-$(CONFIG_MFD_SYSCON)	+= syscon.o
 obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
-obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o vexpress-sysreg.o
+obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-sysreg.o
 obj-$(CONFIG_MFD_RETU)		+= retu-mfd.o
 obj-$(CONFIG_MFD_AS3711)	+= as3711.o
 obj-$(CONFIG_MFD_AS3722)	+= as3722.o
diff --git a/drivers/mfd/vexpress-config.c b/drivers/mfd/vexpress-config.c
deleted file mode 100644
index 84ce6b9..0000000
--- a/drivers/mfd/vexpress-config.c
+++ /dev/null
@@ -1,288 +0,0 @@
-/*
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * Copyright (C) 2012 ARM Limited
- */
-
-#define pr_fmt(fmt) "vexpress-config: " fmt
-
-#include <linux/bitops.h>
-#include <linux/completion.h>
-#include <linux/export.h>
-#include <linux/init.h>
-#include <linux/list.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/slab.h>
-#include <linux/string.h>
-#include <linux/vexpress.h>
-
-
-#define VEXPRESS_CONFIG_MAX_BRIDGES 2
-
-struct vexpress_config_bridge {
-	struct device_node *node;
-	struct vexpress_config_bridge_info *info;
-	struct list_head transactions;
-	spinlock_t transactions_lock;
-} vexpress_config_bridges[VEXPRESS_CONFIG_MAX_BRIDGES];
-
-static DECLARE_BITMAP(vexpress_config_bridges_map,
-		ARRAY_SIZE(vexpress_config_bridges));
-static DEFINE_MUTEX(vexpress_config_bridges_mutex);
-
-struct vexpress_config_bridge *vexpress_config_bridge_register(
-		struct device_node *node,
-		struct vexpress_config_bridge_info *info)
-{
-	struct vexpress_config_bridge *bridge;
-	int i;
-
-	pr_debug("Registering bridge '%s'\n", info->name);
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	i = find_first_zero_bit(vexpress_config_bridges_map,
-			ARRAY_SIZE(vexpress_config_bridges));
-	if (i >= ARRAY_SIZE(vexpress_config_bridges)) {
-		pr_err("Can't register more bridges!\n");
-		mutex_unlock(&vexpress_config_bridges_mutex);
-		return NULL;
-	}
-	__set_bit(i, vexpress_config_bridges_map);
-	bridge = &vexpress_config_bridges[i];
-
-	bridge->node = node;
-	bridge->info = info;
-	INIT_LIST_HEAD(&bridge->transactions);
-	spin_lock_init(&bridge->transactions_lock);
-
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	return bridge;
-}
-EXPORT_SYMBOL(vexpress_config_bridge_register);
-
-void vexpress_config_bridge_unregister(struct vexpress_config_bridge *bridge)
-{
-	struct vexpress_config_bridge __bridge = *bridge;
-	int i;
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++)
-		if (&vexpress_config_bridges[i] == bridge)
-			__clear_bit(i, vexpress_config_bridges_map);
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	WARN_ON(!list_empty(&__bridge.transactions));
-	while (!list_empty(&__bridge.transactions))
-		cpu_relax();
-}
-EXPORT_SYMBOL(vexpress_config_bridge_unregister);
-
-
-struct vexpress_config_func {
-	struct vexpress_config_bridge *bridge;
-	void *func;
-};
-
-struct vexpress_config_func *__vexpress_config_func_get(struct device *dev,
-		struct device_node *node)
-{
-	struct device_node *bridge_node;
-	struct vexpress_config_func *func;
-	int i;
-
-	if (WARN_ON(dev && node && dev->of_node != node))
-		return NULL;
-	if (dev && !node)
-		node = dev->of_node;
-
-	func = kzalloc(sizeof(*func), GFP_KERNEL);
-	if (!func)
-		return NULL;
-
-	bridge_node = of_node_get(node);
-	while (bridge_node) {
-		const __be32 *prop = of_get_property(bridge_node,
-				"arm,vexpress,config-bridge", NULL);
-
-		if (prop) {
-			bridge_node = of_find_node_by_phandle(
-					be32_to_cpup(prop));
-			break;
-		}
-
-		bridge_node = of_get_next_parent(bridge_node);
-	}
-
-	mutex_lock(&vexpress_config_bridges_mutex);
-	for (i = 0; i < ARRAY_SIZE(vexpress_config_bridges); i++) {
-		struct vexpress_config_bridge *bridge =
-				&vexpress_config_bridges[i];
-
-		if (test_bit(i, vexpress_config_bridges_map) &&
-				bridge->node == bridge_node) {
-			func->bridge = bridge;
-			func->func = bridge->info->func_get(dev, node);
-			break;
-		}
-	}
-	mutex_unlock(&vexpress_config_bridges_mutex);
-
-	if (!func->func) {
-		of_node_put(node);
-		kfree(func);
-		return NULL;
-	}
-
-	return func;
-}
-EXPORT_SYMBOL(__vexpress_config_func_get);
-
-void vexpress_config_func_put(struct vexpress_config_func *func)
-{
-	func->bridge->info->func_put(func->func);
-	of_node_put(func->bridge->node);
-	kfree(func);
-}
-EXPORT_SYMBOL(vexpress_config_func_put);
-
-struct vexpress_config_trans {
-	struct vexpress_config_func *func;
-	int offset;
-	bool write;
-	u32 *data;
-	int status;
-	struct completion completion;
-	struct list_head list;
-};
-
-static void vexpress_config_dump_trans(const char *what,
-		struct vexpress_config_trans *trans)
-{
-	pr_debug("%s %s trans %p func 0x%p offset %d data 0x%x status %d\n",
-			what, trans->write ? "write" : "read", trans,
-			trans->func->func, trans->offset,
-			trans->data ? *trans->data : 0, trans->status);
-}
-
-static int vexpress_config_schedule(struct vexpress_config_trans *trans)
-{
-	int status;
-	struct vexpress_config_bridge *bridge = trans->func->bridge;
-	unsigned long flags;
-
-	init_completion(&trans->completion);
-	trans->status = -EFAULT;
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	if (list_empty(&bridge->transactions)) {
-		vexpress_config_dump_trans("Executing", trans);
-		status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-	} else {
-		vexpress_config_dump_trans("Queuing", trans);
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	}
-
-	switch (status) {
-	case VEXPRESS_CONFIG_STATUS_DONE:
-		vexpress_config_dump_trans("Finished", trans);
-		trans->status = status;
-		break;
-	case VEXPRESS_CONFIG_STATUS_WAIT:
-		list_add_tail(&trans->list, &bridge->transactions);
-		break;
-	}
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-
-	return status;
-}
-
-void vexpress_config_complete(struct vexpress_config_bridge *bridge,
-		int status)
-{
-	struct vexpress_config_trans *trans;
-	unsigned long flags;
-	const char *message = "Completed";
-
-	spin_lock_irqsave(&bridge->transactions_lock, flags);
-
-	trans = list_first_entry(&bridge->transactions,
-			struct vexpress_config_trans, list);
-	trans->status = status;
-
-	do {
-		vexpress_config_dump_trans(message, trans);
-		list_del(&trans->list);
-		complete(&trans->completion);
-
-		if (list_empty(&bridge->transactions))
-			break;
-
-		trans = list_first_entry(&bridge->transactions,
-				struct vexpress_config_trans, list);
-		vexpress_config_dump_trans("Executing pending", trans);
-		trans->status = bridge->info->func_exec(trans->func->func,
-				trans->offset, trans->write, trans->data);
-		message = "Finished pending";
-	} while (trans->status == VEXPRESS_CONFIG_STATUS_DONE);
-
-	spin_unlock_irqrestore(&bridge->transactions_lock, flags);
-}
-EXPORT_SYMBOL(vexpress_config_complete);
-
-int vexpress_config_wait(struct vexpress_config_trans *trans)
-{
-	wait_for_completion(&trans->completion);
-
-	return trans->status;
-}
-EXPORT_SYMBOL(vexpress_config_wait);
-
-int vexpress_config_read(struct vexpress_config_func *func, int offset,
-		u32 *data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = false,
-		.data = data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_read);
-
-int vexpress_config_write(struct vexpress_config_func *func, int offset,
-		u32 data)
-{
-	struct vexpress_config_trans trans = {
-		.func = func,
-		.offset = offset,
-		.write = true,
-		.data = &data,
-		.status = 0,
-	};
-	int status = vexpress_config_schedule(&trans);
-
-	if (status == VEXPRESS_CONFIG_STATUS_WAIT)
-		status = vexpress_config_wait(&trans);
-
-	return status;
-}
-EXPORT_SYMBOL(vexpress_config_write);
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index 981bef4..b4138a7 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -16,8 +16,10 @@
 #include <linux/io.h>
 #include <linux/leds.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
+#include <linux/sched.h>
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/timer.h>
@@ -72,9 +74,18 @@
 
 static void __iomem *vexpress_sysreg_base;
 static struct device *vexpress_sysreg_dev;
-static int vexpress_master_site;
+static LIST_HEAD(vexpress_sysreg_config_funcs);
+static struct device *vexpress_sysreg_config_bridge;
 
 
+static int vexpress_sysreg_get_master(void)
+{
+	if (readl(vexpress_sysreg_base + SYS_MISC) & SYS_MISC_MASTERSITE)
+		return VEXPRESS_SITE_DB2;
+
+	return VEXPRESS_SITE_DB1;
+}
+
 void vexpress_flags_set(u32 data)
 {
 	writel(~0, vexpress_sysreg_base + SYS_FLAGSCLR);
@@ -84,7 +95,7 @@ void vexpress_flags_set(u32 data)
 u32 vexpress_get_procid(int site)
 {
 	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
+		site = vexpress_sysreg_get_master();
 
 	return readl(vexpress_sysreg_base + (site == VEXPRESS_SITE_DB1 ?
 			SYS_PROCID0 : SYS_PROCID1));
@@ -114,130 +125,33 @@ void __iomem *vexpress_get_24mhz_clock_base(void)
 }
 
 
-static void vexpress_sysreg_find_prop(struct device_node *node,
-		const char *name, u32 *val)
-{
-	of_node_get(node);
-	while (node) {
-		if (of_property_read_u32(node, name, val) == 0) {
-			of_node_put(node);
-			return;
-		}
-		node = of_get_next_parent(node);
-	}
-}
-
-unsigned __vexpress_get_site(struct device *dev, struct device_node *node)
-{
-	u32 site = 0;
-
-	WARN_ON(dev && node && dev->of_node != node);
-	if (dev && !node)
-		node = dev->of_node;
-
-	if (node) {
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS)
-			site = pdev->resource[0].start;
-	} else if (dev && strncmp(dev_name(dev), "ct:", 3) == 0) {
-		site = VEXPRESS_SITE_MASTER;
-	}
-
-	if (site == VEXPRESS_SITE_MASTER)
-		site = vexpress_master_site;
-
-	return site;
-}
-
-
 struct vexpress_sysreg_config_func {
-	u32 template;
-	u32 device;
+	struct list_head list;
+	struct regmap *regmap;
+	int num_templates;
+	u32 template[0]; /* Keep this last */
 };
 
-static struct vexpress_config_bridge *vexpress_sysreg_config_bridge;
-static struct timer_list vexpress_sysreg_config_timer;
-static u32 *vexpress_sysreg_config_data;
-static int vexpress_sysreg_config_tries;
-
-static void *vexpress_sysreg_config_func_get(struct device *dev,
-		struct device_node *node)
+static int vexpress_sysreg_config_exec(struct vexpress_sysreg_config_func *func,
+		int index, bool write, u32 *data)
 {
-	struct vexpress_sysreg_config_func *config_func;
-	u32 site;
-	u32 position = 0;
-	u32 dcc = 0;
-	u32 func_device[2];
-	int err = -EFAULT;
-
-	if (node) {
-		of_node_get(node);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,site", &site);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,position",
-				&position);
-		vexpress_sysreg_find_prop(node, "arm,vexpress,dcc", &dcc);
-		err = of_property_read_u32_array(node,
-				"arm,vexpress-sysreg,func", func_device,
-				ARRAY_SIZE(func_device));
-		of_node_put(node);
-	} else if (dev && dev->bus == &platform_bus_type) {
-		struct platform_device *pdev = to_platform_device(dev);
-
-		if (pdev->num_resources == 1 &&
-				pdev->resource[0].flags == IORESOURCE_BUS) {
-			site = pdev->resource[0].start;
-			func_device[0] = pdev->resource[0].end;
-			func_device[1] = pdev->id;
-			err = 0;
-		}
-	}
-	if (err)
-		return NULL;
-
-	config_func = kzalloc(sizeof(*config_func), GFP_KERNEL);
-	if (!config_func)
-		return NULL;
-
-	config_func->template = SYS_CFGCTRL_DCC(dcc);
-	config_func->template |= SYS_CFGCTRL_FUNC(func_device[0]);
-	config_func->template |= SYS_CFGCTRL_SITE(site == VEXPRESS_SITE_MASTER ?
-			vexpress_master_site : site);
-	config_func->template |= SYS_CFGCTRL_POSITION(position);
-	config_func->device |= func_device[1];
-
-	dev_dbg(vexpress_sysreg_dev, "func 0x%p = 0x%x, %d\n", config_func,
-			config_func->template, config_func->device);
-
-	return config_func;
-}
-
-static void vexpress_sysreg_config_func_put(void *func)
-{
-	kfree(func);
-}
-
-static int vexpress_sysreg_config_func_exec(void *func, int offset,
-		bool write, u32 *data)
-{
-	int status;
-	struct vexpress_sysreg_config_func *config_func = func;
-	u32 command;
+	u32 command, status;
+	int tries;
+	long timeout;
 
 	if (WARN_ON(!vexpress_sysreg_base))
 		return -ENOENT;
 
+	if (WARN_ON(index > func->num_templates))
+		return -EINVAL;
+
 	command = readl(vexpress_sysreg_base + SYS_CFGCTRL);
 	if (WARN_ON(command & SYS_CFGCTRL_START))
 		return -EBUSY;
 
-	command = SYS_CFGCTRL_START;
+	command = func->template[index];
+	command |= SYS_CFGCTRL_START;
 	command |= write ? SYS_CFGCTRL_WRITE : 0;
-	command |= config_func->template;
-	command |= SYS_CFGCTRL_DEVICE(config_func->device + offset);
 
 	/* Use a canary for reads */
 	if (!write)
@@ -250,90 +164,190 @@ static int vexpress_sysreg_config_func_exec(void *func, int offset,
 	writel(command, vexpress_sysreg_base + SYS_CFGCTRL);
 	mb();
 
-	if (vexpress_sysreg_dev) {
-		/* Schedule completion check */
-		if (!write)
-			vexpress_sysreg_config_data = data;
-		vexpress_sysreg_config_tries = 100;
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(100));
-		status = VEXPRESS_CONFIG_STATUS_WAIT;
-	} else {
-		/* Early execution, no timer available, have to spin */
-		u32 cfgstat;
+	/* The operation can take ages... Go to sleep, 100us initially */
+	tries = 100;
+	timeout = 100;
+	do {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(usecs_to_jiffies(timeout));
+		if (signal_pending(current))
+			return -EINTR;
+
+		status = readl(vexpress_sysreg_base + SYS_CFGSTAT);
+		if (status & SYS_CFGSTAT_ERR)
+			return -EFAULT;
+
+		if (timeout > 20)
+			timeout -= 20;
+	} while (--tries && !(status & SYS_CFGSTAT_COMPLETE));
+	if (WARN_ON_ONCE(!tries))
+		return -ETIMEDOUT;
+
+	if (!write) {
+		*data = readl(vexpress_sysreg_base + SYS_CFGDATA);
+		dev_dbg(vexpress_sysreg_dev, "func %p, read data %x\n",
+				func, *data);
+	}
 
-		do {
-			cpu_relax();
-			cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-		} while (!cfgstat);
+	return 0;
+}
 
-		if (!write && (cfgstat & SYS_CFGSTAT_COMPLETE))
-			*data = readl(vexpress_sysreg_base + SYS_CFGDATA);
-		status = VEXPRESS_CONFIG_STATUS_DONE;
+static int vexpress_sysreg_config_read(void *context, unsigned int index,
+		unsigned int *val)
+{
+	struct vexpress_sysreg_config_func *func = context;
 
-		if (cfgstat & SYS_CFGSTAT_ERR)
-			status = -EINVAL;
-	}
+	return vexpress_sysreg_config_exec(func, index, false, val);
+}
 
-	return status;
+static int vexpress_sysreg_config_write(void *context, unsigned int index,
+		unsigned int val)
+{
+	struct vexpress_sysreg_config_func *func = context;
+
+	return vexpress_sysreg_config_exec(func, index, true, &val);
 }
 
-struct vexpress_config_bridge_info vexpress_sysreg_config_bridge_info = {
-	.name = "vexpress-sysreg",
-	.func_get = vexpress_sysreg_config_func_get,
-	.func_put = vexpress_sysreg_config_func_put,
-	.func_exec = vexpress_sysreg_config_func_exec,
+struct regmap_config vexpress_sysreg_regmap_config = {
+	.lock = vexpress_config_lock,
+	.unlock = vexpress_config_unlock,
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_read = vexpress_sysreg_config_read,
+	.reg_write = vexpress_sysreg_config_write,
+	.reg_format_endian = REGMAP_ENDIAN_LITTLE,
+	.val_format_endian = REGMAP_ENDIAN_LITTLE,
 };
 
-static void vexpress_sysreg_config_complete(unsigned long data)
+static struct regmap *vexpress_sysreg_config_regmap_init(struct device *dev,
+		void *context)
 {
-	int status = VEXPRESS_CONFIG_STATUS_DONE;
-	u32 cfgstat = readl(vexpress_sysreg_base + SYS_CFGSTAT);
-
-	if (cfgstat & SYS_CFGSTAT_ERR)
-		status = -EINVAL;
-	if (!vexpress_sysreg_config_tries--)
-		status = -ETIMEDOUT;
-
-	if (status < 0) {
-		dev_err(vexpress_sysreg_dev, "error %d\n", status);
-	} else if (!(cfgstat & SYS_CFGSTAT_COMPLETE)) {
-		mod_timer(&vexpress_sysreg_config_timer,
-				jiffies + usecs_to_jiffies(50));
-		return;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct vexpress_sysreg_config_func *func;
+	struct property *prop;
+	const __be32 *val = NULL;
+	__be32 energy_quirk[4];
+	int num;
+	u32 site, position, dcc;
+	int err;
+	int i;
+
+	if (dev->of_node) {
+		err = vexpress_config_get_topo(dev->of_node, &site, &position,
+				&dcc);
+		if (err)
+			return ERR_PTR(err);
+
+		prop = of_find_property(dev->of_node,
+				"arm,vexpress-sysreg,func", NULL);
+		if (!prop)
+			return ERR_PTR(-EINVAL);
+
+		num = prop->length / sizeof(u32) / 2;
+		val = prop->value;
+	} else {
+		if (pdev->num_resources != 1 ||
+				pdev->resource[0].flags != IORESOURCE_BUS)
+			return ERR_PTR(-EFAULT);
+
+		site = pdev->resource[0].start;
+		if (site == VEXPRESS_SITE_MASTER)
+			site = vexpress_sysreg_get_master();
+		position = 0;
+		dcc = 0;
+		num = 1;
 	}
 
-	if (vexpress_sysreg_config_data) {
-		*vexpress_sysreg_config_data = readl(vexpress_sysreg_base +
-				SYS_CFGDATA);
-		dev_dbg(vexpress_sysreg_dev, "read data %x\n",
-				*vexpress_sysreg_config_data);
-		vexpress_sysreg_config_data = NULL;
+	/*
+	 * "arm,vexpress-energy" function used to be described
+	 * by its first device only, now it requires both
+	 */
+	if (num == 1 && of_device_is_compatible(dev->of_node,
+			"arm,vexpress-energy")) {
+		num = 2;
+		energy_quirk[0] = *val;
+		energy_quirk[2] = *val++;
+		energy_quirk[1] = *val;
+		energy_quirk[3] = cpu_to_be32(be32_to_cpup(val) + 1);
+		val = energy_quirk;
 	}
 
-	vexpress_config_complete(vexpress_sysreg_config_bridge, status);
-}
+	func = kzalloc(sizeof(*func) + sizeof(*func->template) * num,
+			GFP_KERNEL);
+	if (!func)
+		return NULL;
 
+	func->num_templates = num;
 
-void vexpress_sysreg_setup(struct device_node *node)
-{
-	if (WARN_ON(!vexpress_sysreg_base))
-		return;
+	for (i = 0; i < num; i++) {
+		u32 function, device;
 
-	if (readl(vexpress_sysreg_base + SYS_MISC) & SYS_MISC_MASTERSITE)
-		vexpress_master_site = VEXPRESS_SITE_DB2;
+		if (dev->of_node) {
+			function = be32_to_cpup(val++);
+			device = be32_to_cpup(val++);
+		} else {
+			function = pdev->resource[0].end;
+			device = pdev->id;
+		}
+
+		dev_dbg(dev, "func %p: %u/%u/%u/%u/%u\n",
+				func, site, position, dcc,
+				function, device);
+
+		func->template[i] = SYS_CFGCTRL_DCC(dcc);
+		func->template[i] |= SYS_CFGCTRL_SITE(site);
+		func->template[i] |= SYS_CFGCTRL_POSITION(position);
+		func->template[i] |= SYS_CFGCTRL_FUNC(function);
+		func->template[i] |= SYS_CFGCTRL_DEVICE(device);
+	}
+
+	vexpress_sysreg_regmap_config.max_register = num - 1;
+
+	func->regmap = regmap_init(dev, NULL, func,
+			&vexpress_sysreg_regmap_config);
+
+	if (IS_ERR(func->regmap))
+		kfree(func);
 	else
-		vexpress_master_site = VEXPRESS_SITE_DB1;
+		list_add(&func->list, &vexpress_sysreg_config_funcs);
 
-	vexpress_sysreg_config_bridge = vexpress_config_bridge_register(
-			node, &vexpress_sysreg_config_bridge_info);
-	WARN_ON(!vexpress_sysreg_config_bridge);
+	return func->regmap;
+}
+
+static void vexpress_sysreg_config_regmap_exit(struct regmap *regmap,
+		void *context)
+{
+	struct vexpress_sysreg_config_func *func, *tmp;
+
+	regmap_exit(regmap);
+
+	list_for_each_entry_safe(func, tmp, &vexpress_sysreg_config_funcs,
+			list) {
+		if (func->regmap == regmap) {
+			list_del(&vexpress_sysreg_config_funcs);
+			kfree(func);
+			break;
+		}
+	}
+}
+
+static struct vexpress_config_bridge_ops vexpress_sysreg_config_bridge_ops = {
+	.regmap_init = vexpress_sysreg_config_regmap_init,
+	.regmap_exit = vexpress_sysreg_config_regmap_exit,
+};
+
+int vexpress_sysreg_config_device_register(struct platform_device *pdev)
+{
+	pdev->dev.parent = vexpress_sysreg_config_bridge;
+
+	return platform_device_register(pdev);
 }
 
+
 void __init vexpress_sysreg_early_init(void __iomem *base)
 {
 	vexpress_sysreg_base = base;
-	vexpress_sysreg_setup(NULL);
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 }
 
 void __init vexpress_sysreg_of_early_init(void)
@@ -344,10 +358,14 @@ void __init vexpress_sysreg_of_early_init(void)
 		return;
 
 	node = of_find_compatible_node(NULL, NULL, "arm,vexpress-sysreg");
-	if (node) {
-		vexpress_sysreg_base = of_iomap(node, 0);
-		vexpress_sysreg_setup(node);
-	}
+	if (WARN_ON(!node))
+		return;
+
+	vexpress_sysreg_base = of_iomap(node, 0);
+	if (WARN_ON(!vexpress_sysreg_base))
+		return;
+
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 }
 
 
@@ -470,28 +488,22 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 		return -EBUSY;
 	}
 
-	if (!vexpress_sysreg_base) {
+	if (!vexpress_sysreg_base)
 		vexpress_sysreg_base = devm_ioremap(&pdev->dev, res->start,
 				resource_size(res));
-		vexpress_sysreg_setup(pdev->dev.of_node);
-	}
 
 	if (!vexpress_sysreg_base) {
 		dev_err(&pdev->dev, "Failed to obtain base address!\n");
 		return -EFAULT;
 	}
 
-	setup_timer(&vexpress_sysreg_config_timer,
-			vexpress_sysreg_config_complete, 0);
-
+	vexpress_config_set_master(vexpress_sysreg_get_master());
 	vexpress_sysreg_dev = &pdev->dev;
 
 #ifdef CONFIG_GPIOLIB
 	vexpress_sysreg_gpio_chip.dev = &pdev->dev;
 	err = gpiochip_add(&vexpress_sysreg_gpio_chip);
 	if (err) {
-		vexpress_config_bridge_unregister(
-				vexpress_sysreg_config_bridge);
 		dev_err(&pdev->dev, "Failed to register GPIO chip! (%d)\n",
 				err);
 		return err;
@@ -502,6 +514,10 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 			sizeof(vexpress_sysreg_leds_pdata));
 #endif
 
+	vexpress_sysreg_config_bridge = vexpress_config_bridge_register(
+			&pdev->dev, &vexpress_sysreg_config_bridge_ops, NULL);
+	WARN_ON(!vexpress_sysreg_config_bridge);
+
 	device_create_file(vexpress_sysreg_dev, &dev_attr_sys_id);
 
 	return 0;
@@ -522,7 +538,12 @@ static struct platform_driver vexpress_sysreg_driver = {
 
 static int __init vexpress_sysreg_init(void)
 {
-	vexpress_sysreg_of_early_init();
+	struct device_node *node;
+
+	/* Need the sysreg early, before any other device... */
+	for_each_matching_node(node, vexpress_sysreg_match)
+		of_platform_device_create(node, NULL, NULL);
+
 	return platform_driver_register(&vexpress_sysreg_driver);
 }
 core_initcall(vexpress_sysreg_init);
diff --git a/drivers/power/reset/vexpress-poweroff.c b/drivers/power/reset/vexpress-poweroff.c
index 476aa49..37e7799 100644
--- a/drivers/power/reset/vexpress-poweroff.c
+++ b/drivers/power/reset/vexpress-poweroff.c
@@ -23,13 +23,12 @@
 static void vexpress_reset_do(struct device *dev, const char *what)
 {
 	int err = -ENOENT;
-	struct vexpress_config_func *func =
-			vexpress_config_func_get_by_dev(dev);
+	struct regmap *reg = dev_get_drvdata(dev);
 
-	if (func) {
+	if (reg) {
 		unsigned long timeout;
 
-		err = vexpress_config_write(func, 0, 0);
+		err = regmap_write(reg, 0, 0);
 
 		timeout = jiffies + HZ;
 		while (time_before(jiffies, timeout))
@@ -96,6 +95,7 @@ static int vexpress_reset_probe(struct platform_device *pdev)
 	enum vexpress_reset_func func;
 	const struct of_device_id *match =
 			of_match_device(vexpress_reset_of_match, &pdev->dev);
+	struct regmap *regmap;
 
 	if (match)
 		func = (enum vexpress_reset_func)match->data;
@@ -120,6 +120,11 @@ static int vexpress_reset_probe(struct platform_device *pdev)
 		break;
 	};
 
+	regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+	dev_set_drvdata(&pdev->dev, regmap);
+
 	return 0;
 }
 
diff --git a/drivers/regulator/vexpress.c b/drivers/regulator/vexpress.c
index f3ae28a..2863428 100644
--- a/drivers/regulator/vexpress.c
+++ b/drivers/regulator/vexpress.c
@@ -26,14 +26,14 @@
 struct vexpress_regulator {
 	struct regulator_desc desc;
 	struct regulator_dev *regdev;
-	struct vexpress_config_func *func;
+	struct regmap *regmap;
 };
 
 static int vexpress_regulator_get_voltage(struct regulator_dev *regdev)
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 	u32 uV;
-	int err = vexpress_config_read(reg->func, 0, &uV);
+	int err = regmap_read(reg->regmap, 0, &uV);
 
 	return err ? err : uV;
 }
@@ -43,7 +43,7 @@ static int vexpress_regulator_set_voltage(struct regulator_dev *regdev,
 {
 	struct vexpress_regulator *reg = rdev_get_drvdata(regdev);
 
-	return vexpress_config_write(reg->func, 0, min_uV);
+	return regmap_write(reg->regmap, 0, min_uV);
 }
 
 static struct regulator_ops vexpress_regulator_ops_ro = {
@@ -57,22 +57,17 @@ static struct regulator_ops vexpress_regulator_ops = {
 
 static int vexpress_regulator_probe(struct platform_device *pdev)
 {
-	int err;
 	struct vexpress_regulator *reg;
 	struct regulator_init_data *init_data;
 	struct regulator_config config = { };
 
 	reg = devm_kzalloc(&pdev->dev, sizeof(*reg), GFP_KERNEL);
-	if (!reg) {
-		err = -ENOMEM;
-		goto error_kzalloc;
-	}
+	if (!reg)
+		return -ENOMEM;
 
-	reg->func = vexpress_config_func_get_by_dev(&pdev->dev);
-	if (!reg->func) {
-		err = -ENXIO;
-		goto error_get_func;
-	}
+	reg->regmap = devm_regmap_init_vexpress_config(&pdev->dev);
+	if (IS_ERR(reg->regmap))
+		return PTR_ERR(reg->regmap);
 
 	reg->desc.name = dev_name(&pdev->dev);
 	reg->desc.type = REGULATOR_VOLTAGE;
@@ -80,10 +75,8 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	reg->desc.continuous_voltage_range = true;
 
 	init_data = of_get_regulator_init_data(&pdev->dev, pdev->dev.of_node);
-	if (!init_data) {
-		err = -EINVAL;
-		goto error_get_regulator_init_data;
-	}
+	if (!init_data)
+		return -EINVAL;
 
 	init_data->constraints.apply_uV = 0;
 	if (init_data->constraints.min_uV && init_data->constraints.max_uV)
@@ -97,30 +90,12 @@ static int vexpress_regulator_probe(struct platform_device *pdev)
 	config.of_node = pdev->dev.of_node;
 
 	reg->regdev = devm_regulator_register(&pdev->dev, &reg->desc, &config);
-	if (IS_ERR(reg->regdev)) {
-		err = PTR_ERR(reg->regdev);
-		goto error_regulator_register;
-	}
+	if (IS_ERR(reg->regdev))
+		return PTR_ERR(reg->regdev);
 
 	platform_set_drvdata(pdev, reg);
 
 	return 0;
-
-error_regulator_register:
-error_get_regulator_init_data:
-	vexpress_config_func_put(reg->func);
-error_get_func:
-error_kzalloc:
-	return err;
-}
-
-static int vexpress_regulator_remove(struct platform_device *pdev)
-{
-	struct vexpress_regulator *reg = platform_get_drvdata(pdev);
-
-	vexpress_config_func_put(reg->func);
-
-	return 0;
 }
 
 static struct of_device_id vexpress_regulator_of_match[] = {
@@ -130,7 +105,6 @@ static struct of_device_id vexpress_regulator_of_match[] = {
 
 static struct platform_driver vexpress_regulator_driver = {
 	.probe = vexpress_regulator_probe,
-	.remove = vexpress_regulator_remove,
 	.driver	= {
 		.name = DRVNAME,
 		.owner = THIS_MODULE,
diff --git a/include/linux/vexpress.h b/include/linux/vexpress.h
index 617c01b..ae4d2a1 100644
--- a/include/linux/vexpress.h
+++ b/include/linux/vexpress.h
@@ -15,16 +15,15 @@
 #define _LINUX_VEXPRESS_H
 
 #include <linux/device.h>
+#include <linux/platform_device.h>
 #include <linux/reboot.h>
+#include <linux/regmap.h>
 
 #define VEXPRESS_SITE_MB		0
 #define VEXPRESS_SITE_DB1		1
 #define VEXPRESS_SITE_DB2		2
 #define VEXPRESS_SITE_MASTER		0xf
 
-#define VEXPRESS_CONFIG_STATUS_DONE	0
-#define VEXPRESS_CONFIG_STATUS_WAIT	1
-
 #define VEXPRESS_GPIO_MMC_CARDIN	0
 #define VEXPRESS_GPIO_MMC_WPROT		1
 #define VEXPRESS_GPIO_FLASH_WPn		2
@@ -44,63 +43,30 @@
 	.flags = IORESOURCE_BUS,	\
 }
 
-/* Config bridge API */
+/* Config infrastructure */
 
-/**
- * struct vexpress_config_bridge_info - description of the platform
- * configuration infrastructure bridge.
- *
- * @name:	Bridge name
- *
- * @func_get:	Obtains pointer to a configuration function for a given
- *		device or a Device Tree node, to be used with @func_put
- *		and @func_exec. The node pointer should take precedence
- *		over device pointer when both are passed.
- *
- * @func_put:	Tells the bridge that the function will not be used any
- *		more, so all allocated resources can be released.
- *
- * @func_exec:	Executes a configuration function read or write operation.
- *		The offset selects a 32 bit word of the value accessed.
- *		Must return VEXPRESS_CONFIG_STATUS_DONE when operation
- *		is finished immediately, VEXPRESS_CONFIG_STATUS_WAIT when
- *		will be completed in some time or negative value in case
- *		of error.
- */
-struct vexpress_config_bridge_info {
-	const char *name;
-	void *(*func_get)(struct device *dev, struct device_node *node);
-	void (*func_put)(void *func);
-	int (*func_exec)(void *func, int offset, bool write, u32 *data);
-};
+void vexpress_config_set_master(u32 site);
+u32 vexpress_config_get_master(void);
 
-struct vexpress_config_bridge;
+void vexpress_config_lock(void *arg);
+void vexpress_config_unlock(void *arg);
 
-struct vexpress_config_bridge *vexpress_config_bridge_register(
-		struct device_node *node,
-		struct vexpress_config_bridge_info *info);
-void vexpress_config_bridge_unregister(struct vexpress_config_bridge *bridge);
+int vexpress_config_get_topo(struct device_node *node, u32 *site,
+		u32 *position, u32 *dcc);
 
-void vexpress_config_complete(struct vexpress_config_bridge *bridge,
-		int status);
+/* Config bridge API */
 
-/* Config function API */
+struct vexpress_config_bridge_ops {
+	struct regmap * (*regmap_init)(struct device *dev, void *context);
+	void (*regmap_exit)(struct regmap *regmap, void *context);
+};
 
-struct vexpress_config_func;
+struct device *vexpress_config_bridge_register(struct device *parent,
+		struct vexpress_config_bridge_ops *ops, void *context);
 
-struct vexpress_config_func *__vexpress_config_func_get(struct device *dev,
-		struct device_node *node);
-#define vexpress_config_func_get_by_dev(dev) \
-		__vexpress_config_func_get(dev, NULL)
-#define vexpress_config_func_get_by_node(node) \
-		__vexpress_config_func_get(NULL, node)
-void vexpress_config_func_put(struct vexpress_config_func *func);
+/* Config regmap API */
 
-/* Both may sleep! */
-int vexpress_config_read(struct vexpress_config_func *func, int offset,
-		u32 *data);
-int vexpress_config_write(struct vexpress_config_func *func, int offset,
-		u32 data);
+struct regmap *devm_regmap_init_vexpress_config(struct device *dev);
 
 /* Platform control */
 
@@ -109,19 +75,12 @@ u32 vexpress_get_hbi(int site);
 void *vexpress_get_24mhz_clock_base(void);
 void vexpress_flags_set(u32 data);
 
-#define vexpress_get_site_by_node(node) __vexpress_get_site(NULL, node)
-#define vexpress_get_site_by_dev(dev) __vexpress_get_site(dev, NULL)
-unsigned __vexpress_get_site(struct device *dev, struct device_node *node);
-
 void vexpress_sysreg_early_init(void __iomem *base);
 void vexpress_sysreg_of_early_init(void);
+int vexpress_sysreg_config_device_register(struct platform_device *pdev);
 
 /* Clocks */
 
-struct clk *vexpress_osc_setup(struct device *dev);
-void vexpress_osc_of_setup(struct device_node *node);
-
 void vexpress_clk_init(void __iomem *sp810_base);
-void vexpress_clk_of_init(void);
 
 #endif
