/*
 * RTLinux mutex and condvar implementation
 *
 * Written by Michael Barabanov
 * Rewritten by Victor Yodaiken after a long delay.
 * Copyright (C) Finite State Machine Labs Inc., 1999,2000
 * Released under the terms of the GPL Version 2
 * 
 * StandAlone RTLinux integration 
 * written by Vicente Esteve LLoret <viesllo@inf.upv.es> 
 *
 *
 * 
 */

#include <rtl_conf.h>
#include <rtl_sched.h>
#include <rtl_mutex.h>
#include <rtl_sync.h>

#if _RTL_POSIX_MUTEXS

#ifdef CONFIG_RTL_SRP

/*
 *  ------- Stack Resource Protocol utility functions --------
 */

static srp_stack_t srp_ceil_stack[RTL_SEM_NSEMS_MAX];
static int srp_ceil_stack_count=-1;

inline struct rtl_sched_param *srp_current_sysceil ()
{
	if (srp_ceil_stack_count>=0)
		return &srp_ceil_stack[srp_ceil_stack_count].preempt_level;
        else
                return (struct rtl_sched_param *)0;
}

/* A new mutex is being locked and the system ceil may raise */
static inline int srp_add_sysceil(struct rtl_sched_param *ceil)
{
        if (srp_ceil_stack_count >= RTL_SEM_NSEMS_MAX) {
		//rtl_printf("pthread_mutex_lock: Too many mutex locked (SRP protocol).\n");
		//rtl_printf("Edit RTL_SEM_NSEMS_MAX constant. Current value is: %d", RTL_SEM_NSEMS_MAX); 
                return EBUSY;
	}
        if ( cmp_preempt_level(ceil, srp_current_sysceil()) >= 0 ){
                srp_ceil_stack[++srp_ceil_stack_count].preempt_level = *ceil;
                srp_ceil_stack[srp_ceil_stack_count].owner = pthread_self();
        }
        return 0;
}


/* When calling mutex_unlock, the top of system ceil stack will be removed if apropiate */
static inline void srp_del_sysceil(struct rtl_sched_param *ceil, struct rtl_sched_param *prev_level)
{
        if (cmp_preempt_level(&srp_ceil_stack[srp_ceil_stack_count].preempt_level, ceil) == 0){
                (*prev_level)=srp_ceil_stack[srp_ceil_stack_count--].preempt_level;
        }
        (*prev_level).sched_priority=-1;
}

pthread_t srp_ceiling_owner()
{
        if (srp_ceil_stack_count>=0)
                return srp_ceil_stack[srp_ceil_stack_count].owner;
        else 
                return (pthread_t) 0;
}



int pthread_mutex_register_np(pthread_t thread, pthread_mutex_t *mutex) 
{
	if (!mutex->valid){
		return EINVAL;
	}
        switch (mutex->protocol) {
        case PTHREAD_PRIO_PROTECT:
                if ( (mutex->prioceiling == sched_get_priority_max(SCHED_FIFO)+1) ||
                     (mutex->prioceiling < thread->sched_param.sched_priority)){
                        mutex->prioceiling = thread->sched_param.sched_priority;
                }
                break;
        case PTHREAD_PRIO_SRP:
                if ( (mutex->preemptceiling.sched_deadline == 0) ||
                     (cmp_preempt_level(&(thread->sched_param), &(mutex->preemptceiling)) > 0 ) ) {
                        mutex->preemptceiling = thread->sched_param;
                }
        }
        return 0;
}       

#endif /* CONFIG_RTL_SRP */




int pthread_mutexattr_init(pthread_mutexattr_t *attr)
{
  attr->type = PTHREAD_MUTEX_DEFAULT;
  attr->pshared = PTHREAD_PROCESS_SHARED;
  attr->protocol = PTHREAD_PRIO_NONE;
  attr->prioceiling = sched_get_priority_max(SCHED_FIFO);

#ifdef CONFIG_RTL_SRP 
  attr->preemptceiling.sched_priority = attr->prioceiling+1;
  attr->preemptceiling.sched_deadline = 0LL;
#endif
  return 0;
}

int pthread_mutex_destroy(pthread_mutex_t *mutex)
{
  mutex->valid = 0;
  return 0;
}

int pthread_mutex_init(pthread_mutex_t *mutex,
		    const pthread_mutexattr_t *attr)
{
  pthread_mutexattr_t defattr;
  mutex->valid = 1;
  mutex->busy = 0;
  mutex->flags = 0;
  rtl_wait_init(&mutex->wait);
  rtl_spin_lock_init (&mutex->lock);
  
  if (!attr) 
  {
    pthread_mutexattr_init(&defattr);
    attr = &defattr;
  }
  
  mutex->type = attr->type;
  mutex->protocol = attr->protocol;
  mutex->prioceiling = attr->prioceiling;
#ifdef CONFIG_RTL_SRP
  mutex->preemptceiling = attr->preemptceiling;
#endif
  return 0;
}


static inline int __pthread_mutex_trylock(pthread_mutex_t *mutex)
{
#ifdef _RTL_POSIX_THREAD_PRIO_PROTECT
/* if _RTL_POSIX_THREAD_PRIO_PROTECT, this code is protected by a spinlock */
  if (mutex->protocol == PTHREAD_PRIO_PROTECT) 
  {
    if (RTL_PRIO(RTL_CURRENT) > mutex->prioceiling) 
    {
      return EINVAL;
    }
  mutex->oldprio = RTL_PRIO (RTL_CURRENT);
  RTL_PRIO (RTL_CURRENT) = mutex->prioceiling;
  }
#endif

#ifdef CONFIG_RTL_SRP
  if (mutex->protocol == PTHREAD_PRIO_SRP) {
    srp_add_sysceil(&(mutex->preemptceiling));
  }
#endif
  if (test_and_set_bit(0, &mutex->busy)) 
  {
    return EBUSY;
  }
  return 0;
}

int pthread_mutex_trylock(pthread_mutex_t *mutex)
{
  rtl_irqstate_t flags;
  int ret;
  switch (mutex->type) 
  {
    case PTHREAD_MUTEX_SPINLOCK_NP:
      rtl_no_interrupts (flags);
      if (rtl_spin_trylock(&mutex->lock)) 
      {
        rtl_restore_interrupts (flags);
	return EBUSY;
      } 
      else 
      {
	mutex->flags = flags;
	return 0;
      }
      break;
    case PTHREAD_MUTEX_NORMAL:
    default:
#if _RTL_POSIX_THREAD_PRIO_PROTECT | CONFIG_RTL_SRP
      rtl_spin_lock_irqsave (&mutex->lock, flags);
#endif
      ret = __pthread_mutex_trylock(mutex);
#if _RTL_POSIX_THREAD_PRIO_PROTECT | CONFIG_RTL_SRP
      rtl_spin_unlock_irqrestore(&mutex->lock, flags);
#endif
      return ret;
  }
}


int pthread_mutex_lock(pthread_mutex_t *mutex)
{
  rtl_irqstate_t flags;
  int ret;
  
  switch (mutex->type) 
  {
    case PTHREAD_MUTEX_SPINLOCK_NP:
      rtl_no_interrupts (flags);
      rtl_spin_lock (&mutex->lock);
      mutex->flags = flags;
      return 0;
    case PTHREAD_MUTEX_NORMAL:
    default:
      if (!mutex->valid) 
      {
        return EINVAL;
      }

      rtl_spin_lock_irqsave (&mutex->lock, flags);
      while ((ret = __pthread_mutex_trylock(mutex))) 
      {
        if (ret == EINVAL) 
        {
	  rtl_spin_unlock_irqrestore(&mutex->lock, flags);
	  return ret;
	}
 
        ret = rtl_wait_sleep (&mutex->wait, &mutex->lock);
        rtl_spin_lock(&mutex->lock);

      }
      rtl_spin_unlock_irqrestore(&mutex->lock, flags);
      return 0;
  }
}


int pthread_mutex_unlock(pthread_mutex_t *mutex)
{
  rtl_irqstate_t flags;

#ifdef CONFIG_RTL_SRP
  struct rtl_sched_param srp_prev_sysceil;
#endif
  
  switch (mutex->type) 
  {
    case PTHREAD_MUTEX_SPINLOCK_NP:
      flags = mutex->flags;
      rtl_spin_unlock(&mutex->lock);
      rtl_restore_interrupts (flags);
      return 0;
    case PTHREAD_MUTEX_NORMAL:
    default:
      if (!mutex->valid) 
      {
        return EINVAL;
      }

      rtl_spin_lock_irqsave (&mutex->lock, flags);
#ifdef _RTL_POSIX_THREAD_PRIO_PROTECT
      if (mutex->protocol == PTHREAD_PRIO_PROTECT) 
      {
        RTL_PRIO(RTL_CURRENT) = mutex->oldprio;
      }
#endif
#ifdef CONFIG_RTL_SRP
      if (mutex->protocol == PTHREAD_PRIO_SRP) {
	srp_del_sysceil(&mutex->preemptceiling,&srp_prev_sysceil);
      };
#endif
      clear_bit (0, &mutex->busy);
      rtl_wait_wakeup(&mutex->wait);
      rtl_spin_unlock_irqrestore (&mutex->lock, flags);
			/* XXX we do not call the scheduler here by design;
			 * for fast wakeups, use semaphores & pthread_wakeup_np */
#ifdef CONFIG_RTL_SRP
      if (mutex->protocol == PTHREAD_PRIO_SRP) 
	 rtl_schedule();
#endif
      return 0;
  }
}

#ifdef _RTL_POSIX_THREAD_PRIO_PROTECT
int pthread_mutex_setprioceiling(pthread_mutex_t *mutex, int prioceiling, int *old_ceiling)

{
  int ret;
  ret = pthread_mutex_lock (mutex);
  if (ret) 
  {
    return ret;
  }
  *old_ceiling = mutex->prioceiling;
  mutex->prioceiling = prioceiling;
  pthread_mutex_unlock (mutex);
  return 0;
}
#endif

#ifdef CONFIG_RTL_SRP
int pthread_mutex_setpreemptceiling_np(pthread_mutex_t *mutex, struct rtl_sched_param *preemptceiling, struct rtl_sched_param *old_ceiling)
{
	int ret;
	ret = pthread_mutex_lock (mutex);
	if (ret) {
		return ret;
	}
	*old_ceiling = mutex->preemptceiling;
	mutex->preemptceiling = (*preemptceiling);
	pthread_mutex_unlock (mutex);
	return 0;
}
#endif




#endif // _RTL_POSIX_MUTEXS