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