arch/arm/kernel/hyp-stub.S

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2 /*
   3  * Copyright (c) 2012 Linaro Limited.
   4  */
   5
   6 #include <linux/init.h>
   7 #include <linux/irqchip/arm-gic-v3.h>
   8 #include <linux/linkage.h>
   9 #include <asm/assembler.h>
  10 #include <asm/virt.h>
  11
  12 #ifndef ZIMAGE
  13 /*
  14  * For the kernel proper, we need to find out the CPU boot mode long after
  15  * boot, so we need to store it in a writable variable.
  16  *
  17  * This is not in .bss, because we set it sufficiently early that the boot-time
  18  * zeroing of .bss would clobber it.
  19  */
  20 .data
  21         .align  2
  22 ENTRY(__boot_cpu_mode)
  23         .long   0
  24 .text
  25
  26         /*
  27          * Save the primary CPU boot mode. Requires 2 scratch registers.
  28          */
  29         .macro  store_primary_cpu_mode  reg1, reg2
  30         mrs     \reg1, cpsr
  31         and     \reg1, \reg1, #MODE_MASK
  32         str_l   \reg1, __boot_cpu_mode, \reg2
  33         .endm
  34
  35         /*
  36          * Compare the current mode with the one saved on the primary CPU.
  37          * If they don't match, record that fact. The Z bit indicates
  38          * if there's a match or not.
  39          * Requires 2 additional scratch registers.
  40          */
  41         .macro  compare_cpu_mode_with_primary mode, reg1, reg2
  42         adr_l   \reg2, __boot_cpu_mode
  43         ldr     \reg1, [\reg2]
  44         cmp     \mode, \reg1            @ matches primary CPU boot mode?
  45         orrne   \reg1, \reg1, #BOOT_CPU_MODE_MISMATCH
  46         strne   \reg1, [\reg2]          @ record what happened and give up
  47         .endm
  48
  49 #else   /* ZIMAGE */
  50
  51         .macro  store_primary_cpu_mode  reg1:req, reg2:req
  52         .endm
  53
  54 /*
  55  * The zImage loader only runs on one CPU, so we don't bother with mult-CPU
  56  * consistency checking:
  57  */
  58         .macro  compare_cpu_mode_with_primary mode, reg1, reg2
  59         cmp     \mode, \mode
  60         .endm
  61
  62 #endif /* ZIMAGE */
  63
  64 /*
  65  * Hypervisor stub installation functions.
  66  *
  67  * These must be called with the MMU and D-cache off.
  68  * They are not ABI compliant and are only intended to be called from the kernel
  69  * entry points in head.S.
  70  */
  71 @ Call this from the primary CPU
  72 ENTRY(__hyp_stub_install)
  73         store_primary_cpu_mode  r4, r5
  74 ENDPROC(__hyp_stub_install)
  75
  76         @ fall through...
  77
  78 @ Secondary CPUs should call here
  79 ENTRY(__hyp_stub_install_secondary)
  80         mrs     r4, cpsr
  81         and     r4, r4, #MODE_MASK
  82
  83         /*
  84          * If the secondary has booted with a different mode, give up
  85          * immediately.
  86          */
  87         compare_cpu_mode_with_primary   r4, r5, r6
  88         retne   lr
  89
  90         /*
  91          * Once we have given up on one CPU, we do not try to install the
  92          * stub hypervisor on the remaining ones: because the saved boot mode
  93          * is modified, it can't compare equal to the CPSR mode field any
  94          * more.
  95          *
  96          * Otherwise...
  97          */
  98
  99         cmp     r4, #HYP_MODE
 100         retne   lr                      @ give up if the CPU is not in HYP mode
 101
 102 /*
 103  * Configure HSCTLR to set correct exception endianness/instruction set
 104  * state etc.
 105  * Turn off all traps
 106  * Eventually, CPU-specific code might be needed -- assume not for now
 107  *
 108  * This code relies on the "eret" instruction to synchronize the
 109  * various coprocessor accesses. This is done when we switch to SVC
 110  * (see safe_svcmode_maskall).
 111  */
 112         @ Now install the hypervisor stub:
 113         W(adr)  r7, __hyp_stub_vectors
 114         mcr     p15, 4, r7, c12, c0, 0  @ set hypervisor vector base (HVBAR)
 115
 116         @ Disable all traps, so we don't get any nasty surprise
 117         mov     r7, #0
 118         mcr     p15, 4, r7, c1, c1, 0   @ HCR
 119         mcr     p15, 4, r7, c1, c1, 2   @ HCPTR
 120         mcr     p15, 4, r7, c1, c1, 3   @ HSTR
 121
 122 THUMB(  orr     r7, #(1 << 30)  )       @ HSCTLR.TE
 123 ARM_BE8(orr     r7, r7, #(1 << 25))     @ HSCTLR.EE
 124         mcr     p15, 4, r7, c1, c0, 0   @ HSCTLR
 125
 126         mrc     p15, 4, r7, c1, c1, 1   @ HDCR
 127         and     r7, #0x1f               @ Preserve HPMN
 128         mcr     p15, 4, r7, c1, c1, 1   @ HDCR
 129
 130         @ Make sure NS-SVC is initialised appropriately
 131         mrc     p15, 0, r7, c1, c0, 0   @ SCTLR
 132         orr     r7, #(1 << 5)           @ CP15 barriers enabled
 133         bic     r7, #(3 << 7)           @ Clear SED/ITD for v8 (RES0 for v7)
 134         bic     r7, #(3 << 19)          @ WXN and UWXN disabled
 135         mcr     p15, 0, r7, c1, c0, 0   @ SCTLR
 136
 137         mrc     p15, 0, r7, c0, c0, 0   @ MIDR
 138         mcr     p15, 4, r7, c0, c0, 0   @ VPIDR
 139
 140         mrc     p15, 0, r7, c0, c0, 5   @ MPIDR
 141         mcr     p15, 4, r7, c0, c0, 5   @ VMPIDR
 142
 143 #if !defined(ZIMAGE) && defined(CONFIG_ARM_ARCH_TIMER)
 144         @ make CNTP_* and CNTPCT accessible from PL1
 145         mrc     p15, 0, r7, c0, c1, 1   @ ID_PFR1
 146         ubfx    r7, r7, #16, #4
 147         teq     r7, #0
 148         beq     1f
 149         mrc     p15, 4, r7, c14, c1, 0  @ CNTHCTL
 150         orr     r7, r7, #3              @ PL1PCEN | PL1PCTEN
 151         mcr     p15, 4, r7, c14, c1, 0  @ CNTHCTL
 152         mov     r7, #0
 153         mcrr    p15, 4, r7, r7, c14     @ CNTVOFF
 154
 155         @ Disable virtual timer in case it was counting
 156         mrc     p15, 0, r7, c14, c3, 1  @ CNTV_CTL
 157         bic     r7, #1                  @ Clear ENABLE
 158         mcr     p15, 0, r7, c14, c3, 1  @ CNTV_CTL
 159 1:
 160 #endif
 161
 162 #ifdef CONFIG_ARM_GIC_V3
 163         @ Check whether GICv3 system registers are available
 164         mrc     p15, 0, r7, c0, c1, 1   @ ID_PFR1
 165         ubfx    r7, r7, #28, #4
 166         teq     r7, #0
 167         beq     2f
 168
 169         @ Enable system register accesses
 170         mrc     p15, 4, r7, c12, c9, 5  @ ICC_HSRE
 171         orr     r7, r7, #(ICC_SRE_EL2_ENABLE | ICC_SRE_EL2_SRE)
 172         mcr     p15, 4, r7, c12, c9, 5  @ ICC_HSRE
 173         isb
 174
 175         @ SRE bit could be forced to 0 by firmware.
 176         @ Check whether it sticks before accessing any other sysreg
 177         mrc     p15, 4, r7, c12, c9, 5  @ ICC_HSRE
 178         tst     r7, #ICC_SRE_EL2_SRE
 179         beq     2f
 180         mov     r7, #0
 181         mcr     p15, 4, r7, c12, c11, 0 @ ICH_HCR
 182 2:
 183 #endif
 184
 185         bx      lr                      @ The boot CPU mode is left in r4.
 186 ENDPROC(__hyp_stub_install_secondary)
 187
 188 __hyp_stub_do_trap:
 189 #ifdef ZIMAGE
 190         teq     r0, #HVC_SET_VECTORS
 191         bne     1f
 192         /* Only the ZIMAGE stubs can change the HYP vectors */
 193         mcr     p15, 4, r1, c12, c0, 0  @ set HVBAR
 194         b       __hyp_stub_exit
 195 #endif
 196
 197 1:      teq     r0, #HVC_SOFT_RESTART
 198         bne     2f
 199         bx      r1
 200
 201 2:      ldr     r0, =HVC_STUB_ERR
 202         __ERET
 203
 204 __hyp_stub_exit:
 205         mov     r0, #0
 206         __ERET
 207 ENDPROC(__hyp_stub_do_trap)
 208
 209 /*
 210  * __hyp_set_vectors is only used when ZIMAGE must bounce between HYP
 211  * and SVC. For the kernel itself, the vectors are set once and for
 212  * all by the stubs.
 213  */
 214 ENTRY(__hyp_set_vectors)
 215         mov     r1, r0
 216         mov     r0, #HVC_SET_VECTORS
 217         __HVC(0)
 218         ret     lr
 219 ENDPROC(__hyp_set_vectors)
 220
 221 ENTRY(__hyp_soft_restart)
 222         mov     r1, r0
 223         mov     r0, #HVC_SOFT_RESTART
 224         __HVC(0)
 225         ret     lr
 226 ENDPROC(__hyp_soft_restart)
 227
 228 .align 5
 229 ENTRY(__hyp_stub_vectors)
 230 __hyp_stub_reset:       W(b)    .
 231 __hyp_stub_und:         W(b)    .
 232 __hyp_stub_svc:         W(b)    .
 233 __hyp_stub_pabort:      W(b)    .
 234 __hyp_stub_dabort:      W(b)    .
 235 __hyp_stub_trap:        W(b)    __hyp_stub_do_trap
 236 __hyp_stub_irq:         W(b)    .
 237 __hyp_stub_fiq:         W(b)    .
 238 ENDPROC(__hyp_stub_vectors)
 239