/*
 * RTLinux default scheduler
 * RTLinux has a modular scheduler and this may be replaced if desired.
 *
 * Written by Michael Barabanov, Victor Yodaiken
 * Copyright (C) Finite State Machine Labs Inc., 1998,1999
 * Released under the terms of the GNU  General Public License Version 2
 *
 * StandAlone RTLinux integration 
 * written by Vicente Esteve LLoret <viesllo@inf.upv.es> 
 * Copyright (C) Feb, 2003 OCERA Consortium.
 *
 * 
 */

#include <deblin/vhal.h>
#include <arch/segment.h>
#include <arch/memory.h>
#include <arch/context.h>
#include <arch/rtl_tracer.h>
#include <arch/idle.h>
#include <arch/mprot.h>
#include <rtl_sched.h>
#include <rtl_ipc.h>
#include <rtl_sync.h>
#include <arch/rtl_switch.h>
#include <rtl_conf.h>

// static spinlock_t rtl_tqueue_lock;

// static int rtl_sched_irq;
//static pthread_t zombie_threads;

struct rtl_sched_cpu_struct rtl_scheduler[1];

struct rtl_thread_struct rtl_idle_task;

extern void display_cr3(int num);

int rtl_startup (void *(*fn)(void *),void *data)
{
  void *retval;
  
  STARTUSERCODE();
  
  rtl_allow_interrupts(); 
  
  rtl_schedule();  
  
  retval = (*fn)(data);
  
  return 0;
}

static void rtl_sched_timer_interrupt( struct pt_regs *regs)
{
  rtl_schedule();  
}




static inline struct rtl_thread_struct * find_preemptor(schedule_t *s, struct rtl_thread_struct *chosen){
	struct rtl_thread_struct *t;
	struct rtl_thread_struct *preemptor=0;
	for (t = s->rtl_tasks; t; t = t->next) {
		if (test_bit(RTL_THREAD_TIMERARMED, &t->threadflags)) {
			if (t->sched_param.sched_priority > chosen->sched_param.sched_priority)
			{
				if(!preemptor ||(t->resume_time < preemptor->resume_time))
				{	
					preemptor = t;
				}
			}
		}
	}
	return preemptor;
}





#define do_abort(t) do { clear_bit(RTL_THREAD_TIMERARMED, &t->threadflags); if (t->abort) { t->abort(t->abortdata); }} while (0)

static inline void do_signal(pthread_t t)
{
  rtl_irqstate_t flags;
  rtl_no_interrupts(flags);

  if (test_and_clear_bit(RTL_SIGNAL_SUSPEND, &t->pending)) 
  {
    do_abort(t);
    RTL_MARK_SUSPENDED(t);
  }

  if (test_and_clear_bit(RTL_SIGNAL_WAKEUP, &t->pending)) 
  {
    RTL_MARK_READY(t);
    do_abort(t);
  }

  if (test_and_clear_bit(RTL_SIGNAL_TIMER, &t->pending)) 
  {
    RTL_MARK_READY(t);
    do_abort(t);
  }

  if (!test_bit(RTL_SIGNAL_CANCEL, &t->blocked) && 
      test_and_clear_bit(RTL_SIGNAL_CANCEL, &t->pending)) 
  {
    RTL_MARK_READY(t);
    do_abort(t);
    rtl_restore_interrupts(flags);
    
//		(*rtl_sigcancel_handler)(RTL_SIGNAL_CANCEL);
  }  

  rtl_restore_interrupts(flags);
}

//
//


static inline void do_user_signal(pthread_t t){
  int sig;
  unsigned long flags, initial_interrupt_state;
  void (*fun)(int)=NULL;
  
  rtl_no_interrupts(initial_interrupt_state);
  
  for (sig=RTL_MAX_SIGNAL;sig>=RTL_SIGUSR1;sig--){
    if (test_bit(sig,&t->pending) 
	&& !test_bit(sig,&t->blocked))
    {
      if (rtl_sigact[sig].owner==t)
      {
	fun=rtl_sigact[sig].sa_handler;
	if (fun) 
	{ 
	  // Block signal being managed.
	  set_bit(sig,&t->blocked);
	  set_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&t->pending);
	  rtl_allow_interrupts();
	  STARTUSERCODE();
	  fun(sig);
	  ENDUSERCODE();
	  rtl_no_interrupts(flags); 
	  clear_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&t->pending);
	  clear_bit(sig,&t->blocked);

	  if (test_and_clear_bit(RTL_THREAD_SIGNAL_INTERRUMPIBLE,&t->threadflags))
	  {
	    pthread_kill(t,RTL_SIGNAL_WAKEUP);
	  }
	  clear_bit (RTL_SCHED_TIMER_OK, &LOCAL_SCHED->sched_flags); 
	}
      }
      /* The bit should be pending until unblocked. */
      clear_bit(sig,&t->pending);
    }   
  }
  
  rtl_restore_interrupts(initial_interrupt_state);
}

int rtl_schedule(void)
{
  unsigned long interrupt_state;
  struct rtl_thread_struct *new_task = 0;
  struct rtl_thread_struct *preemptor = 0; 
  struct rtl_thread_struct *t;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  schedule_t *s; 
  hrtime_t now;
  rtl_sigset_t mask;
  char ptr[20];

  
  STARTKERNELCODE(mprot);
  s = LOCAL_SCHED;
idle:
    
  rtl_no_interrupts(interrupt_state);
 //
//  DebugString("Timer                           ");

  now = s->clock->gethrtime(s->clock);
  
  ptr[8]=0;
  hexatoascii((unsigned long) now,ptr);
  PutString(0,68,ptr,0x20);
  hexatoascii((unsigned long) (now>>32),ptr);
  PutString(0,60,ptr,0x20);


  if (s->clock->mode == RTL_CLOCK_MODE_ONESHOT) 
  {
    s->clock->value = now;
  };

  // Verifico Timers.
  // 
  
  for (t = s->rtl_tasks; t; t = t->next) 
  {
    if (test_bit(RTL_THREAD_TIMERARMED, &t->threadflags)) 
    {
      if (now >= t->resume_time) 
      {
        clear_bit(RTL_THREAD_TIMERARMED, &t->threadflags);
	rtl_sigaddset (&t->pending, RTL_SIGNAL_TIMER);
	if (t->period != 0) 
	{
	  t->resume_time += t->period;
	} 
	else 
	{
	  t->resume_time = HRTIME_INFINITY;
	}
      }
    }
    /* and find highest priority runnable task */
    if ((t->pending & ~t->blocked) && (!new_task ||
        (t->sched_param.sched_priority > new_task->sched_param.sched_priority)))    {
          new_task = t;
    }
  };		

  if ((s->clock->mode == RTL_CLOCK_MODE_ONESHOT))// && 
  //    !test_bit (RTL_SCHED_TIMER_OK, &s->sched_flags)) 
  {
    if ((preemptor = find_preemptor(s,new_task))) 
    {
      (s->clock)->settimer(s->clock, preemptor->resume_time - now);
    } else {
      (s->clock)->settimer(s->clock, (HRTICKS_PER_SEC / HZ) / 2);
    }
    set_bit (RTL_SCHED_TIMER_OK, &s->sched_flags);
  }


  if (s->rtl_current != new_task)
  {
#if CONFIG_CONTEXT_MEMORYPROT
    hexatoascii((unsigned long) (new_task->contextid),ptr);
    PutString(1,60,ptr,0x20);
    context_switch(new_task->contextid);
#endif
    rtl_switch_to(&s->rtl_current,new_task);
    RTL_TRACE(TASKIN,new_task->tracer_id);
  };

  // are There Signals ??
  // 

  mask = pthread_self()->pending;

  if (pthread_self()->pending & ~(1 << RTL_SIGNAL_READY)) 
  {
    do_signal(pthread_self());
  };

 
  if (test_bit(RTL_SIGNAL_HANDLER_EXECUTION_INPROGRESS,&pthread_self()->pending)) goto end;
  
  if ((pthread_self()->pending  & ~pthread_self()->blocked) 
       & RTL_THREAD_SIGNALS_MASK) 
           do_user_signal(pthread_self());
 
 
  if (!rtl_sigismember(&pthread_self()->pending, RTL_SIGNAL_READY)) 
  {
    goto idle;
  };

end:	
  rtl_restore_interrupts(interrupt_state);	
  ENDKERNELCODE(mprot);
 
  return mask;
}

/* Note: as per POSIX standard, delivery of the signal is not
   necessarily soon. In RTLinux it waits until the next scheduling
   you must do sigqueue(cpu_id,RTL_RESCHED_SIGNAL) to get it to work.
   but TODO sigqueue
 
   Note that currently no soft signals require any real processing so we 
   don't need to be "in the context" of the receiving thread.
   Signals are hard: meaning we get them in the process context
   anyways (on top of whatever thread is running) or schedule control
   which means that we just manipulate the scheduler status and be done.


 */


#define CHECK_VALID(thread) do { if ((thread)->magic != RTL_THREAD_MAGIC) return ESRCH; } while (0)

int pthread_kill(pthread_t thread, int signal)
{
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  
  STARTKERNELCODE(mprot);
 
  if ((unsigned) signal <= RTL_MAX_SIGNAL) 
  {
    CHECK_VALID(thread);
    if (signal == 0) 
    {
      ENDKERNELCODE(mprot);
      return 0;
    }
    set_bit(signal, &thread->pending);
    ENDKERNELCODE(mprot);
    return 0;
  }
  ENDKERNELCODE(mprot);
  return 0;
}



int pthread_suspend_np (pthread_t thread)
{
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  
  STARTKERNELCODE(mprot);
  
  if (thread == pthread_self()) 
  {
    RTL_MARK_SUSPENDED (pthread_self());
    rtl_schedule();
//		pthread_testcancel();
  }
  else 
  {
    pthread_kill(thread,RTL_SIGNAL_SUSPEND);
  }
  ENDKERNELCODE(mprot);
  return 0;
  
}

int pthread_wakeup_np (pthread_t thread)
{
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  
  STARTKERNELCODE(mprot);
  pthread_kill(thread,RTL_SIGNAL_WAKEUP);
  rtl_schedule();
  ENDKERNELCODE(mprot);
  
  return 0;
}






static void add_to_task_list(pthread_t thread)
{
  schedule_t *s = LOCAL_SCHED;
  thread->next = s->rtl_tasks;
  s->rtl_tasks = thread;
}
 
int __pthread_create (pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg, struct module *creator)
{
  int *stack_addr;
  int stack_size;
  struct rtl_thread_struct *task;
  long interrupt_state;
  pthread_attr_t default_attr;
#if CONFIG_KERNEL_MEMORYPROT
  mprot_t mprot;
#endif
#if CONFIG_CONTEXT_MEMORYPROT
  int contextid;
#endif
  
  STARTKERNELCODE(mprot);
  
  DebugString(" Creando Tarea                                       "); 

  if (!attr)
  {
    pthread_attr_init(&default_attr);
    attr = &default_attr;
  }
  stack_size = attr->stack_size;
   
  stack_addr = kmalloc((stack_size*sizeof(int)), GFP_KERNEL);
#if CONFIG_CONTEXT_MEMORYPROT
  contextid = get_contextid(start_routine);
  map_context(contextid,stack_addr,stack_size*sizeof(int));
#endif

  task = (struct rtl_thread_struct *) stack_addr;
  task->kmalloc_stack_bottom = stack_addr;
  
  *thread = task;
  
  task->magic = RTL_THREAD_MAGIC;
  task->pending = attr->initial_state;
  task->blocked = 0;
  task->abort = 0;
  task->cpu = attr->cpu;
  task->cleanup = 0;
  task->resume_time = HRTIME_INFINITY;
  task->period = 0;
  task->sched_param = attr->sched_param;
  task->stack = (int *) (((int) stack_addr) + (stack_size-1)*sizeof(int));
  
  DebugString("PUntero de Pila               ");
  DebugValue(task->stack);
  
  task->fpu_initialized = 0;
  task->uses_fp = attr->use_fp;
  task->start = (void *) start_routine;
  
#if CONFIG_CONTEXT_MEMORYPROT
  task->contextid = contextid;
#endif
  
  // Esto es dependiente de la implementación de semaforos.
  // Poner dentro de un define!!!.

  ((RT_TASK_IPC *) task)->sem_at = NULL;
  ((RT_TASK_IPC *) task)->magic = RT_TASK_IPC_MAGIC;
  task->user[IPC_DATA_INDEX] = task;

  // Fin de la inicialización dependiente de semáforos.
  

  rtl_no_interrupts(interrupt_state);
  rtl_init_stack(task,start_routine,arg,rtl_startup);
  add_to_task_list(task);
  
//  RTL_MARK_READY(*thread);
//  
  rtl_sigaddset(&task->pending,RTL_SIGNAL_READY);
  
  rtl_restore_interrupts(interrupt_state);
  
  ENDKERNELCODE(mprot);
  
  return 0;
}



    
                   // 
		   // Por ahora Las funciones de inicialización
		   // se ejecutan con las interrupciones
		   // deshabilitadas

int init_sched(void)
{
  schedule_t *s = LOCAL_SCHED;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  unsigned int cpu_id = rtl_getcpuid();
  rtl_irqstate_t interrupt_state;

  STARTKERNELCODE(mprot);
  
  DebugString("Inicializando el Planificador                         ");
 
  rtl_no_interrupts(interrupt_state);
  
  rtl_idle_task.stack = (int *) IDLE_STACK_PTR;
  rtl_idle_task.next = 0;
  rtl_idle_task.sched_param.sched_priority = sched_get_priority_min(0); 
#if CONFIG_CONTEXT_MEMORYPROT
  rtl_idle_task.contextid = get_idle_contextid();
#endif


  rtl_sigaddset (&rtl_idle_task.pending, RTL_SIGNAL_READY);
  
  s->rtl_current = &rtl_idle_task;
  s->rtl_tasks = &rtl_idle_task;
  
  s->sched_flags = 0;

  s->clock = rtl_getbestclock(cpu_id);
  
  if ((s->clock) && 
      rtl_setclockhandler(s->clock, rtl_sched_timer_interrupt) == 0)
  {
    s->clock->init(s->clock);
  };
  DebugString("Voy a habilitar interrupciones en init_sched         "); 
  rtl_restore_interrupts (interrupt_state);
  DebugString("Planificador Inicializado                             "); 
  
  ENDKERNELCODE(mprot);
  
  return 0;
}

int pthread_make_periodic_np (pthread_t p, hrtime_t start_time, hrtime_t period)
{
  rtl_irqstate_t interrupt_state;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  

  STARTKERNELCODE(mprot);
  
  if (period < 0) 
  {
    return EINVAL;
  }

  rtl_no_interrupts(interrupt_state);

  p->period = period;
  __rtl_setup_timeout(p, start_time);

  rtl_schedule();
  rtl_restore_interrupts(interrupt_state);

  ENDKERNELCODE(mprot);
  
  return 0;
}


int pthread_wait_np(void)
{
  rtl_irqstate_t interrupt_state;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  pthread_t self = pthread_self();

  STARTKERNELCODE(mprot);

  rtl_no_interrupts(interrupt_state);
  
  RTL_MARK_SUSPENDED (self);
  __rtl_setup_timeout (self, self->resume_time);
  rtl_schedule();
//	pthread_testcancel();
  rtl_restore_interrupts(interrupt_state);

  ENDKERNELCODE(mprot);
  
  return 0;
}



int usleep (useconds_t interval)
{
  rtl_irqstate_t flags;
#if CONFIG_KERNEL_MEMORYPROT  
  mprot_t mprot;
#endif  
  pthread_t th; 
  hrtime_t save_resume_time;          

  STARTKERNELCODE(mprot);
  th = pthread_self();
  rtl_no_interrupts (flags);
  save_resume_time = th->resume_time;

  RTL_MARK_SUSPENDED(th);

  __rtl_setup_timeout (th,gethrtime() + (long long) interval * 1000LL);

  rtl_schedule();

//	pthread_testcancel();

  th->resume_time = save_resume_time;
  rtl_restore_interrupts (flags);

  ENDKERNELCODE(mprot);
  
  return 0;
}