Diff: rtx/TARGET_CORTEX_A/TOOLCHAIN_ARM/HAL_CA9.c

Revision:

67:63988a2238f7

Parent:

49:77c8e4604045

Child:

68:d3d0e710b443

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rtx/TARGET_CORTEX_A/TOOLCHAIN_ARM/HAL_CA9.c	Fri Feb 27 10:00:40 2015 +0000
@@ -0,0 +1,416 @@
+/*----------------------------------------------------------------------------
+ *      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
+ *---------------------------------------------------------------------------*/