1/* SPDX-License-Identifier: BSD-2-Clause */ 2/* 3 * Copyright (c) 2016, Linaro Limited 4 * Copyright (c) 2014, STMicroelectronics International N.V. 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions are met: 9 * 10 * 1. Redistributions of source code must retain the above copyright notice, 11 * this list of conditions and the following disclaimer. 12 * 13 * 2. Redistributions in binary form must reproduce the above copyright notice, 14 * this list of conditions and the following disclaimer in the documentation 15 * and/or other materials provided with the distribution. 16 * 17 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 * POSSIBILITY OF SUCH DAMAGE. 28 */ 29 30#include <arm32_macros.S> 31#include <arm.h> 32#include <asm.S> 33#include <generated/asm-defines.h> 34#include <keep.h> 35#include <kernel/unwind.h> 36#include <sm/optee_smc.h> 37#include <sm/teesmc_opteed.h> 38#include <sm/teesmc_opteed_macros.h> 39#include <util.h> 40 41 .section .text.sm_asm 42 43FUNC sm_save_modes_regs , : 44UNWIND( .fnstart) 45UNWIND( .cantunwind) 46 /* User mode registers has to be saved from system mode */ 47 cps #CPSR_MODE_SYS 48 stm r0!, {sp, lr} 49 50 cps #CPSR_MODE_IRQ 51 mrs r2, spsr 52 stm r0!, {r2, sp, lr} 53 54 cps #CPSR_MODE_FIQ 55 mrs r2, spsr 56 stm r0!, {r2, sp, lr} 57 58 cps #CPSR_MODE_SVC 59 mrs r2, spsr 60 stm r0!, {r2, sp, lr} 61 62 cps #CPSR_MODE_ABT 63 mrs r2, spsr 64 stm r0!, {r2, sp, lr} 65 66 cps #CPSR_MODE_UND 67 mrs r2, spsr 68 stm r0!, {r2, sp, lr} 69 70 cps #CPSR_MODE_MON 71 bx lr 72UNWIND( .fnend) 73END_FUNC sm_save_modes_regs 74 75/* Restores the mode specific registers */ 76FUNC sm_restore_modes_regs , : 77UNWIND( .fnstart) 78UNWIND( .cantunwind) 79 /* User mode registers has to be saved from system mode */ 80 cps #CPSR_MODE_SYS 81 ldm r0!, {sp, lr} 82 83 cps #CPSR_MODE_IRQ 84 ldm r0!, {r2, sp, lr} 85 msr spsr_fsxc, r2 86 87 cps #CPSR_MODE_FIQ 88 ldm r0!, {r2, sp, lr} 89 msr spsr_fsxc, r2 90 91 cps #CPSR_MODE_SVC 92 ldm r0!, {r2, sp, lr} 93 msr spsr_fsxc, r2 94 95 cps #CPSR_MODE_ABT 96 ldm r0!, {r2, sp, lr} 97 msr spsr_fsxc, r2 98 99 cps #CPSR_MODE_UND 100 ldm r0!, {r2, sp, lr} 101 msr spsr_fsxc, r2 102 103 cps #CPSR_MODE_MON 104 bx lr 105UNWIND( .fnend) 106END_FUNC sm_restore_modes_regs 107 108/* 109 * stack_tmp is used as stack, the top of the stack is reserved to hold 110 * struct sm_ctx, everything below is for normal stack usage. As several 111 * different CPU modes are using the same stack it's important that switch 112 * of CPU mode isn't done until one mode is done. This means FIQ, IRQ and 113 * Async abort has to be masked while using stack_tmp. 114 */ 115LOCAL_FUNC sm_smc_entry , : 116UNWIND( .fnstart) 117UNWIND( .cantunwind) 118 srsdb sp!, #CPSR_MODE_MON 119 push {r0-r7} 120 121 clrex /* Clear the exclusive monitor */ 122 123 /* Find out if we're doing an secure or non-secure entry */ 124 read_scr r1 125 tst r1, #SCR_NS 126 bne .smc_from_nsec 127 128 /* 129 * As we're coming from secure world (NS bit cleared) the stack 130 * pointer points to sm_ctx.sec.r0 at this stage. After the 131 * instruction below the stack pointer points to sm_ctx. 132 */ 133 sub sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0) 134 135 /* Save secure context */ 136 add r0, sp, #SM_CTX_SEC 137 bl sm_save_modes_regs 138 139 /* 140 * On FIQ exit we're restoring the non-secure context unchanged, on 141 * all other exits we're shifting r1-r4 from secure context into 142 * r0-r3 in non-secure context. 143 */ 144 add r8, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0) 145 ldm r8, {r0-r4} 146 mov_imm r9, TEESMC_OPTEED_RETURN_FIQ_DONE 147 cmp r0, r9 148 addne r8, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 149 stmne r8, {r1-r4} 150 151 /* Restore non-secure context */ 152 add r0, sp, #SM_CTX_NSEC 153 bl sm_restore_modes_regs 154 155.sm_ret_to_nsec: 156 /* 157 * Return to non-secure world 158 */ 159 add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8) 160 ldm r0, {r8-r12} 161 162 /* Update SCR */ 163 read_scr r0 164 orr r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ 165 write_scr r0 166 /* 167 * isb not needed since we're doing an exception return below 168 * without dependency to the changes in SCR before that. 169 */ 170 171 add sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 172 b .sm_exit 173 174.smc_from_nsec: 175 /* 176 * As we're coming from non-secure world (NS bit set) the stack 177 * pointer points to sm_ctx.nsec.r0 at this stage. After the 178 * instruction below the stack pointer points to sm_ctx. 179 */ 180 sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 181 182 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */ 183 write_scr r1 184 isb 185 186 add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8) 187 stm r0, {r8-r12} 188 189 mov r0, sp 190 bl sm_from_nsec 191 cmp r0, #0 192 beq .sm_ret_to_nsec 193 194 /* 195 * Continue into secure world 196 */ 197 add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0) 198 199.sm_exit: 200 pop {r0-r7} 201 rfefd sp! 202UNWIND( .fnend) 203END_FUNC sm_smc_entry 204 205/* 206 * FIQ handling 207 * 208 * Saves CPU context in the same way as sm_smc_entry() above. The CPU 209 * context will later be restored by sm_smc_entry() when handling a return 210 * from FIQ. 211 */ 212LOCAL_FUNC sm_fiq_entry , : 213UNWIND( .fnstart) 214UNWIND( .cantunwind) 215 /* FIQ has a +4 offset for lr compared to preferred return address */ 216 sub lr, lr, #4 217 /* sp points just past struct sm_sec_ctx */ 218 srsdb sp!, #CPSR_MODE_MON 219 push {r0-r7} 220 221 clrex /* Clear the exclusive monitor */ 222 223 /* 224 * As we're coming from non-secure world the stack pointer points 225 * to sm_ctx.nsec.r0 at this stage. After the instruction below the 226 * stack pointer points to sm_ctx. 227 */ 228 sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 229 230 /* Update SCR */ 231 read_scr r1 232 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */ 233 write_scr r1 234 isb 235 236 /* Save non-secure context */ 237 add r0, sp, #SM_CTX_NSEC 238 bl sm_save_modes_regs 239 stm r0!, {r8-r12} 240 241 /* Set FIQ entry */ 242 ldr r0, =(thread_vector_table + THREAD_VECTOR_TABLE_FIQ_ENTRY) 243 str r0, [sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)] 244 245 /* Restore secure context */ 246 add r0, sp, #SM_CTX_SEC 247 bl sm_restore_modes_regs 248 249 add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR) 250 251 rfefd sp! 252UNWIND( .fnend) 253END_FUNC sm_fiq_entry 254 255 .section .text.sm_vect_table 256 .align 5 257LOCAL_FUNC sm_vect_table , : 258UNWIND( .fnstart) 259UNWIND( .cantunwind) 260 b . /* Reset */ 261 b . /* Undefined instruction */ 262 b sm_smc_entry /* Secure monitor call */ 263 b . /* Prefetch abort */ 264 b . /* Data abort */ 265 b . /* Reserved */ 266 b . /* IRQ */ 267 b sm_fiq_entry /* FIQ */ 268 269#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP 270 .macro vector_prologue_spectre 271 /* 272 * This depends on SP being 8 byte aligned, that is, the 273 * lowest three bits in SP are zero. 274 * 275 * The idea is to form a specific bit pattern in the lowest 276 * three bits of SP depending on which entry in the vector 277 * we enter via. This is done by adding 1 to SP in each 278 * entry but the last. 279 */ 280 add sp, sp, #1 /* 7:111 Reset */ 281 add sp, sp, #1 /* 6:110 Undefined instruction */ 282 add sp, sp, #1 /* 5:101 Secure monitor call */ 283 add sp, sp, #1 /* 4:100 Prefetch abort */ 284 add sp, sp, #1 /* 3:011 Data abort */ 285 add sp, sp, #1 /* 2:010 Reserved */ 286 add sp, sp, #1 /* 1:001 IRQ */ 287 nop /* 0:000 FIQ */ 288 .endm 289 290 .align 5 291sm_vect_table_a15: 292 vector_prologue_spectre 293 /* 294 * Invalidate the branch predictor for the current processor. 295 * For Cortex-A8 ACTLR[6] has to be set to 1 for BPIALL to be 296 * effective. 297 * Note that the BPIALL instruction is not effective in 298 * invalidating the branch predictor on Cortex-A15. For that CPU, 299 * set ACTLR[0] to 1 during early processor initialisation, and 300 * invalidate the branch predictor by performing an ICIALLU 301 * instruction. See also: 302 * https://github.com/ARM-software/arm-trusted-firmware/wiki/Arm-Trusted-Firmware-Security-Advisory-TFV-6#variant-2-cve-2017-5715 303 */ 304 write_iciallu 305 isb 306 b 1f 307 308 .align 5 309sm_vect_table_bpiall: 310 vector_prologue_spectre 311 /* Invalidate the branch predictor for the current processor. */ 312 write_bpiall 313 isb 314 3151: 316 /* 317 * Only two exception does normally occur, smc and fiq. With all 318 * other exceptions it's good enough to just spinn, the lowest bits 319 * still tells which exception we're stuck with when attaching a 320 * debugger. 321 */ 322 323 /* Test for FIQ, all the lowest bits of SP are supposed to be 0 */ 324 tst sp, #(BIT(0) | BIT(1) | BIT(2)) 325 beq sm_fiq_entry 326 327 /* Test for SMC, xor the lowest bits of SP to be 0 */ 328 eor sp, sp, #(BIT(0) | BIT(2)) 329 tst sp, #(BIT(0) | BIT(1) | BIT(2)) 330 beq sm_smc_entry 331 332 /* unhandled exception */ 333 b . 334#endif /*!CFG_CORE_WORKAROUND_SPECTRE_BP*/ 335UNWIND( .fnend) 336END_FUNC sm_vect_table 337 338/* void sm_init(vaddr_t stack_pointer); */ 339FUNC sm_init , : 340UNWIND( .fnstart) 341 /* Set monitor stack */ 342 mrs r1, cpsr 343 cps #CPSR_MODE_MON 344 /* Point just beyond sm_ctx.sec */ 345 sub sp, r0, #(SM_CTX_SIZE - SM_CTX_NSEC) 346 347#ifdef CFG_INIT_CNTVOFF 348 read_scr r0 349 orr r0, r0, #SCR_NS /* Set NS bit in SCR */ 350 write_scr r0 351 isb 352 353 /* 354 * Accessing CNTVOFF: 355 * If the implementation includes the Virtualization Extensions 356 * this is a RW register, accessible from Hyp mode, and 357 * from Monitor mode when SCR.NS is set to 1. 358 * If the implementation includes the Security Extensions 359 * but not the Virtualization Extensions, an MCRR or MRRC to 360 * the CNTVOFF encoding is UNPREDICTABLE if executed in Monitor 361 * mode, regardless of the value of SCR.NS. 362 */ 363 read_idpfr1 r2 364 mov r3, r2 365 ands r3, r3, #IDPFR1_GENTIMER_MASK 366 beq .no_gentimer 367 ands r2, r2, #IDPFR1_VIRT_MASK 368 beq .no_gentimer 369 mov r2, #0 370 write_cntvoff r2, r2 371 372.no_gentimer: 373 bic r0, r0, #SCR_NS /* Clr NS bit in SCR */ 374 write_scr r0 375 isb 376#endif 377 378 msr cpsr, r1 379 380#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP 381 /* 382 * For unrecognized CPUs we fall back to the vector used for 383 * unaffected CPUs. Cortex A-15 has special treatment compared to 384 * the other affected Cortex CPUs. 385 */ 386 read_midr r1 387 ubfx r2, r1, #MIDR_IMPLEMENTER_SHIFT, #MIDR_IMPLEMENTER_WIDTH 388 cmp r2, #MIDR_IMPLEMENTER_ARM 389 bne 1f 390 391 ubfx r2, r1, #MIDR_PRIMARY_PART_NUM_SHIFT, \ 392 #MIDR_PRIMARY_PART_NUM_WIDTH 393 394 movw r3, #CORTEX_A8_PART_NUM 395 cmp r2, r3 396 movwne r3, #CORTEX_A9_PART_NUM 397 cmpne r2, r3 398 movwne r3, #CORTEX_A17_PART_NUM 399 cmpne r2, r3 400 ldreq r0, =sm_vect_table_bpiall 401 beq 2f 402 403 movw r3, #CORTEX_A15_PART_NUM 404 cmp r2, r3 405 ldreq r0, =sm_vect_table_a15 406 beq 2f 407#endif 408 /* Set monitor vector (MVBAR) */ 4091: ldr r0, =sm_vect_table 4102: write_mvbar r0 411 412 bx lr 413END_FUNC sm_init 414KEEP_PAGER sm_init 415 416 417/* struct sm_nsec_ctx *sm_get_nsec_ctx(void); */ 418FUNC sm_get_nsec_ctx , : 419 mrs r1, cpsr 420 cps #CPSR_MODE_MON 421 mov r0, sp 422 msr cpsr, r1 423 424 /* 425 * As we're in secure mode mon_sp points just beyond sm_ctx.sec 426 * which is sm_ctx.nsec 427 */ 428 bx lr 429END_FUNC sm_get_nsec_ctx 430