#********************************************************************************************************
#                                              uC/OS-II
#                                        The Real-Time Kernel
#
#                    Copyright 1992-2021 Silicon Laboratories Inc. www.silabs.com
#
#                                 SPDX-License-Identifier: APACHE-2.0
#
#               This software is subject to an open source license and is distributed by
#                Silicon Laboratories Inc. pursuant to the terms of the Apache License,
#                    Version 2.0 available at www.apache.org/licenses/LICENSE-2.0.
#
#********************************************************************************************************

#********************************************************************************************************
#
#                                           MPC57xx VLE Port
#                                             GNU Toolchain
#
# Filename : os_cpu_a.S
# Version  : V2.93.01
#********************************************************************************************************


    .include  "os_cpu_a.inc"

#********************************************************************************************************
#                                          PUBLIC FUNCTIONS
#********************************************************************************************************

    .global OSStartHighRdy
    .global OS_CPU_SR_Save
    .global OS_CPU_SR_Restore
    .global OSCtxSw
    .global OSIntCtxSw

    .global OS_ExceptHandler_CriticalInput
    .global OS_ExceptHandler_MachineCheck
    .global OS_ExceptHandler_DataStorage
    .global OS_ExceptHandler_InstructionStorage
    .global OS_ExceptHandler_ExternalInput
    .global OS_ExceptHandler_Alignment
    .global OS_ExceptHandler_Program
    .global OS_ExceptHandler_PerformanceMonitor
    .global OS_ExceptHandler_SystemCall
    .global OS_ExceptHandler_Debug
    .global OS_ExceptHandler_EFPUDataException
    .global OS_ExceptHandler_EFPURoundException
    .global OS_ExceptHandler_EFPUUnavailable
    .global OS_ExceptHandler_IVOR13
    .global OS_ExceptHandler_IVOR14
    .global OS_ExceptHandler_IVOR15


    .extern OSTCBCur                                            # 4 bytes
    .extern OSTCBHighRdy                                        # 4 bytes
    .extern OSPrioCur                                           # 1 byte
    .extern OSPrioHighRdy                                       # 1 byte
    .extern OSTaskSwHook                                        # 4 bytes
    .extern OS_CPU_IntHandler                                   # 4 bytes
    .extern OSIntNestingCtr                                     # 4 bytes
    .extern OS_CPU_ISRStkBase                                   # 4 bytes
    .extern OS_CPU_ISRNestingCtr                                # 1 byte


#********************************************************************************************************
#                                     CODE GENERATION DIRECTIVES
#********************************************************************************************************

    .section  .text


#********************************************************************************************************
#                                   CRITICAL SECTION METHOD 3 FUNCTIONS
#
# Description: Disable/Enable interrupts by preserving the state of interrupts.  Generally speaking you
#              would store the state of the interrupt disable flag in the local variable 'cpu_sr' and then
#              disable interrupts.  'cpu_sr' is allocated in all of uC/OS-II's functions that need to
#              disable interrupts.  You would restore the interrupt disable state by copying back 'cpu_sr'
#              into the CPU's status register.
#
# Prototypes :     OS_CPU_SR  OS_CPU_SR_Save(void);
#                  void       OS_CPU_SR_Restore(OS_CPU_SR cpu_sr);
#
#
# Note(s)    : 1) These functions are used in general like this:
#
#                 void Task (void *p_arg)
#                 {
#                 #if OS_CRITICAL_METHOD == 3          /* Allocate storage for CPU status register */
#                     OS_CPU_SR  cpu_sr;
#                 #endif
#
#                          :
#                          :
#                     OS_ENTER_CRITICAL();             /* cpu_sr = OS_CPU_SaveSR();                */
#                          :
#                          :
#                     OS_EXIT_CRITICAL();              /* OS_CPU_RestoreSR(cpu_sr);                */
#                          :
#                          :
#                 }
#*******************************************************************************************************

OS_CPU_SR_Save:
    mfmsr   r3
    wrteei  0
    se_blr

OS_CPU_SR_Restore:
    wrtee   r3
    se_blr


#********************************************************************************************************
#                                         START MULTITASKING
#
# Description : This function is called by OSStart() to start the highest priority task that was created
#               by your application before calling OSStart().
#
# Arguments   : none
#
# Note(s)     : 1) See Note #2 of OSTaskStkInit() in os_cpu.h for the stack frame's layout.
#
#               2) OSStartHighRdy() MUST:
#                      a) Call OSTaskSwHook() then,
#                      b) Switch to the highest priority task.
#********************************************************************************************************

OSStartHighRdy:
    e_bl        OSTaskSwHook                                    # Call OSTaskSwHook.

    e_li        r0, 1                                           # Set OSRunning to 1.
    e_lis       r11,OSRunning@ha                                # ...
    e_stb       r0,OSRunning@l(r11)                             # ...

    e_lis       r11,OSTCBHighRdy@ha                             # Get pointer to ready task TCB.
    e_lwz       r11,OSTCBHighRdy@l(r11)                         # ...

    e_lis       r12,OSTCBCur@ha                                 # Save as current task TCB ptr.
    e_stw       r11,OSTCBCur@l(r12)                             # ...

    e_lwz       r1, 0(r11)                                      # Get new stack pointer.

    epilogue                                                    # Restore context.

    se_rfi                                                      # Run task.


#********************************************************************************************************
#                                          EXCEPTION VECTORS
#********************************************************************************************************

#********************************************************************************************************
#                                   OS_ExceptHandler_CriticalInput
#                                                IVOR0
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_CriticalInput:
    e_b         OS_ExceptHandler_CriticalInput


#********************************************************************************************************
#                                    OS_ExceptHandler_MachineCheck
#                                                IVOR1
#
# Description:  Unused.
#********************************************************************************************************

OS_ExceptHandler_MachineCheck:
    e_b         OS_ExceptHandler_MachineCheck


#********************************************************************************************************
#                                    OS_ExceptHandler_DataStorage
#                                                IVOR2
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_DataStorage:
    e_b         OS_ExceptHandler_DataStorage


#********************************************************************************************************
#                                 OS_ExceptHandler_InstructionStorage
#                                                IVOR3
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_InstructionStorage:
    e_b         OS_ExceptHandler_InstructionStorage


#********************************************************************************************************
#                                   OS_ExceptHandler_ExternalInput
#                                                IVOR4
#
# Description: Switches to interrupt stack and calls the user-defined function
#              void OS_CPU_IntHandler(void).
#********************************************************************************************************

OS_ExceptHandler_ExternalInput:
    prologue

    e_lis       r11,OS_CPU_ISRNestingCtr@ha                     # Increment the ISR nesting counter to reflect the current nesting level.
    e_or2i      r11,OS_CPU_ISRNestingCtr@l                      # ...
    e_lwz       r12,0(r11)                                      # ...
    e_add2i.    r12,1                                           # ...
    e_stw       r12,0(r11)                                      # ...

    e_cmpl16i   r12,1                                           # Check to see if we are the first interrupt.
    e_bgt       IntStkInUse                                     # If we are nested the ISR stack is already being used.
                                                                # Otherwise, the first interrupt must switch to the ISR stack.

    e_lis       r11,OSTCBCur@ha                                 # Get a pointer to current TCB.
    e_lwz       r11,OSTCBCur@l(r11)                             # ...
    e_stw       r1,0(r11)                                       # Save the current stack pointer in the current TCB.

    e_lis       r11,OS_CPU_ISRStkBase@ha                        # Get a pointer to 8-byte aligned base of the ISR stack.
    e_lwz       r11,OS_CPU_ISRStkBase@l(r11)                    # ...
    mr          r1,r11                                          # Switch to the ISR stack.


IntStkInUse:
                                                                ### ALLOCATE INITIAL FRAME OF THE ISR CALL STACK ###
    e_stwu      0,-4(r1)                                        # Clear the initial frame (Backchain must be null according to EABI)
    e_stwu      0,-4(r1)                                        # ...

    e_bl        OS_CPU_IntHandler                               # Unnested kernel-aware interrupts will return through OSIntCtxSw.

                                                                ### NESTED OR FAST ISRS ONLY ###
    wrteei      0                                               # OS_CPU_IntHandler may have enabled interrupts. Make sure they are disabled.

    e_add2i.    r1,8                                            ### DEALLOCATE INITIAL FRAME OF THE ISR CALL STACK ###

    e_lis       r11,OS_CPU_ISRNestingCtr@ha                     # Decrement the ISR nesting counter to reflect the current nesting level.
    e_or2i      r11,OS_CPU_ISRNestingCtr@l                      # ...
    e_lwz       r12,0(r11)                                      # ...
    e_add2i.    r12,-1                                          # ...
    e_stw       r12,0(r11)                                      # ...

    e_cmpl16i   r12,0                                           # Check to see if we are nested.
    e_bgt       NestedReturn                                    # If we are, the previous frame is in the ISR stack.
                                                                # Otherwise, we must switch back to the task stack.

    e_lis       r11,OSTCBCur@ha                                 # Get pointer to current TCB.
    e_lwz       r11,OSTCBCur@l(r11)                             # ...
    e_lwz       r1,0(r11)                                       # Load stack pointer from current TCB.


NestedReturn:
    epilogue                                                    # If we are nested or not kernel-aware, pop the previous ISR context and rfi.

    se_rfi


#********************************************************************************************************
#                               PERFORM A CONTEXT SWITCH (From an ISR)
#
# Description : This routine is called when an ISR makes a higher priority task ready-to-run.
#
#               Interrupts should be disabled by the kernel before this point.
#********************************************************************************************************

OSIntCtxSw:
    e_bl        OSTaskSwHook                                    # Call OSTaskSwHook.

    e_lis       r11,OSTCBHighRdy@ha                             # Get pointer to ready task TCB.
    e_lwz       r11,OSTCBHighRdy@l(r11)                         # ...

    e_lis       r12,OSTCBCur@ha                                 # Save as current task TCB ptr.
    e_stw       r11,OSTCBCur@l(r12)                             # ...

    e_lis       r12,OSPrioHighRdy@ha                            # Get High Ready Priority.
    e_lbz       r10,OSPrioHighRdy@l(r12)                        # ...

    e_lis       r12,OSPrioCur@ha                                # Save as Current Priority.
    e_stb       r10,OSPrioCur@l(r12)                            # ...

    e_lwz       r1,0(r11)                                       # Get new stack pointer.

    e_lis       r11,OS_CPU_ISRNestingCtr@ha                     # Decrement the ISR nesting counter to reflect the current nesting level.
    e_or2i      r11,OS_CPU_ISRNestingCtr@l                      # ...
    e_lwz       r12,0(r11)                                      # ...
    e_add2i.    r12,-1                                          # ...
    e_stw       r12,0(r11)                                      # ...

    epilogue                                                    # Restore context.

    se_rfi                                                      # Run task


#********************************************************************************************************
#                                     OS_ExceptHandler_Alignment
#                                                IVOR5
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_Alignment:
    e_b         OS_ExceptHandler_Alignment


#********************************************************************************************************
#                                      OS_ExceptHandler_Program
#                                                IVOR6
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_Program:
    e_b         OS_ExceptHandler_Program


#********************************************************************************************************
#                                 OS_ExceptHandler_PerformanceMonitor
#                                                IVOR7
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_PerformanceMonitor:
    e_b         OS_ExceptHandler_PerformanceMonitor


#********************************************************************************************************
#                                     OS_ExceptHandler_SystemCall
#                                                IVOR8
#
# Description: Triggered by the kernel in order to perform a context switch.
#********************************************************************************************************

OS_ExceptHandler_SystemCall:
    e_b         OSCtxSw


#********************************************************************************************************
#                                      TASK LEVEL CONTEXT SWITCH
#
# Description : This routine is called when a task makes a higher priority task ready-to-run.
#               The pseudo code is:
#
#                   OS_CTX_SAVE
#                   OSTCBCur->OSTCBStkPtr = SP
#                   OSTaskSwHook()
#                   OSPrioCur             = OSPrioHighRdy
#                   OSTCBCur              = OSTCBHighRdy
#                   SP                    = OSTCBCur->OSTCBStkPtr
#                   OS_CTX_RESTORE
#                   Return from interrupt/exception
#
#               Interrupts should be disabled by the kernel before this point.
#********************************************************************************************************

OSCtxSw:
    prologue                                                    # Save context.

    e_lis       r11,OSTCBCur@ha                                 # Get pointer to current TCB.
    e_lwz       r11,OSTCBCur@l(r11)                             # ...
    e_stw       r1, 0(r11)                                      # Save stack pointer in current TCB.

    e_bl        OSTaskSwHook                                    # Call OSTaskSwHook.

    e_lis       r11,OSTCBHighRdy@ha                             # Get pointer to ready task TCB.
    e_lwz       r11,OSTCBHighRdy@l(r11)                         # ...

    e_lis       r12,OSTCBCur@ha                                 # Save as current task TCB ptr.
    e_stw       r11,OSTCBCur@l(r12)                             # ...

    e_lis       r12,OSPrioHighRdy@ha                            # Get High Ready Priority.
    e_lbz       r10,OSPrioHighRdy@l(r12)                        # ...

    e_lis       r12,OSPrioCur@ha                                # Save as Current Priority.
    e_stb       r10,OSPrioCur@l(r12)                            # ...

    e_lwz       r1,0(r11)                                       # Get new stack pointer from TCB.

    epilogue                                                    # Restore context.

    se_rfi                                                      # Run task.


#********************************************************************************************************
#                                       OS_ExceptHandler_Debug
#                                                IVOR9
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_Debug:
    e_b         OS_ExceptHandler_Debug


#********************************************************************************************************
#                                 OS_ExceptHandler_EFPUDataException
#                                               IVOR10
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_EFPUDataException:
    e_b         OS_ExceptHandler_EFPUDataException


#********************************************************************************************************
#                                 OS_ExceptHandler_EFPURoundException
#                                               IVOR11
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_EFPURoundException:
    e_b         OS_ExceptHandler_EFPURoundException


#********************************************************************************************************
#                                  OS_ExceptHandler_EFPUUnavailable
#                                               IVOR12
#
# Description: Unused.
#********************************************************************************************************

OS_ExceptHandler_EFPUUnavailable:
    e_b         OS_ExceptHandler_EFPUUnavailable


#********************************************************************************************************
#                                       OS_ExceptHandler_IVOR13
#                                               IVOR13
#
# Description: Reserved.
#********************************************************************************************************

OS_ExceptHandler_IVOR13:
    e_b         OS_ExceptHandler_IVOR13


#********************************************************************************************************
#                                       OS_ExceptHandler_IVOR14
#                                               IVOR14
#
# Description: Reserved.
#********************************************************************************************************

OS_ExceptHandler_IVOR14:
    e_b         OS_ExceptHandler_IVOR14


#********************************************************************************************************
#                                       OS_ExceptHandler_IVOR15
#                                               IVOR15
#
# Description: Reserved.
#********************************************************************************************************

OS_ExceptHandler_IVOR15:
    e_b         OS_ExceptHandler_IVOR15


    .end
