arm: ls1: add error handling case

* Add iomap() error handling case and initialize rcpm_base when system
booting up.
* Only fill secondary_pre_boot_entry with value of SCRATCHRW1 register
when system boot up, do not need to fill it each time doing cpu-hotplug.

Signed-off-by: Zhuoyu Zhang <Zhuoyu.Zhang@freescale.com>
Change-Id: If672bd18074eea6db75c9273a7656aff983ffd9a
Reviewed-on: http://git.am.freescale.net:8181/28866
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Chenhui Zhao <chenhui.zhao@freescale.com>
Reviewed-by: Zhengxiong Jin <Jason.Jin@freescale.com>

2 files changed, 36 insertions, 27 deletions

diff --git a/arch/arm/mach-imx/platsmp.c b/arch/arm/mach-imx/platsmp.c
index 22f9814..140c98c 100644
--- a/arch/arm/mach-imx/platsmp.c
+++ b/arch/arm/mach-imx/platsmp.c
@@ -19,6 +19,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/delay.h>
+#include <asm/smp_plat.h>
 
 #include "common.h"
 #include "hardware.h"
@@ -36,6 +37,8 @@
 #define	DCSR_RCPM2_DEBUG1	0x400
 #define	DCSR_RCPM2_DEBUG2	0x414
 
+#define	CCSR_TWAITSR0		0x04C
+
 #define	STRIDE_4B		4
 
 u32 g_diag_reg;
@@ -43,6 +46,7 @@ static void __iomem *scu_base;
 static void __iomem *dcfg_base;
 static void __iomem *scfg_base;
 static void __iomem *dcsr_rcpm2_base;
+static void __iomem *rcpm_base;
 static u32 secondary_pre_boot_entry;
 
 static struct map_desc scu_io_desc __initdata = {
@@ -134,7 +138,7 @@ static int ls1021a_secondary_iomap(void)
 	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-dcfg");
 	if (!np) {
 		pr_err("%s: failed to find dcfg node.\n", __func__);
-		ret = -EINVAL;
+		ret = -ENODEV;
 		goto dcfg_err;
 	}
 
@@ -149,7 +153,7 @@ static int ls1021a_secondary_iomap(void)
 	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-scfg");
 	if (!np) {
 		pr_err("%s: failed to find scfg node.\n", __func__);
-		ret = -EINVAL;
+		ret = -ENODEV;
 		goto scfg_err;
 	}
 
@@ -164,7 +168,7 @@ static int ls1021a_secondary_iomap(void)
 	np = of_find_compatible_node(NULL, NULL, "fsl,ls1021a-dcsr-rcpm");
 	if (!np) {
 		pr_err("%s: failed to find dcsr node.\n", __func__);
-		ret = -EINVAL;
+		ret = -ENODEV;
 		goto dcsr_err;
 	}
 
@@ -176,8 +180,25 @@ static int ls1021a_secondary_iomap(void)
 		goto dcsr_err;
 	}
 
+	np = of_find_compatible_node(NULL, NULL, "fsl,qoriq-rcpm-2.1");
+	if (!np) {
+		pr_err("%s(): failed to find the RCPM node.\n", __func__);
+		ret = -ENODEV;
+		goto rcpm_err;
+	}
+
+	rcpm_base = of_iomap(np, 0);
+	of_node_put(np);
+	if (!rcpm_base) {
+		pr_err("%s: failed to map rcpm.\n", __func__);
+		ret = -ENOMEM;
+		goto rcpm_err;
+	}
+
 	return 0;
 
+rcpm_err:
+	iounmap(dcsr_rcpm2_base);
 dcsr_err:
 	iounmap(scfg_base);
 scfg_err:
@@ -186,13 +207,18 @@ dcfg_err:
 	return ret;
 }
 
+u32 ls1_get_cpu_arg(int cpu)
+{
+	BUG_ON(!rcpm_base);
+
+	cpu = cpu_logical_map(cpu);
+	return ioread32be(rcpm_base + CCSR_TWAITSR0) & (1 << cpu);
+}
+
 void ls1021a_set_secondary_entry(void)
 {
 	unsigned long paddr;
 
-	secondary_pre_boot_entry = readl_relaxed(dcfg_base +
-						DCFG_CCSR_SCRATCHRW1);
-
 	if (dcfg_base) {
 		paddr = virt_to_phys(secondary_startup);
 		writel_relaxed(cpu_to_be32(paddr),
@@ -259,6 +285,10 @@ static int ls1021a_boot_secondary(unsigned int cpu, struct task_struct *idle)
 static void __init ls1021a_smp_prepare_cpus(unsigned int max_cpus)
 {
 	ls1021a_secondary_iomap();
+
+	secondary_pre_boot_entry = readl_relaxed(dcfg_base +
+						DCFG_CCSR_SCRATCHRW1);
+
 	ls1021a_set_secondary_entry();
 }
 
diff --git a/arch/arm/mach-imx/src.c b/arch/arm/mach-imx/src.c
index e73cbbe..10a6b1a 100644
--- a/arch/arm/mach-imx/src.c
+++ b/arch/arm/mach-imx/src.c
@@ -30,8 +30,6 @@
 #define BP_SRC_SCR_CORE1_RST		14
 #define BP_SRC_SCR_CORE1_ENABLE		22
 
-#define CCSR_TWAITSR0         0x04C
-
 static void __iomem *src_base;
 static DEFINE_SPINLOCK(scr_lock);
 
@@ -116,25 +114,6 @@ void imx_set_cpu_arg(int cpu, u32 arg)
 	writel_relaxed(arg, src_base + SRC_GPR1 + cpu * 8 + 4);
 }
 
-u32 ls1_get_cpu_arg(int cpu)
-{
-	struct device_node *np;
-	void __iomem *ls1_rcpm_base;
-
-	np = of_find_compatible_node(NULL, NULL, "fsl,qoriq-rcpm-2.1");
-	if (!np) {
-		pr_err("%s(): Can not find the RCPM node.\n", __func__);
-		return -ENODEV;
-	}
-
-	ls1_rcpm_base = of_iomap(np, 0);
-	of_node_put(np);
-	WARN_ON(!ls1_rcpm_base);
-
-	cpu = cpu_logical_map(cpu);
-	return ioread32be(ls1_rcpm_base + CCSR_TWAITSR0) & (1 << cpu);
-}
-
 void imx_src_prepare_restart(void)
 {
 	u32 val;