schedule.c

mega128平台下

#include <iom128v.h>
#include <macros.h>
//#include "../api/schedule.h"
typedef struct {
  void (*tp) ();
} TOSH_sched_entry_T;
typedef unsigned char uint8_t;
enum {

  TOSH_MAX_TASKS = 8,
  TOSH_TASK_BITMASK = (TOSH_MAX_TASKS - 1)
};

volatile TOSH_sched_entry_T TOSH_queue[TOSH_MAX_TASKS];
uint8_t TOSH_sched_full;
volatile uint8_t TOSH_sched_free;

uint8_t atomic_start(void)
{
  uint8_t result = SREG;
  CLI();
  return result;
}

void atomic_end(uint8_t oldSreg)
{
  SREG=oldSreg;
}


/* These are provided in HPL.td */
void TOSH_wait()
{
  ;//asm volatile("nop");
  //asm volatile("nop");
}

void TOSH_sleep()
{
    //sbi(MCUCR, 5);
  //sbi(MCUCR, SE);
  //asm volatile ("sleep");
}

void sched_init(void)
{
  int i;
  TOSH_sched_free = 0;
  TOSH_sched_full = 0;
  for (i = 0; i < TOSH_MAX_TASKS; i++)
    TOSH_queue[i].tp = 0x0;
}

/*
 * TOS_post (thread_pointer)
 *  
 * Put the task pointer into the next free slot.
 * Return 1 if successful, 0 if there is no free slot.
 *
 * This function uses a critical section to protect TOSH_sched_free.
 * As tasks can be posted in both interrupt and non-interrupt context,
 * this is necessary.
 */
void TOS_post(void (*tp) ()) {
  uint8_t fInterruptFlags;
  uint8_t tmp;
  
  fInterruptFlags = atomic_start();

  tmp = TOSH_sched_free;
  
  if (TOSH_queue[tmp].tp == 0x0) {
    TOSH_sched_free = (tmp + 1) & TOSH_TASK_BITMASK;
    TOSH_queue[tmp].tp = tp;
    atomic_end(fInterruptFlags);
    //return 1;
  }
  else {	
    atomic_end(fInterruptFlags);
    //return 0;
  }
}

/*
 * TOSH_schedule_task()
 *
 * Remove the task at the head of the queue and execute it, freeing
 * the queue entry. Return 1 if a task was executed, 0 if the queue
 * is empty.
 */

int TOSH_run_next_task ()
{
  uint8_t fInterruptFlags;
  uint8_t old_full;
  void (*func)(void);
  
  fInterruptFlags = atomic_start();
  old_full = TOSH_sched_full;
  func = TOSH_queue[old_full].tp;
  if (func == 0x0)
  {
      atomic_end(fInterruptFlags);
      return 0;
  }

  TOSH_queue[old_full].tp = 0x0;
  TOSH_sched_full = (old_full + 1) & TOSH_TASK_BITMASK;
  atomic_end(fInterruptFlags);
  func();

  return 1;
}

void run_task() {
  while (TOSH_run_next_task()) 
    ;
  TOSH_sleep();
  TOSH_wait();
}



void __nesc_enable_interrupt() {
  SEI();
}