drivers/staging/wilc1000/wilc_msgqueue.c

#include "wilc_msgqueue.h"
#include <linux/spinlock.h>
#include <linux/errno.h>
#include <linux/slab.h>

/*!
 *  @author		syounan
 *  @date		1 Sep 2010
 *  @note		copied from FLO glue implementatuion
 *  @version		1.0
 */
int wilc_mq_create(struct message_queue *mq)
{
	spin_lock_init(&mq->lock);
	sema_init(&mq->sem, 0);
	INIT_LIST_HEAD(&mq->msg_list);
	mq->recv_count = 0;
	mq->exiting = false;
	return 0;
}

/*!
 *  @author		syounan
 *  @date		1 Sep 2010
 *  @note		copied from FLO glue implementatuion
 *  @version		1.0
 */
int wilc_mq_destroy(struct message_queue *mq)
{
	struct message *msg;

	mq->exiting = true;

	/* Release any waiting receiver thread. */
	while (mq->recv_count > 0) {
		up(&mq->sem);
		mq->recv_count--;
	}

	while (!list_empty(&mq->msg_list)) {
		msg = list_first_entry(&mq->msg_list, struct message, list);
		list_del(&msg->list);
		kfree(msg->buf);
	}

	return 0;
}

/*!
 *  @author		syounan
 *  @date		1 Sep 2010
 *  @note		copied from FLO glue implementatuion
 *  @version		1.0
 */
int wilc_mq_send(struct message_queue *mq,
		 const void *send_buf, u32 send_buf_size)
{
	unsigned long flags;
	struct message *new_msg = NULL;

	if (!mq || (send_buf_size == 0) || !send_buf)
		return -EINVAL;

	if (mq->exiting)
		return -EFAULT;

	/* construct a new message */
	new_msg = kmalloc(sizeof(*new_msg), GFP_ATOMIC);
	if (!new_msg)
		return -ENOMEM;

	new_msg->len = send_buf_size;
	INIT_LIST_HEAD(&new_msg->list);
	new_msg->buf = kmemdup(send_buf, send_buf_size, GFP_ATOMIC);
	if (!new_msg->buf) {
		kfree(new_msg);
		return -ENOMEM;
	}

	spin_lock_irqsave(&mq->lock, flags);

	/* add it to the message queue */
	list_add_tail(&new_msg->list, &mq->msg_list);

	spin_unlock_irqrestore(&mq->lock, flags);

	up(&mq->sem);

	return 0;
}

/*!
 *  @author		syounan
 *  @date		1 Sep 2010
 *  @note		copied from FLO glue implementatuion
 *  @version		1.0
 */
int wilc_mq_recv(struct message_queue *mq,
		 void *recv_buf, u32 recv_buf_size, u32 *recv_len)
{
	struct message *msg;
	unsigned long flags;

	if (!mq || (recv_buf_size == 0) || !recv_buf || !recv_len)
		return -EINVAL;

	if (mq->exiting)
		return -EFAULT;

	spin_lock_irqsave(&mq->lock, flags);
	mq->recv_count++;
	spin_unlock_irqrestore(&mq->lock, flags);

	down(&mq->sem);
	spin_lock_irqsave(&mq->lock, flags);

	if (list_empty(&mq->msg_list)) {
		spin_unlock_irqrestore(&mq->lock, flags);
		up(&mq->sem);
		return -EFAULT;
	}
	/* check buffer size */
	msg = list_first_entry(&mq->msg_list, struct message, list);
	if (recv_buf_size < msg->len) {
		spin_unlock_irqrestore(&mq->lock, flags);
		up(&mq->sem);
		return -EOVERFLOW;
	}

	/* consume the message */
	mq->recv_count--;
	memcpy(recv_buf, msg->buf, msg->len);
	*recv_len = msg->len;

	list_del(&msg->list);

	kfree(msg->buf);
	kfree(msg);

	spin_unlock_irqrestore(&mq->lock, flags);

	return 0;
}