ARM: HYP/non-sec: add the option for a second-stage monitor

Allow the switch to a second stage secure monitor just before
switching to non-secure.

This allows a resident piece of firmware to be active once the
kernel has been entered (the u-boot monitor is dead anyway,
its pages being reused).

Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
Acked-by: Ian Campbell <ijc@hellion.org.uk>

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 2a43e3c81e255a1082bc6473a76e35372f163efb..745670e549d90c0870fbcf05f30ad62ca175a53b 100644 (file)
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -44,10 +44,19 @@ _monitor_vectors:
  * ip: target PC
  */
 _secure_monitor:
+#ifdef CONFIG_ARMV7_PSCI
+       ldr     r5, =_psci_vectors              @ Switch to the next monitor
+       mcr     p15, 0, r5, c12, c0, 1
+       isb
+
+       @ Obtain a secure stack, and configure the PSCI backend
+       bl      psci_arch_init
+#endif
+
        mrc     p15, 0, r5, c1, c1, 0           @ read SCR
-       bic     r5, r5, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
+       bic     r5, r5, #0x4a                   @ clear IRQ, EA, nET bits
        orr     r5, r5, #0x31                   @ enable NS, AW, FW bits
-
+                                               @ FIQ preserved for secure mode
        mov     r6, #SVC_MODE                   @ default mode is SVC
        is_cpu_virt_capable r4
 #ifdef CONFIG_ARMV7_VIRT