411 lines
12 KiB
C
411 lines
12 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 "RTX_Config.h"
|
|
#include "rt_HAL_CA.h"
|
|
#include "rt_List.h"
|
|
#include "rt_MemBox.h"
|
|
#include "rt_System.h"
|
|
#include "rt_Task.h"
|
|
#include "rt_TypeDef.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
|
|
*---------------------------------------------------------------------------*/
|