/* $Revision: 1.2 $ */

#include <locore.h>
#include <common.h>
#include <cpu.h>
#include <sem.h>
#include <kern.h>
#include <errno.h>
#include <stdio.h>
#include <string.h>
#include <debug.h>

extern int first_bit();

extern int timer_secs ;
extern int need_resched, interrupt_nest ;
extern int timer_ticks ;

int errno = 0;
tcb_t fake_tcb;
tcb_t *fake_task = &fake_tcb;
tcb_t *cur_task;
tcb_t *priority_list[MAX_PRIORITY_GROUPS][MAX_PRIORITIES_PER_GROUP];
u_int group_ready_mask;
u_int pri_ready_masks[MAX_PRIORITY_GROUPS];
char idle_stack[1024];
int cpu_usage;


#warning assert needed
#define assert(s) ((void)(0))

#define MAXTASKS 64



tcb_t task_pool [MAXTASKS] ;
tcb_t * free_list ;
tcb_t * task_list ;

int t_list_in_list( tcb_t * new ){
  tcb_t * tmp ;
  tmp = task_list ;
  while( tmp ){
    if( new == tmp ) 
      return 1 ;
    tmp = tmp->tcb_next ;
  }
  return 0 ;
}

void check_lists(){
  tcb_t * tmp ;
  for(tmp = free_list ; tmp && tmp->tcb_next != NULL ; tmp=tmp->tcb_next ){
    assert( tmp->tcb_next->tcb_previous == tmp ) ;
  }
  for(tmp = task_list ; tmp && tmp->tcb_next != NULL ; tmp=tmp->tcb_next ){
    assert( tmp->tcb_next->tcb_previous == tmp ) ;
  }
}  
     

void init_task_list(){
  /* Prec: None
   * Post: Initialises task list.
   *       All tcb in free_list
   *       task_list = NULL
   */
  int i ;
  tcb_t * tmp ;

  for( i = 0 ; i < MAXTASKS ; i++ ){
    bzero( &task_pool[i] , sizeof(tcb_t)) ;
  }

  task_list = NULL ;
  free_list = &task_pool[0] ;
  tmp = free_list ;
  tmp->tcb_previous = NULL ;
  tmp->tcb_next=&task_pool[1] ;
  tmp=tmp->tcb_next ;
  for( i = 1 ; i < ((MAXTASKS) - 1) ; i ++ ){
    tmp->tcb_next=&task_pool[i+1] ;
    tmp->tcb_previous=&task_pool[i-1] ;
    tmp = tmp->tcb_next ;
  }
  assert( tmp == &task_pool[(MAXTASKS)-1] ) ;
  tmp->tcb_previous = &task_pool[(MAXTASKS)-2] ;
  tmp->tcb_next = NULL ;
}

tcb_t * t_list_get_free_tcb(){
  /* Prec : None
   * Returns: valid tcb_t * if space for another tcb
   *          NULL if not
   */
  tcb_t * rwert ;
  rwert = NULL  ;
  if( free_list != NULL ){
    rwert = free_list ;
    free_list = free_list->tcb_next ;
    if( free_list != NULL ){
      free_list->tcb_previous = NULL ;
    }
    rwert->tcb_next = NULL ;
    rwert->tcb_previous = NULL ;
  }else{
    errno = ENOMEM ;
    rwert = NULL ;
  }
#ifndef NODEBUG
  check_lists() ;
#endif

  return rwert ;
}

int t_list_free_tcb( tcb_t * superflous ){
  /* Prec : superflous is in task_list 
   * Post : superflous returned to task_free_list
   */
  tcb_t * prev ;
  tcb_t * next ;
  tcb_t * tmp ;

  assert( superflous != NULL ) ;
  assert( t_list_in_list( superflous ) ) ;  
  /* Take it from Task_list */
  prev = superflous->tcb_previous ;
  next = superflous->tcb_next ;
  if( next != NULL ){
    next->tcb_previous = prev ;
  }
  if( prev != NULL ){
    prev->tcb_next = next ;
  }else{
    /* List Node! */
    task_list = next ;
  }
    

  /* To Free List */
  tmp = free_list ;
  free_list = superflous ;
  if( tmp != NULL ){
    superflous->tcb_next = tmp ;
    tmp->tcb_previous = superflous ;
  }else{
    superflous->tcb_next = NULL ;
  }
  superflous->tcb_previous = NULL ;
#ifndef NODEBUG
  check_lists() ;
  assert( ! t_list_in_list( superflous ) ) ;  
#endif
  return 0 ;
}

int t_list_insert( tcb_t * new ){
  /* Prec: Not in list && new != NULL
   * Post: front ( task_list = *new )
   */
  tcb_t * tmp; 

  assert ( new ) ;
  assert( ! t_list_in_list( new ) ) ;
  tmp = task_list ;
  task_list = new ;
  new->tcb_next = tmp ;
  new->tcb_previous =  NULL  ;
  if( tmp != NULL ){
    tmp -> tcb_previous = task_list ;
  }
#ifndef NODEBUG
  check_lists() ;
#endif

  return 0 ;
}

void t_list_dump(){
  tcb_t * tmp ;
  int i = 0 ;

  for( tmp = task_list ; tmp != NULL; tmp = tmp->tcb_next,i++ ){
    printf( "Task Nr: %2d , Name: %6s , adr:%p\n" , i , tmp->tcb_name , tmp ) ;
  }
}

void free_list_dump(){
  tcb_t * tmp ;
  int i = 0 ;
  
  for( tmp = free_list ; tmp != NULL ; tmp = tmp->tcb_next,i++ ){
    printf( "freeTask Nr: %2d , Name: %6s , adr:%p \n" , i , tmp->tcb_name , tmp  ) ;
  }
}


/* int main(){ */
/*   int i ; */
/*   tcb_t * local ; */
/*   char * name ; */
/*   init_task_list() ; */

/*   for( i = 0 ; i < MAXTASKS ; i ++ ){ */
/*     local = t_list_get_free_tcb() ; */
/*     if( local != NULL ){ */
/*       name = (char *)malloc( 10 ) ; */
/*       strcpy( name , "task" ) ; */
/*       sprintf ( &name[4] , "%d" , i ) ; */
/*       local->tcb_name = name ; */
/*       t_list_insert( local ) ; */
/*     }else{ */
/*       perror( "t_list_get_free_tcb()\n" ) ; */
/*     } */
/*   } */
/*   printf("FreeTaskAfterFill\n"); */
/*   free_list_dump() ; */
/*   printf("NonFreeTaskAfterFill\n"); */
/*   t_list_dump() ; */
/*   for( i = 0 ; i < MAXTASKS/2 ; i++ ){ */
/*     printf("deleting task_pool[%d]\n",i); */
/*     t_list_free_tcb( &task_pool[i] ) ; */
/*   } */
/*   printf("FreeTaskAfterHalfDel\n"); */
/*   free_list_dump() ; */
/*   printf("NonFreeTaskAfterHalfDel\n"); */
/*   t_list_dump() ; */
/*   for( i = MAXTASKS/2+1 ; i < MAXTASKS; i++ ){ */
/*     printf("deleting task_pool[%d]\n",i); */
/*     t_list_free_tcb( &task_pool[i] ) ; */
/*   } */
/*   printf("FreeTaskAfterFullDel\n"); */
/*   free_list_dump() ; */
/*   printf("NonFreeTaskAfterFullDel\n"); */
/*   t_list_dump() ; */

/*   t_list_free_tcb( &task_pool[32] ) ; */
/*   printf("NonFreeTaskAfterFullDel\n"); */
/*   t_list_dump() ; */

/* } */



int idle_task(void){
  u_int secs;
  u_int iters;
  u_int max_iters;
  
  secs = timer_secs;
  max_iters = 0;
  iters = 0;
  for (;;) {
    if (timer_secs != secs) {
      if (iters > max_iters)
	max_iters = iters;
      cpu_usage = ((max_iters - iters) * 100) / max_iters;
      debug("%d secs since boot, cpu usage %d%%\n",timer_secs, cpu_usage);
      secs = timer_secs;
      iters = 0;
    } else
      iters++;
  }
}

void kern_init(void){
  int i, j;

  tcb_t * idle_tcb ; 
  for (i = 0; i < MAX_PRIORITY_GROUPS; i++) {
    for (j = 0; j < MAX_PRIORITIES_PER_GROUP; j++)
      priority_list[i][j] = 0;
    pri_ready_masks[i] = 0;
  }
  group_ready_mask = 0;
  cur_task = &fake_tcb;
  fake_tcb.tcb_name = "fake";

  debug("kern_init: spawning idle\n");
  if ( ( idle_tcb = spawn( "idle", idle_stack, idle_task,
	    sizeof(idle_stack), MIN_PRIORITY, 0)) == NULL )
    panic("kern_init: can't spawn idle task\n");
  ready(idle_tcb);
}

void task_main(void){
  if (!cur_task)
    panic("task_main: cur_task is nil!");
  debug("task_main: cur_task %h<%s> main(pc) %h\n",
	cur_task, cur_task->tcb_name, cur_task->tcb_main);
  (*cur_task->tcb_main) ();
  debug("%s @ %h going to exit\n", cur_task->tcb_name, cur_task) ;
  exit(0);
}

void restart(tcb_t * tcb) {
  context_t *tp;

  tp = &tcb->tcb_context;
  ctx_fill(tp, (int) tcb->restart, IPL_ENABLE, 0, 0, tcb->tcb_stkbeg);
  tcb->tcb_stat = TASK_INACTIVE;
  (*tcb->restart) ();
}

tcb_t * spawn(char *name, char *stack, int (*code) (),int stksiz, int priority, int arg){
  context_t *tp;
  tcb_t * new_tcb ;
  int s;

  if (!stack) {
    debug("spawn: stack %h invalid\n", stack);
    return NULL;
  }
  if (priority > MAX_PRIORITY || priority < MIN_PRIORITY) {
    debug("spawn: priority %h invalid\n", priority);
    return NULL;
  }
  new_tcb = t_list_get_free_tcb() ;
  if (!new_tcb) {
    debug("spawn: new_tcb %h invalid\n", new_tcb);
    return NULL;
  }
  t_list_insert( new_tcb ) ;

  s = splhi();
  new_tcb->tcb_group_index = PRI_TO_GROUP_INDEX(priority);
  new_tcb->tcb_pri_index = PRI_TO_PRI_INDEX(priority);
  if (priority_list[new_tcb->tcb_group_index][new_tcb->tcb_pri_index]) {
    debug("spawn: priority slot already occupied by %h\n",
	  priority_list[new_tcb->tcb_group_index][new_tcb->tcb_pri_index]);

    splx(s);
    t_list_free_tcb( new_tcb ) ;
    return NULL;
  }
  debug("spawn: new_tcb %h<%s> stk %h code %h stksz %h pri %h\n",
	new_tcb, name, stack, code, stksiz, priority);
  new_tcb->tcb_main = code;
  new_tcb->restart = code;
  tp = &new_tcb->tcb_context;
  new_tcb->tcb_stkbeg = (int) (stack + stksiz);
  ctx_fill(tp, (int) task_main, IPL_ENABLE, 0, 0, new_tcb->tcb_stkbeg);
  new_tcb->tcb_stkend = new_tcb->tcb_stkbeg;
  new_tcb->tcb_stksiz = stksiz;
  new_tcb->tcb_stat = TASK_INACTIVE;
  new_tcb->tcb_name = name;
  new_tcb->tcb_stdio_buf = 0;
  new_tcb->tcb_wait = 0 ;
  new_tcb->tcb_arg = arg;
  priority_list[new_tcb->tcb_group_index][new_tcb->tcb_pri_index] =
    new_tcb;

  splx(s);
  return new_tcb ;
}

int ready(tcb_t * task) {
  int s;

  if (!task) {
    if (cur_task == 0)
      panic("ready: cur_task == 0, task == 0");
    task = cur_task;
  }
  if (task->tcb_stat & TASK_READY) {
    debug("ready: %h<%s> already ready\n", task, task->tcb_name);

    return -1;
  }
  s = splhi();
  task->tcb_stat &= ~TASK_STATE_MASK;
  task->tcb_stat |= TASK_READY;
  group_ready_mask |= (1 << task->tcb_group_index);
  pri_ready_masks[task->tcb_group_index] |= (1 << task->tcb_pri_index);
  debug("ready: %h marked ready, group_ready_mask %h\n",
	task, group_ready_mask);
  splx(s);
  return 0;
}

int suspend(tcb_t * task){
  /* Prec: Task is really a valid tcb 
   * Post: Task gets Status SUSPENDED 
   * returns: 0 if succeeded
   *          negative if not 
   */

  int s = 0;

  if (!task) {
    if (cur_task == 0)
      panic("suspend: cur_task == 0, task == 0");
    task = cur_task;
  }
  if (task->tcb_stat & TASK_SUSPENDED) {
    debug("suspend: %h<%s> already suspended\n", task, task->tcb_name);
    /* splx is wrong here because there is no corresponding splhi() */
    /* splx(s); */
    return -1;
  }
  s = splhi();
  task->tcb_stat &= ~TASK_STATE_MASK;
  task->tcb_stat |= TASK_SUSPENDED;

  /* Take Task from Ready Queue */
  pri_ready_masks[task->tcb_group_index] &= ~(1 << task->tcb_pri_index);

  /* If there are no Task in this Priority Group then take the whole 
   * Group from ready Queue */
  if (pri_ready_masks[task->tcb_group_index] == 0){
    group_ready_mask &= ~(1 << task->tcb_group_index);
  }
  debug("suspend: %h<%s> suspended\n", task, task->tcb_name);
  splx(s);
  return 0;
}

tcb_t * pick(sem_t * sem){
  int group_index;
  int pri_index;
  tcb_t *retval;

  if (sem) {
    group_index = first_bit(sem->sem_gindex);
    pri_index = first_bit(sem->sem_pindex[group_index]);
    debug("pick: sem %h gmask %h gidx %h pmask=%h pidx %h\n",
	  sem, group_ready_mask, group_index,
	  pri_ready_masks[group_index], pri_index);

    retval = priority_list[group_index][pri_index];
  } else {
    group_index = first_bit(group_ready_mask);
    pri_index = first_bit(pri_ready_masks[group_index]);
    debug("pick: gmask %h gidx %h pmask=%h pidx %h\n",
	  group_ready_mask, group_index,
	  pri_ready_masks[group_index], pri_index);

    retval = priority_list[group_index][pri_index];
  }
  debug("pick: retval %h<%s> == new\n",
	retval, retval->tcb_name);
  return (retval);
}

void reschedule(context_t * trap_context){
  int s;
  tcb_t *new_task, *old_task;

  new_task = pick(0);
  if ( ! new_task ){
    panic( "reschedule pick returned 0\n" ) ;
  }
  if (new_task == cur_task) {
    debug("reschedule: cur %h<%s> == new\n",
	  cur_task, cur_task->tcb_name);
  }else{
    debug("reschedule: cur %h<%s> , new %h<%s>\n",
	  cur_task, cur_task->tcb_name,
	  new_task, new_task->tcb_name);
  }
  s = splhi();
  old_task = cur_task;
  cur_task = new_task;
  if (trap_context && interrupt_nest > 0) {
    old_task->tcb_context = *trap_context;
    old_task->tcb_stat &= ~TASK_RUNNING;
    new_task->tcb_stat |= TASK_RUNNING;
    interrupt_nest--;
    context_intr_swap(&new_task->tcb_context);
  } else {
    old_task->tcb_stat &= ~TASK_RUNNING;
    new_task->tcb_stat |= TASK_RUNNING;
    context_swap(&old_task->tcb_context, &new_task->tcb_context);
  }
  splx(s);
}

void yield(void) {
  debug("yield: cur_task %h<%s>\n", cur_task, cur_task->tcb_name);
  
  if (cur_task == 0)
    panic("yield: cur_task==0");
  suspend(cur_task);
  reschedule(0);
}

void exec(tcb_t * task){
  debug("exec: task %h<%s>\n", task, task->tcb_name);
  ready(task);
  reschedule(0);
}

void finish(tcb_t * task){
  int s;
  
  if (!task) {
    if (cur_task == 0)
      panic("finish: cur_task == 0, task == 0\n");
    task = cur_task;
  }
  debug("finish: task %h<%s>, being suspended and deleted\n",
	task, task->tcb_name);
  t_list_free_tcb( task ) ;
  s = splhi();
  cur_task = &fake_tcb;
  suspend(task);
  priority_list[task->tcb_group_index][task->tcb_pri_index] = 0;
  splx(s);
  reschedule(0);
}

void
exit(int code)
{
  /* XXX do something with 'code' */
  finish(0);
}

void ps_print(tcb_t * tcb){
  context_t *ctx;
  
  ctx = &tcb->tcb_context;
  
  kprintf("%10s %8.0h %8.0h %8.0h %4d %4d %4d ",
	  tcb->tcb_name, tcb, tcb->tcb_stkbeg, ctx->ctx_reg.sp,
	  tcb->tcb_stksiz, tcb->tcb_group_index, tcb->tcb_pri_index);
  if (tcb->tcb_stat == 0) {
    kprintf("INACTIVE\n");
    return;
  }
  if (tcb->tcb_stat & TASK_RUNNING)
    kprintf("RUNNING ");
  if (tcb->tcb_stat & TASK_SUSPENDED)
    kprintf("SUSPEND ");
  if (tcb->tcb_stat & TASK_READY)
    kprintf("READY ");
  kprintf("\n");
}

u_long ps(void){
  int i, j;
  
  kprintf("%10s %10s %10s %10s %4s %4s %4s %8s\n",
	  "NAME", "TCB", "STKTOP", "SP",
	  "SSIZ", "GPRI", "LPRI", "STATUS");
  for (i = 0; i < MAX_PRIORITY_GROUPS; i++) {
    for (j = 0; j < MAX_PRIORITIES_PER_GROUP; j++) {
      if (priority_list[i][j] == 0)
	continue;
      ps_print(priority_list[i][j]);
    }
  }
  return 1UL ;
}

void sleep(int ticks){
  int s = splhi() ;
  cur_task->tcb_wait = timer_ticks + ticks ;
  debug("suspending %s from %d to %d \n" , cur_task->tcb_name , timer_ticks , cur_task->tcb_wait) ;
  suspend( cur_task ) ;
  splx(s);
  reschedule(0) ;
}

void check_sleep_queue(){
  tcb_t * iter = task_list ;
  int modified = 0 ;
  while( iter != NULL ){
    if( iter->tcb_wait != 0 &&  iter->tcb_wait <= timer_ticks ){
      debug("waking up %s\n" , iter->tcb_name ) ;
      iter->tcb_wait = 0 ;
      ready( iter ) ;
      modified = 1 ;
    }
    iter = iter->tcb_next ;
  }
  if(modified)
    need_resched++;
}