1/* 2 * Copyright (c) 2012 Linaro Limited. 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License as published by 6 * the Free Software Foundation; either version 2 of the License, or 7 * (at your option) any later version. 8 * 9 * This program is distributed in the hope that it will be useful, 10 * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 * GNU General Public License for more details. 13 * 14 * You should have received a copy of the GNU General Public License along 15 * with this program; if not, write to the Free Software Foundation, Inc., 16 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 */ 18 19#include <linux/init.h> 20#include <linux/irqchip/arm-gic-v3.h> 21#include <linux/linkage.h> 22#include <asm/assembler.h> 23#include <asm/virt.h> 24 25#ifndef ZIMAGE 26/* 27 * For the kernel proper, we need to find out the CPU boot mode long after 28 * boot, so we need to store it in a writable variable. 29 * 30 * This is not in .bss, because we set it sufficiently early that the boot-time 31 * zeroing of .bss would clobber it. 32 */ 33.data 34 .align 2 35ENTRY(__boot_cpu_mode) 36 .long 0 37.text 38 39 /* 40 * Save the primary CPU boot mode. Requires 3 scratch registers. 41 */ 42 .macro store_primary_cpu_mode reg1, reg2, reg3 43 mrs \reg1, cpsr 44 and \reg1, \reg1, #MODE_MASK 45 adr \reg2, .L__boot_cpu_mode_offset 46 ldr \reg3, [\reg2] 47 str \reg1, [\reg2, \reg3] 48 .endm 49 50 /* 51 * Compare the current mode with the one saved on the primary CPU. 52 * If they don't match, record that fact. The Z bit indicates 53 * if there's a match or not. 54 * Requires 3 additionnal scratch registers. 55 */ 56 .macro compare_cpu_mode_with_primary mode, reg1, reg2, reg3 57 adr \reg2, .L__boot_cpu_mode_offset 58 ldr \reg3, [\reg2] 59 ldr \reg1, [\reg2, \reg3] 60 cmp \mode, \reg1 @ matches primary CPU boot mode? 61 orrne \reg1, \reg1, #BOOT_CPU_MODE_MISMATCH 62 strne \reg1, [\reg2, \reg3] @ record what happened and give up 63 .endm 64 65#else /* ZIMAGE */ 66 67 .macro store_primary_cpu_mode reg1:req, reg2:req, reg3:req 68 .endm 69 70/* 71 * The zImage loader only runs on one CPU, so we don't bother with mult-CPU 72 * consistency checking: 73 */ 74 .macro compare_cpu_mode_with_primary mode, reg1, reg2, reg3 75 cmp \mode, \mode 76 .endm 77 78#endif /* ZIMAGE */ 79 80/* 81 * Hypervisor stub installation functions. 82 * 83 * These must be called with the MMU and D-cache off. 84 * They are not ABI compliant and are only intended to be called from the kernel 85 * entry points in head.S. 86 */ 87@ Call this from the primary CPU 88ENTRY(__hyp_stub_install) 89 store_primary_cpu_mode r4, r5, r6 90ENDPROC(__hyp_stub_install) 91 92 @ fall through... 93 94@ Secondary CPUs should call here 95ENTRY(__hyp_stub_install_secondary) 96 mrs r4, cpsr 97 and r4, r4, #MODE_MASK 98 99 /* 100 * If the secondary has booted with a different mode, give up 101 * immediately. 102 */ 103 compare_cpu_mode_with_primary r4, r5, r6, r7 104 retne lr 105 106 /* 107 * Once we have given up on one CPU, we do not try to install the 108 * stub hypervisor on the remaining ones: because the saved boot mode 109 * is modified, it can't compare equal to the CPSR mode field any 110 * more. 111 * 112 * Otherwise... 113 */ 114 115 cmp r4, #HYP_MODE 116 retne lr @ give up if the CPU is not in HYP mode 117 118/* 119 * Configure HSCTLR to set correct exception endianness/instruction set 120 * state etc. 121 * Turn off all traps 122 * Eventually, CPU-specific code might be needed -- assume not for now 123 * 124 * This code relies on the "eret" instruction to synchronize the 125 * various coprocessor accesses. This is done when we switch to SVC 126 * (see safe_svcmode_maskall). 127 */ 128 @ Now install the hypervisor stub: 129 W(adr) r7, __hyp_stub_vectors 130 mcr p15, 4, r7, c12, c0, 0 @ set hypervisor vector base (HVBAR) 131 132 @ Disable all traps, so we don't get any nasty surprise 133 mov r7, #0 134 mcr p15, 4, r7, c1, c1, 0 @ HCR 135 mcr p15, 4, r7, c1, c1, 2 @ HCPTR 136 mcr p15, 4, r7, c1, c1, 3 @ HSTR 137 138THUMB( orr r7, #(1 << 30) ) @ HSCTLR.TE 139ARM_BE8(orr r7, r7, #(1 << 25)) @ HSCTLR.EE 140 mcr p15, 4, r7, c1, c0, 0 @ HSCTLR 141 142 mrc p15, 4, r7, c1, c1, 1 @ HDCR 143 and r7, #0x1f @ Preserve HPMN 144 mcr p15, 4, r7, c1, c1, 1 @ HDCR 145 146 @ Make sure NS-SVC is initialised appropriately 147 mrc p15, 0, r7, c1, c0, 0 @ SCTLR 148 orr r7, #(1 << 5) @ CP15 barriers enabled 149 bic r7, #(3 << 7) @ Clear SED/ITD for v8 (RES0 for v7) 150 bic r7, #(3 << 19) @ WXN and UWXN disabled 151 mcr p15, 0, r7, c1, c0, 0 @ SCTLR 152 153 mrc p15, 0, r7, c0, c0, 0 @ MIDR 154 mcr p15, 4, r7, c0, c0, 0 @ VPIDR 155 156 mrc p15, 0, r7, c0, c0, 5 @ MPIDR 157 mcr p15, 4, r7, c0, c0, 5 @ VMPIDR 158 159#if !defined(ZIMAGE) && defined(CONFIG_ARM_ARCH_TIMER) 160 @ make CNTP_* and CNTPCT accessible from PL1 161 mrc p15, 0, r7, c0, c1, 1 @ ID_PFR1 162 lsr r7, #16 163 and r7, #0xf 164 cmp r7, #1 165 bne 1f 166 mrc p15, 4, r7, c14, c1, 0 @ CNTHCTL 167 orr r7, r7, #3 @ PL1PCEN | PL1PCTEN 168 mcr p15, 4, r7, c14, c1, 0 @ CNTHCTL 169 mov r7, #0 170 mcrr p15, 4, r7, r7, c14 @ CNTVOFF 171 172 @ Disable virtual timer in case it was counting 173 mrc p15, 0, r7, c14, c3, 1 @ CNTV_CTL 174 bic r7, #1 @ Clear ENABLE 175 mcr p15, 0, r7, c14, c3, 1 @ CNTV_CTL 1761: 177#endif 178 179#ifdef CONFIG_ARM_GIC_V3 180 @ Check whether GICv3 system registers are available 181 mrc p15, 0, r7, c0, c1, 1 @ ID_PFR1 182 ubfx r7, r7, #28, #4 183 teq r7, #0 184 beq 2f 185 186 @ Enable system register accesses 187 mrc p15, 4, r7, c12, c9, 5 @ ICC_HSRE 188 orr r7, r7, #(ICC_SRE_EL2_ENABLE | ICC_SRE_EL2_SRE) 189 mcr p15, 4, r7, c12, c9, 5 @ ICC_HSRE 190 isb 191 192 @ SRE bit could be forced to 0 by firmware. 193 @ Check whether it sticks before accessing any other sysreg 194 mrc p15, 4, r7, c12, c9, 5 @ ICC_HSRE 195 tst r7, #ICC_SRE_EL2_SRE 196 beq 2f 197 mov r7, #0 198 mcr p15, 4, r7, c12, c11, 0 @ ICH_HCR 1992: 200#endif 201 202 bx lr @ The boot CPU mode is left in r4. 203ENDPROC(__hyp_stub_install_secondary) 204 205__hyp_stub_do_trap: 206 teq r0, #HVC_SET_VECTORS 207 bne 1f 208 mcr p15, 4, r1, c12, c0, 0 @ set HVBAR 209 b __hyp_stub_exit 210 2111: teq r0, #HVC_SOFT_RESTART 212 bne 1f 213 bx r1 214 2151: teq r0, #HVC_RESET_VECTORS 216 beq __hyp_stub_exit 217 218 ldr r0, =HVC_STUB_ERR 219 __ERET 220 221__hyp_stub_exit: 222 mov r0, #0 223 __ERET 224ENDPROC(__hyp_stub_do_trap) 225 226/* 227 * __hyp_set_vectors: Call this after boot to set the initial hypervisor 228 * vectors as part of hypervisor installation. On an SMP system, this should 229 * be called on each CPU. 230 * 231 * r0 must be the physical address of the new vector table (which must lie in 232 * the bottom 4GB of physical address space. 233 * 234 * r0 must be 32-byte aligned. 235 * 236 * Before calling this, you must check that the stub hypervisor is installed 237 * everywhere, by waiting for any secondary CPUs to be brought up and then 238 * checking that BOOT_CPU_MODE_HAVE_HYP(__boot_cpu_mode) is true. 239 * 240 * If not, there is a pre-existing hypervisor, some CPUs failed to boot, or 241 * something else went wrong... in such cases, trying to install a new 242 * hypervisor is unlikely to work as desired. 243 * 244 * When you call into your shiny new hypervisor, sp_hyp will contain junk, 245 * so you will need to set that to something sensible at the new hypervisor's 246 * initialisation entry point. 247 */ 248ENTRY(__hyp_set_vectors) 249 mov r1, r0 250 mov r0, #HVC_SET_VECTORS 251 __HVC(0) 252 ret lr 253ENDPROC(__hyp_set_vectors) 254 255ENTRY(__hyp_soft_restart) 256 mov r1, r0 257 mov r0, #HVC_SOFT_RESTART 258 __HVC(0) 259 ret lr 260ENDPROC(__hyp_soft_restart) 261 262ENTRY(__hyp_reset_vectors) 263 mov r0, #HVC_RESET_VECTORS 264 __HVC(0) 265 ret lr 266ENDPROC(__hyp_reset_vectors) 267 268#ifndef ZIMAGE 269.align 2 270.L__boot_cpu_mode_offset: 271 .long __boot_cpu_mode - . 272#endif 273 274.align 5 275ENTRY(__hyp_stub_vectors) 276__hyp_stub_reset: W(b) . 277__hyp_stub_und: W(b) . 278__hyp_stub_svc: W(b) . 279__hyp_stub_pabort: W(b) . 280__hyp_stub_dabort: W(b) . 281__hyp_stub_trap: W(b) __hyp_stub_do_trap 282__hyp_stub_irq: W(b) . 283__hyp_stub_fiq: W(b) . 284ENDPROC(__hyp_stub_vectors) 285 286