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-defines.h> 33#include <asm.S> 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 add sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 168 b .sm_exit 169 170.smc_from_nsec: 171 /* 172 * As we're coming from non-secure world (NS bit set) the stack 173 * pointer points to sm_ctx.nsec.r0 at this stage. After the 174 * instruction below the stack pointer points to sm_ctx. 175 */ 176 sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 177 178 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */ 179 write_scr r1 180 181 add r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8) 182 stm r0, {r8-r12} 183 184 mov r0, sp 185 bl sm_from_nsec 186 cmp r0, #0 187 beq .sm_ret_to_nsec 188 189 /* 190 * Continue into secure world 191 */ 192 add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0) 193 194.sm_exit: 195 pop {r0-r7} 196 rfefd sp! 197UNWIND( .fnend) 198END_FUNC sm_smc_entry 199 200/* 201 * FIQ handling 202 * 203 * Saves CPU context in the same way as sm_smc_entry() above. The CPU 204 * context will later be restored by sm_smc_entry() when handling a return 205 * from FIQ. 206 */ 207LOCAL_FUNC sm_fiq_entry , : 208UNWIND( .fnstart) 209UNWIND( .cantunwind) 210 /* FIQ has a +4 offset for lr compared to preferred return address */ 211 sub lr, lr, #4 212 /* sp points just past struct sm_sec_ctx */ 213 srsdb sp!, #CPSR_MODE_MON 214 push {r0-r7} 215 216 clrex /* Clear the exclusive monitor */ 217 218 /* 219 * As we're coming from non-secure world the stack pointer points 220 * to sm_ctx.nsec.r0 at this stage. After the instruction below the 221 * stack pointer points to sm_ctx. 222 */ 223 sub sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0) 224 225 /* Update SCR */ 226 read_scr r1 227 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */ 228 write_scr r1 229 230 /* Save non-secure context */ 231 add r0, sp, #SM_CTX_NSEC 232 bl sm_save_modes_regs 233 stm r0!, {r8-r12} 234 235 /* Set FIQ entry */ 236 ldr r0, =(thread_vector_table + THREAD_VECTOR_TABLE_FIQ_ENTRY) 237 str r0, [sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)] 238 239 /* Restore secure context */ 240 add r0, sp, #SM_CTX_SEC 241 bl sm_restore_modes_regs 242 243 add sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR) 244 245 rfefd sp! 246UNWIND( .fnend) 247END_FUNC sm_fiq_entry 248 249 .section .text.sm_vect_table 250 .align 5 251LOCAL_FUNC sm_vect_table , : 252UNWIND( .fnstart) 253UNWIND( .cantunwind) 254#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP 255 /* 256 * This depends on SP being 8 byte aligned, that is, the lowest 257 * three bits in SP are zero. 258 * 259 * The idea is to form a specific bit pattern in the lowest three 260 * bits of SP depending on which entry in the vector we enter via. 261 * This is done by adding 1 to SP in each entry but the last. 262 */ 263 add sp, sp, #1 /* 7:111 Reset */ 264 add sp, sp, #1 /* 6:110 Undefined instruction */ 265 add sp, sp, #1 /* 5:101 Secure monitor call */ 266 add sp, sp, #1 /* 4:100 Prefetch abort */ 267 add sp, sp, #1 /* 3:011 Data abort */ 268 add sp, sp, #1 /* 2:010 Reserved */ 269 add sp, sp, #1 /* 1:001 IRQ */ 270 nop /* 0:000 FIQ */ 271 272 /* Invalidate the branch predictor for the current processor. */ 273 write_bpiall 274 isb 275 276 /* 277 * Only two exception does normally occur, smc and fiq. With all 278 * other exceptions it's good enough to just spinn, the lowest bits 279 * still tells which exception we're stuck with when attaching a 280 * debugger. 281 */ 282 283 /* Test for FIQ, all the lowest bits of SP are supposed to be 0 */ 284 tst sp, #(BIT(0) | BIT(1) | BIT(2)) 285 beq sm_fiq_entry 286 287 /* Test for SMC, xor the lowest bits of SP to be 0 */ 288 eor sp, sp, #(BIT(0) | BIT(2)) 289 tst sp, #(BIT(0) | BIT(1) | BIT(2)) 290 beq sm_smc_entry 291 292 /* unhandled exception */ 293 b . 294#else /*!CFG_CORE_WORKAROUND_SPECTRE_BP*/ 295 b . /* Reset */ 296 b . /* Undefined instruction */ 297 b sm_smc_entry /* Secure monitor call */ 298 b . /* Prefetch abort */ 299 b . /* Data abort */ 300 b . /* Reserved */ 301 b . /* IRQ */ 302 b sm_fiq_entry /* FIQ */ 303#endif /*!CFG_CORE_WORKAROUND_SPECTRE_BP*/ 304UNWIND( .fnend) 305END_FUNC sm_vect_table 306 307/* void sm_init(vaddr_t stack_pointer); */ 308FUNC sm_init , : 309UNWIND( .fnstart) 310 /* Set monitor stack */ 311 mrs r1, cpsr 312 cps #CPSR_MODE_MON 313 /* Point just beyond sm_ctx.sec */ 314 sub sp, r0, #(SM_CTX_SIZE - SM_CTX_NSEC) 315 msr cpsr, r1 316 317 /* Set monitor vector (MVBAR) */ 318 ldr r0, =sm_vect_table 319 write_mvbar r0 320 321 bx lr 322END_FUNC sm_init 323KEEP_PAGER sm_init 324 325 326/* struct sm_nsec_ctx *sm_get_nsec_ctx(void); */ 327FUNC sm_get_nsec_ctx , : 328 mrs r1, cpsr 329 cps #CPSR_MODE_MON 330 mov r0, sp 331 msr cpsr, r1 332 333 /* 334 * As we're in secure mode mon_sp points just beyond sm_ctx.sec 335 * which is sm_ctx.nsec 336 */ 337 bx lr 338END_FUNC sm_get_nsec_ctx 339