ENGR00331611 [imx6x] Set SCU CPU Power status register correctly

Backport from 3.10.x kernel.
Set the SCU CPU Power status register to reflect the correct status of
a CPU (active/inactive/not-present).

Signed-off-by: Ranjani Vaidyanathan <Ranjani.Vaidyanathan@freescale.com>

3 files changed, 33 insertions, 4 deletions

diff --git a/arch/arm/mach-mx6/headsmp.S b/arch/arm/mach-mx6/headsmp.S
index 9492d8dc1765..9389a6449551 100644
--- a/arch/arm/mach-mx6/headsmp.S
+++ b/arch/arm/mach-mx6/headsmp.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -18,6 +18,9 @@
 
 #include <linux/linkage.h>
 #include <linux/init.h>
+#include <asm/smp_scu.h>
+
+#define SCU_CPU_STATUS		0x08
 
 ENTRY(v7_invalidate_l1)
         mov     r0, #0
@@ -60,6 +63,16 @@ ENTRY(mx6_secondary_startup)
 	mcr p15, 0, r1, c7, c5, 0 @ Invalidate I-Cache
 	/* Invalidate L1 D-cache */
 	bl      v7_invalidate_l1
+
+	/* Set the CPU status in SCU CPU status register. */
+	mrc	p15, 0, r0, c0, c0, 5
+	and	r0, r0, #3
+	mrc	p15, 4, r1, c15, c0, 0
+	ldr	r2, =SCU_CPU_STATUS
+	orr	r2, r2, r0
+	ldr	r0, =SCU_PM_NORMAL
+	strb	r0, [r1, r2]
+
 	/* Set ARM working mode */
 	msr     cpsr_fsxc, #0xd3
 
diff --git a/arch/arm/mach-mx6/plat_hotplug.c b/arch/arm/mach-mx6/plat_hotplug.c
index 20a33329f472..ab23637fbee4 100644
--- a/arch/arm/mach-mx6/plat_hotplug.c
+++ b/arch/arm/mach-mx6/plat_hotplug.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -22,6 +22,7 @@
 #include <linux/delay.h>
 #include <mach/hardware.h>
 #include <asm/mach-types.h>
+#include <asm/smp_scu.h>
 #include <linux/io.h>
 #include "src-reg.h"
 #include <linux/sched.h>
@@ -92,6 +93,9 @@ void platform_cpu_die(unsigned int cpu)
 	:	"=&r" (v)
 	:	"r" (0), "Ir" (CR_C), "Ir" (0x40)
 	:	"cc");
+
+	scu_power_mode(IO_ADDRESS(SCU_BASE_ADDR), SCU_PM_DORMANT);
+
 	/* Tell cpu0 to kill this core, as this core's cache is
 	already disabled, and we want to set a flag to tell cpu0
 	to kill this core, so I write the flag to this core's SRC
diff --git a/arch/arm/mach-mx6/platsmp.c b/arch/arm/mach-mx6/platsmp.c
index a21f0231f131..4f861f9e96e2 100644
--- a/arch/arm/mach-mx6/platsmp.c
+++ b/arch/arm/mach-mx6/platsmp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2011-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright (C) 2011-2014 Freescale Semiconductor, Inc. All Rights Reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -106,6 +106,7 @@ void __init smp_init_cpus(void)
 {
 	void __iomem *scu_base = scu_base_addr();
 	unsigned int i, ncores;
+	u32 me = smp_processor_id();
 
 	ncores = scu_base ? scu_get_core_count(scu_base) : 1;
 
@@ -121,9 +122,20 @@ void __init smp_init_cpus(void)
 		ncores = NR_CPUS;
 	}
 
-	for (i = 0; i < ncores; i++)
+	if (setup_max_cpus >= ncores)
+		setup_max_cpus = ncores;
+
+	for (i = 0; i < setup_max_cpus; i++)
 		set_cpu_possible(i, true);
 
+	for (i = setup_max_cpus; i < ncores; i++)
+		set_cpu_possible(i, false);
+	/* Set the SCU CPU Power status for each inactive core. */
+	for (i = 0; i < NR_CPUS;  i++) {
+		if (i != me)
+			__raw_writeb(SCU_PM_POWEROFF, scu_base + 0x08 + i);
+	}
+
 	set_smp_cross_call(gic_raise_softirq);
 }
 static void __init wakeup_secondary(void)