+++ /dev/null
-/*----------------------------------------------------------------------------
- * 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
- *---------------------------------------------------------------------------*/