// Chapter 5 9S12C32 C programs
// Jonathan W. Valvano, 2/07/07
// This software accompanies the book,
// Embedded Microcomputer Systems: Real Time Interfacing, Second Edition
// published by Thomson Engineering, 2006


// Program 5.4. C code for the two main programs and shared subroutine.
int Sub(int j){ int i;
  PTM = 1;  // PTM=program is being executed 
  i = j+1;
  return(i);
 }
void ProgA(){ int i;
  i=5;
  while(1) { 
    PTM = 2; 
    i = Sub(i);
  }
}
void ProgB(){ int i;
  i=6;
  while(1) { 
    PTM = 4; 
    i = Sub(i);
  }
}

// Program 5.5. C code for the thread control block.
struct TCB
{   struct TCB *Next;      /* Link to Next TCB */
    unsigned char *SP;     /* Stack Pointer when not running  */
    unsigned short Id;     /* output to PortT visualizing active thread */
    unsigned char MoreStack[49];  /* more stack */
    unsigned char CCR;     /* Initial CCR */
    unsigned char RegB;    /* Initial RegB */
    unsigned char RegA;    /* Initial RegA */
    unsigned short RegX;   /* Initial RegX */
    unsigned short RegY;   /* Initial RegY */
    void (*PC)(void);      /* Initial PC */
};
typedef struct TCB TCBType;
typedef TCBType * TCBPtr;
TCBType sys[3]={
  {  &sys[1],        /* Pointer to Next */
     &sys[0].CCR,    /* Initial SP */
     1,              /* Id */
     { 0},
     0x40,0,0,0,0,   /* CCR,B,A,X,Y */
     ProgA, },       /* Initial PC */
  {  &sys[2],        /* Pointer to Next */
     &sys[1].CCR,    /* Initial SP */
     2,              /* Id */
     { 0},
     0x40,0,0,0,0,   /* CCR,B,A,X,Y */
     ProgA, },       /* Initial PC */
  {  &sys[0],        /* Pointer to Next */
     &sys[2].CCR,    /* Initial SP */
     4,              /* Id */
     { 0},
     0x40,0,0,0,0,   /* CCR,B,A,X,Y */
     ProgB, } };     /* Initial PC */

// Program 5.6. C code for the thread switcher.
TCBPtr RunPt;   /* Pointer to current thread  */
void interrupt 13 ThreadSwitch(){
asm ldx RunPt
asm sts 2,x
  RunPt = RunPt->Next;
  PTT = RunPt->Id;  /* PortH=active thread */
asm   ldx RunPt
asm   lds 2,x
  TC5 = TCNT+20000;  /* Thread runs for 10 ms */
  TFLG1 = 0x20; }     /* ack by clearing C5F */
void main(void){ 
	DDRT = 0xFF;      /* PortT outputs specify the thread which is running */
	DDRM = 0xFF;      /* PortM outputs specify the program which is running */
  RunPt = &sys[0];  /* Specify first thread */
asm  sei
  TFLG1 = 0x20;   /* Clear C5F */
  TIE = 0x20;     /* Arm C5F */
  TSCR1 = 0x80;   /* Enable TCNT*/
  TSCR2 = 0x01;   /* 2MHz TCNT */
  TIOS |= 0x20;   /* Output compare */
  TC5 = TCNT+20000;
  PTT = RunPt->Id;
asm  ldx _RunPt
asm  lds 2,x
asm  cli
asm  rti
}    /* Launch First Thread */

// Program 5.7. C function to create a new thread.
void create(void (*program)(void), int TheId){
  TCBPtr NewPt;     // pointer to new thread control block
  NewPt = (TCBPtr)malloc(sizeof(TCBType)); // space for new TCB
  if(NewPt==0)return;
  NewPt->SP = &(NewPt->CCR);    /* 6812 Stack Pointer when not running  */
  NewPt->Id = TheId;            /* used to visualize active thread */
  NewPt->CCR = 0x40;            /* Initial CCR, I=0 */
  NewPt->RegB = 0;              /* Initial RegB */
  NewPt->RegA = 0;              /* Initial RegA */
  NewPt->RegX = 0;              /* Initial RegX */
  NewPt->RegY = 0;              /* Initial RegY */
  NewPt->PC=program;            /* Initial PC */
  if(RunPt){
    NewPt->Next = RunPt->Next;
    RunPt->Next = NewPt;}       /* will run Next */
  else
    RunPt = NewPt;              /* the first and only thread */

}



// ******** OS_Wait ************
// decrement and spin if less than 0
// input:  pointer to a semaphore
// output: none
void OS_Wait(short *semaPt){ 
  asm sei      // Test and set is atomic
  while(*semaPt <= 0){  // disabled 
    asm cli             // disabled 
    asm nop             // enabled  
    asm sei             // enabled
  }
  (*semaPt)--;          // disabled 
  asm cli               // disabled 
}                       // enabled 
// ******** OS_Signal ************
// increment semaphore 
// input:  pointer to a semaphore
// output: none
void OS_Signal(short *semaPt){ 
unsigned char SaveCCR;
  asm tpa     
  asm staa SaveCCR   // save previous 
  asm sei            // make atomic 
  (*semaPt)++;
  asm ldaa SaveCCR   // recall previous 
  asm tap            // end critical 
}
//Program 5.8. A spinlock counting semaphore.



// Program 5.10. 6812 C code for a spinlock binary semaphore.
void bWait(char *semaphore){
asm  clra          // new value for semaphore
asm loop: minm [2,x]    // test and set (ICC12 version 5)           
asm      bcc loop
}                            
void bSignal(char *semaphore){
  (*semaphore) = 1; // compiler makes this atomic
}



// Program 5.11. C code for a counting semaphore.
struct	sema4    // counting semaphore based on 3 binary semaphores
{   short value; // semaphore value
    char s1;     // binary semaphore
    char s2;     // binary semaphore
    char s3;     // binary semaphore
};
typedef struct sema4 sema4Type;
typedef sema4Type * sema4Ptr;
void Wait(sema4Ptr semaphore){
  bWait(&semaphore->s3);  // wait if other caller to Wait gets here first
  bWait(&semaphore->s1);  // mutual exclusive access to value
  (semaphore->value)--;   // basic function of Wait
  if((semaphore->value)<0){
    bSignal(&semaphore->s1); // end of mutual exclusive access to value
    bWait(&semaphore->s2);   // wait for value to go above 0
  }
  else
    bSignal(&semaphore->s1); // end of mutual exclusive access to value
  bWait(&semaphore->s3);     // let other callers to Wait get in
}                            
void Signal(sema4Ptr semaphore){
  bWait(&semaphore->s1);  // mutual exclusive access to value
  (semaphore->value)++;  // basic function of Signal
  if((semaphore->value)<=0)
    bSignal(&semaphore->s2);   // allow S2 spinner to continue
  bSignal(&semaphore->s1); // end of mutual exclusive access to value
}
void Initialize(sema4Ptr semaphore, short initial){
  semaphore->s1 = 1;   // first one to bWait(s1) continues
  semaphore->s2 = 0;   // first one to bWait(s2) spins
  semaphore->s3 = 1;   // first one to bWait(s3) continues
  semaphore->value=initial;
}






//******************FSM**************************
void FSM(void){ StatePtr Pt;   
  Pt = SA;                 // Initial State
  DDRT = 0x03;             // PT1,PT0 outputs, PT3,PT2 inputs
  PTT = Pt->Out;           // Output depends on the current state
  for(;;) {
    OS_Sleep();            // Runs every 2ms
    Pt = Pt->Next[PTT>>2]; // Next state depends on the input 
    PTT = Pt->Out;         // Output depends on the current state
  }
}
//******************PID**************************
void PID(void){ unsigned char speed,power;   
  PID_Init();              // Initialize
  for(;;) {
    OS_Sleep();            // Runs every 1ms
    speed = PID_In();      // read tachometer 
    power = PID_Calc(speed);
    PID_Out(power);        // adjust power to motor 
  }
}
//******************DAS**************************
void DAS(void){ unsigned char raw;   
  DAS_Init();            // Initialize
  for(;;) {
    OS_Sleep();          // Runs every 1.5ms
    raw = DAS_In();      // read ADC 
    Result = DAS_Calc(raw); 
  }
}
//******************PAN**************************
void PAN(void){ unsigned char input;   
  PAN_Init();            // Initialize
  for(;;) {
    input = PAN_In();    // front panel input
    if(input){
      PAN_Out(input);    // process
    }
  }
}
//Program 5.14. Four user threads.




struct TCB{
  unsigned char *StackPt;      // Stack Pointer
  unsigned char MoreStack[91]; // 100 bytes of stack 
  unsigned char InitialReg[7]; // initial CCR,B,A,X,Y 
  void (*InitialPC)(void);     // starting location
};
typedef struct TCB TCBType;
TCBType *RunPt;            // thread currently running 
#define TheFSM &sys[0]     // finite state machine
#define ThePID &sys[1]     // proportional-integral-derivative
#define TheDAS &sys[2]     // data acquisition system
#define ThePAN &sys[3]     // front panel
TCBType sys[4]={
  { TheFSM.InitialReg[0],{ 0},{0x40,0,0,0,0,0,0},FSM},
  { ThePID.InitialReg[0],{ 0},{0x40,0,0,0,0,0,0},PID},
  { TheDAS.InitialReg[0],{ 0},{0x40,0,0,0,0,0,0},DAS},
  { ThePAN.InitialReg[0],{ 0},{0x40,0,0,0,0,0,0},PAN}
};
//Program 5.15. The thread control blocks.


struct	Node{
  struct Node *Next;        // circular linked list
  TCBType *ThreadPt;        // which thread to run
  unsigned short TimeSlice; // how long to run it
};
typedef struct Node NodeType;
NodeType *NodePt;
NodeType Schedule[22]={ 
{ &Schedule[1], ThePID, 300}, // interval     0,  300
{ &Schedule[2], TheFSM, 100}, // interval   300,  400
{ &Schedule[3], TheDAS,  50}, // interval   400,  450
{ &Schedule[4], ThePAN, 550}, // interval   450, 1000
{ &Schedule[5], ThePID, 300}, // interval  1000, 1300
{ &Schedule[6], ThePAN, 600}, // interval  1300, 1900
{ &Schedule[7], TheDAS,  50}, // interval  1900, 1950
{ &Schedule[8], ThePAN,  50}, // interval  1950, 2000
{ &Schedule[9], ThePID, 300}, // interval  2000, 2300
{ &Schedule[10],TheFSM, 100}, // interval  2300, 2400
{ &Schedule[11],ThePAN, 600}, // interval  2400, 3000
{ &Schedule[12],ThePID, 300}, // interval  3000, 3300
{ &Schedule[13],ThePAN, 100}, // interval  3300, 3400
{ &Schedule[14],TheDAS,  50}, // interval  3400, 3450
{ &Schedule[15],ThePAN, 550}, // interval  3450, 4000
{ &Schedule[16],ThePID, 300}, // interval  4000, 4300
{ &Schedule[17],TheFSM, 100}, // interval  4300, 4400
{ &Schedule[18],ThePAN, 500}, // interval  4400, 4900
{ &Schedule[19],TheDAS,  50}, // interval  4900, 4950
{ &Schedule[20],ThePAN,  50}, // interval  4950, 5000
{ &Schedule[21],ThePID, 300}, // interval  5000, 5300
{ &Schedule[0], ThePAN, 700}  // interval  5300, 6000
};
//Program 5.16. The scheduler defines both the thread and the duration.


void OS_Sleep(void){  // cooperative multitasking 
  asm swi             // suspend this tread and run another
}
interrupt 4 void swiISR(void){
asm ldx RunPt	     // cooperative multitasking
asm sts 0,x       // thread goes to sleep when it is done
  RunPt = ThePAN; // non-real time thread
asm ldx RunPt
asm lds 0,x
}
interrupt 11 void threadSwitchISR(void){
asm ldx RunPt
asm sts 0,x
  NodePt = NodePt->Next;
  RunPt = NodePt->ThreadPt;    // which thread to run
  TC3 = TC3+NodePt->TimeSlice; // Thread runs for a unit of time 
  TFLG1 = 0x08;                // acknowledge by clearing TC3F 
asm ldx RunPt
asm lds 0,x
}
void main(void) {
  NodePt = &Schedule[0];     // first thread to run 
  RunPt = NodePt->ThreadPt; 
  TIOS |= 0x08;    // activate OC3
  TSCR1 = 0x80;    // enable TCNT
  TSCR2 = 0x02;    // usec TCNT 
  TIE |= 0x08;     // Arm TC3 
  TC3 = TCNT+NodePt->TimeSlice; // Thread runs for a unit of time 
  TFLG1 = 0x08;    // Clear C3F 
asm ldx RunPt
asm lds 0,x
asm rti     // Launch First Thread 
}
//Program 5.17. The scheduler defines both the thread and the duration.