/* * Copyright (c) 2014, STMicroelectronics International N.V. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * 1. Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ #include #include #include #include .section .text.sm_asm LOCAL_FUNC sm_save_modes_regs , : /* User mode registers has to be saved from system mode */ cps #CPSR_MODE_SYS stm r0!, {sp, lr} cps #CPSR_MODE_IRQ mrs r2, spsr stm r0!, {r2, sp, lr} cps #CPSR_MODE_SVC mrs r2, spsr stm r0!, {r2, sp, lr} cps #CPSR_MODE_ABT mrs r2, spsr stm r0!, {r2, sp, lr} cps #CPSR_MODE_UND mrs r2, spsr stm r0!, {r2, sp, lr} cps #CPSR_MODE_MON ldm r1, {r2-r3} /* Load SPSR and LR from the stack */ stm r0!, {r2-r3} /* Store SPSR and LR in context */ bx lr END_FUNC sm_save_modes_regs /* Restores the mode specific registers */ LOCAL_FUNC sm_restore_modes_regs , : /* User mode registers has to be saved from system mode */ cps #CPSR_MODE_SYS ldm r0!, {sp, lr} cps #CPSR_MODE_IRQ ldm r0!, {r2, sp, lr} msr spsr_fsxc, r2 cps #CPSR_MODE_SVC ldm r0!, {r2, sp, lr} msr spsr_fsxc, r2 cps #CPSR_MODE_ABT ldm r0!, {r2, sp, lr} msr spsr_fsxc, r2 cps #CPSR_MODE_UND ldm r0!, {r2, sp, lr} msr spsr_fsxc, r2 cps #CPSR_MODE_MON ldm r0!, {r2-r3} /* Load SPSR and LR from context */ stm r1, {r2-r3} /* Store SPSR and LR in stack */ bx lr END_FUNC sm_restore_modes_regs LOCAL_FUNC sm_smc_entry , : srsdb sp!, #CPSR_MODE_MON push {r0-r3} /* Positions relative to stack pointer */ #define SMC_ENTRY_R0R3_OFFS 0 #define SMC_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) /* Clear the exclusive monitor */ clrex /* Find out if we're doing an secure or non-secure entry */ read_scr r1 tst r1, #SCR_NS bne .smc_ret_to_sec .smc_ret_to_nsec: /* Save secure context */ bl sm_get_sec_ctx add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ bl sm_save_modes_regs mov r0, sp mov r1, r4 bl sm_set_nsec_ret_vals /* Restore non-secure context */ bl sm_get_nsec_ctx add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ bl sm_restore_modes_regs ldm r0!, {r4-r12} /* Update SCR */ read_scr r0 orr r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ write_scr r0 b .smc_exit .smc_ret_to_sec: bic r1, r1, #(SCR_NS | SCR_FIQ)/* Clear NS and FIQ bit in SCR */ write_scr r1 /* Save non-secure context */ push {r12, lr} bl sm_get_nsec_ctx pop {r12, lr} add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ bl sm_save_modes_regs stm r0!, {r4-r12} /* * Update secure context with vector depending on SMC function, * also updates entry reason */ mov r0, sp bl sm_set_sec_smc_entry /* Restore secure context */ bl sm_get_sec_ctx add r1, sp, #SMC_ENTRY_SRS_OFFS /* Where srsdb wrote */ bl sm_restore_modes_regs .smc_exit: pop {r0-r3} rfefd sp! END_FUNC sm_smc_entry /* * FIQ handling * * Saves CPU context in per core structure sm_pre_fiq_ctx which * later will be restored in the smc handler when handling a return * from FIQ. */ LOCAL_FUNC sm_fiq_entry , : /* FIQ has a +4 offset for lr compared to preferred return address */ sub lr, lr, #4 srsdb sp!, #CPSR_MODE_MON push {r0-r3} /* Positions relative to stack pointer */ #define FIQ_ENTRY_R0R3_OFFS 0 #define FIQ_ENTRY_SRS_OFFS (4 * 4 + SMC_ENTRY_R0R3_OFFS) /* Update SCR */ read_scr r1 bic r1, r1, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */ write_scr r1 /* Save non-secure context */ push {r12, lr} bl sm_get_nsec_ctx pop {r12, lr} add r1, sp, #FIQ_ENTRY_SRS_OFFS /* Where srsdb wrote */ bl sm_save_modes_regs stm r0!, {r4-r12} pop {r1-r4} /* R0-R3 pushed at entry */ stm r0!, {r1-r4} /* Update secure context with vector for FIQ handling */ bl sm_set_sec_fiq_entry /* Restore secure context */ bl sm_get_sec_ctx mov r1, sp /* No offset from sp now that {R0-R3} are poped */ bl sm_restore_modes_regs rfefd sp! END_FUNC sm_fiq_entry .align 5 LOCAL_FUNC sm_vect_table , : b . /* Reset */ b . /* Undefined instruction */ b sm_smc_entry /* Secure monitor call */ b . /* Prefetch abort */ b . /* Data abort */ b . /* Reserved */ b . /* IRQ */ b sm_fiq_entry /* FIQ */ END_FUNC sm_vect_table /* void sm_init(vaddr_t stack_pointer); */ FUNC sm_init , : push {r0, lr} /* Set monitor stack */ mrs r1, cpsr cps #CPSR_MODE_MON mov sp, r0 msr cpsr, r1 /* Set monitor vector (MVBAR) */ ldr r0, =sm_vect_table write_mvbar r0 pop {r0, pc} END_FUNC sm_init