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