/*
 * Copyright (C) 1999-2003 Paolo Mantegazza <mantegazza@aero.polimi.it>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 *
 */

#ifndef _RTAI_POSIX_H_
#define _RTAI_POSIX_H_

#ifdef __KERNEL__

#include <rtai_malloc.h>
#include <rtai_rwl.h>
#include <rtai_spl.h>
#include <rtai_sem.h>

#define MAX_PRIO       99
#define MIN_PRIO       1
#define STACK_SIZE     8192
#define RR_QUANTUM_NS  1000000

typedef struct rt_semaphore sem_t;

typedef struct rt_semaphore pthread_mutex_t;

typedef unsigned long pthread_mutexattr_t;

typedef struct rt_semaphore pthread_cond_t;

typedef unsigned long pthread_condattr_t;

typedef struct rt_semaphore pthread_barrier_t;

typedef int pthread_barrierattr_t;

typedef RWL pthread_rwlock_t;

typedef int pthread_rwlockattr_t;

typedef struct rt_spl_t pthread_spinlock_t;

typedef struct rt_task_struct *pthread_t;

typedef struct pthread_attr {

    int stacksize;
    int policy;
    int rr_quantum_ns;
    int priority;

} pthread_attr_t;

typedef struct pthread_cookie {

    RT_TASK task;
    SEM sem;
    void (*task_fun)(int);
    int arg;

} pthread_cookie_t;

#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

static inline int sem_init_rt(sem_t *sem, int pshared, unsigned int value)
{
	if (value < SEM_TIMOUT) {
		rt_typed_sem_init(sem, value, pshared | PRIO_Q);
		return 0;
	}
	return -EINVAL;
}

static inline int sem_destroy_rt(sem_t *sem)
{
	if (rt_sem_wait_if(sem) >= 0) {
		rt_sem_signal(sem);
		return rt_sem_delete(sem);
	}
	return -EBUSY;
}

static inline int sem_wait_rt(sem_t *sem)
{
	return rt_sem_wait(sem) < SEM_TIMOUT ? 0 : -1;
}

static inline int sem_trywait_rt(sem_t *sem)
{
	return rt_sem_wait_if(sem) > 0 ? 0 : -EAGAIN;
}

static inline int sem_timedwait_rt(sem_t *sem, const struct timespec *abstime)
{
	return rt_sem_wait_until(sem, timespec2count(abstime)) < SEM_TIMOUT ? 0 : -1;
}

static inline int sem_post_rt(sem_t *sem)
{
	return rt_sem_signal(sem) < SEM_TIMOUT ? 0 : -ERANGE;
}

static inline int sem_getvalue_rt(sem_t *sem, int *sval)
{
	if ((*sval = rt_sem_wait_if(sem)) > 0) {
		rt_sem_signal(sem);
	}
	return 0;
}

static inline int pthread_mutex_init_rt(pthread_mutex_t *mutex, const pthread_mutexattr_t *mutexattr)
{
	rt_typed_sem_init(mutex, 1, RES_SEM);
	return 0;
}

static inline int pthread_mutex_destroy_rt(pthread_mutex_t *mutex)
{
	if (rt_sem_wait_if(mutex) > 0) {
		rt_sem_signal(mutex);
		return rt_sem_delete(mutex);
	}
	return -EBUSY;
}

static inline int pthread_mutex_lock_rt(pthread_mutex_t *mutex)
{
	return rt_sem_wait(mutex) < SEM_TIMOUT ? 0 : -EINVAL;
}

static inline int pthread_mutex_timedlock_rt(pthread_mutex_t *mutex, const struct timespec *abstime)
{
	return rt_sem_wait_until(mutex, timespec2count(abstime)) < SEM_TIMOUT ? 0 : -1;
}

static inline int pthread_mutex_trylock_rt(pthread_mutex_t *mutex)
{
	return rt_sem_wait_if(mutex) > 0 ? 0 : -EBUSY;
}

static inline int pthread_mutex_unlock_rt(pthread_mutex_t *mutex)
{
	return rt_sem_signal(mutex) > 0 ? 0 : -EINVAL;
}

static inline int pthread_cond_init_rt(pthread_cond_t *cond, const pthread_condattr_t *cond_attr)
{
        return sem_init_rt(cond, BIN_SEM, 0);
}

static inline int pthread_cond_destroy_rt(pthread_cond_t *cond)
{
        return sem_destroy_rt((sem_t *)cond);
}

static inline int pthread_cond_signal_rt(pthread_cond_t *cond)
{
	return rt_cond_signal((sem_t *)cond);
}

static inline int pthread_cond_broadcast_rt(pthread_cond_t *cond)
{
	return rt_sem_broadcast(cond);
}

static inline int pthread_cond_wait_rt(pthread_cond_t *cond, pthread_mutex_t *mutex)
{
	return rt_cond_wait(cond, mutex);
}

static inline int pthread_cond_timedwait_rt(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime)
{
	return rt_cond_wait_until(cond, mutex, timespec2count(abstime)) < SEM_TIMOUT ? 0 : -ETIMEDOUT;
}

static inline int pthread_barrier_init_rt(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count)
{
	return sem_init_rt(barrier, CNT_SEM, count);
}

static inline int pthread_barrier_destroy_rt(pthread_barrier_t *barrier)
{
	return sem_destroy_rt(barrier);
}

static inline int pthread_barrier_wait_rt(pthread_barrier_t *barrier)
{
	return rt_sem_wait_barrier(barrier);
}

static inline int pthread_rwlock_init_rt(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr)
{
	return rt_rwl_init((RWL *)rwlock);
}

static inline int pthread_rwlock_destroy_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_delete((RWL *)rwlock);
}

static inline int pthread_rwlock_rdlock_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_rdlock((RWL *)rwlock);
}

static inline int pthread_rwlock_tryrdlock_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_rdlock_if((RWL *)rwlock);
}

static inline int pthread_rwlock_timedrdlock_rt(pthread_rwlock_t *rwlock, struct timespec *abstime)
{
	return rt_rwl_rdlock_until((RWL *)rwlock, timespec2count(abstime));
}

static inline int pthread_rwlock_wrlock_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_wrlock((RWL *)rwlock);
}

static inline int pthread_rwlock_trywrlock_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_wrlock_if((RWL *)rwlock);
}

static inline int pthread_rwlock_timedwrlock_rt(pthread_rwlock_t *rwlock, struct timespec *abstime)
{
	return rt_rwl_wrlock_until((RWL *)rwlock, timespec2count(abstime));
}

static inline int pthread_rwlock_unlock_rt(pthread_rwlock_t *rwlock)
{
	return rt_rwl_unlock((RWL *)rwlock);
}

static inline int pthread_spin_init_rt(pthread_spinlock_t *lock)
{
	return rt_spl_init((SPL *)lock);
}

static inline int pthread_spin_destroy_rt(pthread_spinlock_t *lock)
{
	return rt_spl_delete((SPL *)lock);
}

static inline int pthread_spin_lock_rt(pthread_spinlock_t *lock)
{
	return rt_spl_lock((SPL *)lock);
}

static inline int pthread_spin_trylock_rt(pthread_spinlock_t *lock)
{
	return rt_spl_lock_if((SPL *)lock);
}

static inline int pthread_spin_unlock_rt(pthread_spinlock_t *lock)
{
	return rt_spl_unlock((SPL *)lock);
}

static inline int get_max_priority_rt(int policy)
{
	return MAX_PRIO;
}

static inline int get_min_priority_rt(int policy)
{
	return MIN_PRIO;
}

static void posix_wrapper_fun(pthread_cookie_t *cookie)
{
	cookie->task_fun(cookie->arg);
	rt_sem_broadcast(&cookie->sem);
	rt_sem_delete(&cookie->sem);
	rt_task_suspend(&cookie->task);
} 

static inline int pthread_create_rt(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg)
{
	pthread_cookie_t *cookie;
	cookie = (void *)rt_malloc(sizeof(pthread_cookie_t));
	if (cookie) {
		(cookie->task).magic = 0;
		cookie->task_fun = (void *)start_routine;
		cookie->arg = (int)arg;
		if (!rt_task_init(&cookie->task, (void *)posix_wrapper_fun, (int)cookie,
				(attr) ? attr->stacksize : STACK_SIZE, (attr) ? attr->priority : RT_SCHED_LOWEST_PRIORITY, 1, 0)) {
			*thread = &cookie->task;
			rt_typed_sem_init(&cookie->sem, 0, BIN_SEM | FIFO_Q);
			rt_task_resume(&cookie->task);
        		return 0;
		}
	}
	rt_free(cookie);
       	return ENOMEM;
}

static inline int pthread_yield_rt(void)
{
	rt_task_yield();
	return 0;
}

static inline void pthread_exit_rt(void *retval)
{
	RT_TASK *rt_task;
	rt_task = rt_whoami();
	rt_sem_broadcast((SEM *)(rt_task + 1));
	rt_sem_delete((SEM *)(rt_task + 1));
	rt_task_suspend(rt_task);
}

static inline int pthread_join_rt(pthread_t thread, void **thread_return)
{
	int retval1, retval2;
	if (rt_whoami()->priority != RT_SCHED_LINUX_PRIORITY)
		retval1 = rt_sem_wait((SEM *)(thread + 1));
	else {
		while ((retval1 = rt_sem_wait_if((SEM *)(thread + 1))) <= 0) {
			set_current_state(TASK_INTERRUPTIBLE);
			schedule_timeout(HZ/10);
		}
	}
	if (retval1 != 0xFFFF)
		retval1 = 0;
	retval2 = rt_task_delete(thread);
	rt_free(thread);
	return (retval1) ? retval1 : retval2;
}

static inline int pthread_cancel_rt(pthread_t thread)
{
	int retval;
	if (!thread) {
		thread = rt_whoami();
	}
	retval = rt_task_delete(thread);
	rt_free(thread);
	return retval;
}

static inline int pthread_equal_rt(pthread_t thread1,pthread_t thread2)
{
	return thread1 == thread2;
}

static inline pthread_t pthread_self_rt(void)
{
	return rt_whoami();
}

static inline int pthread_attr_init_rt(pthread_attr_t *attr)
{
	attr->stacksize     = STACK_SIZE;
	attr->policy        = SCHED_FIFO;
	attr->rr_quantum_ns = RR_QUANTUM_NS;
	attr->priority      = 1;
	return 0;
}

static inline int pthread_attr_destroy_rt(pthread_attr_t *attr)
{
	return 0;
}

static inline int pthread_attr_setschedparam_rt(pthread_attr_t *attr, const struct sched_param *param)
{
	if(param->sched_priority < MIN_PRIO || param->sched_priority > MAX_PRIO) {
		return(EINVAL);
	}
	attr->priority = MAX_PRIO - param->sched_priority;
	return 0;
}

static inline int pthread_attr_getschedparam_rt(const pthread_attr_t *attr, struct sched_param *param)
{
	param->sched_priority = MAX_PRIO - attr->priority;
	return 0;
}

static inline int pthread_attr_setschedpolicy_rt(pthread_attr_t *attr, int policy)
{
	if(policy != SCHED_FIFO && policy != SCHED_RR) {
		return EINVAL;
	}
	if ((attr->policy = policy) == SCHED_RR) {
		rt_set_sched_policy(rt_whoami(), SCHED_RR, attr->rr_quantum_ns);
	}
	return 0;
}


static inline int pthread_attr_getschedpolicy_rt(const pthread_attr_t *attr, int *policy)
{
	*policy = attr->policy;
	return 0;
}

static inline int pthread_attr_setschedrr_rt(pthread_attr_t *attr, int rr_quantum_ns)
{
	attr->rr_quantum_ns = rr_quantum_ns;
	return 0;
}


static inline int pthread_attr_getschedrr_rt(const pthread_attr_t *attr, int *rr_quantum_ns)
{
	*rr_quantum_ns = attr->rr_quantum_ns;
	return 0;
}

static inline int pthread_attr_setstacksize_rt(pthread_attr_t *attr, int stacksize)
{
	attr->stacksize = stacksize;
	return 0;
}

static inline int pthread_attr_getstacksize_rt(const pthread_attr_t *attr, int *stacksize)
{
	*stacksize = attr->stacksize;
	return 0;
}

static inline int pthread_attr_setstack_rt(pthread_attr_t *attr, void *stackaddr, int stacksize)
{
	attr->stacksize = stacksize;
	return 0;
}

static inline int pthread_attr_getstack_rt(const pthread_attr_t *attr, void **stackaddr, int *stacksize)
{
	*stacksize = attr->stacksize;
	return 0;
}

static inline void pthread_testcancel_rt(void)
{
	rt_task_delete(rt_whoami());
	pthread_exit_rt(NULL);
}

static inline void clock_gettime_rt(int clockid, struct timespec *current_time)
{
	count2timespec(rt_get_time(), current_time);
}

static inline int nanosleep_rt(const struct timespec *rqtp, struct timespec *rmtp)
{
        RTIME expire;
        if (rqtp->tv_nsec >= 1000000000L || rqtp->tv_nsec < 0 || rqtp->tv_sec <
0) {
                return -EINVAL;
        }
        rt_sleep_until(expire = rt_get_time() + timespec2count(rqtp));
        if ((expire -= rt_get_time()) > 0) {
                if (rmtp) {
                        count2timespec(expire, rmtp);
                }
                return -EINTR;
        }
        return 0;
}

/*
 * DO NOTHING FUNCTIONS
 */

#define pthread_mutexattr_init_rt(attr)
#define pthread_mutexattr_destroy_rt(attr)
#define pthread_mutexattr_getpshared_rt(attr, pshared)
#define pthread_mutexattr_setpshared_rt(attr, pshared)
#define pthread_mutexattr_settype_rt(attr, kind)
#define pthread_mutexattr_gettype_rt(attr, kind)

#define pthread_condattr_init_rt(attr)
#define pthread_condattr_destroy_rt(attr)
#define pthread_condattr_getpshared_rt(attr, pshared)
#define pthread_condattr_setpshared_rt(attr, pshared)

#define pthread_barrierattr_getpshared_rt(attr, pshared)
#define pthread_barrierattr_setpshared_rt(attr, pshared)
#define pthread_barrierattr_getpshared_rt(attr, pshared)
#define pthread_barrierattr_setpshared_rt(attr, pshared)

#define pthread_rwlockattr_init(attr)
#define pthread_rwlockattr_destroy(attr)
#define pthread_rwlockattr_getpshared( ttr, pshared)
#define pthread_rwlockattr_setpshared(attr, pshared)
#define pthread_rwlockattr_getkind_np(attr, pref)
#define pthread_rwlockattr_setkind_np(attr, pref)

#define pthread_detach_rt(thread)
#define pthread_attr_setdetachstate_rt(attr, detachstate)
#define pthread_attr_getdetachstate_rt(attr, detachstate)
#define pthread_setconcurrency_rt(level)
#define pthread_getconcurrency_rt()
#define pthread_attr_setinheritsched_rt(attr, inherit)
#define pthread_attr_getinheritsched_rt(attr, inherit)
#define pthread_attr_setscope_rt(attr, scope)
#define pthread_attr_getscope_rt(attr, scope)
#define pthread_attr_setguardsize_rt(attr, guardsize) 
#define pthread_attr_getguardsize_rt(attr, guardsize)
#define pthread_attr_setstackaddr_rt(attr, stackaddr)
#define pthread_attr_getstackaddr_rt(attr, stackaddr) 
#define pthread_setcancelstate_rt(state, oldstate)
#define pthread_setcanceltype_rt(type, oldtype)

#ifdef __cplusplus
}
#endif /* __cplusplus */

#else  /* !__KERNEL__ */

#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <semaphore.h>
#include <pthread.h>
#include <stdlib.h>

struct task_struct;

#undef  SEM_VALUE_MAX 
#define SEM_VALUE_MAX  (SEM_TIMOUT - 1)
#define SEM_BINARY     (0x7FFFFFFF)

#include <asm/rtai_atomic.h>
#include <rtai_sem.h>

/*
 * SUPPORT STUFF
 */

static inline int MAKE_SOFT(void)
{
	if (rt_is_hard_real_time(rt_buddy())) {
		rt_make_soft_real_time();
		return 1;
	}
	return 0;
}

#define MAKE_HARD(hs)  do { if (hs) rt_make_hard_real_time(); } while (0)

#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

RTAI_PROTO(void, count2timespec,(RTIME rt, struct timespec *t))
{
	t->tv_sec = (rt = count2nano(rt))/1000000000;
	t->tv_nsec = rt - t->tv_sec*1000000000LL;
}

RTAI_PROTO(void, nanos2timespec,(RTIME rt, struct timespec *t))
{
	t->tv_sec = rt/1000000000;
	t->tv_nsec = rt - t->tv_sec*1000000000LL;
}

RTAI_PROTO(RTIME, timespec2count,(const struct timespec *t))
{
	return nano2count(t->tv_sec*1000000000LL + t->tv_nsec);
}

RTAI_PROTO(RTIME, timespec2nanos,(const struct timespec *t))
{
	return t->tv_sec*1000000000LL + t->tv_nsec;
}

/*
 * FUNCTIONS MADE SAFELY USABLE IN HARD REAL TIME, BUT BREAKING HARD REAL TIME
 */

RTAI_PROTO(sem_t *,sem_open_rt,(const char *name, int oflags, int value, int type))
{
	int hs, fd;
	sem_t *sem;
	hs = MAKE_SOFT();
	if ((fd = open(name, O_RDONLY)) > 0) {
		read(fd, &sem, sizeof(int));
		close(fd); 
        	atomic_inc((atomic_t *)(&((int *)sem)[1]));
	} else {
	        struct { int name, value, type; } arg = { nam2num(name), value, (type == SEM_BINARY ? BIN_SEM : CNT_SEM) | PRIO_Q };
		sem = (sem_t *)malloc(sizeof(sem_t));
	        if ((((int *)sem)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_SEM_INIT, &arg).i[LOW]) && (fd = open(name, O_WRONLY | O_CREAT))) {
			write(fd, &sem, sizeof(int));
			close(fd); 
        		((int *)sem)[1] = 1;
		} else {
			free(sem);
			sem = 0;
		}
	}
	MAKE_HARD(hs);
	return sem;
}

RTAI_PROTO(int, sem_init_rt,(sem_t *sem, int pshared, unsigned int value))
{
	int hs;
	if (value <= SEM_VALUE_MAX) {
	        struct { int name, value, type; } arg = { rt_get_name(0), value, (pshared == SEM_BINARY ? BIN_SEM : CNT_SEM) | PRIO_Q };
		hs = MAKE_SOFT();
		((int *)sem)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_SEM_INIT, &arg).i[LOW];
       		((int *)sem)[1] = 0;
		MAKE_HARD(hs);
		return 0;
	}
	errno = EINVAL;
	return -1;
}

RTAI_PROTO(int, sem_close_rt,(sem_t *sem))
{
	int hs, cnt;
	char name[7];
	struct { void *sem; } arg = { ((void **)sem)[0] };
	if (rtai_lxrt(BIDX, SIZARG, SEM_WAIT_IF, &arg).i[LOW] < 0) {
		errno = EBUSY;
		return -1;
	}
	cnt = ((int *)sem)[1];
	if (!cnt || (cnt && atomic_dec_and_test((atomic_t *)&((int *)sem)[1]))) {
		hs = MAKE_SOFT();
		num2nam(rt_get_name(((void **)sem)[0]), name);
		rtai_lxrt(BIDX, SIZARG, LXRT_SEM_DELETE, &arg);
	        if (cnt) {
			unlink(name);
			free((void *)sem);
		}
		MAKE_HARD(hs);
	}
	return 0;
}

RTAI_PROTO(int, sem_destroy_rt,(sem_t *sem))
{
	return sem_close_rt(sem);
}

RTAI_PROTO(int, pthread_create_rt,(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = pthread_create(thread, attr, start_routine, arg);
	MAKE_HARD(hs);
	return ret;
}

RTAI_PROTO(int, pthread_cancel_rt,(pthread_t thread))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = pthread_cancel(thread);
	MAKE_HARD(hs);
	return ret;
}

#define pthread_cleanup_push_rt(routine, arg) \
do { \
	{\
		int __hs_hs_hs__; \
		__hs_hs_hs__ = MAKE_SOFT(); \
		pthread_cleanup_push(routine, arg); \
		MAKE_HARD(__hs_hs_hs__);
	
#define pthread_cleanup_pop_rt(execute) \
		__hs_hs_hs__ = MAKE_SOFT(); \
		pthread_cleanup_pop(execute); \
		MAKE_HARD(__hs_hs_hs__); \
	} \
} while (0)

#define pthread_cleanup_push_defer_rt(routine, arg) \
do { \
	{\
		int __hs_hs_hs__; \
		__hs_hs_hs__ = MAKE_SOFT(); \
		pthread_cleanup_push_defer_np(routine, arg); \
		MAKE_HARD(__hs_hs_hs__);

#define pthread_cleanup_pop_restore_rt(execute) \
		__hs_hs_hs__ = MAKE_SOFT(); \
		pthread_cleanup_pop_restore_np(execute); \
		MAKE_HARD(__hs_hs_hs__); \
	} \
} while (0)

RTAI_PROTO(int, pthread_sigmask_rt,(int how, const sigset_t *newmask, sigset_t *oldmask))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = pthread_sigmask(how, newmask, oldmask);
	MAKE_HARD(hs);
	return ret;
}

RTAI_PROTO(int, pthread_kill_rt,(pthread_t thread, int signo))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = pthread_kill(thread, signo);
	MAKE_HARD(hs);
	return ret;
}


RTAI_PROTO(int, sigwait_rt,(const sigset_t *set, int *sig))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = sigwait(set, sig);
	MAKE_HARD(hs);
	return ret;
}

RTAI_PROTO(pthread_mutex_t *, pthread_mutex_open_rt,(const char *name))
{
	int hs, fd;
	pthread_mutex_t *mutex;
	hs = MAKE_SOFT();
	if ((fd = open(name, O_RDONLY)) > 0) {
		read(fd, &mutex, sizeof(int));
		close(fd); 
        	atomic_inc((atomic_t *)(&((int *)mutex)[1]));
	} else {
	        struct { int name, value, type; } arg = { nam2num(name), 1, RES_SEM };
		mutex = (pthread_mutex_t *)malloc(sizeof(pthread_mutex_t));
        	if ((((int *)mutex)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_SEM_INIT, &arg).i[LOW]) && (fd = open(name, O_WRONLY | O_CREAT))) {
			write(fd, &mutex, sizeof(int));
			close(fd); 
		        ((int *)mutex)[1] = 1;
		} else {
			free(mutex);
			mutex = 0;
		}
	}
	MAKE_HARD(hs);
	return mutex;
}

RTAI_PROTO(int, pthread_mutex_init_rt,(pthread_mutex_t *mutex, const pthread_mutexattr_t *mutexattr))
{
	int hs;
	struct { int name, value, type; } arg = { rt_get_name(0), 1, RES_SEM };
	hs = MAKE_SOFT();
	((int *)mutex)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_SEM_INIT, &arg).i[LOW];
        ((int *)mutex)[1] = 0;
	MAKE_HARD(hs);
	return 0;
}

RTAI_PROTO(int, pthread_mutex_close_rt,(pthread_mutex_t *mutex))
{
	int hs, cnt;
	char name[7];
	struct { void *sem; } arg = { ((void **)mutex)[0] };
	if (rtai_lxrt(BIDX, SIZARG, SEM_WAIT_IF, &arg).i[LOW] < 0) {
		return EBUSY;
	}
	cnt = ((int *)mutex)[1];
	if (!cnt || (cnt && atomic_dec_and_test((atomic_t *)&((int *)mutex)[1]))) {
		hs = MAKE_SOFT();
		num2nam(rt_get_name(((void **)mutex)[0]), name);
		rtai_lxrt(BIDX, SIZARG, LXRT_SEM_DELETE, &arg);
	        if (cnt) {
			unlink(name);
			free((void *)mutex);
		}
		MAKE_HARD(hs);
	}
	return 0;
}

RTAI_PROTO(int, pthread_mutex_destroy_rt,(pthread_mutex_t *mutex))
{
	return pthread_mutex_close_rt(mutex);
}

RTAI_PROTO(pthread_cond_t *, pthread_cond_open_rt,(const char *name))
{
	return (pthread_cond_t *)sem_open_rt(name, 0, 0, SEM_BINARY);
}

RTAI_PROTO(int, pthread_cond_init_rt,(pthread_cond_t *cond, pthread_condattr_t *cond_attr))
{
	return sem_init_rt((sem_t *)cond, SEM_BINARY, 0);
}

RTAI_PROTO(int, pthread_cond_destroy_rt,(pthread_cond_t *cond))
{
	return sem_close_rt((sem_t *)cond);
}

RTAI_PROTO(int, pthread_cond_close_rt,(pthread_cond_t *cond))
{
	return sem_close_rt((sem_t *)cond);
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(pthread_barrier_t *, pthread_barrier_open_rt,(const char *name, unsigned int count))
{
	return (pthread_barrier_t *)sem_open_rt(name, 0, count, 0);
}

RTAI_PROTO(int, pthread_barrier_init_rt,(pthread_barrier_t *barrier, const pthread_barrierattr_t *attr, unsigned int count))
{
	return sem_init_rt((sem_t *)barrier, 0, count);
}

RTAI_PROTO(int, pthread_barrier_destroy_rt,(pthread_barrier_t *barrier))
{
	return sem_close_rt((sem_t *)barrier);
}

RTAI_PROTO(int, pthread_barrier_close_rt,(pthread_barrier_t *barrier))
{
	return sem_close_rt((sem_t *)barrier);
}
#endif

/*
 * DO NOTHING FUNCTIONS (IN RTAI HARD REAL TIME)
 */

#define pthread_attr_setdetachstate_rt(attr, detachstate)
#define pthread_detach_rt(thread)
#define pthread_getconcurrency_rt()
#define pthread_setconcurrency_rt(level)

#define pthread_mutexattr_init_rt(attr)
#define pthread_mutexattr_destroy_rt(attr)
#define pthread_mutexattr_getpshared_rt(attr, pshared)
#define pthread_mutexattr_setpshared_rt(attr, pshared)
#define pthread_mutexattr_settype_rt(attr, kind)
#define pthread_mutexattr_gettype_rt(attr, kind)

#define pthread_condattr_init_rt(attr)
#define pthread_condattr_destroy_rt(attr)
#define pthread_condattr_getpshared_rt(attr, pshared)
#define pthread_condattr_setpshared_rt(attr, pshared)
#ifdef __USE_XOPEN2K
#define pthread_barrierattr_getpshared_rt(attr, pshared)
#define pthread_barrierattr_setpshared_rt(attr, pshared)
#define pthread_barrierattr_getpshared_rt(attr, pshared)
#define pthread_barrierattr_setpshared_rt(attr, pshared)
#endif
#define pthread_rwlockattr_init(attr)
#define pthread_rwlockattr_destroy(attr)
#define pthread_rwlockattr_getpshared( ttr, pshared)
#define pthread_rwlockattr_setpshared(attr, pshared)
#define pthread_rwlockattr_getkind_np(attr, pref)
#define pthread_rwlockattr_setkind_np(attr, pref)

/*
 * FUNCTIONS (LIKELY) SAFELY USABLE IN HARD REAL TIME "AS THEY ARE", 
 * BECAUSE MAKE SENSE IN THE INITIALIZATION PHASE ONLY, I.E. BEFORE 
 * GOING HARD REAL TIME
 */

#define pthread_self_rt                  pthread_self
#define pthread_equal_rt                 pthread_equal
#define pthread_attr_init_rt             pthread_attr_init      
#define pthread_attr_destroy_rt          pthread_attr_destroy
#define pthread_attr_getdetachstate_rt   pthread_attr_getdetachstate
#define pthread_attr_setschedpolicy_rt   pthread_attr_setschedpolicy
#define pthread_attr_getschedpolicy_rt   pthread_attr_getschedpolicy 
#define pthread_attr_setschedparam_rt    pthread_attr_setschedparam
#define pthread_attr_getschedparam_rt    pthread_attr_getschedparam
#define pthread_attr_setinheritsched_rt  pthread_attr_setinheritsched
#define pthread_attr_getinheritsched_rt  pthread_attr_getinheritsched
#define pthread_attr_setscope_rt         pthread_attr_setscope
#define pthread_attr_getscope_rt         pthread_attr_getscope
#ifdef __USE_UNIX98
#define pthread_attr_setguardsize_rt     pthread_attr_setguardsize
#define pthread_attr_getguardsize_rt     pthread_attr_getguardsize
#endif
#define pthread_attr_setstackaddr_rt     pthread_attr_setstackaddr
#define pthread_attr_getstackaddr_rt     pthread_attr_getstackaddr
#ifdef __USE_XOPEN2K
#define pthread_attr_setstack_rt         pthread_attr_setstack
#define pthread_attr_getstack_rt         pthread_attr_getstack
#endif
#define pthread_attr_setstacksize_rt     pthread_attr_setstacksize
#define pthread_attr_getstacksize_rt     pthread_attr_getstacksize

/*
 * WORKING FUNCTIONS USABLE IN HARD REAL TIME, THIS IS THE REAL STUFF
 */

#define pthread_setcancelstate_rt  pthread_setcancelstate
#define pthread_setcanceltype_rt   pthread_setcanceltype

RTAI_PROTO(void, pthread_testcancel_rt,(void))
{
	int oldtype, oldstate;
	pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &oldstate);
	pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, &oldtype);
	if (oldstate != PTHREAD_CANCEL_DISABLE && oldtype != PTHREAD_CANCEL_DEFERRED) {
		MAKE_SOFT();
		rt_task_delete(rt_buddy());
		pthread_exit(NULL);
	}
	pthread_setcanceltype(oldtype, &oldtype);
	pthread_setcancelstate(oldstate, &oldstate);
}

extern int pthread_yield (void);
RTAI_PROTO(int, pthread_yield_rt,(void))
{
	if (rt_is_hard_real_time(rt_buddy())) {
		struct { unsigned long dummy; } arg;
		rtai_lxrt(BIDX, SIZARG, YIELD, &arg);
		return 0;
	}
	return pthread_yield();
}

RTAI_PROTO(void, pthread_exit_rt,(void *retval))
{
	MAKE_SOFT();
	rt_task_delete(rt_buddy());
	pthread_exit(retval);
}

RTAI_PROTO(int, pthread_join_rt,(pthread_t thread, void **thread_return))
{
	int hs, ret;
	hs = MAKE_SOFT();
	ret = pthread_join(thread, thread_return);
	MAKE_HARD(hs);
	return ret;
}

RTAI_PROTO(int, sem_wait_rt,(sem_t *sem))
{
	struct { void *sem; } arg = { ((void **)sem)[0] };
	rtai_lxrt(BIDX, SIZARG, SEM_WAIT, &arg);
	return 0;
}

RTAI_PROTO(int, sem_trywait_rt,(sem_t *sem))
{
	struct { void *sem; } arg = { ((void **)sem)[0] };
	if (rtai_lxrt(BIDX, SIZARG, SEM_WAIT_IF, &arg).i[LOW] > 0) {
		return 0;
	}
	errno = EAGAIN;
	return -1;
}

RTAI_PROTO(int, sem_timedwait_rt,(sem_t *sem, const struct timespec *abstime))
{
	struct { void *sem; RTIME until; } arg = { ((void **)sem)[0], timespec2count(abstime) };
	return rtai_lxrt(BIDX, SIZARG, SEM_WAIT_UNTIL, &arg).i[LOW] < SEM_VALUE_MAX ? 0 : ETIMEDOUT;
}

RTAI_PROTO(int, sem_post_rt,(sem_t *sem))
{
	struct { void *sem; } arg = { ((void **)sem)[0] };
	return rtai_lxrt(BIDX, SIZARG, SEM_SIGNAL, &arg).i[LOW];
}

RTAI_PROTO(int, sem_getvalue_rt,(sem_t *sem, int *sval))
{
	struct { void *sem; } arg = { ((void **)sem)[0] };
	*sval = rtai_lxrt(BIDX, SIZARG, SEM_COUNT, &arg).i[LOW];
	return 0;
}

RTAI_PROTO(int, pthread_mutex_lock_rt,(pthread_mutex_t *mutex))
{
	return sem_wait_rt((sem_t *)mutex);
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(int, pthread_mutex_timedlock_rt,(pthread_mutex_t *mutex, const struct timespec *abstime))
{
	return sem_timedwait_rt((sem_t *)mutex, abstime);
}
#endif

RTAI_PROTO(int, pthread_mutex_trylock_rt,(pthread_mutex_t *mutex))
{
	return sem_trywait_rt((sem_t *)mutex);
}

RTAI_PROTO(int, pthread_mutex_unlock_rt,(pthread_mutex_t *mutex))
{
	return sem_post_rt((sem_t *)mutex);
}

RTAI_PROTO(int, pthread_cond_signal_rt,(pthread_cond_t *cond))
{
	struct { void *cond; } arg = { ((void **)cond)[0] };
	return rtai_lxrt(BIDX, SIZARG, COND_SIGNAL, &arg).i[LOW];
}

RTAI_PROTO(int, pthread_cond_broadcast_rt,(pthread_cond_t *cond))
{
	struct { void *cond; } arg = { ((void **)cond)[0] };
	return rtai_lxrt(BIDX, SIZARG, SEM_BROADCAST, &arg).i[LOW];
}

RTAI_PROTO(int, pthread_cond_wait_rt,(pthread_cond_t *cond, pthread_mutex_t *mutex))
{
	struct { void *cond; void *mutex; } arg = { ((void **)cond)[0], ((void **)mutex)[0] };
	return rtai_lxrt(BIDX, SIZARG, COND_WAIT, &arg).i[LOW];
}

RTAI_PROTO(int, pthread_cond_timedwait_rt,(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime))
{
	struct { void *cond; void *mutex; RTIME time; } arg = { ((void **)cond)[0], ((void **)mutex)[0], timespec2count(abstime) };
	return rtai_lxrt(BIDX, SIZARG, COND_WAIT_UNTIL, &arg).i[LOW] < SEM_TIMOUT ? 0 : -ETIMEDOUT;
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(int, pthread_barrier_wait_rt,(pthread_barrier_t *barrier))
{
	struct { void *sem; } arg = { ((void **)barrier)[0] };
	rtai_lxrt(BIDX, SIZARG, SEM_WAIT_BARRIER, &arg);
	return 0;
}
#endif

#ifdef __USE_UNIX98
RTAI_PROTO(pthread_rwlock_t *, pthread_rwlock_open_rt,(const char *name))
{
	int hs, fd;
	pthread_rwlock_t *rwlock;
	hs = MAKE_SOFT();
	if ((fd = open(name, O_RDONLY)) > 0) {
		read(fd, &rwlock, sizeof(int));
		close(fd); 
        	atomic_inc((atomic_t *)(&((int *)rwlock)[1]));
	} else {
	        struct { int name, value, type; } arg = { nam2num(name), 1, RES_SEM };
		rwlock = (pthread_rwlock_t *)malloc(sizeof(pthread_rwlock_t));
        	if ((((int *)rwlock)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_RWL_INIT, &arg).i[LOW]) && (fd = open(name, O_WRONLY | O_CREAT))) {
			write(fd, &rwlock, sizeof(int));
			close(fd); 
		        ((int *)rwlock)[1] = 1;
		} else {
			free(rwlock);
			rwlock = 0;
		}
	}
	MAKE_HARD(hs);
	return rwlock;
}
#endif /* __USE_UNIX98 */

RTAI_PROTO(int, pthread_rwlock_init_rt,(pthread_rwlock_t *rwlock, pthread_rwlockattr_t *attr))
{
	int hs;
	struct { int name; } arg = { rt_get_name(0) };
	hs = MAKE_SOFT();
	((int *)rwlock)[0] = rtai_lxrt(BIDX, SIZARG, LXRT_RWL_INIT, &arg).i[LOW];
        ((int *)rwlock)[1] = 0;
	MAKE_HARD(hs);
	return 0;
}

RTAI_PROTO(int, pthread_rwlock_close_rt,(pthread_rwlock_t *rwlock))
{
	int hs, cnt;
	char name[7];
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	if (rtai_lxrt(BIDX, SIZARG, RWL_WRLOCK_IF, &arg).i[LOW] < 0) {
		return EBUSY;
	} else {
		rtai_lxrt(BIDX, SIZARG, RWL_UNLOCK, &arg);
		if (rtai_lxrt(BIDX, SIZARG, RWL_RDLOCK_IF, &arg).i[LOW] < 0) {
			return EBUSY;
		}
		rtai_lxrt(BIDX, SIZARG, RWL_UNLOCK, &arg);
	}
	cnt = ((int *)rwlock)[1];
	if (!cnt || (cnt && atomic_dec_and_test((atomic_t *)&((int *)rwlock)[1]))) {
		hs = MAKE_SOFT();
		num2nam(rt_get_name(((void **)rwlock)[0]), name);
		rtai_lxrt(BIDX, SIZARG, LXRT_RWL_DELETE, &arg);
	        if (cnt) {
			unlink(name);
			free((void *)rwlock);
		}
		MAKE_HARD(hs);
	}
	return 0;
}

RTAI_PROTO(int, pthread_rwlock_destroy_rt,(pthread_rwlock_t *rwlock))
{
	return pthread_rwlock_close_rt(rwlock);
}

RTAI_PROTO(int, pthread_rwlock_rdlock_rt,(pthread_rwlock_t *rwlock))
{
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	return rtai_lxrt(BIDX, SIZARG, RWL_RDLOCK, &arg).i[LOW];
}

RTAI_PROTO(int, pthread_rwlock_tryrdlock_rt,(pthread_rwlock_t *rwlock))
{
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	return rtai_lxrt(BIDX, SIZARG, RWL_RDLOCK_IF, &arg).i[LOW];
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(int, pthread_rwlock_timedrdlock_rt,(pthread_rwlock_t *rwlock, struct timespec *abstime))
{
	struct { void *rwlock; RTIME time; } arg = { ((void **)rwlock)[0], timespec2count(abstime) };
	return rtai_lxrt(BIDX, SIZARG, RWL_RDLOCK_UNTIL, &arg).i[LOW];
}
#endif

RTAI_PROTO(int, pthread_rwlock_wrlock_rt,(pthread_rwlock_t *rwlock))
{
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	return rtai_lxrt(BIDX, SIZARG, RWL_WRLOCK, &arg).i[LOW];
}

RTAI_PROTO(int, pthread_rwlock_trywrlock_rt,(pthread_rwlock_t *rwlock))
{
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	return rtai_lxrt(BIDX, SIZARG, RWL_WRLOCK_IF, &arg).i[LOW];
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(int, pthread_rwlock_timedwrlock_rt,(pthread_rwlock_t *rwlock, struct timespec *abstime))
{
	struct { void *rwlock; RTIME time; } arg = { ((void **)rwlock)[0], timespec2count(abstime) };
	return rtai_lxrt(BIDX, SIZARG, RWL_WRLOCK_UNTIL, &arg).i[LOW];
}
#endif

RTAI_PROTO(int, pthread_rwlock_unlock_rt,(pthread_rwlock_t *rwlock))
{
	struct { void *rwlock; } arg = { ((void **)rwlock)[0] };
	return rtai_lxrt(BIDX, SIZARG, RWL_UNLOCK, &arg).i[LOW];
}

#ifdef __USE_XOPEN2K
RTAI_PROTO(int, pthread_spin_init_rt,(pthread_spinlock_t *lock))
{
	return (((int *)lock)[0] = 0);
}

RTAI_PROTO(int, pthread_spin_destroy_rt,(pthread_spinlock_t *lock))
{
	return ((int *)lock)[0] = 0;
}

RTAI_PROTO(int, pthread_spin_lock_rt,(pthread_spinlock_t *lock))
{
	while (atomic_cmpxchg(&lock, 0, 1));
	return 0;
}

RTAI_PROTO(int, pthread_spin_trylock_rt,(pthread_spinlock_t *lock))
{
	if (atomic_cmpxchg(&lock, 0, 1)) {
		return EAGAIN;
	}
	return 0;
}

RTAI_PROTO(int, pthread_spin_unlock_rt,(pthread_spinlock_t *lock))
{
	return ((int *)lock)[0] = 0;
}
#endif

RTAI_PROTO(void, clock_gettime_rt,(int clockid, struct timespec *current_time))
{
	count2timespec(rt_get_time(), current_time);
}

RTAI_PROTO(int, nanosleep_rt,(const struct timespec *rqtp, struct timespec *rmtp))
{
	RTIME expire;
	if (rqtp->tv_nsec >= 1000000000L || rqtp->tv_nsec < 0 || rqtp->tv_sec < 0) {
		return -EINVAL;
	}
	rt_sleep_until(expire = rt_get_time() + timespec2count(rqtp));
	if ((expire -= rt_get_time()) > 0) {
		if (rmtp) {
			count2timespec(expire, rmtp);
		}
		return -EINTR;
	}
        return 0;
}

#ifdef __cplusplus
}
#endif /* __cplusplus */

#endif /* !__KERNEL__ */

#endif /* !_RTAI_POSIX_H_ */