#include "rtos_target_cfg.h"

//-----------------------------------------------------------------------------
//      CODE GENERATION DIRECTIVES
//
//        RSEG CODE:CODE(2)
    .cpu cortex-m3
    .fpu softvfp   
    .syntax unified
    .thumb
    .text	
    .align 2


//-----------------------------------------------------------------------------
//  EQUATES
//
    .equ    NVIC_INT_CTRL        ,     0xE000ED04  // Interrupt control state register.
    .equ    NVIC_PENDSVSET       ,     0x10000000  // Value to trigger PendSV exception.

    .equ    NVIC_SYSPRI14        ,     0xE000ED22  // System priority register (priority 14).
    .equ    NVIC_PENDSV_PRI      ,           0xFF  // PendSV priority value (lowest).
    .equ    NVIC_SYSPRI15        ,     0xE000ED23  // System priority register (priority 15).
    .equ    NVIC_ST_PRI          ,           0xFF  // SysTick priority value (lowest).

    .equ    NVIC_ST_CTRL         ,    0xE000E010   // SysTick Ctrl & Status Reg.
    .equ    NVIC_ST_RELOAD       ,    0xE000E014   // SysTick Reload  Value Reg.
    .equ    NVIC_ST_CTRL_CLK_SRC ,    0x00000004   // Clock Source.
    .equ    NVIC_ST_CTRL_INTEN   ,    0x00000002   // Interrupt enable.
    .equ    NVIC_ST_CTRL_ENABLE  ,    0x00000001   // Counter mode.


//-----------------------------------------------------------------------------
//  PUBLIC FUNCTIONS
//
    .section    .text,"ax"
    .code 16

    .extern OS_ContextSwitchHook
    .global PendSV_Handler
    .global OS_Start
    .global ContextRestore

//-----------------------------------------------------------------------------
//      HANDLE PendSV EXCEPTION
//      void PendSVC_ISR(void)
//
// Note(s) : 1) PendSV is used to cause a context switch.  This is a recommended method for performing
//              context switches with Cortex-M3.  This is because the Cortex-M3 auto-saves half of the
//              processor context on any exception, and restores same on return from exception.  So only
//              saving of R4-R11 is required and fixing up the stack pointers.  Using the PendSV exception
//              this way means that context saving and restoring is identical whether it is initiated from
//              a thread or occurs due to an interrupt or exception.
//
//           2) Pseudo-code is:
//              a) Get the process SP, if 0 then skip (goto f) the saving part (first context switch);
//              b) Save remaining regs r4-r11 on process stack;
//              c) Call OS_ContextSwitchHook for save current task SP and get new task SP;
//              d) Restore R4-R11 from new process stack;
//              e) Perform exception return which will restore remaining context.
//              f) Get SP for the first context switch (R2 hold it);
//                 run SysTick; goto d);
//
//           3) On entry into PendSV handler:
//              a) The following have been saved on the process stack (by processor):
//                 xPSR, PC, LR, R12, R0-R3
//              b) Processor mode is switched to Handler mode (from Thread mode)
//              c) Stack is Main stack (switched from Process stack)
//
//           4) Since PendSV is set to lowest priority in the system (by OS_Start() below), we
//              know that it will only be run when no other exception or interrupt is active, and
//              therefore safe to assume that context being switched out was using the process stack (PSP).
//

.thumb_func
PendSV_Handler:
    CPSID   I                 // Prevent interruption during context switch
    MRS     R0, PSP           // PSP is process stack pointer
    CBZ     R0, nosave        // Skip register save the first time    

    STMDB R0!, {R4-R11}       // Save remaining regs r4-11 on process stack
    // At this point, entire context of process has been saved                                                            

    PUSH    {R14}                        // Save LR exc_return value
    LDR     R1, =OS_ContextSwitchHook    // OS_ContextSwitchHook();
    BLX     R1
    POP     {R14}
    
ContextRestore:
    // R0 is new process SP;
    LDMIA R0!, {R4-R11}       // Restore r4-11 from new process stack
    MSR     PSP, R0           // Load PSP with new process SP
    ORR     LR, LR, #0x04     // Ensure exception return uses process stack
    CPSIE   I
    BX      LR                // Exception return will restore remaining context
nosave:
    MOV R0, R2                // R2 hold the first task SP
    
    LDR     R1, =NVIC_ST_CTRL // Enable and run SysTick
    LDR     R2, =(NVIC_ST_CTRL_CLK_SRC | NVIC_ST_CTRL_INTEN | NVIC_ST_CTRL_ENABLE)
    STR     R2, [R1]
    
    B   ContextRestore

   
//-----------------------------------------------------------------------------
//      START MULTITASKING
//      void OS_Start(TStackItem* sp)
//
// Note(s) : 1) OS_Start() MUST:
//              a) Setup PendSV and SysTick exception priority to lowest;
//              b) Setup SysTick (reload value);
//              c) Enable interrupts (tasks will run with interrupts enabled).
//
.thumb_func
OS_Start:
    LDR     R1, =NVIC_SYSPRI14      // Set the PendSV exception priority (lowest)
    LDR     R2, =NVIC_PENDSV_PRI
    STRB    R2, [R1]
    LDR     R1, =NVIC_SYSPRI15      // Set the SysTick exception priority (lowest)
    LDR     R2, =NVIC_ST_PRI
    STRB    R2, [R1]
    
    LDR     R1, =NVIC_ST_RELOAD     // Setup SysTick
    LDR     R2, =(SYSTICKFREQ/SYSTICKINTRATE-1)  
    STR     R2, [R1]

    MOV     R2, R0                  // Save the first task stack
    MOVS    R0, #0                  // Set the PSP to 0 for initial context switch call
    MSR     PSP, R0

    LDR     R0, =NVIC_INT_CTRL      // Trigger the PendSV exception (causes context switch)
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]

    CPSIE   I                       // Enable interrupts at processor level
loop:
    B       loop                    // Should never get here


    .end