/*---------------------------------------------------------------------------- * RL-ARM - RTX *---------------------------------------------------------------------------- * Name: HAL_CA9.c * Purpose: Hardware Abstraction Layer for Cortex-A9 * Rev.: 3 Sept 2013 *---------------------------------------------------------------------------- * * Copyright (c) 2012 - 2013 ARM Limited * All rights reserved. * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * - Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * - 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. * - Neither the name of ARM nor the names of its contributors may be used * to endorse or promote products derived from this software without * specific prior written permission. * * 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 COPYRIGHT HOLDERS AND 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 "rt_TypeDef.h" #include "RTX_Config.h" #include "rt_System.h" #include "rt_Task.h" #include "rt_List.h" #include "rt_MemBox.h" #include "rt_HAL_CA.h" /*---------------------------------------------------------------------------- * Functions *---------------------------------------------------------------------------*/ //For A-class, set USR/SYS stack __asm void rt_set_PSP (U32 stack) { ARM MRS R1, CPSR CPS #MODE_SYS ;no effect in USR mode ISB MOV SP, R0 MSR CPSR_c, R1 ;no effect in USR mode ISB BX LR } //For A-class, get USR/SYS stack __asm U32 rt_get_PSP (void) { ARM MRS R1, CPSR CPS #MODE_SYS ;no effect in USR mode ISB MOV R0, SP MSR CPSR_c, R1 ;no effect in USR mode ISB BX LR } /*--------------------------- _alloc_box ------------------------------------*/ __asm void *_alloc_box (void *box_mem) { /* Function wrapper for Unprivileged/Privileged mode. */ ARM LDR R12,=__cpp(rt_alloc_box) MRS R2, CPSR LSLS R2, #28 BXNE R12 SVC 0 BX LR } /*--------------------------- _free_box -------------------------------------*/ __asm int _free_box (void *box_mem, void *box) { /* Function wrapper for Unprivileged/Privileged mode. */ ARM LDR R12,=__cpp(rt_free_box) MRS R2, CPSR LSLS R2, #28 BXNE R12 SVC 0 BX LR } /*-------------------------- SVC_Handler -----------------------------------*/ #pragma push #pragma arm __asm void SVC_Handler (void) { PRESERVE8 ARM IMPORT rt_tsk_lock IMPORT rt_tsk_unlock IMPORT SVC_Count IMPORT SVC_Table IMPORT rt_stk_check IMPORT FPUEnable Mode_SVC EQU 0x13 SRSFD SP!, #Mode_SVC ; Push LR_SVC and SPRS_SVC onto SVC mode stack PUSH {R4} ; Push R4 so we can use it as a temp MRS R4,SPSR ; Get SPSR TST R4,#CPSR_T_BIT ; Check Thumb Bit LDRNEH R4,[LR,#-2] ; Thumb: Load Halfword BICNE R4,R4,#0xFF00 ; Extract SVC Number LDREQ R4,[LR,#-4] ; ARM: Load Word BICEQ R4,R4,#0xFF000000 ; Extract SVC Number /* Lock out systick and re-enable interrupts */ PUSH {R0-R3,R12,LR} AND R12, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R12 ; Adjust stack PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack BLX rt_tsk_lock CPSIE i POP {R12, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R12 ; Unadjust stack POP {R0-R3,R12,LR} CMP R4,#0 BNE SVC_User MRS R4,SPSR PUSH {R4} ; Push R4 so we can use it as a temp AND R4, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R4 ; Adjust stack PUSH {R4, LR} ; Store stack adjustment and dummy LR BLX R12 POP {R4, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R4 ; Unadjust stack POP {R4} ; Restore R4 MSR SPSR_CXSF,R4 /* Here we will be in SVC mode (even if coming in from PendSV_Handler or OS_Tick_Handler) */ Sys_Switch LDR LR,=__cpp(&os_tsk) LDM LR,{R4,LR} ; os_tsk.run, os_tsk.new CMP R4,LR BNE switching PUSH {R0-R3,R12,LR} AND R12, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R12 ; Adjust stack PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack CPSID i BLX rt_tsk_unlock POP {R12, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R12 ; Unadjust stack POP {R0-R3,R12,LR} POP {R4} RFEFD SP! ; Return from exception, no task switch switching CLREX CMP R4,#0 ADDEQ SP,SP,#12 ; Original R4, LR & SPSR do not need to be popped when we are paging in a different task BEQ SVC_Next ; Runtask deleted? PUSH {R8-R11} //R4 and LR already stacked MOV R10,R4 ; Preserve os_tsk.run MOV R11,LR ; Preserve os_tsk.new ADD R8,SP,#16 ; Unstack R4,LR LDMIA R8,{R4,LR} SUB SP,SP,#4 ; Make space on the stack for the next instn STMIA SP,{SP}^ ; Put User SP onto stack POP {R8} ; Pop User SP into R8 MRS R9,SPSR STMDB R8!,{R9} ; User CPSR STMDB R8!,{LR} ; User PC STMDB R8,{LR}^ ; User LR SUB R8,R8,#4 ; No writeback for store of User LR STMDB R8!,{R0-R3,R12} ; User R0-R3,R12 MOV R3,R10 ; os_tsk.run MOV LR,R11 ; os_tsk.new POP {R9-R12} ADD SP,SP,#12 ; Fix up SP for unstack of R4, LR & SPSR STMDB R8!,{R4-R7,R9-R12} ; User R4-R11 //If applicable, stack VFP state MRC p15,0,R1,c1,c0,2 ; VFP/NEON access enabled? (CPACR) AND R2,R1,#0x00F00000 CMP R2,#0x00F00000 BNE no_outgoing_vfp VMRS R2,FPSCR STMDB R8!,{R2,R4} ; Push FPSCR, maintain 8-byte alignment VSTMDB R8!,{S0-S31} LDRB R2,[R3,#TCB_STACKF] ; Record in TCB that VFP state is stacked ORR R2,#2 STRB R2,[R3,#TCB_STACKF] no_outgoing_vfp STR R8,[R3,#TCB_TSTACK] MOV R4,LR PUSH {R4} ; Push R4 so we can use it as a temp AND R4, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R4 ; Adjust stack PUSH {R4, LR} ; Store stack adjustment and dummy LR to SVC stack BLX rt_stk_check POP {R4, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R4 ; Unadjust stack POP {R4} ; Restore R4 MOV LR,R4 SVC_Next //R4 == os_tsk.run, LR == os_tsk.new, R0-R3, R5-R12 corruptible LDR R1,=__cpp(&os_tsk) ; os_tsk.run = os_tsk.new STR LR,[R1] LDRB R1,[LR,#TCB_TID] ; os_tsk.run->task_id LSL R1,#8 ; Store PROCID MCR p15,0,R1,c13,c0,1 ; Write CONTEXTIDR LDR R0,[LR,#TCB_TSTACK] ; os_tsk.run->tsk_stack //Does incoming task have VFP state in stack? LDRB R3,[LR,#TCB_STACKF] TST R3,#0x2 MRC p15,0,R1,c1,c0,2 ; Read CPACR ANDEQ R1,R1,#0xFF0FFFFF ; Disable VFP access if incoming task does not have stacked VFP state ORRNE R1,R1,#0x00F00000 ; Enable VFP access if incoming task does have stacked VFP state MCR p15,0,R1,c1,c0,2 ; Write CPACR BEQ no_incoming_vfp ISB ; We only need the sync if we enabled, otherwise we will context switch before next VFP instruction anyway VLDMIA R0!,{S0-S31} LDR R2,[R0] VMSR FPSCR,R2 ADD R0,R0,#8 no_incoming_vfp LDR R1,[R0,#60] ; Restore User CPSR MSR SPSR_CXSF,R1 LDMIA R0!,{R4-R11} ; Restore User R4-R11 ADD R0,R0,#4 ; Restore User R1-R3,R12 LDMIA R0!,{R1-R3,R12} LDMIA R0,{LR}^ ; Restore User LR ADD R0,R0,#4 ; No writeback for load to user LR LDMIA R0!,{LR} ; Restore User PC ADD R0,R0,#4 ; Correct User SP for unstacked user CPSR PUSH {R0} ; Push R0 onto stack LDMIA SP,{SP}^ ; Get R0 off stack into User SP ADD SP,SP,#4 ; Put SP back LDR R0,[R0,#-32] ; Restore R0 PUSH {R0-R3,R12,LR} AND R12, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R12 ; Adjust stack PUSH {R12, LR} ; Store stack adjustment and dummy LR to SVC stack CPSID i BLX rt_tsk_unlock POP {R12, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R12 ; Unadjust stack POP {R0-R3,R12,LR} MOVS PC,LR ; Return from exception /*------------------- User SVC -------------------------------*/ SVC_User LDR R12,=SVC_Count LDR R12,[R12] CMP R4,R12 ; Check for overflow BHI SVC_Done LDR R12,=SVC_Table-4 LDR R12,[R12,R4,LSL #2] ; Load SVC Function Address MRS R4,SPSR ; Save SPSR PUSH {R4} ; Push R4 so we can use it as a temp AND R4, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R4 ; Adjust stack PUSH {R4, LR} ; Store stack adjustment and dummy LR BLX R12 ; Call SVC Function POP {R4, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R4 ; Unadjust stack POP {R4} ; Restore R4 MSR SPSR_CXSF,R4 ; Restore SPSR SVC_Done PUSH {R0-R3,R12,LR} PUSH {R4} ; Push R4 so we can use it as a temp AND R4, SP, #4 ; Ensure stack is 8-byte aligned SUB SP, SP, R4 ; Adjust stack PUSH {R4, LR} ; Store stack adjustment and dummy LR CPSID i BLX rt_tsk_unlock POP {R4, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R4 ; Unadjust stack POP {R4} ; Restore R4 POP {R0-R3,R12,LR} POP {R4} RFEFD SP! ; Return from exception } #pragma pop #pragma push #pragma arm __asm void PendSV_Handler (U32 IRQn) { ARM IMPORT rt_tsk_lock IMPORT IRQNestLevel ADD SP,SP,#8 //fix up stack pointer (R0 has been pushed and will never be popped, R1 was pushed for stack alignment) //Disable systick interrupts, then write EOIR. We want interrupts disabled before we enter the context switcher. PUSH {R0, R1} BLX rt_tsk_lock POP {R0, R1} LDR R1, =__cpp(&GICInterface_BASE) LDR R1, [R1, #0] STR R0, [R1, #0x10] LDR R0, =IRQNestLevel ; Get address of nesting counter LDR R1, [R0] SUB R1, R1, #1 ; Decrement nesting counter STR R1, [R0] BLX __cpp(rt_pop_req) POP {R1, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R1 ; Unadjust stack LDR R0,[SP,#24] MSR SPSR_CXSF,R0 POP {R0-R3,R12} ; Leave SPSR & LR on the stack PUSH {R4} B Sys_Switch } #pragma pop #pragma push #pragma arm __asm void OS_Tick_Handler (U32 IRQn) { ARM IMPORT rt_tsk_lock IMPORT IRQNestLevel ADD SP,SP,#8 //fix up stack pointer (R0 has been pushed and will never be popped, R1 was pushed for stack alignment) PUSH {R0, R1} BLX rt_tsk_lock POP {R0, R1} LDR R1, =__cpp(&GICInterface_BASE) LDR R1, [R1, #0] STR R0, [R1, #0x10] LDR R0, =IRQNestLevel ; Get address of nesting counter LDR R1, [R0] SUB R1, R1, #1 ; Decrement nesting counter STR R1, [R0] BLX __cpp(os_tick_irqack) BLX __cpp(rt_systick) POP {R1, LR} ; Get stack adjustment & discard dummy LR ADD SP, SP, R1 ; Unadjust stack LDR R0,[SP,#24] MSR SPSR_CXSF,R0 POP {R0-R3,R12} ; Leave SPSR & LR on the stack PUSH {R4} B Sys_Switch } #pragma pop /*---------------------------------------------------------------------------- * end of file *---------------------------------------------------------------------------*/