ARM: tegra11x: CPUID virtualization support

This is the first patch to support CPUID virtualization. The goal is
to treat all CPUs as equal in software. In current implementation,
CPU0 is the anchor CPU, which must be the first one brought up, and
the last one taken down. This patch removes that restriction.

the cluster switch still has to start from CPU0 with this patch.

This can not coexist with secure OS

Reviewed-on: http://git-master/r/144610
(cherry picked from commit d32fba4be39e3f9a95ef5ab44d0c64dc6d2808a3)

Change-Id: Ib7fcaae751d17fee839a4f228f5ef5c3ee2390c2
Signed-off-by: Bo Yan <byan@nvidia.com>
Reviewed-on: http://git-master/r/159486
Reviewed-by: Simone Willett <swillett@nvidia.com>
Tested-by: Simone Willett <swillett@nvidia.com>

Rebase-Id: R09e29d45acf92b3ad2d909d5438c3375aa85e7dd

8 files changed, 64 insertions, 27 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 873b90599c57..e034c4b91536 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -516,4 +516,11 @@ config TEGRA_SOCTHERM
         help
 	  Enables use of soctherm for thermal management.
 
+config TEGRA_VIRTUAL_CPUID
+	bool "virtualized CPUID"
+	depends on !TRUSTED_FOUNDATIONS
+	depends on ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE
+	default n
+	help
+	  Enables virtualized CPUID.
 endif
diff --git a/arch/arm/mach-tegra/cpu-tegra.c b/arch/arm/mach-tegra/cpu-tegra.c
index c68d310c2d0a..2318de73c9f7 100644
--- a/arch/arm/mach-tegra/cpu-tegra.c
+++ b/arch/arm/mach-tegra/cpu-tegra.c
@@ -750,10 +750,6 @@ static int tegra_cpu_init(struct cpufreq_policy *policy)
 
 	cpumask_copy(policy->cpus, cpu_possible_mask);
 
-	if (policy->cpu == 0) {
-		register_pm_notifier(&tegra_cpu_pm_notifier);
-	}
-
 	return 0;
 }
 
@@ -782,7 +778,7 @@ static int tegra_cpufreq_policy_notifier(
 			ret ? policy->max : freq_table[i].frequency;
 
 #ifdef CONFIG_TEGRA_THERMAL_THROTTLE
-		if (once && policy->cpu == 0 &&
+		if (once &&
 		    sysfs_merge_group(&policy->kobj, &stats_attr_grp) == 0)
 			once = 0;
 #endif
@@ -834,8 +830,14 @@ static int __init tegra_cpufreq_init(void)
 	freq_table = table_data->freq_table;
 	tegra_cpu_edp_init(false);
 
+	ret = register_pm_notifier(&tegra_cpu_pm_notifier);
+
+	if (ret)
+		return ret;
+
 	ret = cpufreq_register_notifier(
 		&tegra_cpufreq_policy_nb, CPUFREQ_POLICY_NOTIFIER);
+
 	if (ret)
 		return ret;
 
diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index 6827f58180ad..e63ff0a142c9 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -65,13 +65,16 @@ ENTRY(tegra_resume)
 	bl	__invalidate_cpu_state
 
 	cpu_id	r0
+#ifndef CONFIG_TEGRA_VIRTUAL_CPUID
 	cmp	r0, #0				@ CPU0?
 	bne	cpu_resume			@ no
+#endif
 
 #ifndef CONFIG_ARCH_TEGRA_2x_SOC
 	@ Clear the flow controller flags for this CPU.
-	mov32	r2, TEGRA_FLOW_CTRL_BASE+8	@ CPU0 CSR
-	ldr	r1, [r2]
+	cpu_to_csr_reg	r1, r0
+	mov32	r2, TEGRA_FLOW_CTRL_BASE
+	ldr	r1, [r2, r1]
 	orr	r1, r1, #(1 << 15) | (1 << 14)	@ write to clear event & intr
 	movw	r0, #0x3FFD	@ enable, enable_ext, cluster_switch, immed, & bitmaps
 	bic	r1, r1, r0
@@ -312,10 +315,12 @@ __is_not_lp1:
 __is_not_lp2:
 
 #ifdef CONFIG_SMP
+#ifndef CONFIG_TEGRA_VIRTUAL_CPUID
 	/* Can only be secondary boot (initial or hotplug) but CPU 0
 	   cannot be here. */
 	cmp	r10, #0
 	bleq	__die				@ CPU0 cannot be here
+#endif
 	ldr	lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
 	cmp	lr, #0
 	bleq	__die				@ no secondary startup handler
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index e233ede9acc5..d459b5d21c4a 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -435,6 +435,13 @@ void tegra_smp_clear_power_mask()
 }
 #endif
 
+#ifdef CONFIG_TEGRA_VIRTUAL_CPUID
+static int tegra_cpu_disable(unsigned int cpu)
+{
+	return 0;
+}
+#endif
+
 struct smp_operations tegra_smp_ops __initdata = {
 	.smp_init_cpus		= tegra_smp_init_cpus,
 	.smp_prepare_cpus	= tegra_smp_prepare_cpus,
@@ -443,5 +450,8 @@ struct smp_operations tegra_smp_ops __initdata = {
 #ifdef CONFIG_HOTPLUG_CPU
 	.cpu_kill		= tegra_cpu_kill,
 	.cpu_die		= tegra_cpu_die,
+#ifdef CONFIG_TEGRA_VIRTUAL_CPUID
+	.cpu_disable		= tegra_cpu_disable,
+#endif
 #endif
 };
diff --git a/arch/arm/mach-tegra/pm-t3.c b/arch/arm/mach-tegra/pm-t3.c
index be50c5e79bd5..d989a905dfa3 100644
--- a/arch/arm/mach-tegra/pm-t3.c
+++ b/arch/arm/mach-tegra/pm-t3.c
@@ -33,6 +33,7 @@
 #include <mach/irqs.h>
 #include <mach/io_dpd.h>
 
+#include <asm/smp_plat.h>
 #include <asm/cputype.h>
 #include <asm/hardware/gic.h>
 
@@ -221,11 +222,14 @@ void tegra_cluster_switch_prolog(unsigned int flags)
 					? TEGRA_POWER_CLUSTER_LP
 					: TEGRA_POWER_CLUSTER_G;
 	u32 reg;
+	u32 cpu;
+
+	cpu = cpu_logical_map(smp_processor_id());
 
 	/* Read the flow controler CSR register and clear the CPU switch
 	   and immediate flags. If an actual CPU switch is to be performed,
 	   re-write the CSR register with the desired values. */
-	reg = readl(FLOW_CTRL_CPU_CSR(0));
+	reg = readl(FLOW_CTRL_CPU_CSR(cpu));
 	reg &= ~(FLOW_CTRL_CSR_IMMEDIATE_WAKE |
 		 FLOW_CTRL_CSR_SWITCH_CLUSTER);
 
@@ -268,7 +272,7 @@ void tegra_cluster_switch_prolog(unsigned int flags)
 	}
 
 done:
-	writel(reg, FLOW_CTRL_CPU_CSR(0));
+	writel(reg, FLOW_CTRL_CPU_CSR(cpu));
 }
 
 
@@ -336,17 +340,20 @@ static void cluster_switch_epilog_gic(void)
 void tegra_cluster_switch_epilog(unsigned int flags)
 {
 	u32 reg;
+	u32 cpu;
+
+	cpu = cpu_logical_map(smp_processor_id());
 
 	/* Make sure the switch and immediate flags are cleared in
 	   the flow controller to prevent undesirable side-effects
 	   for future users of the flow controller. */
-	reg = readl(FLOW_CTRL_CPU_CSR(0));
+	reg = readl(FLOW_CTRL_CPU_CSR(cpu));
 	reg &= ~(FLOW_CTRL_CSR_IMMEDIATE_WAKE |
 		 FLOW_CTRL_CSR_SWITCH_CLUSTER);
 #if defined(CONFIG_ARCH_TEGRA_HAS_SYMMETRIC_CPU_PWR_GATE)
 	reg &= ~FLOW_CTRL_CSR_ENABLE_EXT_MASK;
 #endif
-	writel(reg, FLOW_CTRL_CPU_CSR(0));
+	writel(reg, FLOW_CTRL_CPU_CSR(cpu));
 
 	/* Perform post-switch LP=>G clean-up */
 	if (!is_lp_cluster()) {
@@ -433,6 +440,12 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 #endif
 
 		} else {
+#ifdef CONFIG_TEGRA_VIRTUAL_CPUID
+			u32 cpu;
+
+			cpu = cpu_logical_map(smp_processor_id());
+			writel(cpu, FLOW_CTRL_MPID);
+#endif
 			last_g2lp = now;
 			tegra_dvfs_rail_off(tegra_cpu_rail, now);
 		}
@@ -449,9 +462,11 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 		if (us)
 			tegra_lp2_set_trigger(0);
 	} else {
-		int cpu = 0;
+		int cpu;
+
+		cpu = cpu_logical_map(smp_processor_id());
 
-		tegra_set_cpu_in_lp2(0);
+		tegra_set_cpu_in_lp2(cpu);
 		cpu_pm_enter();
 		if (!timekeeping_suspended)
 			clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER,
@@ -461,7 +476,7 @@ int tegra_cluster_control(unsigned int us, unsigned int flags)
 			clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT,
 					   &cpu);
 		cpu_pm_exit();
-		tegra_clear_cpu_in_lp2(0);
+		tegra_clear_cpu_in_lp2(cpu);
 	}
 	local_irq_restore(irq_flags);
 
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 751766879f6b..3f5875f14e6e 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -348,18 +348,12 @@ static void set_power_timers(unsigned long us_on, unsigned long us_off,
  */
 static void restore_cpu_complex(u32 mode)
 {
-	int cpu = smp_processor_id();
+	int cpu = cpu_logical_map(smp_processor_id());
 	unsigned int reg;
 #if defined(CONFIG_ARCH_TEGRA_2x_SOC) || defined(CONFIG_ARCH_TEGRA_3x_SOC)
 	unsigned int policy;
 #endif
 
-	BUG_ON(cpu != 0);
-
-#ifdef CONFIG_SMP
-	cpu = cpu_logical_map(cpu);
-#endif
-
 /*
  * On Tegra11x PLLX and CPU burst policy is either preserved across LP2,
  * or restored by common clock suspend/resume procedures. Hence, we don't
@@ -453,15 +447,12 @@ static void restore_cpu_complex(u32 mode)
  */
 static void suspend_cpu_complex(u32 mode)
 {
-	int cpu = smp_processor_id();
+	int cpu = cpu_logical_map(smp_processor_id());
 	unsigned int reg;
 	int i;
 
 	BUG_ON(cpu != 0);
 
-#ifdef CONFIG_SMP
-	cpu = cpu_logical_map(cpu);
-#endif
 	/* switch coresite to clk_m, save off original source */
 	tegra_sctx.clk_csite_src = readl(clk_rst + CLK_RESET_SOURCE_CSITE);
 	writel(3<<30, clk_rst + CLK_RESET_SOURCE_CSITE);
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index 0f962c6370e8..9754ce01eef7 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -101,6 +101,9 @@ int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags);
 	(IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + 0x38)
 #define FLOW_CTRL_CPU_PWR_CSR_RAIL_ENABLE	1
 
+#define FLOW_CTRL_MPID \
+	(IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + 0x3c)
+
 #define FLOW_CTRL_RAM_REPAIR \
 	(IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + 0x40)
 #define FLOW_CTRL_RAM_REPAIR_BYPASS_EN	(1<<2)
diff --git a/arch/arm/mach-tegra/timer-t3.c b/arch/arm/mach-tegra/timer-t3.c
index 8dd4deb72400..85889e7e69c2 100644
--- a/arch/arm/mach-tegra/timer-t3.c
+++ b/arch/arm/mach-tegra/timer-t3.c
@@ -192,8 +192,12 @@ static void tegra3_suspend_wake_timer(unsigned int cpu)
 {
 	cpumask_clear_cpu(cpu, &wake_timer_ready);
 #ifdef CONFIG_SMP
-	/* Reassign the affinity of the wake IRQ to CPU 0. */
-	(void)irq_set_affinity(tegra_lp2wake_irq[cpu].irq, cpumask_of(0));
+	/* Reassign the affinity of the wake IRQ to any ready CPU. */
+	for_each_cpu_not(cpu, &wake_timer_ready)
+	{
+		(void)irq_set_affinity(tegra_lp2wake_irq[cpu].irq,
+			cpumask_of(cpumask_any(&wake_timer_ready)));
+	}
 #endif
 }