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 <kernel/unwind.h> 32 33 .section .text.sm_asm 34 35LOCAL_FUNC sm_save_modes_regs , : 36UNWIND( .fnstart) 37UNWIND( .cantunwind) 38 /* User mode registers has to be saved from system mode */ 39 cps #CPSR_MODE_SYS 40 stm r0!, {sp, lr} 41 42 cps #CPSR_MODE_IRQ 43 mrs r2, spsr 44 stm r0!, {r2, sp, lr} 45 46 cps #CPSR_MODE_SVC 47 mrs r2, spsr 48 stm r0!, {r2, sp, lr} 49 50 cps #CPSR_MODE_ABT 51 mrs r2, spsr 52 stm r0!, {r2, sp, lr} 53 54 cps #CPSR_MODE_UND 55 mrs r2, spsr 56 stm r0!, {r2, sp, lr} 57 58 cps #CPSR_MODE_MON 59 ldm r1, {r2-r3} /* Load SPSR and LR from the stack */ 60 stm r0!, {r2-r3} /* Store SPSR and LR in context */ 61 bx lr 62UNWIND( .fnend) 63END_FUNC sm_save_modes_regs 64 65/* Restores the mode specific registers */ 66LOCAL_FUNC sm_restore_modes_regs , : 67UNWIND( .fnstart) 68UNWIND( .cantunwind) 69 /* User mode registers has to be saved from system mode */ 70 cps #CPSR_MODE_SYS 71 ldm r0!, {sp, lr} 72 73 cps #CPSR_MODE_IRQ 74 ldm r0!, {r2, sp, lr} 75 msr spsr_fsxc, r2 76 77 cps #CPSR_MODE_SVC 78 ldm r0!, {r2, sp, lr} 79 msr spsr_fsxc, r2 80 81 cps #CPSR_MODE_ABT 82 ldm r0!, {r2, sp, lr} 83 msr spsr_fsxc, r2 84 85 cps #CPSR_MODE_UND 86 ldm r0!, {r2, sp, lr} 87 msr spsr_fsxc, r2 88 89 cps #CPSR_MODE_MON 90 ldm r0!, {r2-r3} /* Load SPSR and LR from context */ 91 stm r1, {r2-r3} /* Store SPSR and LR in stack */ 92 bx lr 93UNWIND( .fnend) 94END_FUNC sm_restore_modes_regs 95 96LOCAL_FUNC sm_smc_entry , : 97UNWIND( .fnstart) 98UNWIND( .cantunwind) 99 srsdb sp!, #CPSR_MODE_MON 100 push {r0-r3} 101/* Positions relative to stack pointer */ 102#define SMC_ENTRY_R0R3_OFFS 0 103#define SMC_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) 104 105 /* Clear the exclusive monitor */ 106 clrex 107 108 /* Find out if we're doing an secure or non-secure entry */ 109 read_scr r1 110 tst r1, #SCR_NS 111 bne .smc_ret_to_sec 112 113.smc_ret_to_nsec: 114 /* Save secure context */ 115 bl sm_get_sec_ctx 116 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 117 bl sm_save_modes_regs 118 119 mov r0, sp 120 mov r1, r4 121 bl sm_set_nsec_ret_vals 122 123 /* Restore non-secure context */ 124 bl sm_get_nsec_ctx 125 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 126 bl sm_restore_modes_regs 127 ldm r0!, {r4-r12} 128 129 /* Update SCR */ 130 read_scr r0 131 orr r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ 132 write_scr r0 133 134 b .smc_exit 135 136.smc_ret_to_sec: 137 bic r1, r1, #(SCR_NS | SCR_FIQ)/* Clear NS and FIQ bit in SCR */ 138 write_scr r1 139 140 /* Save non-secure context */ 141 push {r12, lr} 142 bl sm_get_nsec_ctx 143 pop {r12, lr} 144 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 145 bl sm_save_modes_regs 146 stm r0!, {r4-r12} 147 148 /* 149 * Update secure context with vector depending on SMC function, 150 * also updates entry reason 151 */ 152 mov r0, sp 153 bl sm_set_sec_smc_entry 154 155 /* Restore secure context */ 156 bl sm_get_sec_ctx 157 add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ 158 bl sm_restore_modes_regs 159 160.smc_exit: 161 pop {r0-r3} 162 rfefd sp! 163UNWIND( .fnend) 164END_FUNC sm_smc_entry 165 166/* 167 * FIQ handling 168 * 169 * Saves CPU context in per core structure sm_pre_fiq_ctx which 170 * later will be restored in the smc handler when handling a return 171 * from FIQ. 172 */ 173LOCAL_FUNC sm_fiq_entry , : 174UNWIND( .fnstart) 175UNWIND( .cantunwind) 176 /* FIQ has a +4 offset for lr compared to preferred return address */ 177 sub lr, lr, #4 178 srsdb sp!, #CPSR_MODE_MON 179 push {r0-r3} 180/* Positions relative to stack pointer */ 181#define FIQ_ENTRY_R0R3_OFFS 0 182#define FIQ_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) 183 184 /* Update SCR */ 185 read_scr r1 186 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ 187 write_scr r1 188 189 /* Save non-secure context */ 190 push {r12, lr} 191 bl sm_get_nsec_ctx 192 pop {r12, lr} 193 add r1, sp, #FIQ_ENTRY_SRS_OFFS /* Where srsdb wrote */ 194 bl sm_save_modes_regs 195 stm r0!, {r4-r12} 196 pop {r1-r4} /* R0-R3 pushed at entry */ 197 stm r0!, {r1-r4} 198 199 /* Update secure context with vector for FIQ handling */ 200 bl sm_set_sec_fiq_entry 201 202 /* Restore secure context */ 203 bl sm_get_sec_ctx 204 mov r1, sp /* No offset from sp now that {R0-R3} are poped */ 205 bl sm_restore_modes_regs 206 207 rfefd sp! 208UNWIND( .fnend) 209END_FUNC sm_fiq_entry 210 211 .align 5 212LOCAL_FUNC sm_vect_table , : 213UNWIND( .fnstart) 214UNWIND( .cantunwind) 215 b . /* Reset */ 216 b . /* Undefined instruction */ 217 b sm_smc_entry /* Secure monitor call */ 218 b . /* Prefetch abort */ 219 b . /* Data abort */ 220 b . /* Reserved */ 221 b . /* IRQ */ 222 b sm_fiq_entry /* FIQ */ 223UNWIND( .fnend) 224END_FUNC sm_vect_table 225 226/* void sm_init(vaddr_t stack_pointer); */ 227FUNC sm_init , : 228UNWIND( .fnstart) 229 push {r0, lr} 230UNWIND( .save {r0, lr}) 231 232 /* Set monitor stack */ 233 mrs r1, cpsr 234 cps #CPSR_MODE_MON 235 mov sp, r0 236 msr cpsr, r1 237 238 /* Set monitor vector (MVBAR) */ 239 ldr r0, =sm_vect_table 240 write_mvbar r0 241 242 pop {r0, pc} 243UNWIND( .fnend) 244END_FUNC sm_init 245