/*
 * pqueues interface for Real Time Linux.
 *
 * Copyright (©) 1999 Zentropic Computing, All rights reserved
 *  
 * Authors:             Trevor Woolven (trevw@zentropix.com)
 *
 * Original date:       Thu 15 Jul 1999
 *
 * 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 of the
 * License, or (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * Timed services extension and user space integration for RTAI by
 * Paolo Mantegazza <mantegazza@aero.polimi.it>.
 * 2005, cleaned and revised Paolo Mantegazza <mantegazza@aero.polimi.it>.
 *
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/stat.h>
#include <asm/uaccess.h>
#ifdef CONFIG_PROC_FS
#include <linux/proc_fs.h>
extern struct proc_dir_entry *rtai_proc_root;
#endif
#include <rtai_schedcore.h>
#include <rtai_proc_fs.h>
#include <rtai_signal.h>

MODULE_LICENSE("GPL");

#define mq_cond_t                   SEM
#define mq_mutex_t                  SEM
#define mq_mutex_init(mutex, attr)  rt_typed_sem_init(mutex, 1, BIN_SEM | PRIO_Q)
#define mq_mutex_unlock             rt_sem_signal
#define mq_mutex_lock(mutex) \
	do { \
		if (abs(rt_sem_wait(mutex)) >= RTE_LOWERR) { \
			return -EBADF; \
		} \
	} while (0)		
#define mq_mutex_timedlock(mutex, abstime) \
	do { \
		RTIME t = timespec2count(abstime); \
		int ret; \
		if (abs(ret = rt_sem_wait_until(mutex, t)) >= RTE_LOWERR) { \
			return ret == RTE_TIMOUT ? -ETIMEDOUT : -EBADF; \
		} \
	} while (0)
#define mq_mutex_trylock            rt_sem_wait_if
#define mq_mutex_destroy            rt_sem_delete
#define mq_cond_init(cond, attr)    rt_sem_init(cond, 0)
#define mq_cond_wait(cond, mutex) \
	do { \
		rt_sem_signal(mutex); \
		if (abs(rt_sem_wait(cond)) >= RTE_LOWERR) { \
			return -EBADF; \
		} \
 		if (abs(rt_sem_wait(mutex)) >= RTE_LOWERR) { \
			rt_sem_signal(cond); \
			return -EBADF; \
		} \
	} while (0)
#define mq_cond_timedwait(cond, mutex, abstime) \
	do { \
		RTIME t = timespec2count(abstime); \
		int ret; \
		rt_sem_signal(mutex); \
		if (abs(ret = rt_sem_wait_until(cond, t)) >= RTE_LOWERR) { \
			return ret == RTE_TIMOUT ? -ETIMEDOUT : -EBADF; \
		} \
		if (abs(ret = rt_sem_wait_until(mutex, t)) >= RTE_LOWERR) { \
			rt_sem_signal(cond); \
			return ret == RTE_TIMOUT ? -ETIMEDOUT : -EBADF; \
		} \
	} while (0)
#define mq_cond_signal              rt_sem_signal
#define mq_cond_destroy             rt_sem_delete

#ifndef OK
#define OK  0
#endif
#ifndef ERROR
#define ERROR  -1
#endif

///////////////////////////////////////////////////////////////////////////////
//      LOCAL DEFINITIONS
///////////////////////////////////////////////////////////////////////////////

#define MAX_RT_TASKS 128

///////////////////////////////////////////////////////////////////////////////
//      PACKAGE GLOBAL DATA
///////////////////////////////////////////////////////////////////////////////

static int num_pqueues = 0;
static struct _pqueue_descr_struct rt_pqueue_descr[MAX_PQUEUES] = {{0}};
static struct _pqueue_access_struct task_pqueue_access[MAX_RT_TASKS] = {{0}};
static MQ_ATTR default_queue_attrs = { MAX_MSGS, MAX_MSGSIZE, MQ_NONBLOCK, 0 };

static mq_mutex_t pqueue_mutex;

///////////////////////////////////////////////////////////////////////////////
//      LOCAL FUNCTIONS
///////////////////////////////////////////////////////////////////////////////

static int name_to_id(char *name)
{
	int ind;
	for (ind = 0; ind < MAX_PQUEUES; ind++) {
		if (rt_pqueue_descr[ind].q_name[0] && !strcmp(rt_pqueue_descr[ind].q_name, name)) {
			return ind;
		}
	} 
	return ERROR;
}


static inline mq_bool_t is_empty(struct queue_control *q)
{
	return !q->attrs.mq_curmsgs;
}


static inline mq_bool_t is_full(struct queue_control *q)
{
	return q->attrs.mq_curmsgs == q->attrs.mq_maxmsg;
}


static inline MSG_HDR* getnode(Q_CTRL *queue)
{
	return queue->attrs.mq_curmsgs < queue->attrs.mq_maxmsg ? queue->nodes[queue->attrs.mq_curmsgs++] : NULL;
}

static inline int freenode(void *node, Q_CTRL *queue)
{
	if (queue->attrs.mq_curmsgs > 0) {
                queue->nodes[--queue->attrs.mq_curmsgs] = node;
                return 0;
        }
        return -EINVAL;
}


static void insert_message(Q_CTRL *q, MSG_HDR *this_msg)
{
//This function finds the appropriate point in a priority queue to
//insert the supplied message. It preserves FIFO order within each
//priority levela and can therefore be used for FIFO queuing policies
//simply by making the priority equal to the supplied message priority

	MSG_HDR *prev, *insertpt;

//Do a quick check in case the message at the back of the queue has
//a higher priority than this one, in which case this one can just
//go at the back of the queue. 
//Remember that Posix priorities increase from 0 to (at least) 32

	if (((MSG_HDR *)q->tail)->priority >= this_msg->priority) {
		((MSG_HDR*)q->tail)->next = this_msg;
		q->tail = this_msg;
	} else {
		prev = insertpt = q->head;
//POSIX queues preserve FIFO ordering of messages within
//a particular priority level
		while (insertpt->priority >= this_msg->priority) {
		        prev = insertpt;
		        insertpt = insertpt->next; 
		}
//We've now found a message (or messages) of equal or lower
//priority than the one we're trying to put onto the queue
 		if (insertpt == q->head) {
			this_msg->next = q->head;
			q->head = this_msg;
		} else {
			this_msg->next = prev->next;
			prev->next = this_msg;
		}   
	}
}

#undef mqueues
#define mqueues system_data_ptr

static mq_bool_t is_blocking(MSG_QUEUE *q)
{
	int q_ind;
	struct _pqueue_access_data *aces;

	aces = ((QUEUE_CTRL)_rt_whoami()->mqueues)->q_access;
	for (q_ind = 0; q_ind < MQ_OPEN_MAX; q_ind++) {
		if (aces[q_ind].q_id == q->q_id) {
			return !(aces[q_ind].oflags & O_NONBLOCK);
		}
	}
	return FALSE;
}


static mq_bool_t can_access(MSG_QUEUE *q, Q_ACCESS access)
{
	RT_TASK *caller = _rt_whoami();

	if (q->owner == caller ? (((access == FOR_READ) && (q->permissions & S_IRUSR)) || ((access == FOR_WRITE) && (q->permissions & S_IWUSR))) : (((access == FOR_READ) && (q->permissions & S_IRGRP)) || ((access == FOR_WRITE) && (q->permissions & S_IWGRP)))) {
		int q_ind;
		struct _pqueue_access_data *aces;
		struct _pqueue_access_struct *task_queue_data_ptr;
		int q_access_flags = 0;

		task_queue_data_ptr = (QUEUE_CTRL)caller->mqueues;
		if (task_queue_data_ptr == NULL) {
			return FALSE;
		}
		aces = task_queue_data_ptr->q_access;
		for (q_ind = 0; q_ind < MQ_OPEN_MAX; q_ind++) {
			if (aces[q_ind].q_id == q->q_id) {
				q_access_flags = aces[q_ind].oflags;
				goto set_mode;
			}
		}
		return FALSE;
set_mode:	if (access == FOR_WRITE) {
			if ((q_access_flags & O_WRONLY) || (q_access_flags & O_RDWR)) {
				return TRUE;
			}
		} else {
			return TRUE;
		}
	}
	return FALSE;
}


static inline void initialise_queue(Q_CTRL *q)
{
	int msg_size, msg_ind;
	void *msg_ptr;

	msg_size = q->attrs.mq_msgsize + sizeof(MSG_HDR);
	msg_ptr = q->base;
	q->nodes = msg_ptr + msg_size*q->attrs.mq_maxmsg; 
	for (msg_ind = 0; msg_ind < q->attrs.mq_maxmsg; msg_ind++) {
		q->nodes[msg_ind] = msg_ptr;
		((MSG_HDR *)msg_ptr)->size = 0;
		((MSG_HDR *)msg_ptr)->priority = MQ_MIN_MSG_PRIORITY;
		((MSG_HDR *)msg_ptr)->next = NULL;
		msg_ptr += msg_size;
	}
}


static void delete_queue(int q_index)
{
	rt_free(rt_pqueue_descr[q_index].data.base);

	rt_pqueue_descr[q_index].owner = NULL;
	rt_pqueue_descr[q_index].open_count = 0;
	strcpy(rt_pqueue_descr[q_index].q_name, "\0");
	rt_pqueue_descr[q_index].q_id = INVALID_PQUEUE;
	rt_pqueue_descr[q_index].data.base = NULL;
	rt_pqueue_descr[q_index].data.head = NULL;
	rt_pqueue_descr[q_index].data.tail = NULL;
	rt_pqueue_descr[q_index].data.attrs = (MQ_ATTR){ 0, 0, 0, 0 };
	rt_pqueue_descr[q_index].permissions = 0;

	mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	mq_mutex_destroy(&rt_pqueue_descr[q_index].mutex);
	mq_cond_destroy(&rt_pqueue_descr[q_index].emp_cond);
	mq_cond_destroy(&rt_pqueue_descr[q_index].full_cond);

	if (num_pqueues > 0) {	
		num_pqueues--;	
	}
}

static void signal_suprt_fun_mq(void *fun_arg)
{			
	struct suprt_fun_arg { RT_TASK *sigtask; RT_TASK *task; mqd_t mq; } arg = *(struct suprt_fun_arg *)fun_arg;
	
	arg.sigtask = RT_CURRENT;
	if (!rt_request_signal_(arg.sigtask, arg.task, (arg.mq + MAXSIGNALS))) {
		while (rt_wait_signal(arg.sigtask, arg.task)) {
			rt_pqueue_descr[arg.mq - 1].notify.data._sigev_un._sigev_thread._function((sigval_t)rt_pqueue_descr[arg.mq - 1].notify.data.sigev_value.sival_ptr);
		}
	} else {
		rt_task_resume(arg.task);
	}
}

int rt_request_signal_mq(mqd_t mq)
{
		RT_TASK *sigtask;
		struct suprt_fun_arg { RT_TASK *sigtask; RT_TASK *task; mqd_t mq; } arg = { NULL, rt_whoami(), mq };
		if ((sigtask = rt_malloc(sizeof(RT_TASK)))) {
			rt_task_init_cpuid(sigtask, (void *)signal_suprt_fun_mq, (long)&arg, SIGNAL_TASK_STACK_SIZE, arg.task->priority, 0, 0, RT_CURRENT->runnable_on_cpus);
			rt_task_resume(sigtask);
			rt_task_suspend(arg.task);
			return arg.task->retval;
		}
	return -EINVAL;
}

///////////////////////////////////////////////////////////////////////////////
//      POSIX MESSAGE QUEUES API
///////////////////////////////////////////////////////////////////////////////

RTAI_SYSCALL_MODE mqd_t _mq_open(char *mq_name, int oflags, mode_t permissions, struct mq_attr *mq_attr, long space)
{
	int q_index, t_index, q_ind;
	int spare_count = 0, first_spare = 0;
	mq_bool_t q_found = FALSE;
	RT_TASK *this_task = _rt_whoami();
	struct _pqueue_access_struct *task_data_ptr;

	task_data_ptr = (QUEUE_CTRL)this_task->mqueues;

	mq_mutex_lock(&pqueue_mutex);
	if ((q_index = name_to_id(mq_name)) >= 0) {
//========================
// OPEN AN EXISTING QUEUE
//========================
		if ((oflags & O_CREAT) && (oflags & O_EXCL)) {
			mq_mutex_unlock(&pqueue_mutex);
			return -EEXIST;
		}
		if (task_data_ptr == NULL) {
			for (t_index = 0; t_index < MAX_RT_TASKS; t_index++) {
				if (task_pqueue_access[t_index].this_task == NULL) {
					task_data_ptr = &(task_pqueue_access[t_index]);
					task_data_ptr->this_task = this_task;
					this_task->mqueues = task_data_ptr;
					break;
				}
			}
			if (t_index == MAX_RT_TASKS) {
				mq_mutex_unlock(&pqueue_mutex);
				return -ENOMEM;
			}
		}
	//Now record that this task has opened this queue and
	//the access permissions required
	//Check first to see if this task has already opened this queue
	//and while doing so, record the number of spare 'slots' for this
	//task to have further opened queues
	mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
		for (q_ind = 0; q_ind < MQ_OPEN_MAX; q_ind++) {
			if (task_data_ptr->q_access[q_ind].q_id == rt_pqueue_descr[q_index].q_id) { 
				q_found = TRUE;
				break;
			} else if(task_data_ptr->q_access[q_ind].q_id == INVALID_PQUEUE) {
				if (spare_count == 0) {
					first_spare = q_ind;
				}			
				spare_count++;
			}
		}	
	//If the task has not already opened this queue and there are no
	//more available slots, can't do anymore...
		if (!q_found && spare_count == 0) {
			mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
			mq_mutex_unlock(&pqueue_mutex);
			return -EINVAL;
		}
	//Either the queue has already been opened and so we can re-use
	//it's slot, or a new one is being opened in an unused slot
		if (!q_found) {
	    //Open a new one, using the first free slot
			task_data_ptr->n_open_pqueues++;
			q_ind = first_spare;
		}
		task_data_ptr->q_access[q_ind].q_id = rt_pqueue_descr[q_index].q_id;
		task_data_ptr->q_access[q_ind].oflags = oflags;
		mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	} else if (oflags & O_CREAT) {
//================
// CREATE A QUEUE 
//================
		if(num_pqueues >= MAX_PQUEUES) {
			mq_mutex_unlock(&pqueue_mutex);
			return -ENOMEM;
		}
	//Check the size of the name
		if( strlen(mq_name) >= MQ_NAME_MAX) {
			mq_mutex_unlock(&pqueue_mutex);
			return -ENAMETOOLONG;
		}
	//Allocate a task pqueue access structure to this task, if necessary.
	//Otherwise, check that this task has not already opened too many 
	//queues
	//
		if (task_data_ptr == NULL) {
	    //Find a spare task pqueue access slot for this task
			for (t_index = 0; t_index < MAX_RT_TASKS; t_index++) {
				if (task_pqueue_access[t_index].this_task == NULL) {
					task_data_ptr = &task_pqueue_access[t_index];
					task_data_ptr->this_task = this_task;
					this_task->mqueues = task_data_ptr;
					break;
				}
			}
			if (t_index == MAX_RT_TASKS) {
				mq_mutex_unlock(&pqueue_mutex);
				return -ENOMEM;
			}
		} else if (task_data_ptr->n_open_pqueues >= MQ_OPEN_MAX) {
			mq_mutex_unlock(&pqueue_mutex);
			return -EINVAL;
 		}
	//Look for default queue attributes
		if (mq_attr == NULL) {
			mq_attr = &default_queue_attrs;
		}
	//Find a spare descriptor for this queue
		for (q_index = 0; q_index < MAX_PQUEUES; q_index++) {
			if (rt_pqueue_descr[q_index].q_id == INVALID_PQUEUE) {
				int msg_size, queue_size;
				void *mem_ptr;
		//Get memory for the data queue
				msg_size = mq_attr->mq_msgsize + sizeof(MSG_HDR);
				queue_size = (msg_size + sizeof(void *))*mq_attr->mq_maxmsg;
				mem_ptr = rt_malloc(queue_size);
				if(mem_ptr == NULL) {
					mq_mutex_unlock(&pqueue_mutex);
					return -ENOMEM;
				}
				rt_pqueue_descr[q_index].data.base = mem_ptr; 
		//Initialise the Message Queue descriptor
    				rt_pqueue_descr[q_index].owner = this_task;
		    		rt_pqueue_descr[q_index].open_count = 0;
				strcpy(rt_pqueue_descr[q_index].q_name, mq_name);
				rt_pqueue_descr[q_index].q_id = q_index + 1;
				rt_pqueue_descr[q_index].marked_for_deletion = FALSE;
				rt_pqueue_descr[q_index].data.head = 
				rt_pqueue_descr[q_index].data.tail = rt_pqueue_descr[q_index].data.base;
				rt_pqueue_descr[q_index].data.attrs = *(mq_attr);
				rt_pqueue_descr[q_index].data.attrs.mq_curmsgs = 0;
				rt_pqueue_descr[q_index].permissions = permissions;
		//Initialise conditional variables used for blocking
				mq_cond_init(&rt_pqueue_descr[q_index].emp_cond, NULL);
				mq_cond_init(&rt_pqueue_descr[q_index].full_cond, NULL);
				mq_mutex_init(&rt_pqueue_descr[q_index].mutex, NULL);

		//Clear the queue contents
				initialise_queue(&rt_pqueue_descr[q_index].data);
		//Initialise the Task Queue access descriptor
				q_ind = task_data_ptr->n_open_pqueues++;
				task_data_ptr->q_access[q_ind].q_id = q_index + 1;
				task_data_ptr->q_access[q_ind].oflags = oflags;
				break;
			}
        	}
		if(q_index >= MAX_PQUEUES) {
			mq_mutex_unlock(&pqueue_mutex);
			return -EMFILE;
		}
		num_pqueues++;
	} else {
//==============================
// OPENING A NON-EXISTANT QUEUE
//==============================
		mq_mutex_unlock(&pqueue_mutex);
		return -ENOENT;
	}
	
	// Return the message queue's id and mark it as open
	rt_pqueue_descr[q_index].open_count++;
	mq_mutex_unlock(&pqueue_mutex);
	
	// Prepare notify task 
	if ((oflags & O_NOTIFY_NP) && space == 0)	{
		rt_request_signal_mq(rt_pqueue_descr[q_index].q_id);
	}
	
	return (mqd_t)rt_pqueue_descr[q_index].q_id;
}
EXPORT_SYMBOL(_mq_open);

RTAI_SYSCALL_MODE size_t _mq_receive(mqd_t mq, char *msg_buffer, size_t buflen, unsigned int *msgprio, int space)
{
	int q_index = mq - 1, size;
	MQMSG *msg_ptr;
	MSG_QUEUE *q;

	if (q_index < 0 || q_index >= MAX_PQUEUES) { 
		return -EBADF;
	}
	q = &rt_pqueue_descr[q_index];
	if (buflen < q->data.attrs.mq_msgsize) {
		return -EMSGSIZE;
	}
	if (can_access(q, FOR_READ) == FALSE) {
		return -EINVAL;
	}
	if (is_blocking(q)) {
		mq_mutex_lock(&q->mutex);
	} else if (mq_mutex_trylock(&q->mutex) <= 0) {
		return -EAGAIN;
	}
    	while (is_empty(&q->data)) {
		if (is_blocking(q)) {
			mq_cond_wait(&q->emp_cond, &q->mutex);
		} else {
			mq_mutex_unlock(&q->mutex);
			return -EAGAIN;
		}
	}
    	msg_ptr = q->data.head;
        if (msg_ptr->hdr.size <= buflen) {
		size = msg_ptr->hdr.size;
		if (space) {
			memcpy(msg_buffer, &msg_ptr->data, size);
			if (msgprio) {
				*msgprio = msg_ptr->hdr.priority;
			}
		} else {
			rt_copy_to_user(msg_buffer, &msg_ptr->data, size);
			if (msgprio) {
				rt_put_user(msg_ptr->hdr.priority, msgprio);
			}
		}
	} else {
		size = ERROR;
	}
	q->data.head = msg_ptr->hdr.next;
	msg_ptr->hdr.size = 0;
    	msg_ptr->hdr.next = NULL;
    	freenode(msg_ptr, &q->data);
	if(q->data.head == NULL) {
		q->data.head = q->data.tail = q->data.nodes[0];
	}
	mq_cond_signal(&q->full_cond);
	mq_mutex_unlock(&q->mutex);
	return size;
}
EXPORT_SYMBOL(_mq_receive);

RTAI_SYSCALL_MODE size_t _mq_timedreceive(mqd_t mq, char *msg_buffer, size_t buflen, unsigned int *msgprio, const struct timespec *abstime, int space)
{
	int q_index = mq - 1, size;
	MQMSG *msg_ptr;
	MSG_QUEUE *q;
	struct timespec time;
	
	if (q_index < 0 || q_index >= MAX_PQUEUES) { 
		return -EBADF;
	}
	q = &rt_pqueue_descr[q_index];
	if (buflen < q->data.attrs.mq_msgsize) {
		return -EMSGSIZE;
	}
	if (can_access(q, FOR_READ) == FALSE) {
		return -EINVAL;
	}
	if (!space) {
		rt_copy_from_user(&time, abstime, sizeof(struct timespec));
		abstime = &time;
	}
	if (is_blocking(q)) {
		mq_mutex_timedlock(&q->mutex, abstime);
	} else if (mq_mutex_trylock(&q->mutex) <= 0) {
		return -EAGAIN;
	}
	while (is_empty(&q->data)) {
		if (is_blocking(q)) {
			mq_cond_timedwait(&q->emp_cond, &q->mutex, abstime);
		} else {
			return -EAGAIN;
		}
	}
	msg_ptr = q->data.head;
	if (msg_ptr->hdr.size <= buflen) {
		size = msg_ptr->hdr.size;
		if (space) {
			memcpy(msg_buffer, &msg_ptr->data, size);
			if (msgprio) {
				*msgprio = msg_ptr->hdr.priority;
			}
		} else {
			rt_copy_to_user(msg_buffer, &msg_ptr->data, size);
			if (msgprio) {
				rt_put_user(msg_ptr->hdr.priority, msgprio);
			}
		}
	} else {
		size = ERROR;
	}
	q->data.head = msg_ptr->hdr.next;
	msg_ptr->hdr.size = 0;
	msg_ptr->hdr.next = NULL;
    	freenode(msg_ptr, &q->data);
	if(q->data.head == NULL) {
		q->data.head = q->data.tail = q->data.nodes[0];
	}
	mq_cond_signal(&q->full_cond);
	mq_mutex_unlock(&q->mutex);
	return size;
}
EXPORT_SYMBOL(_mq_timedreceive);

RTAI_SYSCALL_MODE int _mq_send(mqd_t mq, const char *msg, size_t msglen, unsigned int msgprio, int space)
{
	int q_index = mq - 1;
	MSG_QUEUE *q;
	MSG_HDR *this_msg;
	mq_bool_t q_was_empty;

	if (q_index < 0 || q_index >= MAX_PQUEUES) { 
		return -EBADF;
	}
	q = &rt_pqueue_descr[q_index];
	if( can_access(q, FOR_WRITE) == FALSE) {
		return -EINVAL;
	}
	if(msgprio > MQ_PRIO_MAX) {
		return -EINVAL;
	}
	if (is_blocking(q)) {
		mq_mutex_lock(&q->mutex);
	} else if (mq_mutex_trylock(&q->mutex) <= 0) {
		return -EAGAIN;
	}
	q_was_empty = is_empty(&q->data);
    while (is_full(&q->data)) {
		if (is_blocking(q)) {
			mq_cond_wait(&q->full_cond, &q->mutex);
		} else {
			mq_mutex_unlock(&q->mutex);
			return -EAGAIN;
		}
       	}
	if( (this_msg = getnode(&q->data)) == NULL) {
		mq_mutex_unlock(&q->mutex);
		return -ENOBUFS;
	}
	if (msglen > q->data.attrs.mq_msgsize) {
		mq_mutex_unlock(&q->mutex);
		return -EMSGSIZE;
	}
	this_msg->size = msglen;
	this_msg->priority = msgprio;
	if (space) {
		memcpy(&((MQMSG *)this_msg)->data, msg, msglen);
	} else {
		rt_copy_from_user(&((MQMSG *)this_msg)->data, msg, msglen);
	}
	insert_message(&q->data, this_msg);
	mq_cond_signal(&q->emp_cond);
	if(q_was_empty && rt_pqueue_descr[q_index].notify.task != NULL) {
		rt_trigger_signal((MAXSIGNALS + mq), rt_pqueue_descr[q_index].notify.task);
		rt_pqueue_descr[q_index].notify.task = NULL;
	}
	mq_mutex_unlock(&q->mutex);
	return msglen;
}
EXPORT_SYMBOL(_mq_send);

RTAI_SYSCALL_MODE int _mq_timedsend(mqd_t mq, const char *msg, size_t msglen, unsigned int msgprio, const struct timespec *abstime, int space)
{
	int q_index = mq - 1;
	MSG_QUEUE *q;
	MSG_HDR *this_msg;
	mq_bool_t q_was_empty;
	struct timespec time;

	if (q_index < 0 || q_index >= MAX_PQUEUES) { 
		return -EBADF;
	}
	q = &rt_pqueue_descr[q_index];
	if (can_access(q, FOR_WRITE) == FALSE) {
		return -EINVAL;
	}
	if (msgprio > MQ_PRIO_MAX) {
		return -EINVAL;
	}			
	if (!space) {
		rt_copy_from_user(&time, abstime, sizeof(struct timespec));
		abstime = &time;
	}
	if (is_blocking(q)) {
		mq_mutex_timedlock(&q->mutex, abstime);
	} else if (mq_mutex_trylock(&q->mutex) <= 0) {
		return -EAGAIN;
	}
	q_was_empty = is_empty(&q->data);
	while (is_full(&q->data)) {
		if (is_blocking(q)) {
			mq_cond_timedwait(&q->full_cond, &q->mutex, abstime);
		} else {
			mq_mutex_unlock(&q->mutex);
			return -EAGAIN;
		}
	}
	if ((this_msg = getnode(&q->data)) == NULL) {
		mq_mutex_unlock(&q->mutex);
		return -ENOBUFS;
	}
	if (msglen > q->data.attrs.mq_msgsize) {
		mq_mutex_unlock(&q->mutex);
		return -EMSGSIZE;
	}
	this_msg->size = msglen;
	this_msg->priority = msgprio;
	if (space) {
		memcpy(&((MQMSG *)this_msg)->data, msg, msglen);
	} else {
		rt_copy_from_user(&((MQMSG *)this_msg)->data, msg, msglen);
	}
	insert_message(&q->data, this_msg);
	mq_cond_signal(&q->emp_cond);
	if (q_was_empty && rt_pqueue_descr[q_index].notify.task != NULL) {
		rt_trigger_signal((MAXSIGNALS + mq), rt_pqueue_descr[q_index].notify.task);
		rt_pqueue_descr[q_index].notify.task = NULL;
	}
	mq_mutex_unlock(&q->mutex);
	return msglen;

}
EXPORT_SYMBOL(_mq_timedsend);

RTAI_SYSCALL_MODE int mq_close(mqd_t mq)
{
	int q_index = mq - 1;
	int q_ind;
	RT_TASK *this_task = _rt_whoami();
	struct _pqueue_access_struct *task_queue_data_ptr;

	if (q_index < 0 || q_index >= MAX_PQUEUES) { 
		return -EINVAL;
	}
	task_queue_data_ptr = (QUEUE_CTRL)this_task->mqueues;
	if (task_queue_data_ptr == NULL ) {
	    return -EINVAL;
	}
	mq_mutex_lock(&pqueue_mutex);
	for (q_ind = 0; q_ind < MQ_OPEN_MAX; q_ind++) {
		if (task_queue_data_ptr->q_access[q_ind].q_id == mq) {
			task_queue_data_ptr->q_access[q_ind].q_id = INVALID_PQUEUE;
			task_queue_data_ptr->q_access[q_ind].usp_notifier = NULL;
			rt_release_signal((mq + MAXSIGNALS), task_queue_data_ptr->this_task);
			task_queue_data_ptr->n_open_pqueues--;
			break;
	  	}
	}
	if (q_ind == MQ_OPEN_MAX) {
		mq_mutex_unlock(&pqueue_mutex);
		return -EINVAL;
	}
	mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
	if (rt_pqueue_descr[q_index].notify.task == this_task) {
		rt_pqueue_descr[q_index].notify.task = NULL;
	}
        if (--rt_pqueue_descr[q_index].open_count <= 0 &&
 	    rt_pqueue_descr[q_index].marked_for_deletion == TRUE ) {
		delete_queue(q_index);
	}
	mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	mq_mutex_unlock(&pqueue_mutex);
        return OK;
}
EXPORT_SYMBOL(mq_close);

RTAI_SYSCALL_MODE int mq_getattr(mqd_t mq, struct mq_attr *attrbuf)
{
	int q_index = mq - 1;

	if (0 <= q_index && q_index < MAX_PQUEUES) { 
		*attrbuf = rt_pqueue_descr[q_index].data.attrs;
		return OK;
	}
	return -EBADF;
}
EXPORT_SYMBOL(mq_getattr);

RTAI_SYSCALL_MODE int mq_setattr(mqd_t mq, const struct mq_attr *new_attrs, struct mq_attr *old_attrs)
{
	int q_index = mq - 1;
	int q_ind;
	RT_TASK *this_task = _rt_whoami();
	struct _pqueue_access_struct *task_queue_data_ptr;

	if (q_index < 0 || q_index >= MAX_PQUEUES) {
		return -EBADF;
	}
	if (old_attrs != NULL) { 
		*old_attrs = rt_pqueue_descr[q_index].data.attrs;
	}
	task_queue_data_ptr = (QUEUE_CTRL)this_task->mqueues;
	if (task_queue_data_ptr == NULL) {
		return -EINVAL;
	}
	for (q_ind = 0; q_ind < MQ_OPEN_MAX; q_ind++) {
		if (task_queue_data_ptr->q_access[q_ind].q_id == mq) {
			if(new_attrs->mq_flags == MQ_NONBLOCK) {
				task_queue_data_ptr->q_access[q_ind].oflags |= O_NONBLOCK;
			} else if (new_attrs->mq_flags == MQ_BLOCK) {
				task_queue_data_ptr->q_access[q_ind].oflags &= ~O_NONBLOCK;
			} else {
				return -EINVAL;
			}
		       	break;
	  	}
	}
	if (q_ind == MQ_OPEN_MAX) {
		return -EINVAL;
	}
	mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
	rt_pqueue_descr[q_index].data.attrs.mq_flags = new_attrs->mq_flags;
	mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	return OK;
}
EXPORT_SYMBOL(mq_setattr);

RTAI_SYSCALL_MODE int mq_reg_usp_notifier(mqd_t mq, RT_TASK *task, struct sigevent *usp_notification)
{
	mq_mutex_lock(&rt_pqueue_descr[mq -1].mutex);
	((QUEUE_CTRL)task->mqueues)->q_access[mq -1].usp_notifier = usp_notification;
	rt_copy_to_user(usp_notification, &rt_pqueue_descr[mq -1].notify.data, sizeof(struct sigevent));
	mq_mutex_unlock(&rt_pqueue_descr[mq -1].mutex);
	return 0;
}

RTAI_SYSCALL_MODE int _mq_notify(mqd_t mq, RT_TASK *task, long space, long rem, const struct sigevent *notification)
{
	int q_index = mq - 1;
	int rtn;
	if (q_index < 0 || q_index >= MAX_PQUEUES) {
		return -EBADF;
	}
	if (rem) {
		if (rt_pqueue_descr[q_index].notify.task == task) {
			mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
			rt_pqueue_descr[q_index].notify.task = NULL;
			mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
			return OK;
		} else {
			return -EBUSY;
		}
	}
	if (!space && !task->rt_signals) {
		rt_request_signal_mq(mq);
	} else if (!space && !((struct rt_signal_t *)task->rt_signals)[MAXSIGNALS + mq].sigtask) {
		rt_request_signal_mq(mq);
	}
	if (!space && (notification->sigev_notify != SIGEV_THREAD)){
		return ERROR;
	}
	mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
	if (rt_pqueue_descr[q_index].notify.task == NULL) {
        	rt_pqueue_descr[q_index].notify.task = task;
	        rt_pqueue_descr[q_index].notify.data = *notification;
	        if (space) {
	        	if (((QUEUE_CTRL)task->mqueues)->q_access[mq -1].usp_notifier) {
					rt_copy_to_user(((QUEUE_CTRL)task->mqueues)->q_access[mq -1].usp_notifier, &rt_pqueue_descr[mq -1].notify.data, sizeof(struct sigevent));
					rtn = OK;
	        	} else {
					rtn = O_NOTIFY_NP;
	        	}
	        } else {
				rtn = OK;
	        }
	} else {
		rtn = -EBUSY;
	}
	mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	return rtn;
}
EXPORT_SYMBOL(_mq_notify);

RTAI_SYSCALL_MODE int mq_unlink(char *mq_name)
{
	int q_index, rtn;

	mq_mutex_lock(&pqueue_mutex);
	q_index = name_to_id(mq_name);

	if (q_index < 0) {
		mq_mutex_unlock(&pqueue_mutex);
		return -ENOENT;
	}
	mq_mutex_lock(&rt_pqueue_descr[q_index].mutex);
	if (rt_pqueue_descr[q_index].open_count > 0) {
		strcpy(rt_pqueue_descr[q_index].q_name, "\0");
		rt_pqueue_descr[q_index].marked_for_deletion = TRUE;
		rtn = rt_pqueue_descr[q_index].open_count;
	} else {
		delete_queue(q_index);
		rtn = OK;
	}
	mq_mutex_unlock(&rt_pqueue_descr[q_index].mutex);
	mq_mutex_unlock(&pqueue_mutex);
	return rtn;
}
EXPORT_SYMBOL(mq_unlink);

///////////////////////////////////////////////////////////////////////////////
//      PROC FILESYSTEM SECTION
///////////////////////////////////////////////////////////////////////////////

#ifdef CONFIG_PROC_FS

static int pqueue_read_proc(char *page, char **start, off_t off, int count,
			    int *eof, void *data)
{
PROC_PRINT_VARS;
int ind;

    PROC_PRINT("\nRTAI Posix Queue Status\n");
    PROC_PRINT("-----------------------\n\n");
    PROC_PRINT("MAX_PQUEUES = %2d (system wide)\n", MAX_PQUEUES);
    PROC_PRINT("MQ_OPEN_MAX = %2d (per RT task)\n", MQ_OPEN_MAX);
    PROC_PRINT("MQ_NAME_MAX = %d\n", MQ_NAME_MAX);

    PROC_PRINT("\nID  NOpen  NMsgs  MaxMsgs  MaxSz  Perms  Del  Name\n");
    PROC_PRINT("--------------------------------------------------------------------------------\n");
    for (ind = 0; ind < MAX_PQUEUES; ind++) {
	if (rt_pqueue_descr[ind].q_name[0] || rt_pqueue_descr[ind].open_count) {
	    PROC_PRINT( "%-3d %-6d ",
			rt_pqueue_descr[ind].q_id,
			rt_pqueue_descr[ind].open_count
			);
	    PROC_PRINT( "%-6ld %-6ld   %-5ld  ",
		        rt_pqueue_descr[ind].data.attrs.mq_curmsgs,
			rt_pqueue_descr[ind].data.attrs.mq_maxmsg,
			rt_pqueue_descr[ind].data.attrs.mq_msgsize
			);
	    PROC_PRINT( "%-4o   %c    %s\n",
			rt_pqueue_descr[ind].permissions,
			rt_pqueue_descr[ind].marked_for_deletion ? '*' : ' ',
			rt_pqueue_descr[ind].q_name
			);
	}
    }
    PROC_PRINT_DONE;
}

static struct proc_dir_entry *proc_rtai_pqueue;

static int pqueue_proc_register(void)
{
    proc_rtai_pqueue = create_proc_entry("pqueue", 0, rtai_proc_root);
    proc_rtai_pqueue->read_proc = pqueue_read_proc;
    return 0;
}

static int pqueue_proc_unregister(void)
{
    remove_proc_entry("pqueue", rtai_proc_root);
    return 0;
}
#endif

///////////////////////////////////////////////////////////////////////////////
//      MODULE CONTROL
///////////////////////////////////////////////////////////////////////////////

struct rt_native_fun_entry rt_pqueue_entries[] = {
	{ { UR1(1, 5) | UR2(4, 6), _mq_open },  	        MQ_OPEN },
        { { 1, _mq_receive },  		                MQ_RECEIVE },
        { { 1, _mq_send },    		                MQ_SEND },
        { { 1, mq_close },                              MQ_CLOSE },
        { { UW1(2, 3), mq_getattr },                    MQ_GETATTR },
        { { UR1(2, 4) | UW1(3, 4), mq_setattr },	MQ_SETATTR },
        { { UR1(5, 6), _mq_notify },                     MQ_NOTIFY },
        { { UR1(1, 2), mq_unlink },                     MQ_UNLINK },      
        { { 1, _mq_timedreceive },		  	MQ_TIMEDRECEIVE },
        { { 1, _mq_timedsend }, 	       		MQ_TIMEDSEND },
        { { 1,	mq_reg_usp_notifier }, 	       		MQ_REG_USP_NOTIFIER },
	{ { 0, 0 },  		      	       		000 }
};

extern int set_rt_fun_entries(struct rt_native_fun_entry *entry);
extern void reset_rt_fun_entries(struct rt_native_fun_entry *entry);

int __rtai_mq_init(void) 
{
	num_pqueues = 0;
	mq_mutex_init(&pqueue_mutex, NULL);
#ifdef CONFIG_PROC_FS
	pqueue_proc_register();
#endif
	printk(KERN_INFO "RTAI[mq]: loaded.\n");
	return set_rt_fun_entries(rt_pqueue_entries);
	return OK;
}

void __rtai_mq_exit(void) 
{
	mq_mutex_destroy(&pqueue_mutex);
	reset_rt_fun_entries(rt_pqueue_entries);
#ifdef CONFIG_PROC_FS
	pqueue_proc_unregister();
#endif
	printk(KERN_INFO "RTAI[mq]: unloaded.\n");
}

#ifndef CONFIG_RTAI_MQ_BUILTIN
module_init(__rtai_mq_init);
module_exit(__rtai_mq_exit);
#endif /* !CONFIG_RTAI_MQ_BUILTIN */
