From: 黄涛 <huangtao@rock-chips.com>
Date: Mon, 5 Mar 2012 11:02:06 +0000 (+0800)
Subject: rk30: add basic support for smp
X-Git-Tag: firefly_0821_release~9595^2~94
X-Git-Url: http://demsky.eecs.uci.edu/git/?a=commitdiff_plain;h=56049fd39e1b9e42877e2ecb6076a1e8c522c6e3;p=firefly-linux-kernel-4.4.55.git
rk30: add basic support for smp
---
diff --git a/arch/arm/mach-rk30/Makefile b/arch/arm/mach-rk30/Makefile
index c7cdbe174c82..7d2f9d805f2d 100644
--- a/arch/arm/mach-rk30/Makefile
+++ b/arch/arm/mach-rk30/Makefile
@@ -3,9 +3,12 @@ obj-y += common.o
obj-y += devices.o
obj-y += io.o
obj-y += iomux.o
+obj-y += pmu.o
obj-y += reset.o
obj-y += timer.o
obj-$(CONFIG_FIQ) += fiq.o
+obj-$(CONFIG_SMP) += platsmp.o headsmp.o
+obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
obj-$(CONFIG_LOCAL_TIMERS) += localtimer.o
obj-$(CONFIG_PM) += pm.o
diff --git a/arch/arm/mach-rk30/headsmp.S b/arch/arm/mach-rk30/headsmp.S
new file mode 100644
index 000000000000..5599b86dfc4d
--- /dev/null
+++ b/arch/arm/mach-rk30/headsmp.S
@@ -0,0 +1,121 @@
+#include <linux/linkage.h>
+#include <linux/init.h>
+#include <asm/memory.h>
+
+ .section ".text.head", "ax"
+ __CPUINIT
+
+/*
+ * The secondary kernel init calls v7_flush_dcache_all before it enables
+ * the L1; however, the L1 comes out of reset in an undefined state, so
+ * the clean + invalidate performed by v7_flush_dcache_all causes a bunch
+ * of cache lines with uninitialized data and uninitialized tags to get
+ * written out to memory, which does really unpleasant things to the main
+ * processor. We fix this by performing an invalidate, rather than a
+ * clean + invalidate, before jumping into the kernel.
+ */
+ENTRY(v7_invalidate_l1)
+ mov r0, #0
+ mcr p15, 0, r0, c7, c5, 0 @ invalidate I cache
+ mcr p15, 2, r0, c0, c0, 0
+ mrc p15, 1, r0, c0, c0, 0
+
+ ldr r1, =0x7fff
+ and r2, r1, r0, lsr #13
+
+ ldr r1, =0x3ff
+
+ and r3, r1, r0, lsr #3 @ NumWays - 1
+ add r2, r2, #1 @ NumSets
+
+ and r0, r0, #0x7
+ add r0, r0, #4 @ SetShift
+
+ clz r1, r3 @ WayShift
+ add r4, r3, #1 @ NumWays
+1: sub r2, r2, #1 @ NumSets--
+ mov r3, r4 @ Temp = NumWays
+2: subs r3, r3, #1 @ Temp--
+ mov r5, r3, lsl r1
+ mov r6, r2, lsl r0
+ orr r5, r5, r6 @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
+ mcr p15, 0, r5, c7, c6, 2
+ bgt 2b
+ cmp r2, #0
+ bgt 1b
+ dsb
+ isb
+ mov pc, lr
+ENDPROC(v7_invalidate_l1)
+
+ENTRY(rk30_secondary_startup)
+ bl v7_invalidate_l1
+ b secondary_startup
+ENDPROC(rk30_secondary_startup)
+
+ENTRY(rk30_sram_secondary_startup)
+ ldr pc, 1f
+ .word 0xdeadbeaf
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+ ldr pc, 1f
+1: .long rk30_secondary_startup - PAGE_OFFSET + PLAT_PHYS_OFFSET
+ENDPROC(rk30_sram_secondary_startup)
diff --git a/arch/arm/mach-rk30/hotplug.c b/arch/arm/mach-rk30/hotplug.c
new file mode 100644
index 000000000000..5ea1ae3d48fe
--- /dev/null
+++ b/arch/arm/mach-rk30/hotplug.c
@@ -0,0 +1,75 @@
+/*
+ * RK30 SMP cpu-hotplug support
+ *
+ * Copyright (C) 2012 ROCKCHIP, Inc.
+ * Copyright (C) 2002 ARM Ltd.
+ * 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 version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/smp.h>
+#include <linux/delay.h>
+
+#include <asm/cacheflush.h>
+#include <asm/smp_plat.h>
+#include <asm/system.h>
+
+#include <mach/pmu.h>
+
+static cpumask_t dead_cpus;
+
+int platform_cpu_kill(unsigned int cpu)
+{
+ int k;
+
+ /* this function is running on another CPU than the offline target,
+ * here we need wait for shutdown code in platform_cpu_die() to
+ * finish before asking SoC-specific code to power off the CPU core.
+ */
+ for (k = 0; k < 1000; k++) {
+ if (cpumask_test_cpu(cpu, &dead_cpus)) {
+ pmu_set_power_domain(PD_A9_1, false);
+ return 1;
+ }
+
+ mdelay(1);
+ }
+
+ return 0;
+}
+
+/*
+ * platform-specific code to shutdown a CPU
+ *
+ * Called with IRQs disabled
+ */
+void platform_cpu_die(unsigned int cpu)
+{
+ /* hardware shutdown code running on the CPU that is being offlined */
+ flush_cache_all();
+ dsb();
+
+ /* notify platform_cpu_kill() that hardware shutdown is finished */
+ cpumask_set_cpu(cpu, &dead_cpus);
+
+ /* wait for SoC code in platform_cpu_kill() to shut off CPU core
+ * power. CPU bring up starts from the reset vector.
+ */
+ while (1)
+ cpu_do_idle();
+}
+
+int platform_cpu_disable(unsigned int cpu)
+{
+ cpumask_clear_cpu(cpu, &dead_cpus);
+ /*
+ * we don't allow CPU 0 to be shutdown (it is still too special
+ * e.g. clock tick interrupts)
+ */
+ return cpu == 0 ? -EPERM : 0;
+}
diff --git a/arch/arm/mach-rk30/include/mach/io.h b/arch/arm/mach-rk30/include/mach/io.h
index ce788b9b04c1..99ccf481a53a 100644
--- a/arch/arm/mach-rk30/include/mach/io.h
+++ b/arch/arm/mach-rk30/include/mach/io.h
@@ -33,6 +33,7 @@
#define RK30_L2MEM_PHYS 0x10000000
#define RK30_L2MEM_SIZE SZ_512K
#define RK30_IMEM_PHYS 0x10080000
+#define RK30_IMEM_BASE IOMEM(0xFEF00000)
#define RK30_IMEM_SIZE SZ_64K
#define RK30_GPU_PHYS 0x10090000
#define RK30_GPU_SIZE SZ_64K
diff --git a/arch/arm/mach-rk30/include/mach/memory.h b/arch/arm/mach-rk30/include/mach/memory.h
index 37d66ae13993..ab0b7fc032b2 100644
--- a/arch/arm/mach-rk30/include/mach/memory.h
+++ b/arch/arm/mach-rk30/include/mach/memory.h
@@ -2,6 +2,7 @@
#define __MACH_MEMORY_H
#include <linux/version.h>
+#include <mach/io.h>
/*
* Physical DRAM offset.
@@ -11,10 +12,10 @@
/*
* SRAM memory whereabouts
*/
-#define SRAM_CODE_OFFSET 0xFEF00100
-#define SRAM_CODE_END 0xFEF02FFF
-#define SRAM_DATA_OFFSET 0xFEF03000
-#define SRAM_DATA_END 0xFEF03FFF
+#define SRAM_CODE_OFFSET (RK30_IMEM_BASE + 0x0100)
+#define SRAM_CODE_END (RK30_IMEM_BASE + 0x2FFF)
+#define SRAM_DATA_OFFSET (RK30_IMEM_BASE + 0x3000)
+#define SRAM_DATA_END (RK30_IMEM_BASE + 0x3FFF)
#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 34))
#define dmac_clean_range(start, end) dmac_map_area(start, end - start, DMA_TO_DEVICE)
diff --git a/arch/arm/mach-rk30/include/mach/sram.h b/arch/arm/mach-rk30/include/mach/sram.h
new file mode 100644
index 000000000000..8fcb97883798
--- /dev/null
+++ b/arch/arm/mach-rk30/include/mach/sram.h
@@ -0,0 +1 @@
+#include <plat/sram.h>
diff --git a/arch/arm/mach-rk30/platsmp.c b/arch/arm/mach-rk30/platsmp.c
new file mode 100644
index 000000000000..ebcedb01f8b8
--- /dev/null
+++ b/arch/arm/mach-rk30/platsmp.c
@@ -0,0 +1,85 @@
+/*
+ * RK30 SMP source file. It contains platform specific fucntions
+ * needed for the linux smp kernel.
+ *
+ * Copyright (C) 2012 ROCKCHIP, Inc.
+ * All Rights Reserved
+ */
+
+#include <linux/init.h>
+#include <linux/smp.h>
+#include <linux/io.h>
+#include <linux/version.h>
+
+#include <asm/cacheflush.h>
+#include <asm/hardware/gic.h>
+#include <asm/smp_scu.h>
+
+#include <mach/pmu.h>
+
+void __cpuinit platform_secondary_init(unsigned int cpu)
+{
+ /*
+ * if any interrupts are already enabled for the primary
+ * core (e.g. timer irq), then they will not have been enabled
+ * for us: do so
+ */
+ gic_secondary_init(0);
+}
+
+extern void rk30_sram_secondary_startup(void);
+
+int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+ static bool copied;
+
+ if (!copied) {
+ unsigned long sz = 0x100;
+
+ memcpy(RK30_SCU_BASE + sz - 4, (void *)rk30_sram_secondary_startup + sz - 4, 4);
+ memcpy(RK30_IMEM_BASE, rk30_sram_secondary_startup, sz);
+ flush_icache_range((unsigned long)RK30_IMEM_BASE, (unsigned long)RK30_IMEM_BASE + sz);
+ copied = true;
+ }
+
+ dsb_sev();
+ pmu_set_power_domain(PD_A9_1, true);
+
+ return 0;
+}
+
+/*
+ * Initialise the CPU possible map early - this describes the CPUs
+ * which may be present or become present in the system.
+ */
+void __init smp_init_cpus(void)
+{
+ unsigned int i, ncores = scu_get_core_count(RK30_SCU_BASE);
+
+ if (ncores > nr_cpu_ids) {
+ pr_warn("SMP: %u cores greater than maximum (%u), clipping\n",
+ ncores, nr_cpu_ids);
+ ncores = nr_cpu_ids;
+ }
+
+ for (i = 0; i < ncores; i++)
+ set_cpu_possible(i, true);
+
+ set_smp_cross_call(gic_raise_softirq);
+}
+
+void __init platform_smp_prepare_cpus(unsigned int max_cpus)
+{
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(3, 1, 0))
+ int i;
+
+ /*
+ * Initialise the present map, which describes the set of CPUs
+ * actually populated at the present time.
+ */
+ for (i = 0; i < max_cpus; i++)
+ set_cpu_present(i, true);
+#endif
+
+ scu_enable(RK30_SCU_BASE);
+}
diff --git a/arch/arm/mach-rk30/pmu.c b/arch/arm/mach-rk30/pmu.c
new file mode 100644
index 000000000000..ec26036dba3a
--- /dev/null
+++ b/arch/arm/mach-rk30/pmu.c
@@ -0,0 +1,44 @@
+#include <linux/spinlock.h>
+#include <mach/pmu.h>
+#include <mach/sram.h>
+
+static void __sramfunc pmu_set_power_domain_sram(enum pmu_power_domain pd, bool on)
+{
+ u32 mask = 1 << pd;
+ u32 val = readl_relaxed(RK30_PMU_BASE + PMU_PWRDN_CON);
+
+ if (on)
+ val &= ~mask;
+ else
+ val |= mask;
+ writel_relaxed(val, RK30_PMU_BASE + PMU_PWRDN_CON);
+ dsb();
+
+ while (pmu_power_domain_is_on(pd) != on)
+ ;
+}
+
+static noinline void do_pmu_set_power_domain(enum pmu_power_domain pd, bool on)
+{
+ static unsigned long save_sp;
+
+ DDR_SAVE_SP(save_sp);
+ pmu_set_power_domain_sram(pd, on);
+ DDR_RESTORE_SP(save_sp);
+}
+
+/*
+ * software should power down or power up power domain one by one. Power down or
+ * power up multiple power domains simultaneously will result in chip electric current
+ * change dramatically which will affect the chip function.
+ */
+static DEFINE_SPINLOCK(pmu_pd_lock);
+
+void pmu_set_power_domain(enum pmu_power_domain pd, bool on)
+{
+ unsigned long flags;
+
+ spin_lock_irqsave(&pmu_pd_lock, flags);
+ do_pmu_set_power_domain(pd, on);
+ spin_unlock_irqrestore(&pmu_pd_lock, flags);
+}
diff --git a/arch/arm/plat-rk/include/plat/sram.h b/arch/arm/plat-rk/include/plat/sram.h
index c86b80a065b4..a987f729645a 100644
--- a/arch/arm/plat-rk/include/plat/sram.h
+++ b/arch/arm/plat-rk/include/plat/sram.h
@@ -3,6 +3,8 @@
#ifdef CONFIG_PLAT_RK
+#include <linux/init.h>
+
/* Tag variables with this */
#define __sramdata __section(.sram.data)
/* Tag constants with this */
@@ -24,7 +26,7 @@ static inline unsigned long ddr_save_sp(unsigned long new_sp)
}
// save_sp ±ØÐ붨ÒåΪȫ¾Ö±äÁ¿
-#define DDR_SAVE_SP(save_sp) do { save_sp = ddr_save_sp((SRAM_DATA_END&(~7))); } while (0)
+#define DDR_SAVE_SP(save_sp) do { save_sp = ddr_save_sp(((unsigned long)SRAM_DATA_END & (~7))); } while (0)
#define DDR_RESTORE_SP(save_sp) do { ddr_save_sp(save_sp); } while (0)
extern void __sramfunc sram_printch(char byte);
diff --git a/arch/arm/plat-rk/sram.c b/arch/arm/plat-rk/sram.c
index 8d8fb062f148..0edbf2f82c63 100644
--- a/arch/arm/plat-rk/sram.c
+++ b/arch/arm/plat-rk/sram.c
@@ -23,7 +23,7 @@ extern char __sram_data_start, __ssram_data, __esram_data;
static struct map_desc sram_code_iomap[] __initdata = {
{
- .virtual = SRAM_CODE_OFFSET & PAGE_MASK,
+ .virtual = (unsigned long)SRAM_CODE_OFFSET & PAGE_MASK,
.pfn = __phys_to_pfn(0x0),
.length = 1024*1024,
.type = MT_MEMORY