ARM: mvebu: prepare PMSU code for cpuidle and Armada 375/38x SMP

Hello,

The PMSU (Power Management Service Unit) code in arch/arm/mach-mvebu/pmsu.c needs several changes and improvements to properly support cpuidle on Armada 370/XP and SMP on Armada 375/38x.

The problem with the current implementations are:

* The base address and length of the PMSU registers that we had originally chosen are wrong: the base starts 0x100 too late, and therefore we miss some PMSU registers that are important for the cpuidle operation. In other words, the PMSU reg specification was <0x22100 0x400>, but it needs to be <0x22000 0x1000>.

This patch set solves this by introducing a new DT binding for the PMSU that assumes the reg property will be <0x22000 0x1000>. In addition to this, backward compatibility with the old DT property is preserved, by doing some calculation inside pmsu.c to re-adjust <0x22100 0x400> to the now correct <0x22000 0x1000>. The addition of the new DT binding will allow, one day, to get rid of the old deprecated binding.

* The PMSU Device Tree node was used to describe both the PMSU registers themselves, but also the CPU reset registers. But this was a mistake: on Armada 375, we have the same CPU reset registers to start the secondary processors, but we don't have a PMSU. Therefore, the PMSU should be split in two parts: one part really doing the PMSU itself, and one part taking care of the CPU reset registers.

This is achieved by adding a cpu-reset.c driver in arch/arm/mach-mvebu/, which uses a new DT node with a compatible property named "marvell,armada-<chip>-cpu-reset".

This driver is backward compatible with old DT, and therefore if no "marvell,armada-<chip>-cpu-reset" compatible string is found, it will look for a PMSU compatible string, and use the second reg specifier of it to get the location of the CPU register registers.

Note that we have considered using the "reset framework" to support this mechanism. It was working perfectly fine, except that the Device Tree backward compatibility requirement makes it way too
complicated: when we're booted with an old Device Tree, we would have to artifically create a node for the CPU reset device, and then for each CPU, add a "reset" property that contains a phandle to the CPU reset device node we created. While this is therorically possible, the existing OF API in the kernel does not easily allow the allocation of phandle numbers.

Moreover, the "reset framework" is mainly useful for device drivers, so that they don't depend on a SoC-specific set of functions to manage the reset line of their devices. However, in our case, it's the SoC-specific SMP code that calls into SoC-specific CPU reset code, so using the reset framework doesn't bring a huge benefit.

* Setting the boot address of the secondary processor is not only useful for SMP, but also for cpuidle, so this functionality in pmsu.c is split into a separate function, so that it can be re-used by the cpuidle code. It is also useful for the Armada 38x SMP, which also uses the PMSU to set the secondary processor PMSU.

The patch series was successfully tested by using both the old and new Device Trees: the backward compatibility logic has been tested.

Details of the patches:

* PATCH 1 introduces the cpu-reset.c driver, with its normal DT binding.

* PATCH 2 changes the pmsu.c driver to remove the CPU reset logic, and rely on the cpu-reset.c driver. It also introduces the backward compatibility code in the cpu-reset.c driver so that things continue to work normally.

* PATCH 3 improves the pmsu.c driver to properly request its register area instead of using just of_iomap().

* PATCH 4 introduces a new DT binding for the PMSU, which expects a larger register area. It also adds the necessary backward compatibility code to properly support the old DT binding by adjusting manually the base/size of the registers within the driver.

* PATCH 5 switches the Armada 370/XP device trees to use the new CPU reset and PMSU bindings.

* PATCH 6 splits into a separate function the code that sets the boot address of secondary CPUs.

In terms of dependencies, we will have 4 interconnected patch series:
Armada 375/38x coherency support, this PMSU rework series, the cpuidle series, and the Armada 375/38x SMP support. The relations are as follows

  ---------------        ---------------------
  | pmsu-rework |        | 375/38x coherency |
  ---------------        ---------------------
        ||   ||                 ||
	||   \\____________     ||
	\/                \/    \/
  ---------------        ---------------
  |   cpuidle   |        | 375/38x SMP |
  ---------------        ---------------

In other words: cpuidle needs pmsu-rework, and 375/38x SMP needs pmsu-rework and 375/38x coherency. Gregory will soon resend a new version of his cpuidle patch series based on top of these PMSU changes, and I will soon post the 375/38x SMP support.

Best regards,

Thomas

Gregory CLEMENT (3):
ARM: mvebu: extend the PMSU registers
ARM: mvebu: switch to the new PMSU binding in Armada 370/XP Device
Tree
ARM: mvebu: use a separate function to set the boot address of CPUs

Thomas Petazzoni (3):
ARM: mvebu: introduce CPU reset code
ARM: mvebu: start using the CPU reset driver
ARM: mvebu: improve PMSU driver to request its resource

 .../devicetree/bindings/arm/armada-370-xp-pmsu.txt |  14 ++-
 .../devicetree/bindings/arm/armada-cpu-reset.txt   |  17 ++++
 arch/arm/boot/dts/armada-370-xp.dtsi               |   5 +
 arch/arm/boot/dts/armada-370.dtsi                  |   5 +
 arch/arm/boot/dts/armada-xp.dtsi                   |   6 +-
 arch/arm/mach-mvebu/Makefile                       |   2 +-
 arch/arm/mach-mvebu/armada-370-xp.h                |   3 +-
 arch/arm/mach-mvebu/common.h                       |   1 +
 arch/arm/mach-mvebu/cpu-reset.c                    | 109 +++++++++++++++++++++
 arch/arm/mach-mvebu/pmsu.c                         |  79 +++++++++++----
 10 files changed, 209 insertions(+), 32 deletions(-)
 create mode 100644 Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
 create mode 100644 arch/arm/mach-mvebu/cpu-reset.c

1.8.3.2

The Armada 370 and Armada XP have registers that allow to reset the CPUs, which is particularly useful to take the secondary CPUs out of reset in the context of the SMP support.

Unfortunately, an implementation mistake was originally made and the support for these registers was integrated into the PMSU driver, which is in fact completely unrelated. And it turns out that the Armada 375 has the same CPU reset registers, but does not have the PMSU registers.

Therefore, this commit creates a small CPU reset driver. All it does is provide a simple mvebu_cpu_reset_deassert() function that the SMP support code can call to take secondary CPUs out of reset. As of this commit, the driver isn't being used, it will be used through changes in the following commits.

Note that we initially planned to use the 'reset controller' framework, but it requires the addition of "resets" properties in the Device Tree, which are causing too many problems if we want to keep the Device Tree backward compatibility. Moreover, the 'reset controller' framework is mainly useful when a device driver needs to request a reset of its device from a separate reset controller. In our case, the CPU reset handling and the SMP core code are both located in arch/arm/mach-mvebu/ and are tightly linked together, so there's no real benefit in going through a separate framework.

---
 .../devicetree/bindings/arm/armada-cpu-reset.txt   | 17 +++++
 arch/arm/mach-mvebu/Makefile                       |  2 +-
 arch/arm/mach-mvebu/armada-370-xp.h                |  3 +-
 arch/arm/mach-mvebu/common.h                       |  1 +
 arch/arm/mach-mvebu/cpu-reset.c                    | 89 ++++++++++++++++++++++
 5 files changed, 110 insertions(+), 2 deletions(-)
 create mode 100644 Documentation/devicetree/bindings/arm/armada-cpu-reset.txt
 create mode 100644 arch/arm/mach-mvebu/cpu-reset.c

new file mode 100644
index 0000000..384cbf8
@@ -0,0 +1,17 @@
+Marvell Armada CPU reset controller
+===================================
+
+Required properties:
+
+- compatible: Should be "marvell,armada-<chip>-cpu-reset". Supported
+  values are:
+    marvell,armada-370-cpu-reset
+    marvell,armada-xp-cpu-reset
+
+- reg: should be register base and length as documented in the
+  datasheet for the CPU reset registers
+
+cpurst: cpurst at 20800 {
+       compatible = "marvell,armada-xp-cpu-reset";
+       reg = <0x20800 0x20>;
+};

index 2c1db29..0e0eab5 100644
@@ -3,7 +3,7 @@ ccflags-$(CONFIG_ARCH_MULTIPLATFORM) := -I$(srctree)/$(src)/include \
 
 AFLAGS_coherency_ll.o		:= -Wa,-march=armv7-a
 
-obj-y				 += system-controller.o mvebu-soc-id.o
+obj-y				 += system-controller.o mvebu-soc-id.o cpu-reset.o
 obj-$(CONFIG_MACH_MVEBU_V7)      += board-v7.o
 obj-$(CONFIG_ARCH_MVEBU)	 += coherency.o coherency_ll.o pmsu.o
 obj-$(CONFIG_SMP)                += platsmp.o headsmp.o

index 237c86b..991ab32 100644
@@ -18,7 +18,8 @@
 #ifdef CONFIG_SMP
 #include <linux/cpumask.h>
 
-#define ARMADA_XP_MAX_CPUS 4
+#define ARMADA_370_MAX_CPUS 1
+#define ARMADA_XP_MAX_CPUS  4
 
 void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq);
 void armada_xp_mpic_smp_cpu_init(void);

index 55449c4..cfb129b 100644
@@ -18,6 +18,7 @@
 #include <linux/reboot.h>
 
 void mvebu_restart(enum reboot_mode mode, const char *cmd);
+int mvebu_cpu_reset_deassert(int cpu);
 
 void armada_xp_cpu_die(unsigned int cpu);
 

1.8.3.2

Hi Thomas,

[...]

Gregory Clement, Free Electrons
Kernel, drivers, real-time and embedded Linux
development, consulting, training and support.
http://free-electrons.com

Dear Gregory CLEMENT,

I found it weird to have a compatible string marked as supported in the DT binding document, but not actually supported by the kernel. I know it's possible, but I found it odd, especially considering the fact that mapping these registers, even if unused, isn't costing much.

I don't have a strong opinion on this, so if others voice in this way, I'll change it.

Thomas

Thomas Petazzoni, CTO, Free Electrons
Embedded Linux, Kernel and Android engineering
http://free-electrons.com

Me neither I don't have a strong opinion on ths, so with our without this line, you can add my:

and also:

by the way

Gregory Clement, Free Electrons
Kernel, drivers, real-time and embedded Linux
development, consulting, training and support.
http://free-electrons.com

[...]

[...]

[...]

Dear Sebastian Hesselbarth,

Just a safety check to ensure we're not writing outside of the CPU reset registers that have been mapped. I more or less tend to believe that it's nicer to have APIs that do some minimal amount of checking and return an error, rather than having things completely blow up when arguments are wrong.

But I certainly understand that this is a very subjective view, and I wouldn't mind changing the code according to your suggestion if other people agree with it.

Thomas

Thomas Petazzoni, CTO, Free Electrons
Embedded Linux, Kernel and Android engineering
http://free-electrons.com

This commit changes the PMSU driver to no longer map itself the CPU reset registers, and instead call into the CPU reset driver to deassert the secondary CPUs for SMP booting.

In order to provide Device Tree backward compatibility, the CPU reset driver is extended to not only support its official compatible strings "marvell,armada-370-cpu-reset" and "marvell,armada-xp-cpu-reset", but to also look at the PMSU compatible string "marvell,armada-370-xp-pmsu" to find the CPU reset registers address. This allows old Device Tree to work correctly with newer kernel versions. Therefore, the CPU reset driver implements the following logic:

* If one of the normal compatible strings "marvell,armada-<chip>-cpu-reset" is found, then we map its first memory resource as the CPU reset registers.

* Otherwise, if none of the normal compatible strings have been found, we look for the "marvell,armada-370-xp-pmsu" compatible string, and we map the second memory as the CPU reset registers.

---
 arch/arm/mach-mvebu/cpu-reset.c | 56 ++++++++++++++++++++++++++++-------------
 arch/arm/mach-mvebu/pmsu.c      | 20 +++++++--------
 2 files changed, 48 insertions(+), 28 deletions(-)

index 2819887..e3821b1 100644
@@ -46,43 +46,63 @@ int mvebu_cpu_reset_deassert(int cpu)
 	return 0;
 }
 
-static int __init mvebu_cpu_reset_init(void)
+static int mvebu_cpu_reset_map(struct device_node *np, int res_idx)
 {
-	struct device_node *np;
-	const struct of_device_id *match;
 	struct resource res;
-	int ret = 0;
 
-	np = of_find_matching_node_and_match(NULL, of_cpu_reset_table,
-					     &match);
-	if (!np)
-		return 0;
-
-	if (of_address_to_resource(np, 0, &res)) {
+	if (of_address_to_resource(np, res_idx, &res)) {
 		pr_err("unable to get resource\n");
-		ret = -ENOENT;
-		goto out;
+		return -ENOENT;
 	}
 
 	if (!request_mem_region(res.start, resource_size(&res),
 				np->full_name)) {
 		pr_err("unable to request region\n");
-		ret = -EBUSY;
-		goto out;
+		return -EBUSY;
 	}
 
 	cpu_reset_base = ioremap(res.start, resource_size(&res));
 	if (!cpu_reset_base) {
 		pr_err("unable to map registers\n");
 		release_mem_region(res.start, resource_size(&res));
-		ret = -ENOMEM;
-		goto out;
+		return -ENOMEM;
 	}
 
-	ncpus = (int) match->data;
+	return 0;
+}
 
-out:
+int __init mvebu_cpu_reset_init(void)
+{
+	struct device_node *np;
+	const struct of_device_id *match;
+	int res_idx;
+	int ret;
+
+	np = of_find_matching_node_and_match(NULL, of_cpu_reset_table,
+					     &match);
+	if (np) {
+		res_idx = 0;
+		ncpus = (int) match->data;
+	} else {
+		/*
+		 * This code is kept for backward compatibility with
+		 * old Device Trees.
+		 */
+		np = of_find_compatible_node(NULL, NULL, "marvell,armada-370-xp-pmsu");
+		if (np) {
+			pr_warn(FW_WARN "deprecated pmsu binding\n");
+			res_idx = 1;
+			ncpus = ARMADA_XP_MAX_CPUS;
+		}
+	}
+
+	/* No reset node found */
+	if (!np)
+		return 0;
+
+	ret = mvebu_cpu_reset_map(np, res_idx);
 	of_node_put(np);
+
 	return ret;
 }
 

index d71ef53..1807639 100644
@@ -21,14 +21,14 @@
 #include <linux/of_address.h>
 #include <linux/io.h>
 #include <linux/smp.h>
+#include <linux/resource.h>
 #include <asm/smp_plat.h>
+#include "common.h"
 #include "pmsu.h"
 
 static void __iomem *pmsu_mp_base;
-static void __iomem *pmsu_reset_base;
 
 #define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	((cpu * 0x100) + 0x24)
-#define PMSU_RESET_CTL_OFFSET(cpu)		(cpu * 0x8)
 
 static struct of_device_id of_pmsu_table[] = {
 	{.compatible = "marvell,armada-370-xp-pmsu"},

@@ -38,11 +38,11 @@ static struct of_device_id of_pmsu_table[] = {
 #ifdef CONFIG_SMP
 int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
 {
-	int reg, hw_cpu;
+	int hw_cpu, ret;
 
-	if (!pmsu_mp_base || !pmsu_reset_base) {
+	if (!pmsu_mp_base) {
 		pr_warn("Can't boot CPU. PMSU is uninitialized\n");
-		return 1;
+		return -ENODEV;
 	}
 
 	hw_cpu = cpu_logical_map(cpu_id);

@@ -50,10 +50,11 @@ int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
 	writel(virt_to_phys(boot_addr), pmsu_mp_base +
 			PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
 
-	/* Release CPU from reset by clearing reset bit*/
-	reg = readl(pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
-	reg &= (~0x1);
-	writel(reg, pmsu_reset_base + PMSU_RESET_CTL_OFFSET(hw_cpu));
+	ret = mvebu_cpu_reset_deassert(hw_cpu);
+	if (ret) {
+		pr_warn("unable to boot CPU: %d\n", ret);
+		return ret;
+	}
 
 	return 0;
 }

@@ -67,7 +68,6 @@ static int __init armada_370_xp_pmsu_init(void)
 	if (np) {
 		pr_info("Initializing Power Management Service Unit\n");
 		pmsu_mp_base = of_iomap(np, 0);
-		pmsu_reset_base = of_iomap(np, 1);
 		of_node_put(np);
 	}
 

1.8.3.2

Until now, the PMSU driver was using of_iomap() to map its registers, but of_iomap() doesn't call request_mem_region(). This commit fixes the memory mapping code of the PMSU to do so, which will also be useful for a later commit since we will need to adjust the resource base address and size for Device Tree backward compatibility.

---
 arch/arm/mach-mvebu/pmsu.c | 36 +++++++++++++++++++++++++++++++-----
 1 file changed, 31 insertions(+), 5 deletions(-)

index 1807639..b337fe5 100644
@@ -16,6 +16,8 @@
  * other SOC units
  */
 
+#define pr_fmt(fmt) "mvebu-pmsu: " fmt
+
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/of_address.h>

@@ -63,15 +65,39 @@ int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
 static int __init armada_370_xp_pmsu_init(void)
 {
 	struct device_node *np;
+	struct resource res;
+	int ret = 0;
 
 	np = of_find_matching_node(NULL, of_pmsu_table);
-	if (np) {
-		pr_info("Initializing Power Management Service Unit\n");
-		pmsu_mp_base = of_iomap(np, 0);
-		of_node_put(np);
+	if (!np)
+		return 0;
+
+	pr_info("Initializing Power Management Service Unit\n");
+
+	if (of_address_to_resource(np, 0, &res)) {
+		pr_err("unable to get resource\n");
+		ret = -ENOENT;
+		goto out;
 	}
 
-	return 0;
+	if (!request_mem_region(res.start, resource_size(&res),
+				np->full_name)) {
+		pr_err("unable to request region\n");
+		ret = -EBUSY;
+		goto out;
+	}
+
+	pmsu_mp_base = ioremap(res.start, resource_size(&res));
+	if (!pmsu_mp_base) {
+		pr_err("unable to map registers\n");
+		release_mem_region(res.start, resource_size(&res));
+		ret = -ENOMEM;
+		goto out;
+	}
+
+ out:
+	of_node_put(np);
+	return ret;
 }
 
 early_initcall(armada_370_xp_pmsu_init);

1.8.3.2

From: Gregory CLEMENT <[email protected]>

The initial binding for PMSU was wrong, as it didn't take into account all the registers from the PMSU and moreover it referred to the CPU reset registers which are not part of PMSU.

The Power Management Unit Service block also controls the Coherency Fabric subsystem. These registers are needed for the CPU idle implementation for the Armada 370/XP, it allows to enter a deep CPU idle state where the Coherency Fabric and the L2 cache are powered down.

This commit adds support for a new compatible for the PMSU node which includes the registers related to the coherency fabric. It also keeps compatibility with the old compatible string.

---
 .../devicetree/bindings/arm/armada-370-xp-pmsu.txt         | 14 ++++++--------
 arch/arm/mach-mvebu/pmsu.c                                 | 14 ++++++++++++--
 2 files changed, 18 insertions(+), 10 deletions(-)

index 926b4d6..9761887 100644
@@ -4,17 +4,15 @@ Available on Marvell SOCs: Armada 370 and Armada XP
 
 Required properties:
 
-- compatible: "marvell,armada-370-xp-pmsu"
+- compatible: should be "marvell,armada-370-pmsu", whereas
+  "marvell,armada-370-xp-pmsu" is deprecated and will be removed
 
-- reg: Should contain PMSU registers location and length. First pair
-  for the per-CPU SW Reset Control registers, second pair for the
-  Power Management Service Unit.
+- reg: Should contain PMSU registers location and length.
 
 Example:
 
-armada-370-xp-pmsu at d0022000 {
-	compatible = "marvell,armada-370-xp-pmsu";
-	reg = <0xd0022100 0x430>,
-	      <0xd0020800 0x20>;
+armada-370-xp-pmsu at 22000 {
+	compatible = "marvell,armada-370-pmsu";
+	reg = <0x22000 0x1000>;
 };
 

index b337fe5..4ae3ea1 100644
@@ -30,10 +30,14 @@
 
 static void __iomem *pmsu_mp_base;
 
-#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	((cpu * 0x100) + 0x24)
+#define PMSU_BASE_OFFSET    0x100
+#define PMSU_REG_SIZE	    0x1000
+
+#define PMSU_BOOT_ADDR_REDIRECT_OFFSET(cpu)	((cpu * 0x100) + 0x124)
 
 static struct of_device_id of_pmsu_table[] = {
-	{.compatible = "marvell,armada-370-xp-pmsu"},
+	{ .compatible = "marvell,armada-370-pmsu", },
+	{ .compatible = "marvell,armada-370-xp-pmsu", },
 	{ /* end of list */ },
 };
 

@@ -80,6 +84,12 @@ static int __init armada_370_xp_pmsu_init(void)
 		goto out;
 	}
 
+	if (of_device_is_compatible(np, "marvell,armada-370-xp-pmsu")) {
+		pr_warn(FW_WARN "deprecated pmsu binding\n");
+		res.start = res.start - PMSU_BASE_OFFSET;
+		res.end = res.start + PMSU_REG_SIZE - 1;
+	}
+
 	if (!request_mem_region(res.start, resource_size(&res),
 				np->full_name)) {
 		pr_err("unable to request region\n");

1.8.3.2

From: Gregory CLEMENT <[email protected]>

Following the introduction of the new PMSU Device Tree binding, as well as the separate CPU reset binding, this commit switches the Armada 370 and Armada XP Device Trees to use them.

The PMSU node is moved from the Armada XP specific armada-xp.dtsi to the common Armada 370/XP armada-370-xp.dtsi because the PMSU is in fact available at the same location on both SOCs.

The CPU reset node is then added on both Armada 370 and Armada XP, with a different compatible string. On Armada 370, the CPU reset driver is not really needed as Armada 370 is single core and the only use of the CPU reset driver is to boot secondary processors, but it still makes sense to have this CPU reset register described in the Device Tree.

---
 arch/arm/boot/dts/armada-370-xp.dtsi | 5 +++++
 arch/arm/boot/dts/armada-370.dtsi    | 5 +++++
 arch/arm/boot/dts/armada-xp.dtsi     | 6 +++---
 3 files changed, 13 insertions(+), 3 deletions(-)

index 74b5964..d286d67 100644
@@ -199,6 +199,11 @@
 				interrupts = <37>, <38>, <39>, <40>, <5>, <6>;
 			};
 
+			armada-370-pmsu at 22000 {
+				compatible = "marvell,armada-370-pmsu";
+				reg = <0x22000 0x1000>;
+			};
+
 			usb at 50000 {
 				compatible = "marvell,orion-ehci";
 				reg = <0x50000 0x500>;

index 0d8530c..4636a22 100644
@@ -196,6 +196,11 @@
 				clocks = <&coreclk 2>;
 			};
 
+			cpurst at 20800 {
+				compatible = "marvell,armada-370-cpu-reset";
+				reg = <0x20800 0x8>;
+			};
+
 			usb at 50000 {
 				clocks = <&coreclk 0>;
 			};

index b8b84a2..4d36a12 100644
@@ -111,9 +111,9 @@
 				clock-names = "nbclk", "fixed";
 			};
 
-			armada-370-xp-pmsu at 22000 {
-				compatible = "marvell,armada-370-xp-pmsu";
-				reg = <0x22100 0x400>, <0x20800 0x20>;
+			cpurst at 20800 {
+				compatible = "marvell,armada-xp-cpu-reset";
+				reg = <0x20800 0x20>;
 			};
 
 			eth2: ethernet at 30000 {

1.8.3.2

From: Gregory CLEMENT <[email protected]>

Setting the start (or boot) address of a CPU is no more used only during SMP bring up on Armada 370/XP, but it will also be used by the CPU idle function of Armada XP, and by the Armada 38x SMP support.

Therefore this commit creates a separate PMSU function to set the boot address of a CPU with the PMSU.

---
 arch/arm/mach-mvebu/pmsu.c | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

index 4ae3ea1..8361281 100644
@@ -41,6 +41,12 @@ static struct of_device_id of_pmsu_table[] = {
 	{ /* end of list */ },
 };
 
+static void mvebu_pmsu_set_cpu_boot_addr(int hw_cpu, void *boot_addr)
+{
+	writel(virt_to_phys(boot_addr), pmsu_mp_base +
+		PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+}
+
 #ifdef CONFIG_SMP
 int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
 {

@@ -53,8 +59,7 @@ int armada_xp_boot_cpu(unsigned int cpu_id, void *boot_addr)
 
 	hw_cpu = cpu_logical_map(cpu_id);
 
-	writel(virt_to_phys(boot_addr), pmsu_mp_base +
-			PMSU_BOOT_ADDR_REDIRECT_OFFSET(hw_cpu));
+	mvebu_pmsu_set_cpu_boot_addr(hw_cpu, boot_addr);
 
 	ret = mvebu_cpu_reset_deassert(hw_cpu);
 	if (ret) {

1.8.3.2