/*******************************************************************
*                  SEGGER MICROCONTROLLER SYSTEME GmbH             *
*          Solutions for real time microcontroller applications    *
********************************************************************
*                                                                  *
*      (C) 1996-2007   SEGGER Microcontroller Systeme GmbH         *
*                                                                  *
*      www.segger.com  Support: support@segger.com                 *
*                                                                  *
********************************************************************

--------------------------------------------------------------------
File        : RTOS.ASM
Purpose     : Assembler kernel for the OS and AVR32-A core
--------------- END-OF-HEADER ----------------------------------------
*/

/*********************************************************************
*
*       ASM header
*
**********************************************************************
*/
        .file    "RTOS.asm"

        .section .text, "ax"

/*********************************************************************
*
*       Const defines
*
**********************************************************************
*/
         .equ DISP_TASK_pStack,                4           // pStack offset in TCB
         .equ SR,                              0           // Status register offset
         .equ COUNT,                           264
         .equ COMPARE,                         268
         .equ SVC_MODE,                        0x00400000  // Supervisor mode
         .equ OS_CPU_SR_OFFSET,                0           // Status  Register offset in System Register               
         .equ OS_CPU_SR_GM_OFFSET,            16           // Status  Register, Global Interrupt Mask Offset           
         .equ OS_CPU_SR_MX_OFFSET,            22           // Status  Register, Execution Mode Mask offset             
         .equ OS_CPU_RSR_SUP_OFFSET,          20           // Status  Register, to return on Supervisor Context        
         .equ OS_CPU_RAR_SUP_OFFSET,          52           // Address Register, to return on Supervisor Context        
         .equ OS_CPU_RSR_INT0_OFFSET,         24           // Status  Register, to return on INT0       Context        
         .equ OS_CPU_RAR_INT0_OFFSET,         56           // Address Register, to return on INT0       Context        
         .equ OS_CPU_RSR_INT1_OFFSET,         28           // Status  Register, to return on INT1       Context        
         .equ OS_CPU_RAR_INT1_OFFSET,         60           // Address Register, to return on INT1       Context        
         .equ OS_CPU_RSR_INT2_OFFSET,         32           // Status  Register, to return on INT2       Context        
         .equ OS_CPU_RAR_INT2_OFFSET,         64           // Address Register, to return on INT2       Context        
         .equ OS_CPU_RSR_INT3_OFFSET,         36           // Status  Register, to return on INT3       Context        
         .equ OS_CPU_RAR_INT3_OFFSET,         68           // Address Register, to return on INT3       Context        
         .equ MMUCR,                          288 
         

/*********************************************************************
*
*       PUBLICS
*
**********************************************************************
*/
         .global OS_Start
         .global OS_StartTask
         .global OS_Switch
         .global OS_SwitchFromInt
         .global OS_CallISR
         .global OS_CallNestableISR
         .global OS_GetIpl
         .global OS_GetCompareCount
         .global OS_SetCompare
         .global OS_GetCompare
         .global OS_GetCount
         .global OS_IsInInt
         .global OS_GetSP
         .global OS_GetSysStackSize
         .global OS_GetSysStackBase
         .global OS_GetIntStackSize
         .global OS_GetIntStackBase
         .global OS_Prepare_Stackpointer
         .global scall_handler
         
/*********************************************************************
*
*       Externals, code
*
**********************************************************************
*/
         .extern OS_DecRegionCnt
         .extern OS_ChangeTask
         .extern OS_GetRegionCnt
         .extern OS_EnterInterrupt
         .extern OS_LeaveInterrupt
         .extern OS__EnterInterrupt
         .extern OS__LeaveInterrupt
         .extern OS__EnterNestableInterrupt
         .extern OS__LeaveNestableInterrupt

/*********************************************************************
*
*       Externals, data
*
**********************************************************************
*/
         .extern OS_Counters
         .extern OS_pCurrentTask
         .extern OS_Ipl_EI
         .extern OS_Ipl_DI
         .extern OS_ISR_StackFrameAddr

/*********************************************************************
*
*       MACROS to simplify assembler code
*
**********************************************************************
*/
         .macro NOPS 
           nop
           nop
         .endm

         .macro MODE_SVC_EI MACRO
           mov       R11, LO(OS_Ipl_EI)
           orh       R11, HI(OS_Ipl_EI)
           ld.w      R12, R11[0]
           sbr       R12, 22
           mov       R11, 0
           scall
         .endm


         .macro MODE_SVC_DI MACRO
           mov       R11, LO(OS_Ipl_DI)
           orh       R11, HI(OS_Ipl_DI)
           ld.w      R12, R11[0]
           sbr       R12, 22
           mov       R11, 0
           scall
         .endm

/*********************************************************************
*
*       Code starts here
*
**********************************************************************
*/

/********************************************************************
*
*     OS_Switch: Scheduler entrance point, cooperative
*/
OS_Switch:
/* push Permanent registers, OS_Counters and LR */
         mov       R11, LO(OS_Counters)
         orh       R11, HI(OS_Counters)
         ld.w      R10, R11[0x0]
         pushm     R0-R3, R4-R7, R10, LR
         rcall     OS_EnableInterrupts

/* save SP in task control block */
         mov       R11,LO(OS_pCurrentTask)
         orh       R11,HI(OS_pCurrentTask)
         ld.w      R12,R11[0x0]
         st.w      R12[DISP_TASK_pStack],SP

         MODE_SVC_EI

/*********************************************************************
*
*     OS_Start
*/
OS_Start:
         mov       R8, 0     
         mtsr      MMUCR, R8                               // Disable MMU 
OS_SwitchAfterISR_Idle:
         lddpc     SP, .LC1
         rcall     OS_ChangeTask
/*** switch back to application mode */
         mov       R8, LO(OS_Ipl_DI)                       // Load OS_Ipl_DI address
         orh       R8, HI(OS_Ipl_DI)
         ld.w      R8, R8[0]                               // R9 = OS_Ipl_DI
         mtsr      SR, R8
         NOPS
         
         
/*** Restore task SP */
         mov       R11,LO(OS_pCurrentTask)
         orh       R11,HI(OS_pCurrentTask)
         ld.w      R12,R11[0x0]
         ld.w      SP,R12[DISP_TASK_pStack]

/* pop Permanent registers, OS_Counters and LR */
         popm      R0-R3, R4-R7, R10, LR
         mov       R11,LO(OS_Counters)
         orh       R11,HI(OS_Counters)
         st.w      R11[0], R10
         ret       LR
         nop
         nop
.LC1:    .long     _estack

/********************************************************************
*
*      OS_StartTask
*/
OS_StartTask:
         rcall     OS_EnableInterrupts
         mov       R12, R7                                 // void * pContext 
         popm      PC

/*********************************************************************
*
*      OS_SwitchFromInt
*
* Stack frame (pointed to by OS_ISR_StackFrameAddr)
*   Stat                                   OS_CallISR, local variable
*   R7, LR                                 OS_CallISR

*
*   SR             <- low addr             hardware
*   PC
*   LR
*   R12
*   R11
*   R10
*   R9
*   R8             <- high addr
*
* Return value
*   0  Switch prepared. Return addr. has been modified
*   1  No switch prepared because this code is executed from within a nested (L1) interrupt
*/
OS_SwitchFromInt:
/*
* Check if we are on topmost ISTACK
*
* SR is located in register set from current mode, so get current mode and SR
*/
          mfsr     R8, SR                                  // Load SR and decide which RSR should be used
          andh     R8, 0x1C0                               // Mask all bits w/o the mode bits  
          lsr      R8, 0x16                                // Shift right 6 bits, so we have in R8 the mode
          cp.w     R8, 0x02                                // Mode = INT0?
          breq     ModeINT0
          cp.w     R8, 0x03                                // Mode = INT1?
          breq     ModeINT1
          cp.w     R8, 0x04                                // Mode = INT2?
          breq     ModeINT2                             
          mfsr     R12, OS_CPU_RSR_INT3_OFFSET             // Get saved status register from INT0 context
          bral     ModeIntDone
ModeINT0:
          mfsr     R12, OS_CPU_RSR_INT0_OFFSET             // Get saved status register from INT0 context
          bral     ModeIntDone
ModeINT1:
          mfsr     R12, OS_CPU_RSR_INT1_OFFSET             // Get saved status register from INT0 context
          bral     ModeIntDone
ModeINT2:
          mfsr     R12, OS_CPU_RSR_INT2_OFFSET             // Get saved status register from INT0 context
ModeIntDone:
/* if ((mode != 0) && (mode != 1)) {   // Is the previous mode not Application or SuperVisor mode?
*  return 1;    // Can not switch
*  }
*/ 
         andh      R12,0x180,COH                           // M[1:2] == 0 ?
         retne     1
/*
* *** Check if idle. If so, replace return addr with OS_SwitchAfterISR_Idle and return.
*/
_SwitchFromInt:
         mfsr      R10, SR                                 // Get status register
         pushm     R10, LR                                 // Save status and link register on system stack
         mov       R12,LO(OS_pCurrentTask)                 // Load @OS_pCurrentTask
         orh       R12,HI(OS_pCurrentTask)
         ld.w      R12,R12[0x0]                            // Load OS_pCurrentTask
         cp.w      R12,0x0                                 // OS_pCurrentTask == 0?
         breq      StoreIdle

/*** Copy return adr. and status register onto user stack. This is done only if a task has been interrupted, not if the Idle-loop has been interrupted. */

          cp.w     R8, 0x02                                // Mode = INT0?
          breq     _ModeINT0
          cp.w     R8, 0x03                                // Mode = INT1?
          breq     _ModeINT1
          cp.w     R8, 0x04                                // Mode = INT2?
          breq     _ModeINT2                             
          mfsr     R12, OS_CPU_RSR_INT3_OFFSET             // Get saved status register from INT0 context
          mfsr     R11, OS_CPU_RAR_INT3_OFFSET             // Get saved return register from INT0 context
          bral     _ModeIntDone
_ModeINT0:
          mfsr     R12, OS_CPU_RSR_INT0_OFFSET             // Get saved status register from INT0 context
          mfsr     R11, OS_CPU_RAR_INT0_OFFSET             // Get saved return register from INT0 context
          bral     _ModeIntDone
_ModeINT1:
          mfsr     R12, OS_CPU_RSR_INT1_OFFSET             // Get saved status register from INT0 context
          mfsr     R11, OS_CPU_RAR_INT1_OFFSET             // Get saved return register from INT0 context
          bral     _ModeIntDone
_ModeINT2:
          mfsr     R12, OS_CPU_RSR_INT2_OFFSET             // Get saved status register from INT0 context
          mfsr     R11, OS_CPU_RAR_INT2_OFFSET             // Get saved return register from INT0 context
_ModeIntDone:

/*
* *** Switch to application mode with disabled interrupts and save status and return register on application mode stack  
*/
         mov       R9, LO(OS_Ipl_DI)                       // Switch to application mode to use application mode sp
         orh       R9, HI(OS_Ipl_DI)                     
         ld.w      R9, R9[0x0]                             // Load OS_Ipl_DI
         mtsr      SR, R9                                  // Set application mode with disabled interrupts
         NOPS
         stm       --SP, R11, R12                          // Store status register & return addr on application mode stack
         MODE_SVC_DI                                       // Switch back to supervisor mode
/*
* *** Replace the return adr with OS_SwitchAfterISR
*/
         mov       R10,LO(OS_SwitchAfterISR)               // Load OS_SwitchAfterISR addr
         orh       R10,HI(OS_SwitchAfterISR)
/*
* *** Replace the return adr in context of current mode
*/
ReplaceReturnAddr:
         cp.w     R8, 0x02                                 // Mode = INT0?
         breq     __ModeINT0
         cp.w     R8, 0x03                                 // Mode = INT1?
         breq     __ModeINT1
         cp.w     R8, 0x04                                 // Mode = INT2?
         breq     __ModeINT2                             
         mtsr      OS_CPU_RAR_INT3_OFFSET, R10             // Store new return addr  
         mtsr      OS_CPU_RSR_INT3_OFFSET, R9              // Store new status register (application mode with disabled interrupts)
         bral     __ModeIntDone
__ModeINT0:
         mtsr      OS_CPU_RAR_INT0_OFFSET, R10             // Store new return addr  
         mtsr      OS_CPU_RSR_INT0_OFFSET, R9              // Store new status register (application mode with disabled interrupts)
         bral      __ModeIntDone
__ModeINT1:
         mtsr      OS_CPU_RAR_INT1_OFFSET, R10             // Store new return addr  
         mtsr      OS_CPU_RSR_INT1_OFFSET, R9              // Store new status register (application mode with disabled interrupts)
          bral     __ModeIntDone
__ModeINT2:
         mtsr      OS_CPU_RAR_INT2_OFFSET, R10             // Store new return addr  
         mtsr      OS_CPU_RSR_INT2_OFFSET, R9              // Store new status register (application mode with disabled interrupts)
__ModeIntDone:

/*
* *** return to ISR
*/
         popm      R10, LR                                 // Get status register and return register from stack
         mtsr      SR, R10                                 // Restore status register
         NOPS
         ret       0                                       // return to interrupt routine

StoreIdle:
         mov       R10, LO(OS_SwitchAfterISR_Idle)         // Load OS_SwitchAfterISR_Idle addr
         orh       R10, HI(OS_SwitchAfterISR_Idle)
         mov       R9,  LO(SVC_MODE)                       // After Idle, we are o.k. to run in supervisor mode
         orh       R9,  HI(SVC_MODE)
         bral      ReplaceReturnAddr
         

/********************************************************************
*
*      OS_SwitchAfterISR
*
*
* App Stack (Task stack)
*   PC   SwitchFromInt
*   SR   SwitchFromInt
*   R8
*   R9
*   R10
*   R11
*   R12
*   LR
*   <Content of SVC stack>
*   NumItemsOnSVCStack
*/
OS_SwitchAfterISR:
         stm       --SP, R8-R12, LR                        // Save scratch registers & LR
/*** Copy SVC stack contents + Cnt -> App stack */
         mov       R9, SP                                  // store appliction stack pointer in R12
         MODE_SVC_DI                                       // Supervisor mode with disabled interrupts
         mov       R12, LO(_estack)
         orh       R12, HI(_estack)
         mov       R10, SP
         sub       R12, R10                                // R11 = countof Bytes on supervisor stack
         asr       R12,0x2                                 // R11 = countof words on supervisor stack
         breq      SaveStackDone
         mov       R11, R12
SaveStackLoop:
         ld.w      R8,R10++
         st.w      --R9,R8
         sub       R11,0x1
         brne      SaveStackLoop
SaveStackDone:
         st.w      --R9,R12                                // store number of words on application stack
/*** back to APP Mode */
         mov       R10, LO(OS_Ipl_DI)                      // Application mode with disabled ints
         orh       R10, HI(OS_Ipl_DI)
         ld.w      R10, R10[0]                             // Load OS_Ipl_DI
         mtsr      SR, R10
         NOPS
         mov       SP, R9

         rcall     OS_Switch
         rcall     OS_DecRegionCnt                         // OS_RegionCnt--
         popm      R10                                     // Get Numwords from application stack
         mov       R9, SP                                  // Get application stack pointer
         pushm     R10
         cp.w      R10, 0                                  // if (NumWords == 0)
         breq      RestoreStackDone
/*** Restore sys stack */
         MODE_SVC_DI
RestoreStackLoop:
         ld.w      R8,R9++                                 // Pop from App stack
         st.w      --SP,R8                                 // Push onto Sys stack
         sub       R10,0x1
         brne      RestoreStackLoop

         mov       R10, LO(OS_Ipl_DI)                      // Application mode with disabled ints
         orh       R10, HI(OS_Ipl_DI)
         ld.w      R10, R10[0]                             // Load OS_Ipl_DI
         mtsr      SR, R10
         NOPS
RestoreStackDone:
         popm      R10                                     // Get Num Words on 
         lsl       R10, 0x2
         mov       R11, 32
         add       R10, R11
         add       R10, SP                                 // R10 = SP + sizeof(R8-R12, LR) + NumWords
         mov       SP, R10

         MODE_SVC_DI
         ld.w      R11, R10[-0x8]                          // Get return addr and status register from application mode stack
         ld.w      R12, R10[-0x4]
         mtsr      OS_CPU_RAR_SUP_OFFSET, R12              // Get saved return register from current context
         mtsr      OS_CPU_RSR_SUP_OFFSET, R11              // Get saved status register from current context
         ld.w      LR,  R10[-0x20]                         // Restore scratch register
         ld.w      R12, R10[-0x1C]
         ld.w      R11, R10[-0x18]
         ld.w      R9,  R10[-0x10]
         ld.w      R8,  R10[-0x0C]
         ld.w      R10, R10[-0x14]
         rets


/**********************************************************************
*
*      scall_handler(int EnableDisableInterrupt, int Mode)
*
* Parameter:
*
*  Note (1)
*  scall:
*  AVR32A:
*  If ( SR[M2:M0] == {B'000 or B'001} ) {
*    *(--SP SVC ) <- PC + 2;
*    *(--SP SVC ) <- SR;
*    PC <- EVBA + 0x100;
*    SR[M2:M0] <- B'001;
*  } else {
*    LR Current Context <- PC + 2;
*    PC <- EVBA + 0x100;
*  }
*
*  AVR32B:
*  If ( SR[M2:M0] == {B'000 or B'001} ) {
*    RAR_SUP <- PC + 2;
*    RSR_SUP <- SR;
*    PC <- EVBA | 0x100;
*    SR[M2:M0] <- B'001;
*  } else {
*    LR Current Context <- PC + 2;
*    PC <- EVBA | 0x100;
*  }
*
*  rets:
*  If ( SR[M2:M0] == B'f000 ) {
*     Issue Privilege Violation Exception;
*  } else
*    if ( SR[M2:M0] == B'f001 ) {
*       SR <- *(SP SYS ++)
*       PC <- *(SP SYS ++)
*    } else {
*      PC <- LR Current Context
*    }
*  }
*/
         .section .scall.text,"ax"
scall_handler:
         pushm     R10
         mov       R10, R12
         andh      R10, 0x0000                              // mask out high byte
         brne      scall_GetCompareCount                    // check low byte, parameter in low byte (0=normal scall,1=COUNT/COMPARE)
         mfsr      R10, SR                                  // if ((SR & 0x00800000) == 0) {
         andh      R10, 0x180,COH                           //   M[2:1] == 0 ?
         brne      scall_FromISR
         mfsr      R10, OS_CPU_RSR_SUP_OFFSET               // Retrieve SR from previous context
         and       R10, R11                                 // Mask out bits
         or        R10, R12                                 // Set new bit mask
         mtsr      OS_CPU_RSR_SUP_OFFSET, R10               // Store SR back
         popm      R10
         rets
scall_FromISR:
         mfsr      R10, SR                                  // if ((SR & 0x00800000) == 0) {
         and       R10, R11
         or        R10, R12
         mtsr      SR, R10
         NOPS
         popm      R10
         rets
scall_GetCompareCount:
         mfsr      R10, COUNT
         mfsr      R11, COMPARE
         sub       R12, R11, R10
         popm      R10
         rets

/*********************************************************************
*
*     OS_CallISR
*
*
* An assembler code tempalte can be gerated with the following code:
*
* int  OS__EnterInterrupt(unsigned * pStat);
* void OS__LeaveInterrupt(unsigned * pStat);
*
* void OS_CallISR (void (*pRoutine)(void)) {
*   unsigned Stat;
*   if (OS__EnterInterrupt(&Stat) == 1) {
*     OS_ISR_StackFrameAddr = 0; //"SP";
*   }
*   pRoutine();
*   OS__LeaveInterrupt(&Stat);
* }
*/
OS_CallISR:
         STM       --SP,R7,LR
//   unsigned Stat;
         SUB       SP,0x4                                   // Create space for local variable "Stat"
         MOV       R7,R12                                   // Copy pRoutine into a preserved register
//   OS_EnterInterrupt(&Stat);
         MOV       R12,SP
         RCALL     OS__EnterInterrupt
//   if (OS_EnterInterrupt() == 1) {
         CP.w      R12,0x1
         BRNE      OS_CallISR_NotTheFirstCall
//     OS_ISR_StackFrameAddr = 0; // In ASM function, replace by OS_ISR_StackFrameAddr <- SP
         MOV       R11,LO(OS_ISR_StackFrameAddr)
         ORH       R11,HI(OS_ISR_StackFrameAddr)
         ST.w      R11[0x0], SP
//   }
OS_CallISR_NotTheFirstCall:
//   pRoutine();
         ICALL     R7
//   OS_LeaveInterrupt(&Stat);
         MOV       R12,SP
         RCALL     OS__LeaveInterrupt
// }
         SUB       SP,-0x4
         LDM       SP++,R7,PC,R12=0x0

/*********************************************************************
*
*     OS_CallNestableISR
*
*
* An assembler code tempalte can be gerated with the following code:
*
* int  OS__EnterNestableInterrupt(unsigned * pStat);
* void OS__LeaveNestableInterrupt(unsigned * pStat);
*
* void OS_CallISR (void (*pRoutine)(void)) {
*   unsigned Stat;
*   if (OS__EnterNestableInterrupt(&Stat) == 1) {
*     OS_ISR_StackFrameAddr = 0; //"SP";
*   }
*   pRoutine();
*   OS__LeaveNestableInterrupt(&Stat);
* }
*/
OS_CallNestableISR:
         STM       --SP,R7,LR
//   unsigned Stat;
         SUB       SP,0x4                                   // Create space for local variable "Stat"
         MOV       R7,R12                                   // Copy pRoutine into a preserved register
//   OS_EnterInterrupt(&Stat);
         MOV       R12,SP
         RCALL     OS__EnterNestableInterrupt
//   if (OS_EnterInterrupt() == 1) {
         CP.w      R12,0x1
         BRNE      OS_CallNestableISR_NotTheFirstCall
//     OS_ISR_StackFrameAddr = 0; // In ASM function, replace by OS_ISR_StackFrameAddr <- SP
         MOV       R11,LO(OS_ISR_StackFrameAddr)
         ORH       R11,HI(OS_ISR_StackFrameAddr)
         ST.w      R11[0x0], SP
//   }
OS_CallNestableISR_NotTheFirstCall:
//   pRoutine();
         ICALL     R7
//   OS_LeaveInterrupt(&Stat);
         MOV       R12,SP
         RCALL     OS__LeaveNestableInterrupt
// }
         SUB       SP,-0x4
         LDM       SP++,R7,PC,R12=0x0

/********************************************************************
*
*     OS_GetIpl
*/
OS_GetIpl:
         mfsr      R12, SR
         andh      R12,0x1E,COH
         ret       R12

/********************************************************************
*
*     OS_GetCompareCount
*/
OS_GetCompareCount:
         pushm     R10-R11, LR
         mov       R12, LO(0x0001)
         orh       R12, HI(0x0000)
         scall
         popm      R10-R11, LR
         ret       R12

/********************************************************************
*
*     OS_SetCompare
*/
OS_SetCompare:
         mtsr      COMPARE, R12
         mov       PC, LR

/********************************************************************
*
*     OS_GetCompare
*/
OS_GetCompare:
         mfsr      R12, COMPARE
         mov       PC, LR

/********************************************************************
*
*     OS_GetCount
*/
OS_GetCount:
         mfsr      R12, COUNT
         mov       PC, LR

/********************************************************************
*
*     OS_IsInInt
*/
OS_IsInInt:
         mov       R12, 0x00
         ret       R12
 
  
/*********************************************************************
*
*       OS_GetIntStackBase
*/
OS_GetIntStackBase:
	     lda.w     R12, _stack
         ret       R12 

/*********************************************************************
*
*       OS_GetIntStackSize
*/
OS_GetIntStackSize:
         lda.w     R11, _stack
         lda.w     R12, _estack
         sub       R12, R11
         ret       R12

/*********************************************************************
*
*       OS_GetSysStackBase
*/
OS_GetSysStackBase:
         lda.w     R12, _stack
         ret       R12  

/*********************************************************************
*
*       OS_GetSysStackSize
*/
OS_GetSysStackSize:
         lda.w     R11, _stack
         lda.w     R12, _estack
         sub       R12, R11
         ret       R12  

/*********************************************************************
*
*       OS_GetSP
*/
OS_GetSP:
         mov       R12,SP
         ret       R12        


/*********************************************************************
*
*       OS_Prepare_Stackpointer
*/
OS_Prepare_Stackpointer:
         lddpc SP, .LC2
.LC2:    .long	   _estack


         .end

     