/*----------------------------------------------------------------------------
 *      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
 *---------------------------------------------------------------------------*/