1/* 2 * Copyright (c) 2014, STMicroelectronics International N.V. 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright notice, 9 * this list of conditions and the following disclaimer. 10 * 11 * 2. Redistributions in binary form must reproduce the above copyright notice, 12 * this list of conditions and the following disclaimer in the documentation 13 * and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 * POSSIBILITY OF SUCH DAMAGE. 26 */ 27 28#include <asm.S> 29#include <arm.h> 30#include <arm32_macros.S> 31#include <sm/teesmc.h> 32#include <kernel/unwind.h> 33 34 .section .text.sm_asm 35 36LOCAL_FUNC sm_save_modes_regs , : 37UNWIND( .fnstart) 38UNWIND( .cantunwind) 39 /* User mode registers has to be saved from system mode */ 40 cps #CPSR_MODE_SYS 41 stm r0!, {sp, lr} 42 43 cps #CPSR_MODE_IRQ 44 mrs r2, spsr 45 stm r0!, {r2, sp, lr} 46 47 cps #CPSR_MODE_SVC 48 mrs r2, spsr 49 stm r0!, {r2, sp, lr} 50 51 cps #CPSR_MODE_ABT 52 mrs r2, spsr 53 stm r0!, {r2, sp, lr} 54 55 cps #CPSR_MODE_UND 56 mrs r2, spsr 57 stm r0!, {r2, sp, lr} 58 59 cps #CPSR_MODE_MON 60 ldm r1, {r2-r3} /* Load SPSR and LR from the stack */ 61 stm r0!, {r2-r3} /* Store SPSR and LR in context */ 62 bx lr 63UNWIND( .fnend) 64END_FUNC sm_save_modes_regs 65 66/* Restores the mode specific registers */ 67LOCAL_FUNC sm_restore_modes_regs , : 68UNWIND( .fnstart) 69UNWIND( .cantunwind) 70 /* User mode registers has to be saved from system mode */ 71 cps #CPSR_MODE_SYS 72 ldm r0!, {sp, lr} 73 74 cps #CPSR_MODE_IRQ 75 ldm r0!, {r2, sp, lr} 76 msr spsr_fsxc, r2 77 78 cps #CPSR_MODE_SVC 79 ldm r0!, {r2, sp, lr} 80 msr spsr_fsxc, r2 81 82 cps #CPSR_MODE_ABT 83 ldm r0!, {r2, sp, lr} 84 msr spsr_fsxc, r2 85 86 cps #CPSR_MODE_UND 87 ldm r0!, {r2, sp, lr} 88 msr spsr_fsxc, r2 89 90 cps #CPSR_MODE_MON 91 ldm r0!, {r2-r3} /* Load SPSR and LR from context */ 92 stm r1, {r2-r3} /* Store SPSR and LR in stack */ 93 bx lr 94UNWIND( .fnend) 95END_FUNC sm_restore_modes_regs 96 97LOCAL_FUNC sm_smc_entry , : 98UNWIND( .fnstart) 99UNWIND( .cantunwind) 100 srsdb sp!, #CPSR_MODE_MON 101 push {r0-r3} 102/* Positions relative to stack pointer */ 103#define SMC_ENTRY_R0R3_OFFS 0 104#define SMC_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) 105 106 /* Clear the exclusive monitor */ 107 clrex 108 109 /* Find out if we're doing an secure or non-secure entry */ 110 read_scr r1 111 tst r1, #SCR_NS 112 bne .smc_ret_to_sec 113 114.smc_ret_to_nsec: 115 /* Save secure context */ 116 bl sm_get_sec_ctx 117 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 118 bl sm_save_modes_regs 119 120 mov r0, sp 121 mov r1, r4 122 bl sm_set_nsec_ret_vals 123 124 /* Restore non-secure context */ 125 bl sm_get_nsec_ctx 126 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 127 bl sm_restore_modes_regs 128 ldm r0!, {r4-r12} 129 130 /* Update SCR */ 131 read_scr r0 132 orr r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ 133 write_scr r0 134 135 b .smc_exit 136 137.smc_ret_to_sec: 138 bic r1, r1, #(SCR_NS | SCR_FIQ)/* Clear NS and FIQ bit in SCR */ 139 write_scr r1 140 141 /* Save non-secure context */ 142 push {r12, lr} 143 bl sm_get_nsec_ctx 144 pop {r12, lr} 145 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 146 bl sm_save_modes_regs 147 stm r0!, {r4-r12} 148 149 /* 150 * Update secure context with vector depending on SMC function, 151 * also updates entry reason 152 */ 153 mov r0, sp 154 bl sm_set_sec_smc_entry 155 156 /* Restore secure context */ 157 bl sm_get_sec_ctx 158 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 159 bl sm_restore_modes_regs 160 161.smc_exit: 162 pop {r0-r3} 163 rfefd sp! 164UNWIND( .fnend) 165END_FUNC sm_smc_entry 166 167/* 168 * FIQ handling 169 * 170 * Saves CPU context in per core structure sm_pre_fiq_ctx which 171 * later will be restored in the smc handler when handling a return 172 * from FIQ. 173 */ 174LOCAL_FUNC sm_fiq_entry , : 175UNWIND( .fnstart) 176UNWIND( .cantunwind) 177 /* FIQ has a +4 offset for lr compared to preferred return address */ 178 sub lr, lr, #4 179 srsdb sp!, #CPSR_MODE_MON 180 push {r0-r3} 181/* Positions relative to stack pointer */ 182#define FIQ_ENTRY_R0R3_OFFS 0 183#define FIQ_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) 184 185 /* Update SCR */ 186 read_scr r1 187 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ 188 write_scr r1 189 190 /* Save non-secure context */ 191 push {r12, lr} 192 bl sm_get_nsec_ctx 193 pop {r12, lr} 194 add r1, sp, #FIQ_ENTRY_SRS_OFFS /* Where srsdb wrote */ 195 bl sm_save_modes_regs 196 stm r0!, {r4-r12} 197 pop {r1-r4} /* R0-R3 pushed at entry */ 198 stm r0!, {r1-r4} 199 200 /* Update secure context with vector for FIQ handling */ 201 bl sm_set_sec_fiq_entry 202 203 /* Restore secure context */ 204 bl sm_get_sec_ctx 205 mov r1, sp /* No offset from sp now that {R0-R3} are poped */ 206 bl sm_restore_modes_regs 207 208 rfefd sp! 209UNWIND( .fnend) 210END_FUNC sm_fiq_entry 211 212 .align 5 213LOCAL_FUNC sm_vect_table , : 214UNWIND( .fnstart) 215UNWIND( .cantunwind) 216 b . /* Reset */ 217 b . /* Undefined instruction */ 218 b sm_smc_entry /* Secure monitor call */ 219 b . /* Prefetch abort */ 220 b . /* Data abort */ 221 b . /* Reserved */ 222 b . /* IRQ */ 223 b sm_fiq_entry /* FIQ */ 224UNWIND( .fnend) 225END_FUNC sm_vect_table 226 227/* void sm_init(vaddr_t stack_pointer); */ 228FUNC sm_init , : 229UNWIND( .fnstart) 230 push {r0, lr} 231UNWIND( .save {r0, lr}) 232 233 /* Set monitor stack */ 234 mrs r1, cpsr 235 cps #CPSR_MODE_MON 236 mov sp, r0 237 msr cpsr, r1 238 239 /* Set monitor vector (MVBAR) */ 240 ldr r0, =sm_vect_table 241 write_mvbar r0 242 243 pop {r0, pc} 244UNWIND( .fnend) 245END_FUNC sm_init 246