/* Round Robin Scheduler 
   Last updated 29.10.00
 */

#include "H8.h"
#include "RCX_Interrupts.h"
#include "Timers.h"

/* 
 * Process descriptors and global variables.
 *
 * Each process is identified by an integer index. This is used as
 * an index to access the process descriptor for each process.
 * Main program has index 0. 
 *
 */

#define MaxProcesses 5

struct  ProcessDescriptor_t           { word SavedSP; };
typedef struct ProcessDescriptor_t    ProcessDescriptor;


static ProcessDescriptor ProcessTable [ MaxProcesses ];

static int Current     = 0;  /* Index of current process */
static int LastProcess = 0;  /* Index of last entry used in process table */

/* Common stack area for all processes */

#define StackSize 150
#define StackAreaSize (MaxProcesses * StackSize)

static word ProcessStack [ StackAreaSize ];


/* Scheduler interface */

int16 StartProcess ( void (* p)(void) )
{  
  int16 result, StackPointerIndex;

  Disable();
  if ( LastProcess == (MaxProcesses - 1) )
     result = -1;
  else {
     LastProcess = LastProcess + 1;
     result = LastProcess;
    
     StackPointerIndex = (LastProcess+1)*StackSize;
     /* Initial stack depends on the generated code for T0Handler */
     ProcessStack[ --StackPointerIndex] = (word) p; /* pc */
     ProcessStack[ --StackPointerIndex] = 0;        /* cc */
     ProcessStack[ --StackPointerIndex] = 0;        /* r6 */
     ProcessStack[ --StackPointerIndex] = T0_DispatcherReturnAddress;
     ProcessStack[ --StackPointerIndex] = 0;        /* r6 */

     ProcessStack[ --StackPointerIndex] = 0;        /* r0 */
     ProcessStack[ --StackPointerIndex] = 0;        /* r1 */
     ProcessStack[ --StackPointerIndex] = 0;        /* r2 */
     ProcessStack[ --StackPointerIndex] = 0;        /* r3 */
     ProcessStack[ --StackPointerIndex] = 0;        /* r4 */
     ProcessStack[ --StackPointerIndex] = 0;        /* r5 */

     ProcessTable[ LastProcess ].SavedSP= (word) &(ProcessStack[StackPointerIndex]);
  }
  Enable();
  return result;
}


#define semaphore byte

/* Semaphore operations */
void Signal ( semaphore  * s )
{  
   Disable();
   *s = TRUE;
   Enable();
}

void Wait ( semaphore * s )
{  
   Disable();
   while ( ! (*s) ){
     Enable();

     Disable();
   }
   *s = FALSE;
   Enable();
}



/* The timer T0 is used to interrupt periodically. 
 * On each interrupt the T0Handler selects the
 * next process to be scheduled.
 *
 * The time slice is 1 msec.
 */

static word MSCount = 0; /* Keeps track of time */

static word SavedSP;

void T0Handler ( void ) 
{
  /* Stack after C prolog:
        C prolog         r6 address of T0Handler
        RCX dispatcher   return to T0 dispatcher
                         r6 and 
        H8 hardware      cc and
                         pc of interrupted process
     This is used in StartProcess of the Scheduler. Change initial
     stack if stack content changes because of register saving
     in the c prolog.
  */
  /* Save remaining registers of interrupted process */
  asm("
        push r0
        push r1
        push r2
        push r3 
        push r4
        push r5
      "
     );

  /* Save stack pointer in C variable */
  asm("
        mov.w sp,%0
      "
       : "=r" (SavedSP)
      );
  
  ProcessTable[ Current ].SavedSP = SavedSP;

  /* Round robin scheduling */
  Current = Current + 1;
  if ( Current > LastProcess ) Current = 0;

  SavedSP = ProcessTable[ Current ].SavedSP;

  MSCount = MSCount + 1;

  T0_TCSR &= ~bit6; /*clear CMFA flag */

  /* Restore stack pointer from C variable */
  asm("
        mov.w %0,sp
      "
       : 
       : "r" (SavedSP)
     ); 
     
  /* Restore registers and start scheduled process */
  asm("
        pop r5
        pop r4
        pop r3
        pop r2
        pop r1
        pop r0
      "
     );
}	

void  SchedulerInit ( void )
{
  T0_TCR    &= ~bit6;  /* Disable T0 interrupts */ 

  /* Install T0 interrupt handler */
  T0_Interrupt_Handler = T0Handler;

  /* Set T0 to interrupt every msec. An internal clock is
     used to increment the time counter TCNT each 2 usec. 
     Time counter A is set to 250 to request an interrupt on
     compare-match A, i.e. an interrupt period of 1 msec.
   */
  T0_TCSR &= 0;    /* Clear all flags and select no output */
  /* Internal clock 2 usec, T0_TCNT = 0 on compare-match A */
  T0_TCR  |= ( bit3 | bit1 ); STCR = 0;  
  T0_TCORA = 250;  
  T0_TCNT  =   0; 

  T0_TCR    |= bit6;   /* Enable T0 interrupts */

}	

void SchedulerClose( void )
{
  T0_TCR    &= ~bit6;  /* Disable T0 interrupts */ 
}