;********************************************************************************************************
;                                              uC/OS-II
;                                        The Real-Time Kernel
;
;                    Copyright 1992-2020 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.
;
;********************************************************************************************************

;********************************************************************************************************
;
;                                       ATmega256  Specific code
;
; Filename : os_cpu_a.s90
; Version  : V2.93.00
;********************************************************************************************************

#include        <os_cpu_i.s90>


;********************************************************************************************************
;                                          PUBLIC DECLARATIONS
;********************************************************************************************************

                MODULE  OS_CPU_A

                PUBLIC  OS_CPU_SR_Save
                PUBLIC  OS_CPU_SR_Restore
                PUBLIC  OSStartHighRdy
                PUBLIC  OSCtxSw
                PUBLIC  OSIntCtxSw

;********************************************************************************************************
;                                         EXTERNAL DECLARATIONS
;********************************************************************************************************

                EXTERN  OSIntExit
                EXTERN  OSIntNesting
                EXTERN  OSPrioCur
                EXTERN  OSPrioHighRdy
                EXTERN  OSRunning
                EXTERN  OSTaskSwHook
                EXTERN  OSTCBCur
                EXTERN  OSTCBHighRdy


;********************************************************************************************************
;                            DISABLE/ENABLE INTERRUPTS USING OS_CRITICAL_METHOD #3
;
; Description : These functions are used to disable and enable interrupts using OS_CRITICAL_METHOD #3.
;
;               OS_CPU_SR  OSCPUSaveSR (void)
;                     Get current value of SREG
;                     Disable interrupts
;                     Return original value of SREG
;
;               void  OSCPURestoreSR (OS_CPU_SR cpu_sr)
;                     Set SREG to cpu_sr
;                     Return
;********************************************************************************************************

                   RSEG    FARCODE:CODE:NOROOT(1)

OS_CPU_SR_Save:    IN      R16,SREG                 ; Get current state of interrupts disable flag
                   CLI                              ; Disable interrupts
                   RET                              ; Return original SREG value in R16


OS_CPU_SR_Restore: OUT     SREG,R16                 ; Restore SREG
                   RET                              ; Return


;********************************************************************************************************
;                               START HIGHEST PRIORITY TASK READY-TO-RUN
;
; Description : This function is called by OSStart() to start the highest priority task that was created
;               by your application before calling OSStart().
;
; Note(s)     : 1) The (data)stack frame is assumed to look as follows:
;
;                                                       Hardware Stack points to EMPTY
;                                                       <----------------------------\
;                                                PC (H)                              |
;                                                PC (M)                              |
;                                                PC (L)                              |
;                                                                                    |
;                                                                                    |
;                  OSTCBHighRdy->OSTCBStkPtr --> SPL of (return) stack pointer   ----/   (Low memory)
;                                                SPH of (return) stack pointer
;                                                Flags to load in status register
;                                                EIND
;                                                RAMPZ
;                                                R31
;                                                R30
;                                                R27
;                                                .
;                                                .
;                                                R0                                      (High memory)
;
;                  where the stack pointer points to the task start address.
;
;
;               2) OSStartHighRdy() MUST:
;                      a) Call OSTaskSwHook() then,
;                      b) Set OSRunning to TRUE,
;                      c) Switch to the highest priority task.
;********************************************************************************************************

OSStartHighRdy: CALL    OSTaskSwHook                ; Invoke user defined context switch hook
                LDS     R16,OSRunning               ; Indicate that we are multitasking
                INC     R16                         ;
                STS     OSRunning,R16               ;

                LDS     R30,OSTCBHighRdy            ; Let Z point to TCB of highest priority task
                LDS     R31,OSTCBHighRdy+1          ; ready to run
                LD      R28,Z+                      ; Load Y (R29:R28) pointer
                LD      R29,Z+                      ;

                POP_SP                              ; Restore stack pointer
                POP_SREG_INT                        ; Restore status register (DISABLE interrupts)
                POP_ALL                             ; Restore all registers
                RETI


;********************************************************************************************************
;                                       TASK LEVEL CONTEXT SWITCH
;
; Description : This function is called when a task makes a higher priority task ready-to-run.
;
; 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:
;
;                                       SP+0 --> LSB of task code address
;                                         +1     MSB of task code address                (High memory)
;
;               3) The saved context of the task to resume looks as follows:
;
;                                                       Hardware Stack points to EMPTY
;                                                       <----------------------------\
;                                                PC (H)                              |
;                                                PC (M)                              |
;                                                PC (L)                              |
;                                                                                    |
;                                                                                    |
;                  OSTCBHighRdy->OSTCBStkPtr --> SPL of (return) stack pointer   ----/   (Low memory)
;                                                SPH of (return) stack pointer
;                                                Flags to load in status register
;                                                EIND
;                                                RAMPZ
;                                                R31
;                                                R30
;                                                R27
;                                                .
;                                                .
;                                                R0                                      (High memory)
;********************************************************************************************************

OSCtxSw:        PUSH_ALL                            ; Save current task's context
                PUSH_SREG
                PUSH_SP

                LDS     R30,OSTCBCur                ; Z = OSTCBCur->OSTCBStkPtr
                LDS     R31,OSTCBCur+1              ;
                ST      Z+,R28                      ; Save Y (R29:R28) pointer
                ST      Z+,R29                      ;

                CALL    OSTaskSwHook                ; Call user defined task switch hook

                LDS     R16,OSPrioHighRdy           ; OSPrioCur = OSPrioHighRdy
                STS     OSPrioCur,R16

                LDS     R30,OSTCBHighRdy            ; Let Z point to TCB of highest priority task
                LDS     R31,OSTCBHighRdy+1          ; ready to run
                STS     OSTCBCur,R30                ; OSTCBCur = OSTCBHighRdy
                STS     OSTCBCur+1,R31              ;

                LD      R28,Z+                      ; Restore Y pointer
                LD      R29,Z+                      ;

                POP_SP                              ; Restore stack pointer
                LD      R16,Y+                      ; Restore status register
                SBRC    R16,7                       ; Skip next instruction in interrupts DISABLED
                RJMP    OSCtxSw_1

                OUT     SREG,R16                    ; Interrupts of task to return to are DISABLED
                POP_ALL
                RET

OSCtxSw_1:      CBR     R16,BIT07                   ; Interrupts of task to return to are ENABLED
                OUT     SREG,R16
                POP_ALL                             ; Restore all registers
                RETI


;*********************************************************************************************************
;                                INTERRUPT LEVEL CONTEXT SWITCH
;
; Description : This function is called by OSIntExit() to perform a context switch to a task that has
;               been made ready-to-run by an ISR.
;
; 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:
;
;                                                       Hardware Stack points to EMPTY
;                                                       <----------------------------\
;                                                PC (H)                              |
;                                                PC (M)                              |
;                                                PC (L)                              |
;                                                                                    |
;                                                                                    |
;                  OSTCBCur->OSTCBStkPtr ------> SPL of (return) stack pointer    ---/   (Low memory)
;                                                SPH of (return) stack pointer
;                                                Flags to load in status register
;                                                EIND
;                                                RAMPZ
;                                                R31
;                                                R30
;                                                R27
;                                                .
;                                                .
;                                                R0                                      (High memory)
;
;               3) The saved context of the task to resume looks as follows:
;
;                                                       Hardware Stack points to EMPTY
;                                                       <----------------------------\
;                                                PC (H)                              |
;                                                PC (M)                              |
;                                                PC (L)                              |
;                                                                                    |
;                                                                                    |
;                  OSTCBHighRdy->OSTCBStkPtr --> SPL of (return) stack pointer    ---/   (Low memory)
;                                                SPH of (return) stack pointer
;                                                Flags to load in status register
;                                                EIND
;                                                RAMPZ
;                                                R31
;                                                R30
;                                                R27
;                                                .
;                                                .
;                                                R0                                      (High memory)
;*********************************************************************************************************

OSIntCtxSw:
                CALL    OSTaskSwHook                ; Call user defined task switch hook

                LDS     R16,OSPrioHighRdy           ; OSPrioCur = OSPrioHighRdy
                STS     OSPrioCur,R16

                LDS     R30,OSTCBHighRdy            ; Let Z point to TCB of highest priority task
                LDS     R31,OSTCBHighRdy+1          ; ready to run
                STS     OSTCBCur,R30                ; OSTCBCur = OSTCBHighRdy
                STS     OSTCBCur+1,R31              ;

                LD      R28,Z+                      ; Restore Y pointer
                LD      R29,Z+                      ;

                POP_SP                              ; Restore stack pointer
                LD      R16,Y+                      ; Restore status register
                SBRC    R16,7                       ; Skip next instruction in interrupts DISABLED
                RJMP    OSIntCtxSw_1

                OUT     SREG,R16                    ; Interrupts of task to return to are DISABLED
                POP_ALL
                RET

OSIntCtxSw_1:   CBR     R16,BIT07                   ; Interrupts of task to return to are ENABLED
                OUT     SREG,R16
                POP_ALL                             ; Restore all registers
                RETI

                END
