475 lines
16 KiB
ArmAsm
475 lines
16 KiB
ArmAsm
/*----------------------------------------------------------------------------
|
|
* 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.
|
|
*---------------------------------------------------------------------------*/
|
|
|
|
.global rt_set_PSP
|
|
.global rt_get_PSP
|
|
.global _alloc_box
|
|
.global _free_box
|
|
.global PendSV_Handler
|
|
.global OS_Tick_Handler
|
|
|
|
.EQU CPSR_T_BIT, 0x20
|
|
.EQU CPSR_I_BIT, 0x80
|
|
.EQU CPSR_F_BIT, 0x40
|
|
|
|
.EQU MODE_USR, 0x10
|
|
.EQU MODE_FIQ, 0x11
|
|
.EQU MODE_IRQ, 0x12
|
|
.EQU MODE_SVC, 0x13
|
|
.EQU MODE_ABT, 0x17
|
|
.EQU MODE_UND, 0x1B
|
|
.EQU MODE_SYS, 0x1F
|
|
|
|
.EQU TCB_TID, 3 /* 'task id' offset */
|
|
.EQU TCB_STACKF, 32 /* 'stack_frame' offset */
|
|
.EQU TCB_TSTACK, 36 /* 'tsk_stack' offset */
|
|
|
|
.extern rt_alloc_box
|
|
.extern os_tsk
|
|
.extern GICInterface_BASE
|
|
.extern rt_pop_req
|
|
.extern os_tick_irqack
|
|
.extern rt_systick
|
|
|
|
/*----------------------------------------------------------------------------
|
|
* Functions
|
|
*---------------------------------------------------------------------------*/
|
|
.text
|
|
@ For A-class, set USR/SYS stack
|
|
@ __asm void rt_set_PSP (U32 stack) {
|
|
rt_set_PSP:
|
|
.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) {
|
|
rt_get_PSP:
|
|
.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) {
|
|
_alloc_box:
|
|
/* Function wrapper for Unprivileged/Privileged mode. */
|
|
.arm
|
|
|
|
LDR R12,=rt_alloc_box @ __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) {
|
|
_free_box:
|
|
/* Function wrapper for Unprivileged/Privileged mode. */
|
|
.arm
|
|
|
|
LDR R12,=rt_free_box @ __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) {
|
|
|
|
.type SVC_Handler, %function
|
|
.global SVC_Handler
|
|
SVC_Handler:
|
|
@ PRESERVE8
|
|
.arm
|
|
.extern rt_tsk_lock
|
|
.extern rt_tsk_unlock
|
|
.extern SVC_Count
|
|
.extern SVC_Table
|
|
.extern rt_stk_check
|
|
.extern FPUEnable
|
|
|
|
.EQU Mode_SVC, 0x13
|
|
|
|
SRSDB 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,=os_tsk @ __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,=os_tsk @ __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) {
|
|
PendSV_Handler:
|
|
.arm
|
|
|
|
.extern rt_tsk_lock
|
|
.extern 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, =GICInterface_BASE @ __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 rt_pop_req @ __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) {
|
|
OS_Tick_Handler:
|
|
.arm
|
|
|
|
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, =GICInterface_BASE @ __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 os_tick_irqack @ __cpp(os_tick_irqack)
|
|
BLX rt_systick @ __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
|
|
|
|
.global __set_PSP
|
|
@ __STATIC_ASM void __set_PSP(uint32_t topOfProcStack)
|
|
@ {
|
|
__set_PSP:
|
|
@ PRESERVE8
|
|
.arm
|
|
|
|
BIC R0, R0, #7 @ensure stack is 8-byte aligned
|
|
MRS R1, CPSR
|
|
CPS #MODE_SYS @no effect in USR mode
|
|
MOV SP, R0
|
|
MSR CPSR_c, R1 @no effect in USR mode
|
|
ISB
|
|
BX LR
|
|
|
|
@ }
|
|
|
|
.global __set_CPS_USR
|
|
@ __STATIC_ASM void __set_CPS_USR(void)
|
|
@ {
|
|
__set_CPS_USR:
|
|
.arm
|
|
|
|
CPS #MODE_USR
|
|
BX LR
|
|
@ }
|
|
|
|
.END
|
|
/*----------------------------------------------------------------------------
|
|
* end of file
|
|
*---------------------------------------------------------------------------*/
|