/*
 * RTLinux semaphore implementation
 *
 * Written by Michael Barabanov <baraban@fsmlabs.com>
 * Copyright (C) Finite State Machine Labs Inc., 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_sema.h>
#include <rtl_sync.h>
#include <errno.h>

#if _RTL_POSIX_SEMS

int sem_init(sem_t *sem, int pshared, unsigned int value)
{
  rtl_wait_init (&sem->wait);
  sem->value = value;
  rtl_spin_lock_init(&sem->lock);
  return 0;
}

int sem_destroy(sem_t *sem)
{
  return 0;
}

int sem_getvalue(sem_t *sem, int *sval)
{
  return sem->value;
}

int __sem_trywait(sem_t *sem)
{
  if (sem->value > 0) 
  {
    --(sem->value);
    return 0;
  } 
  else 
  {
    errno = EAGAIN;
    return -1;
  }
}

int sem_trywait(sem_t *sem)
{
  rtl_irqstate_t flags;
  int ret;
  rtl_spin_lock_irqsave (&sem->lock, flags);
  ret = __sem_trywait(sem);
  rtl_spin_unlock_irqrestore(&sem->lock, flags);
  return ret;
}

int sem_wait(sem_t *sem)
{
  int ret;
  rtl_irqstate_t flags;
  rtl_spin_lock_irqsave (&sem->lock, flags);
  while (__sem_trywait(sem)) 
  {
    if (errno != EAGAIN) 
    {
      rtl_spin_unlock_irqrestore(&sem->lock, flags);
      return -1;
    }
    ret = rtl_wait_sleep (&sem->wait, &sem->lock);
//		pthread_testcancel();
    rtl_spin_lock(&sem->lock);
  }

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



int sem_post(sem_t *sem)
{
  rtl_irqstate_t flags;
  rtl_spin_lock_irqsave (&sem->lock, flags);
  
  ++(sem->value);
  rtl_wait_wakeup (&sem->wait);

  rtl_spin_unlock_irqrestore(&sem->lock, flags);
  rtl_schedule(); /* make it fast! */
  return 0;
}
#endif // _RTL_POSIX_SEMS