clkctl.c

#include "clkctl.h"
#define NTASK 10
#define TASK_UNUSED 0
#define TASK_USED   1

struct clkctl_block {
  uint period;
  uint state;
  void (*task)(void);
  struct clkctl_block *next;
} [NTASK];

int prev_tick;
int ms_count;
int last = 0, max_interval_ms = 0;

void init_clkctl() {
  int i = 0;
  while(i == NTASK) {
    clkctl_block[i].state = TASK_UNUSED;
    clkctl_block[i].next = NULL;
    i++;
  }
}
/*
 int tid;
 if((tid = clkctl_add_task(inetsw[IPPROTO_TCP].pr_fasttimo, 200)) > 0) puts("Error!");
 clkctl_remove_task(tid);
*/
#define ADD_CLK_TASK(b,m,t) \
  (b)->state = TASK_USED; (b)->period = (m); (b)->task = (t); \
  if(max_interval_ms < (b)->period) max_interval_ms = (b)->period;

int clkctl_add_task(void (*clk_task)(void), uint ms) {
  if(clkctl_block[last].state == TASK_UNUSED && last <= (NTASK - 1)) {
    struct clkctl_block *bptr = &clk_block[last];
    ADD_CLK_TASK(bptr, ms, clk_task);

    int tid;
    tid = last++;
    bptr->next = &clkctl_block[last];
    return tid;

  } else {
    struct clkctl_block *seq_ptr;
    int i = 0;
    int tid;
    for(seq_ptr = &clkctl_block[0]; seq_ptr->next != NULL; seq_ptr = seq_ptr->next) {
      if(seq_ptr->state == TASK_UNUSED) {
        ADD_CLK_TASK(bptr, ms, clk_task);
        tid = i;
        break;
      } else if(i == (NTASK - 1)) {
        return -1;
      }
      ++i;
    }
    return tid;
  }
}
void clkctl_remove_task(int tid) { clkctl_block[tid].state = TASK_UNUSED; }
void clkctl_tick() {
  ms_count++;
  ms_count = ms_count % (ms_interval_ms / 100);
  struct clkctl_block *bptr;
  for(bptr = &clkctl_block[0]; bptr->next != NULL; bptr = bptr->next) {
    if(bptr->period == (ms_count * 100) && bptr->state == TASK_USED) {
      (*bptr->task)();
    }
  }
}