/* $Revision: 1.2 $ */

#include <locore.h>
#include <sem.h>
#include <kern.h>
#include <stdio.h>

#include <debug.h>

extern tcb_t   *cur_task;
extern int      interrupt_nest;
extern int      need_resched;

void sem_init(sem_t * sem){
  int s = splhi();
  int i;
  
  sem->sem_count = 0;
  sem->sem_gindex = 0;
  for (i = 0; i < MAX_PRIORITIES_PER_GROUP; i++) {
    sem->sem_pindex[i] = 0;
  }
  splx(s);
}

void sem_p(sem_t * sem){
  /* Prec: sem is valid Semaphore 
   * Post: decrement Semaphore count by one,
   *       if < 0 suspend calling task
   */
  int s = splhi();
  
  if (--sem->sem_count < 0) {
    sem->sem_count = 0 ;
    sem->sem_gindex |= (1 << cur_task->tcb_group_index);
    sem->sem_pindex[cur_task->tcb_group_index] |=
      (1 << cur_task->tcb_pri_index);
    debug("sem_p: task %h<%s> gidx %h pidx %h yields.\n",
	  cur_task, cur_task->tcb_name, sem->sem_gindex , sem->sem_pindex);
    splx(s);
    yield();
  }
}

int sem_v(sem_t * sem){
  tcb_t          *task;
  int             s;
  
  if (sem->sem_gindex == 0){	/* nobody waiting */
    sem->sem_count++ ;          /* although it must be counted up */
    debug("sem_v: nobody waiting, count=%d\n",sem->sem_count);
    return 0;
  }
  s = splhi();
  
  /* changes: test if sem_count would be above 0 
   * but dont count up
   */
  if (( sem->sem_count + 1 ) > 0) {
    /* pick highest priority task waiting on this semaphore */
    task = pick(sem);
    if(!task){ 
      panic("sem_v: pick returned NULL \n");
    }
    debug("sem_v: pick returned %h <%s>\n", task , task->tcb_name);

    sem->sem_gindex &= ~(1 << task->tcb_group_index);
    sem->sem_pindex[task->tcb_group_index] &=
      ~(1 << task->tcb_pri_index);

    if (!(task->tcb_stat & TASK_SUSPENDED)) {
      debug("sem_v: task %h<%s> stat %h not suspended\n",
	    task, task->tcb_name, task->tcb_stat);
      splx(s);
      return -1;
    }
    if (interrupt_nest == 0 &&
	task->tcb_group_index > cur_task->tcb_group_index &&
	task->tcb_pri_index > cur_task->tcb_pri_index) {
      debug("sem_v: task %h<%s> made to run.\n",
	    task, task->tcb_name);
      /* changed: if sem_count == 0 and nobody waiting count sem_count up
       * else keep it 0 because other tasks must be awakened 
       */
      if ( sem->sem_gindex == 0 ){
	sem->sem_count ++ ;
	debug("sem_v: no other tasks waiting count = %d \n" , sem->sem_count ) ;
      }
      splx(s);

      exec(task);
    } else {
      debug("sem_v: task %h<%s> made ready.\n",
	    task, task->tcb_name);
      ready(task);
      need_resched++;
      /* changed: if sem_count == 0 and nobody waiting count sem_count up
       * else keep it 0 because other tasks must be awakened 
       */
      if ( sem->sem_gindex == 0 ){
	sem->sem_count ++ ;
	debug("sem_v: no other tasks waiting count = %d \n" , sem->sem_count ) ;
      }

      splx(s);
    }
  }
  return 1;
}