DRA7: fix ABB efuse offset for OPP_NOM

[u-boot] / arch / arm / cpu / armv7 / nonsec_virt.S

diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index 3dd60b7137a8c50ba159f8bc91428fc263ba93b4..6367e09612bcd04056c079c35dbd79033d49a157 100644 (file)
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -1,25 +1,9 @@
 /*
- * code for switching cores into non-secure state
+ * code for switching cores into non-secure state and into HYP mode
  *
  * Copyright (c) 2013  Andre Przywara <andre.przywara@linaro.org>
  *
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
+ * SPDX-License-Identifier:    GPL-2.0+
  */
 
 #include <config.h>
@@ -28,15 +12,17 @@
 #include <asm/armv7.h>
 
 .arch_extension sec
+.arch_extension virt
 
-/* the vector table for secure state */
+       .align  5
+/* the vector table for secure state and HYP mode */
 _monitor_vectors:
        .word 0 /* reset */
        .word 0 /* undef */
        adr pc, _secure_monitor
        .word 0
        .word 0
-       .word 0
+       adr pc, _hyp_trap
        .word 0
        .word 0
 
@@ -47,16 +33,58 @@ _monitor_vectors:
  * to non-secure state.
  * We use only r0 and r1 here, due to constraints in the caller.
  */
-       .align  5
 _secure_monitor:
        mrc     p15, 0, r1, c1, c1, 0           @ read SCR
        bic     r1, r1, #0x4e                   @ clear IRQ, FIQ, EA, nET bits
        orr     r1, r1, #0x31                   @ enable NS, AW, FW bits
 
+#ifdef CONFIG_ARMV7_VIRT
+       mrc     p15, 0, r0, c0, c1, 1           @ read ID_PFR1
+       and     r0, r0, #CPUID_ARM_VIRT_MASK    @ mask virtualization bits
+       cmp     r0, #(1 << CPUID_ARM_VIRT_SHIFT)
+       orreq   r1, r1, #0x100                  @ allow HVC instruction
+#endif
+
        mcr     p15, 0, r1, c1, c1, 0           @ write SCR (with NS bit set)
 
+#ifdef CONFIG_ARMV7_VIRT
+       mrceq   p15, 0, r0, c12, c0, 1          @ get MVBAR value
+       mcreq   p15, 4, r0, c12, c0, 0          @ write HVBAR
+#endif
+
        movs    pc, lr                          @ return to non-secure SVC
 
+_hyp_trap:
+       mrs     lr, elr_hyp     @ for older asm: .byte 0x00, 0xe3, 0x0e, 0xe1
+       mov pc, lr                              @ do no switch modes, but
+                                               @ return to caller
+
+/*
+ * 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
+       mov     r12, r0                         @ save GICC address
+#ifdef CONFIG_ARMV7_VIRT
+       bl      _switch_to_hyp
+#endif
+
+       ldr     r1, [r12, #GICC_IAR]            @ acknowledge IPI
+       str     r1, [r12, #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.
  *
@@ -138,3 +166,27 @@ 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
+
+ENTRY(_switch_to_hyp)
+       mov     r0, lr
+       mov     r1, sp                          @ save SVC copy of LR and SP
+       isb
+       hvc #0                   @ for older asm: .byte 0x70, 0x00, 0x40, 0xe1
+       mov     sp, r1
+       mov     lr, r0                          @ restore SVC copy of LR and SP
+
+       bx      lr
+ENDPROC(_switch_to_hyp)