From 3a63866fddef19f5c91c38b4738dd6d967518aa4 Mon Sep 17 00:00:00 2001
From: =?utf8?q?=E9=BB=84=E6=B6=9B?= <huangtao@rock-chips.com>
Date: Thu, 15 Mar 2012 16:14:46 +0800
Subject: [PATCH] rk30: fix fiq support when smp

---
 arch/arm/mach-rk30/fiq.c     | 22 +++++--------------
 arch/arm/mach-rk30/platsmp.c | 41 ++++++++++++++++++++++++++++++++++++
 2 files changed, 46 insertions(+), 17 deletions(-)

diff --git a/arch/arm/mach-rk30/fiq.c b/arch/arm/mach-rk30/fiq.c
index 48c3b35d8d2d..2c8d176f9781 100644
--- a/arch/arm/mach-rk30/fiq.c
+++ b/arch/arm/mach-rk30/fiq.c
@@ -92,29 +92,17 @@ void rk_irq_clearpending(int irq)
 
 void rk30_fiq_init(void)
 {
-	void __iomem *base = RK30_GICD_BASE;
 	unsigned int gic_irqs, i;
 
 	// read gic info to know how many irqs in our chip
-	gic_irqs = readl_relaxed(base + GIC_DIST_CTR) & 0x1f;
-	//set all the interrupt to non-secure state
+	gic_irqs = readl_relaxed(RK30_GICD_BASE + GIC_DIST_CTR) & 0x1f;
+	// set all the interrupt to non-secure state
 	for (i = 0; i < (gic_irqs + 1); i++) {
-		/*
-		 * In any system that implements the ARM Security Extensions,
-		 * to support a consistent model for message passing between
-		 * processors, ARM strongly recommends that all processors reserve:
-		 * ID0-ID7 for Non-secure interrupts
-		 * ID8-ID15 for Secure interrupts.
-		 */
-		if (i == 0) {
-			writel_relaxed(0xffff00ff, base + GIC_DIST_SECURITY + (i<<2));
-		} else {
-			writel_relaxed(0xffffffff, base + GIC_DIST_SECURITY + (i<<2));
-		}
+		writel_relaxed(0xffffffff, RK30_GICD_BASE + GIC_DIST_SECURITY + (i<<2));
 	}
 	dsb();
 
-	writel_relaxed(0x3, base + GIC_DIST_CTRL);
-	writel_relaxed(0x1f, RK30_GICC_BASE + GIC_CPU_CTRL);
+	writel_relaxed(0x3, RK30_GICD_BASE + GIC_DIST_CTRL);
+	writel_relaxed(0xf, RK30_GICC_BASE + GIC_CPU_CTRL);
 	dsb();
 }
diff --git a/arch/arm/mach-rk30/platsmp.c b/arch/arm/mach-rk30/platsmp.c
index 41752d902300..c727174f658e 100644
--- a/arch/arm/mach-rk30/platsmp.c
+++ b/arch/arm/mach-rk30/platsmp.c
@@ -17,6 +17,39 @@
 
 #include <mach/pmu.h>
 
+#ifdef CONFIG_FIQ
+static void gic_raise_softirq_non_secure(const struct cpumask *mask, unsigned int irq)
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 2, 0))
+	unsigned long map = *cpus_addr(*mask);
+#else
+	int cpu;
+	unsigned long map = 0;
+
+	/* Convert our logical CPU mask into a physical one. */
+	for_each_cpu(cpu, mask)
+		map |= 1 << cpu_logical_map(cpu);
+#endif
+
+	/*
+	 * Ensure that stores to Normal memory are visible to the
+	 * other CPUs before issuing the IPI.
+	 */
+	dsb();
+
+	/* this always happens on GIC0 */
+	writel_relaxed(map << 16 | irq | 0x8000, RK30_GICD_BASE + GIC_DIST_SOFTINT);
+}
+
+static void gic_secondary_init_non_secure(void)
+{
+#define GIC_DIST_SECURITY	0x080
+	writel_relaxed(0xffffffff, RK30_GICD_BASE + GIC_DIST_SECURITY);
+	writel_relaxed(0xf, RK30_GICC_BASE + GIC_CPU_CTRL);
+	dsb();
+}
+#endif
+
 void __cpuinit platform_secondary_init(unsigned int cpu)
 {
 	/*
@@ -25,6 +58,10 @@ void __cpuinit platform_secondary_init(unsigned int cpu)
 	 * for us: do so
 	 */
 	gic_secondary_init(0);
+
+#ifdef CONFIG_FIQ
+	gic_secondary_init_non_secure();
+#endif
 }
 
 extern void rk30_sram_secondary_startup(void);
@@ -66,7 +103,11 @@ void __init smp_init_cpus(void)
 	for (i = 0; i < ncores; i++)
 		set_cpu_possible(i, true);
 
+#ifdef CONFIG_FIQ
+	set_smp_cross_call(gic_raise_softirq_non_secure);
+#else
 	set_smp_cross_call(gic_raise_softirq);
+#endif
 }
 
 void __init platform_smp_prepare_cpus(unsigned int max_cpus)
-- 
2.34.1