summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--arch/arm/cpu/armv7/nonsec_virt.S35
-rw-r--r--arch/arm/cpu/armv7/virt-v7.c16
-rw-r--r--arch/arm/include/asm/armv7.h1
-rw-r--r--arch/arm/include/asm/gic.h2
-rw-r--r--include/common.h2
5 files changed, 55 insertions, 1 deletions
diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 3dd60b7..cbee8f7 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -58,6 +58,28 @@ _secure_monitor:
movs pc, lr @ return to non-secure SVC
/*
+ * Secondary CPUs start here and call the code for the core specific parts
+ * of the non-secure and HYP mode transition. The GIC distributor specific
+ * code has already been executed by a C function before.
+ * Then they go back to wfi and wait to be woken up by the kernel again.
+ */
+ENTRY(_smp_pen)
+ mrs r0, cpsr
+ orr r0, r0, #0xc0
+ msr cpsr, r0 @ disable interrupts
+ ldr r1, =_start
+ mcr p15, 0, r1, c12, c0, 0 @ set VBAR
+
+ bl _nonsec_init
+
+ ldr r1, [r0, #GICC_IAR] @ acknowledge IPI
+ str r1, [r0, #GICC_EOIR] @ signal end of interrupt
+
+ adr r0, _smp_pen @ do not use this address again
+ b smp_waitloop @ wait for IPIs, board specific
+ENDPROC(_smp_pen)
+
+/*
* Switch a core to non-secure state.
*
* 1. initialize the GIC per-core interface
@@ -138,3 +160,16 @@ ENTRY(_nonsec_init)
bx lr
ENDPROC(_nonsec_init)
+
+#ifdef CONFIG_SMP_PEN_ADDR
+/* void __weak smp_waitloop(unsigned previous_address); */
+ENTRY(smp_waitloop)
+ wfi
+ ldr r1, =CONFIG_SMP_PEN_ADDR @ load start address
+ ldr r1, [r1]
+ cmp r0, r1 @ make sure we dont execute this code
+ beq smp_waitloop @ again (due to a spurious wakeup)
+ mov pc, r1
+ENDPROC(smp_waitloop)
+.weak smp_waitloop
+#endif
diff --git a/arch/arm/cpu/armv7/virt-v7.c b/arch/arm/cpu/armv7/virt-v7.c
index 068ac85..a0b8742 100644
--- a/arch/arm/cpu/armv7/virt-v7.c
+++ b/arch/arm/cpu/armv7/virt-v7.c
@@ -79,6 +79,17 @@ static unsigned long get_gicd_base_address(void)
#endif
}
+static void kick_secondary_cpus_gic(unsigned long gicdaddr)
+{
+ /* kick all CPUs (except this one) by writing to GICD_SGIR */
+ writel(1U << 24, gicdaddr + GICD_SGIR);
+}
+
+void __weak smp_kick_all_cpus(void)
+{
+ kick_secondary_cpus_gic(gic_dist_addr);
+}
+
int armv7_switch_nonsec(void)
{
unsigned int reg;
@@ -115,7 +126,10 @@ int armv7_switch_nonsec(void)
for (i = 1; i <= itlinesnr; i++)
writel((unsigned)-1, gic_dist_addr + GICD_IGROUPRn + 4 * i);
- /* call the non-sec switching code on this CPU */
+ smp_set_core_boot_addr((unsigned long)_smp_pen, -1);
+ smp_kick_all_cpus();
+
+ /* call the non-sec switching code on this CPU also */
_nonsec_init();
return 0;
diff --git a/arch/arm/include/asm/armv7.h b/arch/arm/include/asm/armv7.h
index b352d43..2efd4bc 100644
--- a/arch/arm/include/asm/armv7.h
+++ b/arch/arm/include/asm/armv7.h
@@ -82,6 +82,7 @@ int armv7_switch_nonsec(void);
/* defined in assembly file */
unsigned int _nonsec_init(void);
+void _smp_pen(void);
#endif /* CONFIG_ARMV7_NONSEC */
#endif /* ! __ASSEMBLY__ */
diff --git a/arch/arm/include/asm/gic.h b/arch/arm/include/asm/gic.h
index c2b1e28..a0891cc 100644
--- a/arch/arm/include/asm/gic.h
+++ b/arch/arm/include/asm/gic.h
@@ -13,5 +13,7 @@
#define GIC_CPU_OFFSET_A15 0x2000
#define GICC_CTLR 0x0000
#define GICC_PMR 0x0004
+#define GICC_IAR 0x000C
+#define GICC_EOIR 0x0010
#endif
diff --git a/include/common.h b/include/common.h
index 8addf43..4d2a56d 100644
--- a/include/common.h
+++ b/include/common.h
@@ -627,6 +627,8 @@ void ft_pci_setup(void *blob, bd_t *bd);
#endif
#endif
+void smp_set_core_boot_addr(unsigned long addr, int corenr);
+void smp_kick_all_cpus(void);
/* $(CPU)/serial.c */
int serial_init (void);