;*********************************************************************************************************
;                                PERFORM A CONTEXT SWITCH (From an ISR)
;                                        void OSIntCtxSw(void)
;
; Note(s): 1) Upon entry,
;             OSTCBCur     points to the OS_TCB of the task to suspend
;             OSTCBHighRdy points to the OS_TCB of the task to resume
;
;          2) The stack frame of the task to suspend looks as follows:
;
;                                   return address    <-- +1
;                                   ISTATUS           <-- +8
;                                   %g0-%g7           <-- +8
;                                   CWP_ISR(%i)       <-- +16
;                                   CWP_TASK(%i-%L)   <-- +16
;                                   ...
;                                   ...
;                                   CWP_HL-1(%i-%L)   <-- +16
;                                   CWPHILIMIT(%i-%L) <-- +16
;                                   STATUS(ISR)       <-- +24
; OSTCBHighRdy->OSTCBStkPtr -->     Compiler space    <-- 0
;
;
;          3) The stack frame of the task to resume looks as follows:
;
;                                   return address    <-- +1
;                                   ISTATUS           <-- +8
;                                   %g0-%g7           <-- +8
;                                   CWP_ISR(%i)       <-- +16
;                                   CWP_TASK(%i-%L)   <-- +16
;                                   ...
;                                   ...
;                                   CWP_HL-1(%i-%L)   <-- +16
;                                   CWPHILIMIT(%i-%L) <-- +16
;                                   STATUS(ISR)       <-- +24
; OSTCBHighRdy->OSTCBStkPtr -->     Compiler space    <-- 0
;
;*********************************************************************************************************

OSIntCtxSw:

   ; This Subroutine is called from OSTickISR();
   ; same as OSCtxSw() (see above)

   ; Call OSTaskSwHook()
    .if (OS_CPU_HOOKS_EN == 1)
      MOVI32   %g0, OSTaskSwHook@h
      CALL     %g0
      NOP
    .endif

   ; OSTCBCur = OSTCBHighRdy
      MOVI32   %g0, OSTCBHighRdy
      LD       %g1, [%g0]                    ; %g1 = HighRdy TCB
      MOVI32   %g0, OSTCBCur
      ST       [%g0], %g1

   ; OSPrioCur = OSPrioHighRdy
      MOVI32   %g2, OSPrioHighRdy
      LD       %g3, [%g2]                    ; %g3 = HighPrioRdy
      MOVI32   %g2, OSPrioCur
      ST       [%g2], %g3


   ; Switch to the highest priority task.
   ; %sp = OSTCBHighRdy->OSTCBStkPtr (%g1 = OSTCBHighRdy)
      ;MOVI32   %g0, OSTCBHighRdy          ; %g0 = &OSTCBHighRdy
      ;LD       %g1, [%g0]                 ; %g1 = &OS_TCB
      LD       %sp, [%g1]                 ; stack is the first element

      LOAD_CONTEXT

      TRET           %o7
      NOP





; ==========================================================================================
; == void OSNiosCallISR();
; ==========================================================================================
; parameters : %o0 = irq ; %o1 = UserISR
      .text
      .global   OSNiosCallISR

OSNiosCallISR:
      SAVE_CONTEXT                          ; affects (%sp, CWP, %g0, %g1, %g2, %g6 and %g7)
  ; %g7 = STATUS(Interrupt)

  ; Call OSIntEnter() or OSIntNesting++;
  ; warning !! OSIntNesting is 8bits value !!
      MOVI32    %g2, OSIntNesting           ; %g2 = &OSIntNesting
      LD        %g1, [%g2]                  ; %g1 = OSIntNesting
      EXT8D     %g1, %g2                    ; extract byte
      ADDI      %g1, 1                      ; %g1 ++
      FILL8     %r0, %g1                    ; %g0 = %g1 %g1 %g1 %g1
      ST8D      [%g2], %r0                  ; store %g1




  ; if (OSIntNesting == 1) OSTCBCur->OSTCBStkPtr = %sp
  ; %g1 = OSIntNesting
      CMPI      %g1, 1                      ; %g1 == 1
      SKPS      cc_eq                       ; %g1 == 0 ?
      BR        nextUISR                    ; no
      ;NOP                                   ; yes
      MOVI32   %g0, OSTCBCur                ; %g0 = &OSTCBHighRdy (delay slot)
      LD       %g1, [%g0]                   ; %g1 = &OS_TCB
      ST       [%g1], %sp                   ; stack is the first element

      ; extract IPRI : %g7 = STATUS(interrupt)
      LSRI      %g7, 9                      ; %g7 = XXXXXXXXXXIIIIII
      PFX       %hi(0x003F)
      AND       %g7, %lo(0x003F)            ; %g7 = 0000000000IIIIII

nextUISR:
   ; Call UserISR() from Table ( must clears the interrupt )
      MOVI32    %g1, OSNiosUserISRTable
      LSLI      %g7, regwidth/2             ; %g7 = %g7*regwidth
      ADD       %g1, %g7                    ; %g1 = ptr(UserISR@h)
      LD        %g0, [%g1]                  ; %g0 = UserISR@h
      CALL      %g0
      NOP

   ; Call OSIntExit()
      MOVI32   %g0, OSIntExit@h
      CALL     %g0
      NOP

      LOAD_CONTEXT

      TRET           %o7
      NOP











; ==========================================================================================
; == USEFULL FUNCTIONS
; ==========================================================================================
      .text
      .global getStatus
      .global getFP
      .global getSP
      .global getReg
      .global getITStatus
      .global setITStatus
      .global getHILIMIT
      .global getIPRI

   ; int getStatus()
   ; return = %o0 = %ctl0
getStatus:
      RDCTL    %o0
      JMP      %o7
      NOP


   ; int getFP()
   ; return = %o0 = %fp = %i6
getFP:
      MOV      %o0, %fp
      JMP      %o7
      NOP



   ; int getSP()
   ; return = %o0 = %sp = %o6
getSP:
      MOV      %o0, %sp
      JMP      %o7
      NOP



   ; int getReg()
   ; return = %o0 = %reg
getReg:
      MOV   %o0, %sp
      JMP   %o7
      NOP


   ; OS_CPU_SR getITStatus()
   ; return = %o0 = X0000...000     X = 1 or 0
getITStatus:
      RDCTL     %o0
      PFX       %hi(0x8000)
      AND       %o0, %lo(0x8000)
      disable_interrupt
      JMP       %o7
      NOP

   ; void setITStatus(OS_CPU_SR status)
   ; in = %o0 = status
setITStatus:
      RDCTL     %o1
      PFX       %hi(0x7FFF)
      AND       %o1, %lo(0x7FFF)
      OR        %o0, %o1
      WRCTL     %o0
      NOP
      JMP       %o7
      NOP

  ; INT16U getHIMILIT();
  ; read HI_LIMIT from %ctl2
  ; return = %o0 = 00000000000HHHHH
getHILIMIT:
      PFX       2
      RDCTL     %o0                               ; %o0 = XXXXXXHHHHHLLLLL
      LSRI      %o0, 5                            ; %o0 = XXXXXXXXXXXHHHHH
      PFX       %hi(0x001F)
      AND       %o0, %lo(0x001F)                  ; %o0 = 00000000000HHHHH
      JMP       %o7
      NOP

  ; INT16U getIPRI();
  ; read IPRI from %ctl0
  ; return = %o0 = IPRI
getIPRI:
      RDCTL     %o0
      LSRI      %o0, 9                      ; %g7 = XXXXXXXXXXIIIIII
      PFX       %hi(0x003F)
      AND       %o0, %lo(0x003F)            ; %g7 = 0000000000IIIIII
      JMP       %o7
      NOP