#
# OS_MPC565_A.s
#
# uC/OS-II MPC565 PowerPC Specific Assembly Language Functions
#
# Functions:
#   void OSStartHighRdy (void)
#   void OSCtxSw (void)                 
#   void OSIntCtxSw (void)
#   void decrementer_exception (void)
#
# Author: Harry E. Barnett 11/11/99
# harryb@hbbse.com http://www.hbbse.com
#
# Modified by Peter S 5/4/01
#   added: floating point support
#   added: SaveSPRs, SaveGPRs, RestoreGPRs, RestoreSPRs macros
#   moved: Stack Frame definition and macros to OS_MPC565_A.i
#   removed MSR,R2,R13 from stack frame
#
# Version V1.00
#
#########################################################################
    .title  "UCOS-II MPC565 Power PC Assembly Language Functions"
    .file   "OS_MPC565_A.s"

    .global OSStartHighRdy
    .global OSCtxSw
    .global OSIntCtxSw
    .global decrementer_exception
    .global dec_init

    .text
    .align  2

    .extern OSTCBHighRdy    ;pointer to highest priority ready task
    .extern OSTCBCur        ;pointer to current tasks TCB
    .extern OSTaskSwHook
    .extern OSRunning
    .extern OSTimeTick
    .extern OSIntExit
    .extern StackFrameSize

    .include OS_CPU_A.i  ;stack frame definition & macros


.page
#########################################################################
#   Start multitasking
#   void OSStartHighRdy(void)
#########################################################################
OSStartHighRdy:
# Call OSTaskSwHook
    bl          OSTaskSwHook

# Set OSRunning to TRUE
    li          r0,1
    lis         r11,OSRunning@ha
    stb         r0,OSRunning@l(r11)

# Get pointer to ready task TCB
    lis         r11,OSTCBHighRdy@ha
    lwz         r11,OSTCBHighRdy@l(r11)

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

# Get new stack pointer
    lwz         r1,0(r11)

# Restore context
    RestoreGPRs                         ;Restore General Purpose Registers
    RestoreSPRs                         ;Restore Special Purpose Registers
    rfi


.page
#########################################################################
#   Perform a context switch from task level
#   void OSCtxSw(void)  Note: This is the System Call Exception Handler             
########################################################################
OSCtxSw:
#Save context
    SaveSPRs                            ;Save Special Purpose Registers
    mtspr       EID,r0                  ;Set MSR RI bit  
    SaveGPRs    2,29                    ;Save General Purpose Registers

# Get pointer to current TCB
    lis         r11,OSTCBCur@ha
    lwz         r11,OSTCBCur@l(r11)

# Save stack pointer in current TCB
    stw         r1,0(r11)

# Call OSTaskSwHook
    bl          OSTaskSwHook

# Get pointer to ready task TCB
    lis         r11,OSTCBHighRdy@ha
    lwz         r11,OSTCBHighRdy@l(r11)

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

# Get High Ready Priority
    lis         r12,OSPrioHighRdy@ha
    lbz         r10,OSPrioHighRdy@l(r12)

# Save as Current Priority
    lis         r12,OSPrioCur@ha
    stb         r10,OSPrioCur@l(r12)

# Get new stack pointer
    lwz         r1,0(r11)

# Restore context
    RestoreGPRs 2,29                    ;Restore General Purpose Registers
    mtspr       EID,r0                  ;enter critical region  
    RestoreSPRs                         ;Restore Special Purpose Registers
    rfi


.page
#########################################################################
#   Perform a context switch from a Decrementer Exception (OS tick)
#   void OSIntCtxSw(void)                    
#########################################################################
OSIntCtxSw:
#  OSIntCtxSw SP needs to be adjusted based OSIntExit stack usage
#    addi        r1,r1,0x08       ;not needed starting in uC/OS-II rev 2.51

# Call OSTaskSwHook
    bl          OSTaskSwHook

#  Get pointer to ready task TCB
    lis         r11,OSTCBHighRdy@ha
    lwz         r11,OSTCBHighRdy@l(r11)

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

# Get High Ready Priority
    lis         r12,OSPrioHighRdy@ha
    lbz         r10,OSPrioHighRdy@l(r12)

# Save as Current Priority
    lis         r12,OSPrioCur@ha
    stb         r10,OSPrioCur@l(r12)

#  Get new stack pointer
    lwz         r1,0(r11)

# Restore context
    RestoreGPRs                         ;Restore General Purpose Registers
    mtspr       EID,r0                  ;enter critcal region
    RestoreSPRs                         ;Restore Special Purpose Registers
    rfi


.page
#########################################################################
#   Decrementer Exception Handler (OS Tick)
#   void DECIntr (void)
#########################################################################
decrementer_exception:
    SaveSPRs                            ;Save Special Purpose Registers
    mtspr       EIE,r0                  ;allow interrupts & mark as recoverable
    SaveGPRs    3,28                    ;Save General Purpose Registers

    mtspr       EID,r0                  ;enter critical region  
    lwz         r4,dec_init@sdarx(r0)   ;get decrementer initial value,
    mfspr       r5,DEC                  ;compensate for interrupt latency by
    add         r4,r4,r5                ;adding decrementer underflow,
    mtspr       DEC,r4                  ;set decrementer to new value
    mtspr       EIE,r0                  ;exit critical region

    lbz         r12,OSIntNesting@sdarx(r0)  ;OSIntNesting++; 
    addi        r12,r12,1   
    stb         r12,OSIntNesting@sdarx(r0)

    lwz         r11,OSTCBCur@sdarx(r0)  ;Save stack pointer in current TCB 
    stw         r1,0(r11)

    bl          OSTimeTick
    bl          OSIntExit

    RestoreGPRs 3,28                    ;Restore General Purpose Registers
    mtspr       EID,r0                  ;enter critical region  
    RestoreSPRs                         ;Restore Special Purpose Registers
    rfi

    .end