3 files changed, 191 insertions, 44 deletions

diff --git a/arch/arm/mach-rockchip/headsmp.S b/arch/arm/mach-rockchip/headsmp.S
index 73206e360e3..46c22dedf63 100644
--- a/arch/arm/mach-rockchip/headsmp.S
+++ b/arch/arm/mach-rockchip/headsmp.S
@@ -16,7 +16,10 @@
 #include <linux/init.h>
 
 ENTRY(rockchip_secondary_startup)
-	bl	v7_invalidate_l1
+	mrc	p15, 0, r0, c0, c0, 0	@ read main ID register
+	ldr	r1, =0x00000c09		@ Cortex-A9 primary part number
+	teq	r0, r1
+	beq	v7_invalidate_l1
 	b	secondary_startup
 ENDPROC(rockchip_secondary_startup)
 
diff --git a/arch/arm/mach-rockchip/platsmp.c b/arch/arm/mach-rockchip/platsmp.c
index 189684f5592..f26fcdca244 100644
--- a/arch/arm/mach-rockchip/platsmp.c
+++ b/arch/arm/mach-rockchip/platsmp.c
@@ -19,7 +19,11 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
 
+#include <linux/reset.h>
+#include <linux/cpu.h>
 #include <asm/cacheflush.h>
 #include <asm/cp15.h>
 #include <asm/smp_scu.h>
@@ -37,23 +41,78 @@ static int ncores;
 
 #define PMU_PWRDN_SCU		4
 
-static void __iomem *pmu_base_addr;
+static struct regmap *pmu;
 
-static inline bool pmu_power_domain_is_on(int pd)
+static int pmu_power_domain_is_on(int pd)
 {
-	return !(readl_relaxed(pmu_base_addr + PMU_PWRDN_ST) & BIT(pd));
+	u32 val;
+	int ret;
+
+	ret = regmap_read(pmu, PMU_PWRDN_ST, &val);
+	if (ret < 0)
+		return ret;
+
+	return !(val & BIT(pd));
 }
 
-static void pmu_set_power_domain(int pd, bool on)
+struct reset_control *rockchip_get_core_reset(int cpu)
 {
-	u32 val = readl_relaxed(pmu_base_addr + PMU_PWRDN_CON);
-	if (on)
-		val &= ~BIT(pd);
+	struct device *dev = get_cpu_device(cpu);
+	struct device_node *np;
+
+	/* The cpu device is only available after the initial core bringup */
+	if (dev)
+		np = dev->of_node;
 	else
-		val |=  BIT(pd);
-	writel(val, pmu_base_addr + PMU_PWRDN_CON);
+		np = of_get_cpu_node(cpu, 0);
 
-	while (pmu_power_domain_is_on(pd) != on) { }
+	return of_reset_control_get(np, NULL);
+}
+
+static int pmu_set_power_domain(int pd, bool on)
+{
+	u32 val = (on) ? 0 : BIT(pd);
+	int ret;
+
+	/*
+	 * We need to soft reset the cpu when we turn off the cpu power domain,
+	 * or else the active processors might be stalled when the individual
+	 * processor is powered down.
+	 */
+	if (read_cpuid_part() != ARM_CPU_PART_CORTEX_A9) {
+		struct reset_control *rstc = rockchip_get_core_reset(pd);
+
+		if (IS_ERR(rstc)) {
+			pr_err("%s: could not get reset control for core %d\n",
+			       __func__, pd);
+			return PTR_ERR(rstc);
+		}
+
+		if (on)
+			reset_control_deassert(rstc);
+		else
+			reset_control_assert(rstc);
+
+		reset_control_put(rstc);
+	}
+
+	ret = regmap_update_bits(pmu, PMU_PWRDN_CON, BIT(pd), val);
+	if (ret < 0) {
+		pr_err("%s: could not update power domain\n", __func__);
+		return ret;
+	}
+
+	ret = -1;
+	while (ret != on) {
+		ret = pmu_power_domain_is_on(pd);
+		if (ret < 0) {
+			pr_err("%s: could not read power domain state\n",
+				 __func__);
+			return ret;
+		}
+	}
+
+	return 0;
 }
 
 /*
@@ -63,7 +122,9 @@ static void pmu_set_power_domain(int pd, bool on)
 static int __cpuinit rockchip_boot_secondary(unsigned int cpu,
 					     struct task_struct *idle)
 {
-	if (!sram_base_addr || !pmu_base_addr) {
+	int ret;
+
+	if (!sram_base_addr || !pmu) {
 		pr_err("%s: sram or pmu missing for cpu boot\n", __func__);
 		return -ENXIO;
 	}
@@ -75,7 +136,24 @@ static int __cpuinit rockchip_boot_secondary(unsigned int cpu,
 	}
 
 	/* start the core */
-	pmu_set_power_domain(0 + cpu, true);
+	ret = pmu_set_power_domain(0 + cpu, true);
+	if (ret < 0)
+		return ret;
+
+	if (read_cpuid_part() != ARM_CPU_PART_CORTEX_A9) {
+		/* We communicate with the bootrom to active the cpus other
+		 * than cpu0, after a blob of initialize code, they will
+		 * stay at wfe state, once they are actived, they will check
+		 * the mailbox:
+		 * sram_base_addr + 4: 0xdeadbeaf
+		 * sram_base_addr + 8: start address for pc
+		 * */
+		udelay(10);
+		writel(virt_to_phys(rockchip_secondary_startup),
+			sram_base_addr + 8);
+		writel(0xDEADBEAF, sram_base_addr + 4);
+		dsb_sev();
+	}
 
 	return 0;
 }
@@ -110,8 +188,6 @@ static int __init rockchip_smp_prepare_sram(struct device_node *node)
 		return -EINVAL;
 	}
 
-	sram_base_addr = of_iomap(node, 0);
-
 	/* set the boot function for the sram code */
 	rockchip_boot_fn = virt_to_phys(rockchip_secondary_startup);
 
@@ -125,54 +201,115 @@ static int __init rockchip_smp_prepare_sram(struct device_node *node)
 	return 0;
 }
 
-static void __init rockchip_smp_prepare_cpus(unsigned int max_cpus)
+static struct regmap_config rockchip_pmu_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+};
+
+static int __init rockchip_smp_prepare_pmu(void)
 {
 	struct device_node *node;
-	unsigned int i;
+	void __iomem *pmu_base;
 
-	node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-scu");
+	/*
+	 * This function is only called via smp_ops->smp_prepare_cpu().
+	 * That only happens if a "/cpus" device tree node exists
+	 * and has an "enable-method" property that selects the SMP
+	 * operations defined herein.
+	 */
+	node = of_find_node_by_path("/cpus");
+
+	pmu = syscon_regmap_lookup_by_phandle(node, "rockchip,pmu");
+	of_node_put(node);
+	if (!IS_ERR(pmu))
+		return 0;
+
+	pmu = syscon_regmap_lookup_by_compatible("rockchip,rk3066-pmu");
+	if (!IS_ERR(pmu))
+		return 0;
+
+	/* fallback, create our own regmap for the pmu area */
+	pmu = NULL;
+	node = of_find_compatible_node(NULL, NULL, "rockchip,rk3066-pmu");
 	if (!node) {
-		pr_err("%s: missing scu\n", __func__);
-		return;
+		pr_err("%s: could not find pmu dt node\n", __func__);
+		return -ENODEV;
 	}
 
-	scu_base_addr = of_iomap(node, 0);
-	if (!scu_base_addr) {
-		pr_err("%s: could not map scu registers\n", __func__);
-		return;
+	pmu_base = of_iomap(node, 0);
+	if (!pmu_base) {
+		pr_err("%s: could not map pmu registers\n", __func__);
+		return -ENOMEM;
 	}
 
-	node = of_find_compatible_node(NULL, NULL, "rockchip,rk3066-smp-sram");
-	if (!node) {
-		pr_err("%s: could not find sram dt node\n", __func__);
-		return;
+	pmu = regmap_init_mmio(NULL, pmu_base, &rockchip_pmu_regmap_config);
+	if (IS_ERR(pmu)) {
+		int ret = PTR_ERR(pmu);
+
+		iounmap(pmu_base);
+		pmu = NULL;
+		pr_err("%s: regmap init failed\n", __func__);
+		return ret;
 	}
 
-	if (rockchip_smp_prepare_sram(node))
-		return;
+	return 0;
+}
 
-	node = of_find_compatible_node(NULL, NULL, "rockchip,rk3066-pmu");
+static void __init rockchip_smp_prepare_cpus(unsigned int max_cpus)
+{
+	struct device_node *node;
+	unsigned int i;
+
+	node = of_find_compatible_node(NULL, NULL, "rockchip,rk3066-smp-sram");
 	if (!node) {
-		pr_err("%s: could not find pmu dt node\n", __func__);
+		pr_err("%s: could not find sram dt node\n", __func__);
 		return;
 	}
 
-	pmu_base_addr = of_iomap(node, 0);
-	if (!pmu_base_addr) {
-		pr_err("%s: could not map pmu registers\n", __func__);
+	sram_base_addr = of_iomap(node, 0);
+	if (!sram_base_addr) {
+		pr_err("%s: could not map sram registers\n", __func__);
 		return;
 	}
 
-	/* enable the SCU power domain */
-	pmu_set_power_domain(PMU_PWRDN_SCU, true);
-
-	/*
-	 * While the number of cpus is gathered from dt, also get the number
-	 * of cores from the scu to verify this value when booting the cores.
-	 */
-	ncores = scu_get_core_count(scu_base_addr);
+	if (rockchip_smp_prepare_pmu())
+		return;
 
-	scu_enable(scu_base_addr);
+	if (read_cpuid_part() == ARM_CPU_PART_CORTEX_A9) {
+		if (rockchip_smp_prepare_sram(node))
+			return;
+
+		/* enable the SCU power domain */
+		pmu_set_power_domain(PMU_PWRDN_SCU, true);
+
+		node = of_find_compatible_node(NULL, NULL, "arm,cortex-a9-scu");
+		if (!node) {
+			pr_err("%s: missing scu\n", __func__);
+			return;
+		}
+
+		scu_base_addr = of_iomap(node, 0);
+		if (!scu_base_addr) {
+			pr_err("%s: could not map scu registers\n", __func__);
+			return;
+		}
+
+		/*
+		 * While the number of cpus is gathered from dt, also get the
+		 * number of cores from the scu to verify this value when
+		 * booting the cores.
+		 */
+		ncores = scu_get_core_count(scu_base_addr);
+		pr_err("%s: ncores %d\n", __func__, ncores);
+
+		scu_enable(scu_base_addr);
+	} else {
+		unsigned int l2ctlr;
+
+		asm ("mrc p15, 1, %0, c9, c0, 2\n" : "=r" (l2ctlr));
+		ncores = ((l2ctlr >> 24) & 0x3) + 1;
+	}
 
 	/* Make sure that all cores except the first are really off */
 	for (i = 1; i < ncores; i++)
diff --git a/arch/arm/mach-rockchip/rockchip.c b/arch/arm/mach-rockchip/rockchip.c
index 8ab9e0e7ff0..d226b71d21d 100644
--- a/arch/arm/mach-rockchip/rockchip.c
+++ b/arch/arm/mach-rockchip/rockchip.c
@@ -24,6 +24,12 @@
 #include <asm/hardware/cache-l2x0.h>
 #include "core.h"
 
+static void __init rockchip_dt_init(void)
+{
+	of_platform_populate(NULL, of_default_bus_match_table, NULL, NULL);
+	platform_device_register_simple("cpufreq-dt", 0, NULL, 0);
+}
+
 static const char * const rockchip_board_dt_compat[] = {
 	"rockchip,rk2928",
 	"rockchip,rk3066a",
@@ -37,4 +43,5 @@ DT_MACHINE_START(ROCKCHIP_DT, "Rockchip Cortex-A9 (Device Tree)")
 	.l2c_aux_val	= 0,
 	.l2c_aux_mask	= ~0,
 	.dt_compat	= rockchip_board_dt_compat,
+	.init_machine	= rockchip_dt_init,
 MACHINE_END