// Put your name & group number here!

/*
  PreLab 9 Skeleton 
  Philip Koopman
  10-28-2009
  lab_9.c    version 1.0 

  Change history:
  CJS - 11.01.2009 - In Timer ISR, added code to switch stack to main loop stack 
                     regardless of currentTask.  Prevents stack smashing bug when
                     all tasks are finished and task0 re-initializes
  akane - 03.20.2012 - In GetSwitches(), inverting PORTB before reading since switches  
  					 are active low which makes markings on switches confusing

  
  This program uses the modclock library.  It must be included in the project.
  This program runs stand-alone on the CPU module. The protoboard is not required.
  CPU module jumpers will have to be changed from the default configuration to enable
        the CPU module switches and LEDs.  Jumpers installed will  enable the corresponding items
        in the below SW and LED list as indicated on the silkscreen lettering on the CPU board.

  Port assignments are according to Table 6 of the module user guide:
    SW 3-1  PB0  (dip rocker switch position 1)
    SW 3-2  PB1
    SW 3-3  PB2
    SW 3-4  PB3
    LED 1   PB4  (active low)
    LED 2   PB5
    LED 3   PB6
    LED 4   PB7
  Note that you will have 4 input bit and 4 output bits on PB
  
  This code, as is, does the following:
     - Each task controls an individual LED; LED is on while task is active
     - Each LED "on" time represents a task that computes for that period of time
         (so the point is not to blink the light at a certain rate; it is to show how long tasks are active)
         If a task is suspended by another task, its light remains on while it is suspended.
     - DIP switches 1-4 control corresponding tasks.  Dip switch = ON means task is active
         - DIP switches are in reverse physical location order from LEDs (but are labelled correctly on the module)
         - DIP switches are sampled by Task 1, so it takes a while for them to be sampled (sometimes a long while)
         - DIP switches do not halt a task immediately; what they do is prevent re-launch at next period       

   Terminology:   _scheduler_ is the thing that decides what tasks to turn on and off and when
                  _tasker_ is the thing that finds highest priority running task and restarts its execution
                    (tasker is either in main loop or timer interrupt, depending on scheduling #define used)
 
   Testing note:  we can tell if we are schedulable if we sometimes get all "OFF" LEDs for tasking that uses periods

   Optimization note:  would help if there were a "sleep" function to switch tasks before the next 8 msec tick,
          especially for Task0 (the scheduler) so it could run more often.
*/

// -----------------------

//#define SIMSWITCHES  // disable when running on real hardware; fakes all switches on for simulation

// Define EXACTLY ONE of below to enable different executives

//#define UNTIMED_CYCLIC_EXEC
//#define CYCLIC_EXEC
//#define MULTI_RATE_COOPERATIVE
//#define PRIORITIZED_COOPERATIVE
#define PREEMPTIVE


//#define SOLNP  // enables prioritized subset with solution

// -----------------------

// Workload parameters

// Amount of CPU time to burn (in msec)
#define TASKRUN2  117
#define TASKRUN3  227
#define TASKRUN4  450
#define TASKRUN5  1100

// Task period (in msec)
#define PERIOD0      5   // if you make this too fast the scheduler overhead gets out of hand (8 msec per run)
#define PERIOD1     20
#define PERIOD2    500
#define PERIOD3   1000
#define PERIOD4   5000
#define PERIOD5  10000

// Preemptive Utilization ~= 8/50 + 8/250 + 117/500 + 227/1000 + 450/5000 + 1100/10000 = .853 + other tasks + B

#define STATIC_PERIOD 5000   // 5 second period for single-rate cyclic exec  

//*****************
//  Includes
//*****************

#include <hidef.h>      /* common defines and macros */
#include <mc9s12c128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12c128"

#include "modclock.h"

//*****************
//  Typedefs
//*****************
typedef char bool;
#define TRUE  1
#define FALSE 0
#define ON    1
#define OFF   0 

typedef   signed char   int8;
typedef unsigned char  uint8;
typedef   signed int   int16;
typedef unsigned int  uint16;
typedef   signed long  int32;
typedef unsigned long uint32;

typedef void (*Ptr2Function)(void);

//***************************
//*  Function prototypes
//***************************
void SetLED(uint8 position, bool flag);  // CAUTION -- enables interrupts as side effect
uint32 TimeNow(void);                    // CAUTION -- enables interrupts as a side effect

void InitSwitchesLED(void);
uint8 GetSwitches(void);
void QuerySwitch(uint8 Task, uint8 Switch); 
void InitPCB(void);

void Task0(void);    
void Task1(void);
void Task2(void);
void Task3(void);
void Task4(void);
void Task5(void);

void main(void);

void WasteMsec(uint16 WaitTime);
void SetupTimer(void);
void interrupt 16 TimerHandler(void); 

#ifdef PREEMPTIVE
//  Below functions are only needed for PREEMPTIVE

uint8 AddrHi(Ptr2Function Ptr);
uint8 AddrLo(Ptr2Function Ptr);
void TaskTerminate(void);
void CreateLaunchStack(uint8 Task); 

#endif PREEMPTIVE



//*****************************
//   Switch and LED interface
//*****************************

// Define switch and LED bit positions
#define  SW3_1  0x01
#define  SW3_2  0x02
#define  SW3_3  0x04
#define  SW3_4  0x08
#define  LED1   0x10
#define  LED2   0x20
#define  LED3   0x40
#define  LED4   0x80

// ----------

// Initialize Switches and LEDs attached to port B
//
void InitSwitchesLED(void) 
{
  // Set up port B   PB0..PB3 input      PB4..PB7  output
  DDRB = 0x00 | LED1 | LED2 | LED3 | LED4 ;  // input bits are 0 for DDR

  // Inputs on Port B need pull-up resistors to work properly with CPU module
  PUCR |= 0x02;   // see MC9S12C128V1 data sheet section 4.3.2.10
  
  // Turn off all LEDs at startup
  PORTB = LED1 | LED2 | LED3 | LED4; 
}

// ----------

// This routine sets the LED outputs given an LED bit position and an On/Off flag
// Port B outputs are a shared resource
// For a preemptive system,  LEDValue needs to be protected to avoid run-time hazards
//
void SetLED(uint8 position, bool flag)
{ 
  // Remember current LED values in a static variable for updates later
  //  yes, we could read PORTB, but sometimes output ports get corrupted; this is more robust
  static uint8 LEDValue = 0xFF;    // inverted output values (1 = LED off); tracks PORTB contents

#ifdef PREEMPTIVE
  DisableInterrupts;    // Need atomic update to LEDValue 
  // TO DO: Somewhere below in this procedure we need to enable interrupts -- put that code 
  //        in the right place with the right #ifdef for compilation
#endif // PREEMPTIVE

  if (flag) 
  {  // turn LED on by zeroing appropriate bit
    LEDValue = LEDValue & (position ^ 0xFF);
  } else
  {  // turn LED off by setting appropriate bit
    LEDValue = LEDValue | position;
  }

  //output new value to LEDs
  PORTB = LEDValue;

}


// ----------

// Get switch values while masking bits associated with LED outputs
//
uint8 GetSwitches(void) 
{
#ifdef SIMSWITCHES  // disable when running on real hardware; fakes all switches on for simulation
 return(SW3_1 | SW3_2 | SW3_3 | SW3_4);  // all switches on for simulation
#else
 return(~PORTB & (SW3_1 | SW3_2 | SW3_3 | SW3_4));	// invert PORTB since switches active low
#endif // SIMSWITCHES
}


//*****************************
//   Tasking Support
//*****************************

#define NTASKS  6     // total number of tasks in this workload, not counting main loop

uint8 CurrentTask = NTASKS;    // current task executing is initialized to be main loop

// Process control block for tracking task status
//
typedef struct PCBstruct
{ Ptr2Function TaskPtr;  // points to start of task code
  bool   LaunchRequest;     // true when task is being requested by a switch
  bool   ReadyToLaunch;  // true when task is eligible to launched (i.e., called at initial entry point)
  bool   Running;        // true when task is actually running (post-launch)
  uint8 * SPsave;
  uint16 Period;         // period in msec
  uint32 NextTime;       // next start time in msec
} PCBstruct;

PCBstruct PCB[NTASKS+1];   // allocate one extra entry for main loop SP save

// Reserve a separate stack area for each task
//
#define STACKSIZE  64             //  size is in bytes for each stack
uint8 STACK[NTASKS][STACKSIZE];   // Don't need a stack for main loop; already have one by default

// ----------

// Initialize PCB entries before starting tasker
//
void InitPCB(void) 
{
  // Initialize PCB entries for the tasks  (Note that PCB[NTASKS] doesn't need initialization)
  //   SPsave doesn't need to be initialized; that's done when setting up stack upon task launch
  
  uint32 TempTime; // store time to avoid time changing between uses

  // Set up task pointers and initial run status
  PCB[0].TaskPtr = &Task0;  PCB[0].LaunchRequest = TRUE;  // always run scheduler
  PCB[1].TaskPtr = &Task1;  PCB[1].LaunchRequest = TRUE;  // always run switch checker
  PCB[2].TaskPtr = &Task2;  PCB[2].LaunchRequest = FALSE;  // other tasks off by default until switches are set
  PCB[3].TaskPtr = &Task3;  PCB[3].LaunchRequest = FALSE; 
  PCB[4].TaskPtr = &Task4;  PCB[4].LaunchRequest = FALSE;
  PCB[5].TaskPtr = &Task5;  PCB[5].LaunchRequest = FALSE;
  
  // Set up task periods
  //    Period is in msec -- what period actually does varies depending on scheduling technique
  PCB[0].Period = PERIOD0; 
  PCB[1].Period = PERIOD1; 
  PCB[2].Period = PERIOD2; 
  PCB[3].Period = PERIOD3; 
  PCB[4].Period = PERIOD4; 
  PCB[5].Period = PERIOD5; 
  
  // Set task running status flags
  PCB[0].ReadyToLaunch = TRUE;     PCB[0].Running = FALSE;   // Always launch scheduler
  PCB[1].ReadyToLaunch = FALSE;    PCB[1].Running = FALSE;   
  PCB[2].ReadyToLaunch = FALSE;    PCB[2].Running = FALSE;   
  PCB[3].ReadyToLaunch = FALSE;    PCB[3].Running = FALSE;   
  PCB[4].ReadyToLaunch = FALSE;    PCB[4].Running = FALSE;   
  PCB[5].ReadyToLaunch = FALSE;    PCB[5].Running = FALSE;   
  
  // Set all NextTime values with same current time to initialize task timing
  TempTime = 0;    
  PCB[0].NextTime = 0;
  PCB[1].NextTime = 0;
  PCB[2].NextTime = 0;
  PCB[3].NextTime = 0;
  PCB[4].NextTime = 0;
  PCB[5].NextTime = 0;
}
    
// --------------------

#ifdef PREEMPTIVE

// Helper functions to extract low and high byte of function addresses to save on return stack
uint8 AddrHi(Ptr2Function Ptr)
{ return ( ((uint16)(Ptr)>>8) & 0xFF); }

uint8 AddrLo(Ptr2Function Ptr)
{ return ( (uint8)(Ptr) & 0xFF ); }

// ----------

// Call when task terminates after running
//   The idea here is that when a finite-duration task completes, it does an RTS to somewhere
//    but that RTS isn't to the main loop, because it was never actually called from the main loop.
//    So we fake it by letting the RTS actually be a "goto" to TaskTerminate which cleans up the
//    task and just loops forever.  At the end of the current time slice TaskTerminate never
//    gets restarted since the Running flag for that task has been set to false.
//
void TaskTerminate(void)
{ PCB[CurrentTask].Running = FALSE;
  
  // Ask yourself -- what is the infinite loop below really doing for us?
  for(;;){}   // there is no return address for us, so loop until tasker takes CPU away from us
}

// ----------

// Create a task launch image onto the task's stack
//    Launch stack consists of:    TOP:  ->  return address to start TaskTerminate
//                                       ->  return address to restart suspended task
//                                       ->  Dummy values to restore registers if an RTI is executed
// This needs to be called every time you launch a stack to set up a clean stack image for that task
//    (Note that the pointer to TaskTerminate probably never gets altered, but the launch address
//     is overwritten as soon as the RTI to launch the task is executed the first time.)
//
void CreateLaunchStack(uint8 Task) 
{ static uint8 StackIdx; 
  
  // Set up fresh stack image for executing the newly launched task
  // Current stack already has RTI information for restarting old task; don't worry about it
  StackIdx = STACKSIZE;
  STACK[Task][--StackIdx] = AddrLo(&TaskTerminate);   // return to termination routine when completed
  STACK[Task][--StackIdx] = AddrHi(&TaskTerminate);  

  // -----------------------------------------------------------
  // TO DO:  set up the rest of values for this stack that will work if it is encountered by an RTI
  //         The result of an RTI should be resumption of the task at the current TaskPtr value
  //         You MUST use AddrLo() and AddrHi() functions appropriately
  

  // -----------------------------------------------------------
     
  // Tag task for launch by putting info into the task's PCB entry
  //
  PCB[Task].SPsave = &STACK[Task][StackIdx];  // points to newly created stack data
  PCB[Task].ReadyToLaunch = FALSE;            // we just launched it; don't relaunch right away
  PCB[Task].Running = TRUE;                   // setting this TRUE tells tasker it is fair game to run

  // That's it; the task will start running at its initial entry point as soon as the tasker sees it
}

#endif PREEMPTIVE


//*****************************
//   Scheduler
//*****************************
                                      
// Scheduler task
// Executing this task examines all the ReadyToRun tasks and launches those whose period has elapsed
//
void Task0(void) 
{
 uint8 ThisTask;     // loop variable to iterate across tasks

 uint32 TempTime;    // volatile gets around compiler warning for null scheduler version

 TempTime = TimeNow(); // stash copy of current time so all tasks see the same time for scheduling 
 
#ifdef UNTIMED_CYCLIC_EXEC
  // Runs all tasks in cyclic order without regard to period information
  //    Per-task period information is ignored
  // ReadyToLaunch (i.e., start task) set to true whenever LaunchRequest has been made by a switch
   
  // for all tasks, launch if requested to do so by switch or other source
  for (ThisTask = 0; ThisTask < NTASKS;  ThisTask++) 
  { 
    PCB[ThisTask].ReadyToLaunch = PCB[ThisTask].LaunchRequest;
    PCB[ThisTask].NextTime = TempTime;    // Run again as soon as possible  (not used by rest of code for this scheduling policy)
  }
#endif // UNTIMED_CYCLIC_EXEC


#ifdef CYCLIC_EXEC
  // Runs all tasks in cyclic order, then waits and relaunches cycle at end of whole-cycle period
  //    Per-task period information is ignored
  // ReadyToLaunch (i.e., start task) set to true whenever LaunchRequest has been made by a switch
   
  // for all tasks, launch if being requested by a switch or otherwise ready to run

  // -------------------------------------------------------------------
  // TO DO:  set ReadyToLaunch and NextTime for all tasks
   
  // -------------------------------------------------------------------

#endif // CYCLIC_EXEC


#ifdef MULTI_RATE_COOPERATIVE
   // Runs all tasks in cyclic order IF it has been long enough since the last time they were ReadyToLaunch

   // Run only tasks whose NextTime has arrived; use Period information to update NextTime
   // ReadyToLaunch is set to true when new LaunchRequest received AND when period length has been satisfied

   // for all tasks, launch task if time has come
   for (ThisTask = 0; ThisTask < NTASKS;  ThisTask++) 
   { 
     if(     PCB[ThisTask].LaunchRequest             // being requested by switch
         && !PCB[ThisTask].ReadyToLaunch             // not already waiting to launch (don't double-launch)
         &&  PCB[ThisTask].NextTime <= TempTime      // and it's been at least one more period
         // Don't bother checking for whether it is already running, since this is a non-preemptive option
       ) 
     { 
       PCB[ThisTask].ReadyToLaunch = TRUE;
       PCB[ThisTask].NextTime += PCB[ThisTask].Period;  // Launch exactly once for each period elapsed
     }
   }
#endif  // MULTI_RATE_COOPERATIVE


#ifdef PRIORITIZED_COOPERATIVE
  // Runs all tasks in prioritized cooperative order IF it has been long enough since the last time they were ReadyToLaunch

  // -------------------------------------------------------------------
  // TO DO:  set ReadyToLaunch and NextTime for all tasks
  //      be sure that tasks already ReadyToLaunch are not double-launched
   
  // -------------------------------------------------------------------

#endif // PRIORITIZED_COOPERATIVE


#ifdef PREEMPTIVE
   // Launch when LaunchRequest AND when period length has been satisfied
   // This approach differs in that main loop isn't doing the launching (calling) for us.
   // Instead, we launch by setting up a stack image and letting the tasker start running it
   //   (The tasker is oblivious as to whether it is a new launch or a resumption of execution)


   // We don't protect access to the PCB since this is the highest priority task that should be messing with it
   //    except for the tasker.  The tasker will ignore the PCB until the Running flag is set, so do that last   
   // For all tasks, launch task if time has come
   for (ThisTask = 1; ThisTask < NTASKS;  ThisTask++) 
   { 
     if(     PCB[ThisTask].LaunchRequest              // being requested by switch
         && !PCB[ThisTask].Running                    // not already running
         && !PCB[ThisTask].ReadyToLaunch              // not already waiting to launch (don't double-launch)
         &&  PCB[ThisTask].NextTime <= TempTime       // and it's been at least one more period
       ) 
     { 
       PCB[ThisTask].ReadyToLaunch = TRUE;  // In case we catch a task switch while this is running
       PCB[ThisTask].NextTime += PCB[ThisTask].Period;

       // Set up fresh stack image for executing the newly launched task
       // Current stack already has RTI information saved for restarting old task; don't worry about it
       CreateLaunchStack(ThisTask);   // sets Running flag

       // Next time this task is run, tasker will use this saved stack info to do an RTI and launch task
     }
   }
#endif

}

//********************************************
//   Check switches to see which tasks to run
//********************************************


// Query an individual switch, and set LaunchRequest accordingly
//
void QuerySwitch(uint8 Task, uint8 Switch) 
{ 
  if (GetSwitches() & Switch)
  { 
    if(PCB[Task].LaunchRequest == FALSE)   // This happens only if it is a new request
    { PCB[Task].LaunchRequest = TRUE;      // Assert a request for the task
      if (TimeNow() >= PCB[Task].NextTime + PCB[Task].Period)  // Debounce switch to avoid too-frequent triggering
      {  // Note: this means task initially runs one period from reboot at the very earliest
         PCB[Task].NextTime = TimeNow();    // Set start time to right now if at least one period since last time
      }
    }
  }
  else   // Switch is off
  { PCB[Task].LaunchRequest = FALSE; 
  }
}

// ----------

// Task to read switches and set requests for task launch based on those values
//
void Task1(void) 
{ 
  // For each task, make ready to run according to corresponding switch
  //   Note that each task always runs to completion (subject to preemption) once restarted
  //   But each task isn't re-launched unless switch stays ON
  QuerySwitch(2, SW3_1);
  QuerySwitch(3, SW3_2);
  QuerySwitch(4, SW3_3);
  QuerySwitch(5, SW3_4);
}
 
//********************************************
//   Workload tasks
//********************************************

//       DUMMY WORKLOAD TASKS (turn on LEDs for varying amounts of time)

// task 2 turns on LED #1 and stays busy for a while
//
void Task2(void) 
{ SetLED(LED1, ON);
  WasteMsec(TASKRUN2);    // pretend we're doing useful work for a while
  SetLED(LED1, OFF); 
}


// task 3 turns on LED #2 and stays busy for a while
//
void Task3(void) 
{ SetLED(LED2, ON);
  WasteMsec(TASKRUN3);    // pretend we're doing useful work for a while
  SetLED(LED2, OFF); 
}


// task 4 turns on LED #3 and stays busy for a while
//
void Task4(void) 
{ SetLED(LED3, ON);
  WasteMsec(TASKRUN4);    // pretend we're doing useful work for a while
  SetLED(LED3, OFF); 
}


// task 5 turns on LED #4 and stays busy for a while
//
void Task5(void) 
{ SetLED(LED4, ON);
  WasteMsec(TASKRUN5);    // pretend we're doing useful work for a while
  SetLED(LED4, OFF); 
}


//***************************************************************************
//        Main Function, which is the scheduler for non-preemptive tasks
//***************************************************************************

void main(void) {

// Perform setup tasks
  clockSetup();       // run module at 8 MHz
  InitSwitchesLED();  // set up Port B for CPU module switches and LEDs  (be sure to install jumpers!)
  SetupTimer();       // init time of day ISR and variables
  InitPCB();

  CurrentTask = NTASKS;   // current task is main loop
  EnableInterrupts;   // this starts the tasker

  //main task loop - this gets executed forever
  for(;;) {

    //****************************
    //  Begin the main loop task
    //****************************

#ifdef UNTIMED_CYCLIC_EXEC
   // Multi-rate cyclic executive
   // Does not wait for tasks -- runs them back to back as fast as it can
   //   Note: this means it ignores periods in PCB, which is what it is intended to do
   //         DO NOT attempt to make this code variant obey per-task periods!
   // After one cycle, we're done; main task loop will come back to do it again
   
   for (CurrentTask = 0; CurrentTask < NTASKS;  CurrentTask++) 
   { if(PCB[CurrentTask].ReadyToLaunch)    // scheduler tells us when to launch
     {
       PCB[CurrentTask].ReadyToLaunch = FALSE;  // don't re-launch until scheduler says to
       PCB[CurrentTask].Running = TRUE;
       PCB[CurrentTask].TaskPtr();  // call the task, and run it to completion
       PCB[CurrentTask].Running = FALSE;
     }
   }
   PCB[0].ReadyToLaunch = TRUE;  // always launch the scheduler next time around

#endif  // UNTIMED_CYCLIC_EXEC


#ifdef CYCLIC_EXEC
  // Cyclic executive framework for cooperative tasking
  //   Runs one copy of task every STATIC_PERIOD msec

  uint32 StartTime;  // Cyclic exec runs once every cyclic period, not on a per-task period basis
  // Record start time of loop so we know how long to wait until next main loop iteration
  StartTime = TimeNow();

   // -------------------------------------------------------------------
   // TO DO: Run all tasks round-robin
   //        When done with loop, wait until end of STATIC_PERIOD, then let main's outer loop start everything again

   
   // at this point we have executed all tasks and waiting until end of loop period

// -------------------------------------------------------------------

#endif  // CYCLIC_EXEC


#if defined(MULTI_RATE_COOPERATIVE)
   // Multi-rate cyclic executive framework for cooperative tasking
   //  Runs tasks at defined periods, but executes tasks round-robin if they are ready

   // for all tasks, execute if ready to launch   (scheduler sets ReadyToLaunch based on LaunchRequest)
   for (CurrentTask = 0; CurrentTask < NTASKS;  CurrentTask++) 
   { if(PCB[CurrentTask].ReadyToLaunch)    // scheduler tells us when to launch
     {
       PCB[CurrentTask].ReadyToLaunch = FALSE;  // don't re-launch until scheduler says to
       PCB[CurrentTask].Running = TRUE;
       PCB[CurrentTask].TaskPtr();  // call the task, and run it to completion
       PCB[CurrentTask].Running = FALSE;
     }
   }
   PCB[0].ReadyToLaunch = TRUE;  // always launch the scheduler next time around

#endif  // MULTI_RATE_COOPERATIVE


#ifdef PRIORITIZED_COOPERATIVE
   // Prioritized multirate cooperative tasking
   //   Run tasks at defined periods, but execute in prioritize order if they are ready

   // Run scheduler to update ReadyToLaunch flags for all other tasks
   PCB[0].TaskPtr();  // Need to reschedule every time, so just call it explicitly
                      //  If you skip this, there is a bug.  Who schedules the scheduler?

   //   Execute exactly ONE task using this loop; let main outer loop come back again later
   for (CurrentTask = 1; CurrentTask < NTASKS;  CurrentTask++) 
   {

  // -------------------------------------------------------------------
  // TO DO:  Find highest priority task that is ReadyToLaunch and execute it

     
    }
#endif  // PRIORITIZED_COOPERATIVE

#ifdef PREEMPTIVE
   // Prioritized preemptive tasker
   //   For this case we don't actually call the tasks
   //   Instead, we set up an RTI-compatible stack image and launch them for the tasker to start executing 
   //   The scheduler is just another task with this approach

   // Looking for ready tasks has to happen in the ISR service routine, which is now the task switcher   
#endif


    //**************************
    //  End the main loop task
    //**************************
 
  }
  /* please make sure that you never leave this function */
}


//*****************************************************
//     Timer Task & ISR;   includes Preemptive Tasker
//*****************************************************

uint32 TimeInMsec;   // 32 bit integer msec of current time since system boot
                     //    CAUTION -- this code doesn't handle rollover!
uint32 CurrentTime;  // 8.24 fixed point clock ticks; integer portion is in msec


// "WasteMsec()" waits specified number of milliseconds based on time-wasting loop
// loop must be hand-tuned for compiler settings and hardware (currently 8 MHz CPU clock)
// set to slightly faster rather than slower than 1 msec to ensure schedulability
// for example:  msec(50)  waits slightly less than 50 msec
//
void WasteMsec(uint16 WaitTime)
{ volatile uint16 i;   // i is volatile to force wasting time updating memory location
  while (WaitTime > 0)
  { WaitTime--;
    for (i = 0; i < 443; i++) 
    { asm NOP;
      asm NOP;
      asm NOP;
      asm NOP;
    }
  }
}

// ----------

// Return current time value
//
uint32 TimeNow(void) 
{ uint32 TempTime;

  // -------------------------------------------------------------------
  // TO DO:  Something we consider "obvious" is wrong with this procedure; fix it.
  //     Hint: where does TimeInMsec get set?  Is that a potential problem for this routine?
 
  
  // Get a clean copy of time and return it
  TempTime = TimeInMsec;



  // -------------------------------------------------------------------

  return(TempTime);  
}

// ----------

// Timer setup
// Call once at start of program to initialize counter/timer and time variables
//
void SetupTimer( void ) {

 // Initialize timer variables
 TimeInMsec = 0;
 CurrentTime = 0;

 // set TN = 1   Timer Enable   TSCR1 bit 7
 TSCR1 |= 0x80;
 
 // set  PR[2:0]  Timer prescale in bottom 3 bits of TSCR2
 TSCR2 = (TSCR2 & 0x78) | 0x80 | 0x00;   // enable TOF interrupt; set 0x07  bus clock / 1
 // Timer interrupt enabled at this point

}


// ----------
 
// Timer ISR
//
//Increment the timer counter and update current time in msec when it increments
//
#define TIMEINCR 0x083126E9   // 137438953  => just over 8 msec per TCNT rollover   8 MHz  divider of 1

void interrupt 16 TimerHandler(void) 
{ 
#ifdef PREEMPTIVE
   static uint8 * SPtemp;  // makes it easier to in-line assembly to save SP value
#ifndef SOLNP
   static uint8 i;  // temp var, but make it static to avoid stack complications
#endif // !SOLNP   
#endif // PREEMPTIVE

   TFLG2 = 0x80;  // Clear TOF; acknowledge interrupt
  
   CurrentTime += TIMEINCR;  // Increment time using 8.24 fixed point  (i.e., integer scaled to 2**-24 secs)
   TimeInMsec += (CurrentTime >> 24) & 0xFF;  // add integer portion to msec time count
   CurrentTime &= 0x00FFFFFF;                 // strip off integer portion, since that went into TimeInMsec

#ifdef PREEMPTIVE
   // This is the tasker in preemptive mode; switch to highest priority running task
   
   // Save current SP
   { asm    STS  SPtemp;  }
   PCB[CurrentTask].SPsave = SPtemp;

   // Set SP to main loop stack during scheduling to avoid crash when
   //         scheduling Task 0
   SPtemp = PCB[NTASKS].SPsave;
   { asm    LDS  SPtemp; }

   
   // Check to see if it is time to run the scheduler   (who schedules the scheduler?)
   //    we could check all the tasks this way, but we want to keep this ISR as fast as possible, so don't do that
   if(    !PCB[0].Running                   // it's not already running
       &&  PCB[0].NextTime <= TimeInMsec    // and it's been a period since last launch
 
 //  Calling TimeNow() results in a bug, because it reenables interrupts and crashes if scheduler period too fast
 //    It's safe to use TimeInMsec directly because interrupts are already masked
 //      &&  PCB[0].NextTime <= TimeNow() 
 
       ) 
     { 
       PCB[0].NextTime += PCB[0].Period;
       if(PCB[0].NextTime <= TimeInMsec)
       { PCB[0].NextTime = TimeInMsec + PCB[0].Period;  // Catch up if behind
       }

       // Create a launch image on the scheduler's stack; we're going to go to it at the next ISR below
       CreateLaunchStack(0);            // Note: double-launching is OK; if we missed a period no point running it twice

     } 



  // -------------------------------------------------------------------
  // LAB ONLY TO DO:  The below code compiled when SOLNP is undefined is round-robin
  //         tasking code.  It should work OK for your prelab if you fill in all the other missing
  //         code properly for this program file for the main lab assignment.
  //   Once you get everything working for the lab portion, define SOLNP and test your prioritized solution
  
#ifdef SOLNP
  // Write a prioritized solution that executes tasks in prioritized order rather than
  //   executing them in round robin order

#endif // SOLNP

#ifndef SOLNP
   for (i = NTASKS; i>0; i--)  // iterate enough times to check all tasks
   { CurrentTask++;   // advance to next task in round robin order
     if (CurrentTask >= NTASKS) { CurrentTask = 0; }   // Wrap around 
     if (PCB[CurrentTask].Running) { break; } // found next active task
   }  // end EITHER when we've found a running task or have checked all tasks
   
   if (!PCB[CurrentTask].Running) 
   { CurrentTask = NTASKS;  // if no tasks are running, default to main loop
   }  
   
#endif // SOLNP

  // -------------------------------------------------------------------

   // Have found the highest priority running task OR have defaulted to main loop task
   //    (this is the only place we use the main loop PCB entry, but it saves having special code for that case
   SPtemp = PCB[CurrentTask].SPsave;
   
   // Restore SP for this newly selected task (or same task that was just interrupted, depending on situation)
   { asm    LDS  SPtemp; }
   
   // RTI at end of this ISR starts up the newly selected task 
#endif //PREEMPTIVE

// The end of this routine is an RTI, because this is an ISR
}

// -----------