minix/lib/libddekit/src/msg_queue.c

#include "common.h"

#include <ddekit/memory.h>
#include <ddekit/minix/msg_queue.h>
#include <ddekit/panic.h>
#include <ddekit/semaphore.h>

#define MESSAGE_QUEUE_SIZE 16

#ifdef DDEBUG_LEVEL_MSG_Q
#undef DDEBUG
#define DDEBUG DDEBUG_LEVEL_MSG_Q
#endif

#include "debug.h"

struct ddekit_minix_msg_q {
	
	unsigned from, to;

	message messages[MESSAGE_QUEUE_SIZE];
	ddekit_sem_t *msg_w_sem, *msg_r_sem;
	int msg_r_pos, msg_w_pos;
	
	struct ddekit_minix_msg_q *next;
};

PRIVATE struct ddekit_minix_msg_q * _list = NULL;
FORWARD _PROTOTYPE(void _ddekit_minix_queue_msg,
                   (struct ddekit_minix_msg_q *mq, message *m));

/*****************************************************************************
 *      ddekit_minix_create_msg_q                                            *
 ****************************************************************************/
PUBLIC struct ddekit_minix_msg_q *
ddekit_minix_create_msg_q(unsigned from, unsigned to)
{
	struct ddekit_minix_msg_q *mq =  (struct ddekit_minix_msg_q *)
	    ddekit_simple_malloc(sizeof(struct ddekit_minix_msg_q));
	
	mq->from = from;
	mq->to   = to;
	mq->msg_w_pos = 0;
	mq->msg_r_pos = 0;

	mq->msg_r_sem = ddekit_sem_init(0);
	mq->msg_w_sem = ddekit_sem_init(MESSAGE_QUEUE_SIZE);

	/* TODO: check for overlapping message ranges */
	mq->next = _list;
	_list     = mq;
	
	DDEBUG_MSG_VERBOSE("created msg_q from %x to %x\n", from , to);

	return mq;	
}

/*****************************************************************************
 *      ddekit_minix_deregister_msg_q                                        *
 ****************************************************************************/
PUBLIC void ddekit_minix_deregister_msg_q(struct ddekit_minix_msg_q *mq)
{
	struct ddekit_minix_msg_q *prev =_list, *it;

	for (it = _list->next; it != NULL ; it = it->next) {
		if (it == mq) {
			prev->next = it->next;
			break;
		}
		prev=it;
	}

	ddekit_sem_deinit(mq->msg_r_sem);
	ddekit_sem_deinit(mq->msg_w_sem);

	ddekit_simple_free(mq);

	DDEBUG_MSG_VERBOSE("destroyed msg_q from \n");
}

/*****************************************************************************
 *     _ddekit_minix_queue_msg                                               *
 ****************************************************************************/
PRIVATE void 
_ddekit_minix_queue_msg(struct ddekit_minix_msg_q *mq, message *m)
{
	int full;
	full = ddekit_sem_down_try(mq->msg_w_sem);

	if (full) {
		/* Our message queue is full... inform the sender. */
		int result;
		DDEBUG_MSG_WARN("Receive queue is full. Ommiting ingoing msg.\n");

		m->m_type = TASK_REPLY;
		m->REP_STATUS = EAGAIN;
		result = asynsend(m->m_source, m);
		
		if (result != 0) {
			ddekit_panic("unable to send reply to %d: %d\n",
					m->m_source, result);
		}

	} else {
		/* queue the message */
		memcpy(&mq->messages[mq->msg_w_pos], m, sizeof(message));

		if (++mq->msg_w_pos == MESSAGE_QUEUE_SIZE) {
			mq->msg_w_pos = 0;
		}
		DDEBUG_MSG_VERBOSE("ddekit_minix_queue_msg: queueing msg %x\n",
		                    m->m_type);
		ddekit_sem_up(mq->msg_r_sem);
	}
}

/*****************************************************************************
 *       ddekit_minix_queue_msg                                              *
 ****************************************************************************/
void ddekit_minix_queue_msg(message *m) 
{
	struct ddekit_minix_msg_q *it, *mq = NULL;
	
	for (it = _list; it !=NULL ; it = it->next) {
		if (m->m_type >= it->from && m->m_type <= it->to) {
			mq = it;
			break;
		}
	}
	if (mq == NULL) {
		DDEBUG_MSG_VERBOSE("no q for msgtype %x\n", m->m_type);
		return;
	}
	_ddekit_minix_queue_msg(mq,m);
}

/*****************************************************************************
 *        ddekit_minix_rcv                                                   *
 ****************************************************************************/
PUBLIC void ddekit_minix_rcv(struct ddekit_minix_msg_q *mq, message *m) 
{
	DDEBUG_MSG_VERBOSE("waiting for message");
	
	ddekit_sem_down(mq->msg_r_sem);

	memcpy(m, &mq->messages[mq->msg_r_pos], sizeof(message));

	if (++mq->msg_r_pos == MESSAGE_QUEUE_SIZE) {
		mq->msg_r_pos = 0;
	}
	
	DDEBUG_MSG_VERBOSE("unqueing message");
	
	ddekit_sem_up(mq->msg_w_sem);
}