#include "ioCC2430.h"
#include <iar_check_symbols.h>

            ; name conflict when named mac_scheduler
            MODULE  mac_scheduler_a

; External symbols and constants
MAC_TASK_QUEUE_SIZE     EQU     03H
NO_TASK                 EQU     0FFH

            EXTERN  pMacTaskQueues
            EXTERN  pMacTasks

#if (__CODE_MODEL__ == __CM_BANKED__)
            EXTERN  ?BCALL
MAC_TASK_INFO_SIZE      EQU     10

#else
MAC_TASK_INFO_SIZE      EQU     9
#endif


;;-------------------------------------------------------------------------------------------------------
;;  void mschDoTask(void)
;;
;;  DESCRIPTION:
;;      Used by the timer module to start or resume a task at every backoff slot boundary
;;      This routine is written in assembler to make the function reentrant, and increase execution
;;      speed.
;;
;;  C EQUIVALENT:
;;      void mschDoTask(void) {
;;          UINT8 taskNumber;
;;          MAC_TASK_QUEUE *pQueue;
;;          MAC_TASK_INFO *pTask;
;;
;;          // Execute tasks by priority
;;          UINT8 n = MAC_TASK_PRIORITY_COUNT - 1;
;;          do {
;;
;;              // Stop if there's a task in progress
;;              pQueue = &pMacTaskQueues[n];
;;              if (pQueue->taskInProgress) return;
;;
;;              // If there's a valid task in the queue, then start it
;;              DISABLE_GLOBAL_INT();
;;              taskNumber = pQueue->firstTask;
;;              if (taskNumber != NO_TASK) {
;;                  pQueue->taskInProgress = TRUE;
;;                  pTask = &pMacTasks[taskNumber];
;;                  ENABLE_GLOBAL_INT();
;;                  ((TFPTR) pTask->pTaskFunc)(pTask);
;;                  pQueue->taskInProgress = FALSE;
;;                  return;
;;              }
;;              ENABLE_GLOBAL_INT();
;;          } while (n--);
;;      }
;;-------------------------------------------------------------------------------------------------------

                RSEG    RCODE
                PUBLIC  mschDoTask;
mschDoTask:
                ; R7 = Current task priority
                MOV     R7,#03H;


priorityLoop:   ; Let DPTR point to the current task queue
                ; DPTR = sizeof(MAC_TASK_QUEUE) * priority + pMacTaskQueues
                ;      = 3 * R7 + pMacTaskQueues
                MOV     A, #MAC_TASK_QUEUE_SIZE;
                MOV     B, R7;
                MUL     AB;
                ADD     A, #LOW pMacTaskQueues;
                MOV     DPL, A;
                CLR     A;
                ADDC    A, #HIGH pMacTaskQueues;
                MOV     DPH, A;

                ; Disable all interrupts
                CLR     EA;

                ; Get the queue parameters
                ; R6 = taskNumber = pQueue->firstTask
                MOVX    A, @DPTR;
                MOV     R6, A;
                INC     DPTR;
                INC     DPTR;
                ; A = pQueue->taskInProgress
                MOVX    A, @DPTR;

                ; Abort if there's a task already in progress
                JNZ     exit;

                ; Skip this round if no task is found
                CJNE    R6, #NO_TASK, foundTask;
                SJMP    endLoop;


foundTask:      ; Set the task in progress flag
                MOV     A, #01H
                MOVX    @DPTR, A;
                PUSH    DPH;
                PUSH    DPL;

                ; Get the pointer to the task info and put it in R2 and R3
                ; [R3:R2] = sizeof(MAC_TASK_INFO) * taskNumber(R6) + pMacTasks
                MOV     A, #MAC_TASK_INFO_SIZE;
                MOV     B, R6;
                MUL     AB;
                ADD     A, #LOW pMacTasks;
                MOV     R2, A;
                CLR     A;
                ADDC    A, #HIGH pMacTasks;
                MOV     R3, A;

                ; Set DPTR to point to the task structure
                MOV     DPL, R2;
                MOV     DPH, R3;

#if (__CODE_MODEL__ == __CM_BANKED__)
#else
                ; set up return address
                MOV     A, #LOW callReturned
                PUSH    A
                MOV     A, #HIGH callReturned
                PUSH    A
#endif
                ; Put the function address into DPTR and the bank number into A
                MOVX    A, @DPTR;
                MOV     R4, A
                INC     DPTR;
                MOVX    A, @DPTR;
                MOV     R5, A;
                INC     DPTR;
                MOVX    A, @DPTR;
                MOV     DPL, R4;
                MOV     DPH, R5;

                ; Turn interrupts back on
                SETB    EA;

                ; Make the banked function call
#if (__CODE_MODEL__ == __CM_BANKED__)

                LCALL   ?BCALL;
#else
                CLR     A
                JMP     @A+DPTR   ; Call subroutine
#endif

                ; Clear the task in progress flag (the DPTR was pushed @ foundTask)
callReturned:   POP     DPL;
                POP     DPH;
                CLR     A;
                MOVX    @DPTR, A;

                ; We're done!!!
                SJMP    exit;


endLoop:        ; Turn interrupts back on
                SETB    EA;

                ; Lower the priority level, and continue on the next loop
                DEC     R7;

                ; Exit when the priority 0 has been checked
                CJNE    R7, #0FFH, priorityLoop;


exit:           ; Turn interrupts back on
                SETB    EA;

                ; Return :)
                RET;

                END;