ARM: shmobile: r8a73a4 SMP and CPU Hotplug prototype v2

@@ -87,6 +87,7 @@ static const char *ape6evm_boards_compat
 };
 
 DT_MACHINE_START(APE6EVM_DT, "ape6evm")
+	.smp		= smp_ops(r8a73a4_smp_ops),
 	.init_irq	= irqchip_init,
 	.init_time	= shmobile_timer_init,
 	.init_machine	= ape6evm_add_standard_devices,

@@ -4,5 +4,6 @@
 void r8a73a4_add_standard_devices(void);
 void r8a73a4_clock_init(void);
 void r8a73a4_pinmux_init(void);
+extern struct smp_operations r8a73a4_smp_ops;
 
 #endif /* __ASM_R8A73A4_H__ */

@@ -194,6 +194,7 @@ static const char *r8a73a4_boards_compat
 };
 
 DT_MACHINE_START(R8A73A4_DT, "Generic R8A73A4 (Flattened Device Tree)")
+	.smp		= smp_ops(r8a73a4_smp_ops),
 	.init_irq	= irqchip_init,
 	.init_machine	= r8a73a4_add_standard_devices_dt,
 	.init_time	= shmobile_timer_init,

89 lines not on this page · raw message

@@ -36,6 +36,8 @@
 
 #define APMU	0xe6150000
 #define CA15WUPCR 0x2010
+#define CA15PSTR 0x2040
+#define CA15CPUNCR(n) (0x2100 + (0x10 * (n))) 
 
 #define MERAM	0xe8080000
 #define CCI_BASE	0xf0190000

@@ -83,7 +85,93 @@ static int __cpuinit r8a73a4_boot_second
 	return 0;
 }
 
+#ifdef CONFIG_HOTPLUG_CPU
+static int r8a73a4_cpu_kill(unsigned int cpu)
+{
+	void __iomem *p;
+	int ret = 0;
+	int k;
+
+	/* this function is running on another CPU than the offline target,
+	 * here all we need to do is to wait for the SoC to enter sleep mode.
+	 */
+	p = ioremap_nocache(APMU, 0x3000);
+	for (k = 0; k < 1000; k++) {
+		if (((__raw_readl(p + CA15PSTR) >> (cpu * 4)) & 0x03) == 3) {
+			ret = 1;
+			break;
+		}
+
+		mdelay(1);
+	}
+	iounmap(p);
+
+	return ret;
+}
+
+/* nicked from arch/arm/mach-exynos/hotplug.c */
+static inline void cpu_enter_lowpower_a15(void)
+{
+	unsigned int v;
+
+	asm volatile(
+	"	mrc	p15, 0, %0, c1, c0, 0\n"
+	"	bic	%0, %0, %1\n"
+	"	mcr	p15, 0, %0, c1, c0, 0\n"
+	  : "=&r" (v)
+	  : "Ir" (CR_C)
+	  : "cc");
+
+	flush_cache_louis();
+
+	asm volatile(
+	/*
+	* Turn off coherency
+	*/
+	"	mrc	p15, 0, %0, c1, c0, 1\n"
+	"	bic	%0, %0, %1\n"
+	"	mcr	p15, 0, %0, c1, c0, 1\n"
+	: "=&r" (v)
+	: "Ir" (0x40)
+	: "cc");
+
+	isb();
+	dsb();
+}
+
+static int r8a73a4_do_idle_core_standby(unsigned long unused)
+{
+	cpu_do_idle(); /* WFI triggers Core Standby */
+	return 0;
+}
+
+static void r8a73a4_cpu_die(unsigned int cpu)
+{
+	void __iomem *p;
+
+	/* hardware shutdown code running on the CPU that is being offlined */
+
+	/* select CPU shutdown mode */
+	p = ioremap_nocache(APMU, 0x3000);
+	__raw_writel(3, p + CA15CPUNCR(cpu)); /* CA15 Core Standby */
+	iounmap(p);
+
+	cpu_enter_lowpower_a15();
+	cpu_suspend(0, r8a73a4_do_idle_core_standby); /* really needed? */
+}
+
+static int r8a73a4_cpu_disable(unsigned int cpu)
+{
+	return 0; /* hotplug of any CPU is tested OK */
+}
+#endif
+
 struct smp_operations r8a73a4_smp_ops __initdata = {
 	.smp_prepare_cpus	= r8a73a4_smp_prepare_cpus,
 	.smp_boot_secondary	= r8a73a4_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_kill		= r8a73a4_cpu_kill,
+	.cpu_die		= r8a73a4_cpu_die,
+	.cpu_disable		= r8a73a4_cpu_disable,
+#endif
 };

@@ -45,6 +45,20 @@
 #define CCI_SNOOP	0x0000
 #define CCI_STATUS	0x000c
 
+static inline void r8a73a4_execute_wfe_as_nop(void)
+{
+	unsigned int v;
+
+	/* set bit 7 in ACTLR */
+	asm volatile(
+	"	mrc	p15, 0, %0, c1, c0, 1\n"
+	"	orr	%0, %0, #0x40\n"
+	"	mcr	p15, 0, %0, c1, c0, 1\n"
+	  : "=&r" (v)
+	  :
+	  : "cc");
+}
+
 static void __init r8a73a4_smp_prepare_cpus(unsigned int max_cpus)
 {
 	u32 bar;

@@ -71,6 +85,8 @@ static void __init r8a73a4_smp_prepare_c
 	while (__raw_readl(p + CCI_STATUS))
 		/* wait for pending bit low */;
 	iounmap(p);
+
+	r8a73a4_execute_wfe_as_nop();
 }
 
 static int __cpuinit r8a73a4_boot_secondary(unsigned int cpu, struct task_struct *idle)

@@ -85,6 +101,13 @@ static int __cpuinit r8a73a4_boot_second
 	return 0;
 }
 
+static void __cpuinit r8a73a4_secondary_init(unsigned int cpu)
+{
+	/* CA15 need SW workaround */
+	if (cpu_logical_map(cpu) < 4)
+		r8a73a4_execute_wfe_as_nop();
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 static int r8a73a4_cpu_kill(unsigned int cpu)
 {

@@ -169,6 +192,7 @@ static int r8a73a4_cpu_disable(unsigned
 struct smp_operations r8a73a4_smp_ops __initdata = {
 	.smp_prepare_cpus	= r8a73a4_smp_prepare_cpus,
 	.smp_boot_secondary	= r8a73a4_boot_secondary,
+	.smp_secondary_init	= r8a73a4_secondary_init,
 #ifdef CONFIG_HOTPLUG_CPU
 	.cpu_kill		= r8a73a4_cpu_kill,
 	.cpu_die		= r8a73a4_cpu_die,

@@ -31,17 +31,22 @@
 #include <asm/smp_plat.h>
 
 #define SYSC	0xe6180000
+#define CA7BAR	0x4020
 #define CA15BAR	0x6020
 #define RESCNT	0x801c
 
 #define APMU	0xe6150000
+#define CA7WUPCR 0x1010
 #define CA15WUPCR 0x2010
+#define CA7PSTR	0x1040
 #define CA15PSTR 0x2040
+#define CA7CPUNCR(n) (0x1100 + (0x10 * (n))) 
 #define CA15CPUNCR(n) (0x2100 + (0x10 * (n))) 
 
 #define MERAM	0xe8080000
 #define CCI_BASE	0xf0190000
 #define CCI_SLAVE3	0x4000
+#define CCI_SLAVE4	0x5000
 #define CCI_SNOOP	0x0000
 #define CCI_STATUS	0x000c
 

@@ -75,13 +80,16 @@ static void __init r8a73a4_smp_prepare_c
 	p = ioremap_nocache(SYSC, 0x9000);
 	bar = (MERAM >> 8) & 0xfffffc00;
 	__raw_writel(bar, p + CA15BAR);
+	__raw_writel(bar, p + CA7BAR);
 	__raw_writel(bar | 0x10, p + CA15BAR);
+	__raw_writel(bar | 0x10, p + CA7BAR);
 	__raw_writel(__raw_readl(p + RESCNT) & ~(1 << 10), p + RESCNT);
 	iounmap(p);
 
 	/* enable snoop and DVM */
 	p = ioremap_nocache(CCI_BASE, 0x8000);
 	__raw_writel(3, p + CCI_SLAVE3 + CCI_SNOOP);	/* ca15 */
+	__raw_writel(3, p + CCI_SLAVE4 + CCI_SNOOP);	/* ca7 */
 	while (__raw_readl(p + CCI_STATUS))
 		/* wait for pending bit low */;
 	iounmap(p);

@@ -92,10 +100,14 @@ static void __init r8a73a4_smp_prepare_c
 static int __cpuinit r8a73a4_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
 	void __iomem *p;
+	int cluster_cpu = cpu_logical_map(cpu) & 3;
 
 	/* wake up CPU core */
 	p = ioremap_nocache(APMU, 0x3000);
-	__raw_writel(1 << (cpu_logical_map(cpu) & 3), p + CA15WUPCR);
+	if (cpu_logical_map(cpu) < 4) 
+		__raw_writel(1 << cluster_cpu, p + CA15WUPCR);
+	else
+		__raw_writel(1 << cluster_cpu, p + CA7WUPCR);
 	iounmap(p);
 
 	return 0;

@@ -112,6 +124,7 @@ static void __cpuinit r8a73a4_secondary_
 static int r8a73a4_cpu_kill(unsigned int cpu)
 {
 	void __iomem *p;
+	int offs;
 	int ret = 0;
 	int k;
 

@@ -120,9 +133,18 @@ static int r8a73a4_cpu_kill(unsigned int
 	 */
 	p = ioremap_nocache(APMU, 0x3000);
 	for (k = 0; k < 1000; k++) {
-		if (((__raw_readl(p + CA15PSTR) >> (cpu * 4)) & 0x03) == 3) {
-			ret = 1;
-			break;
+		if (cpu < 4) { /* CA15 Core Standby */
+			offs = cpu * 4;
+			if (((__raw_readl(p + CA15PSTR) >> offs) & 0x03) == 3) {
+				ret = 1;
+				break;
+			}
+		} else { /* CA7 Core Standby */
+			offs = (cpu - 4) * 4;
+			if (((__raw_readl(p + CA7PSTR) >> offs) & 0x03) == 3) {
+				ret = 1;
+				break;
+			}
 		}
 
 		mdelay(1);

@@ -176,7 +198,11 @@ static void r8a73a4_cpu_die(unsigned int
 
 	/* select CPU shutdown mode */
 	p = ioremap_nocache(APMU, 0x3000);
-	__raw_writel(3, p + CA15CPUNCR(cpu)); /* CA15 Core Standby */
+	if (cpu < 4)
+		__raw_writel(3, p + CA15CPUNCR(cpu)); /* CA15 Core Standby */
+	else
+		__raw_writel(3, p + CA7CPUNCR(cpu - 4)); /* CA7 Core Standby */
+
 	iounmap(p);
 
 	cpu_enter_lowpower_a15();

@@ -50,6 +50,8 @@
 #define CCI_SNOOP	0x0000
 #define CCI_STATUS	0x000c
 
+#define CNTFID0	0xe6080020
+
 static inline void r8a73a4_execute_wfe_as_nop(void)
 {
 	unsigned int v;

@@ -64,6 +66,32 @@ static inline void r8a73a4_execute_wfe_a
 	  : "cc");
 }
 
+static u32 r8a73a4_arch_freq;
+
+static inline void r8a73a4_reset_arch_timer(void)
+{
+	/* Program CNTFRQ from the frequency table */
+	asm volatile("mcr p15, 0, %0, c14, c0, 0" : : "r" (r8a73a4_arch_freq));
+
+	/* CNTVOFF has to be initialized either from non-secure Hypervisor
+	 * mode or secure Monitor mode with SCR.NS==1. If TrustZone is enabled
+	 * then it should be handled by the secure code
+	 */
+	asm volatile(
+	"cps	0x16				@ Monitor mode"
+	"mrc	p15, 0, r1, c1, c1, 0"
+	"orr	r0, r1, #1			@ Set SCR.NS (non-secure)"
+	"mcr	p15, 0, r0, c1, c1, 0"
+	"isb"
+	"mov	r0, #0"
+	"mcrr	p15, 4, r0, r0, c14		@ CNTVOFF"
+	"isb"
+	"mcr	p15, 0, r1, c1, c1, 0		@ Restore SCR"
+	"isb"
+	"cps	0x13				@ Back to SVC mode"
+	: : : "r0", "r1");
+}
+
 static void __init r8a73a4_smp_prepare_cpus(unsigned int max_cpus)
 {
 	u32 bar;

@@ -94,6 +122,11 @@ static void __init r8a73a4_smp_prepare_c
 		/* wait for pending bit low */;
 	iounmap(p);
 
+	/* Retrieve frequency to be able to later program CNTFRQ */
+	p = ioremap_nocache(CNTFID0, 16);
+	r8a73a4_arch_freq = __raw_readl(p);
+	iounmap(p);
+
 	r8a73a4_execute_wfe_as_nop();
 }
 

@@ -115,9 +148,11 @@ static int __cpuinit r8a73a4_boot_second
 
 static void __cpuinit r8a73a4_secondary_init(unsigned int cpu)
 {
-	/* CA15 need SW workaround */
+	/* CA15 and CA7 need SW workaround */
 	if (cpu_logical_map(cpu) < 4)
 		r8a73a4_execute_wfe_as_nop();
+	else
+		r8a73a4_reset_arch_timer();
 }
 
 #ifdef CONFIG_HOTPLUG_CPU