/*
 * POSIX-Compatible Application-Defined Scheduling.
 *
 * Written by J. Vidal
 * Copyright (C) Dec, 2002 OCERA Consortium.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2.
 *
 * These are extensions to the RTLinux code to provide 
 * POSIX-Compatible Application-Defined Scheduling.
 */

#include <rtl_sched.h>
#include <cpu_tracer.h>

#ifdef _RTL_POSIX_APP_SCHEDULING

/* queues managment stuff */
#define queue_empty(q) ((q)->rear == (q)->front)? 1 : 0
/*
  Insert item in circular queue stored in c_array[0:size-1]
  rear points to the last item, and front is one position
  counterclockwise from the first item in q.
*/
#define queue(q,item) (int) ({\
 int q_err=0; \
 (q)->rear=((q)->rear+1) % (q)->size; \
  if (queue_empty((q))) { /* queue full */ \
    if ((q)->front == 0) {\
      (q)->rear=(q)->size - 1; \
    } else { \
      (q)->rear--;\
    }\
    q_err=QUEUE_FULL;\
  } else {\
    (q)->c_array[(q)->rear]=*item;\
  }\
 q_err;  /* return value*/ \
})

/* Removes and returns the front element of the queue c_array[0:size-1] */
#define dequeue(q,item) (int) ({\
  int q_err=0; \
  if (queue_empty((q))) {  \
    q_err=QUEUE_EMPTY;  \
  } else { \
    (q)->front=(((q)->front+1) % (q)->size); \
    *item=(q)->c_array[(q)->front]; \
  }\
  q_err; /* return value*/ \
})

/*
************************************************************** 
Queues managment for application schedulers events threatment.
************************************************************** 
*/
inline int queue_event(event_queue_t *q, struct posix_appsched_event *item){
  return queue(q,item);
}

inline int dequeue_event(event_queue_t * q, struct posix_appsched_event *item){
  return dequeue(q,item);
}

/*
  Check whatever the events queue is empty.
  If it is, implies that the application-defined scheduler
  could be suspended by last call to posix_appsched_invoke_scheduler.
  So, it must be woken up so it begins to
  process the event when it will take the CPU.
*/
inline int send_event (struct posix_appsched_event *event, pthread_t th) {
  int err;
  err=queue_event(th->appsched.events, event);
  if (!err) RTL_MARK_READY(th);
  return err;
}
 
int generate_event(pthread_t th, pthread_t appscheduler, int code,union posix_appsched_eventinfo *info, size_t info_size) {
  struct posix_appsched_event event;
  /*
    Is event filtered or is a system scheduled thread, 
    or alternatively there is no application scheduler? 
  */
  if (!appscheduler || 
      rtl_sigismember(&appscheduler->appsched.events_mask,code)){
    return -1;
  }

  event.thread=th;
  event.event_code=code;

  if (info) {
    event.event_info=*info;
  } 
  event.info_size=info_size;
  return send_event(&event,appscheduler);
}
 
/*
  ************************************************************** 
  Queues managment for application schedulers actions threatment.
  ************************************************************** 
*/
inline int queue_action(action_queue_t *q, posix_appsched_action *item){
  return queue(q,item);
}

inline int dequeue_action(action_queue_t *q, posix_appsched_action *item){
  return dequeue(q,item);
}

int posix_appsched_actions_init(posix_appsched_actions_t * sched_actions){
  rtl_init_actions_queue(sched_actions);
  return 0;
};


int posix_appsched_actions_destroy(posix_appsched_actions_t * sched_actions){
  // In rtlinux memory can't be freed from real time tasks.
  return 0;
}

int posix_appsched_actions_addactivate(posix_appsched_actions_t * sched_actions,
				   pthread_t thread)
{
  posix_appsched_action act;

  act.action = POSIX_APPSCHED_ACTIVATE;
  act.thread = thread;

 return queue_action(sched_actions, &act);
}

int posix_appsched_actions_addsuspend(posix_appsched_actions_t * sched_actions,
				  pthread_t thread)
{
  posix_appsched_action act;

  act.action = POSIX_APPSCHED_SUSPEND;
  act.thread = thread;

  return queue_action(sched_actions, &act);
}

int posix_appsched_actions_addlock(posix_appsched_actions_t *sched_actions,
					  pthread_t thread,/* const*/ pthread_mutex_t * mutex){
  
  posix_appsched_action act;
  
  act.action = POSIX_APPSCHED_LOCK;
  act.thread = thread;
  act.mutex=mutex;
  
  return queue_action(sched_actions, &act);
  
}

/* 
   TODO: Implement message passing to the application scheduler 
   Explicit Scheduler Invocation 
*/
int posix_appsched_invoke_scheduler(void *msg, size_t msg_size){
  int flags,err;
  
  rtl_no_interrupts(flags);
  err=generate_event(pthread_self(),appscheduler(pthread_self()),POSIX_APPSCHED_EXPLICIT_CALL,NULL,0);
  rtl_schedule();
  rtl_restore_interrupts(flags);
  return err;

}

/* Execute Scheduling Actions */
int posix_appsched_execute_actions(/*const*/
			       posix_appsched_actions_t *sched_actions,
			       const rtl_sigset_t *set,
			       const struct timespec *timeout,
			       struct timespec *current_time,
			       struct posix_appsched_event *event){
  int ret=0,remaining_events=1; /* STILL REMAINING EVENTS TO PROCESS?*/
  posix_appsched_action act;
  pthread_t self = pthread_self();
  rtl_sigset_t oset;
  int intr_state;

  rtl_no_interrupts(intr_state);
  //Backup old blocked mask.
  oset=self->blocked;

  /* Execute actions. */
  if (sched_actions)
    while (!dequeue_action(sched_actions, &act)) {
      
      switch (act.action) {
      case POSIX_APPSCHED_ACTIVATE:
	RTL_MARK_READY(act.thread);
	break;
	
      case POSIX_APPSCHED_SUSPEND:
	RTL_MARK_SUSPENDED(act.thread);
	break;
	
      case POSIX_APPSCHED_LOCK:
	switch(act.thread->appsched.status){
	case WAITING_TO_LOCK_APPSCHED_MUTEX:
	case TRYING_TO_LOCK_APPSCHED_MUTEX:
	  if (act.mutex != act.thread->appsched.appsched_mutex_where_waiting){
	    //  Trying to lock a no valid mutex for this tread.
	    return EPERM;
	  } else if (mutex_owner(act.mutex) !=  NULL){
	    //  The mutex is already owned
	    return EBUSY;
	  } else {
	    mutex_owner(act.mutex)=act.thread;
	    RTL_MARK_READY(act.thread);
	  }
	  break;
	case APPSCHED_SUSPENDED: 
	  //APPSCHED_SUSPENDED
	  return EINVAL;
	default:
	  break;
	}
	break;
      default:
	break;
      }
    } //end while
  
    /* Process event */
    if (event) {
      if ((QUEUE_EMPTY==dequeue_event(self->appsched.events, event))) {
      remaining_events=0;
      event->event_code = -1;
      event->thread = NULL;        
    }
  }

  // set up timeout. timeouts are absolute or relative deppending on appsched flags.
  if (timeout){
    if (self->appsched.flags &  POSIX_APPSCHED_ABSTIMEOUT  ){
      __rtl_setup_timeout(self, timespec_to_ns(timeout));  
    }else{
      __rtl_setup_timeout(self, clock_gethrtime(self->appsched.clockid) + timespec_to_ns(timeout));  
    }
  }
  
  // Set signals mask.
  if (set){
    self->blocked= (oset & ~RTL_THREAD_SIGNALS_MASK) | (~(*set) & RTL_THREAD_SIGNALS_MASK) ;     
  }
  
  /*
    if there aren't more events to process, then block app. scheduler 
    until new event arrives, the timeout expires or a signal of the 
    requested set arrives. Whatever happens earlier.
  */
  if (!remaining_events){
    /* 
       if there are no more events to process, 
       the application scheduler is suspended 
    */
    
    RTL_MARK_SUSPENDED(self);
    // Leave the CPU.
    ret=rtl_schedule();
    
    if (RTL_TIMED_OUT(&ret)) { 
         event->event_code = POSIX_APPSCHED_TIMEOUT;
    }
  }

  // Restore old blocked mask backup.
  self->blocked=oset;
  
  //Actualize current time.
  if (current_time) {
    *current_time = timespec_from_ns(clock_gethrtime(self->appsched.clockid));
  }
    
  rtl_restore_interrupts(intr_state);

  return 0;
}

#endif /* #ifdef _RTL_POSIX_APP_SCHEDULING */