417 lines
15 KiB
C
417 lines
15 KiB
C
/*----------------------------------------------------------------------------
|
|
* 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
|
|
*---------------------------------------------------------------------------*/
|