/*****************************************************************************/
/* Copyright (c) 2008 NXP B.V.                                               */
/*                                                                           */
/* 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, using version 2 of the License.             */
/*                                                                           */
/* 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.                                                                      */
/*                                                                           */
/*****************************************************************************/

#include "common.h"

#if defined (CONFIG_TIME_DOCTOR)
#include "linux/timedoctor.h"
#endif
/* Internal OSAL queue representation */

struct osal_queue_element {
	struct list_head list;	/* link elements into queue or free list */
	int size;		/* size of data currently stored */
	void *data;		/* buffer to store data in */
};

struct osal_queue {
	struct list_head queue;	/* the queue */
	struct list_head pool;	/* free element pool */
	spinlock_t lock;	/* lock to guard lists */
	int size;		/* maximum size of element */

	wait_queue_head_t wait;
	int dead;		/* if non-zero, queue was destroyed */
};


/* Since pool should be allocated at queue allocation time, we have to
 * provide custom allocate and deallocate routines */

static inline void free_element_list(struct list_head *l)
{
	struct list_head *p1, *p2;
	struct osal_queue_element *e;
	
	list_for_each_safe(p1, p2, l) {
		e = list_entry(p1, struct osal_queue_element, list);
		kfree(e->data);
		kfree(e);
	}
}

static struct osal_queue *init_osal_queue(int n, int size)
{
	struct osal_queue *oq;
	struct osal_queue_element *e;
	int i;

	oq = (struct osal_queue *) kmalloc(sizeof(*oq), GFP_KERNEL);
	if (!oq)
		return 0;

	/* better to initialize all fields in one place */
	INIT_LIST_HEAD(&oq->queue);
	INIT_LIST_HEAD(&oq->pool);
	oq->lock = SPIN_LOCK_UNLOCKED;
	oq->size = size;
	init_waitqueue_head(&oq->wait);
	oq->dead = 0;

	/* now allocate element pool */
	for (i = 0; i < n; i++) {
		e = (struct osal_queue_element *) kmalloc(sizeof(*e), GFP_KERNEL);
		if (!e)
			goto cleanup;
		e->data = kmalloc(size, GFP_KERNEL);
		if (!e->data) {
			kfree(e);
			goto cleanup;
		}
		list_add(&e->list, &oq->pool);
	}

	return oq;

cleanup:
	free_element_list(&oq->pool);
	kfree(oq);
	return 0;
}

static void free_osal_queue(struct osal_queue *oq)
{
	free_element_list(&oq->queue);
	free_element_list(&oq->pool);
	kfree(oq);
}

static struct handle_table queue_handle_table;
#define allocate_osal_queue_handle(handle) \
	ALLOCATE_HANDLE_MACRO(&queue_handle_table, handle, TMOSAL_ERR_QUE_FULL)
#define bind_osal_queue_to_handle(handle, os) \
	BIND_OBJECT_MACRO(&queue_handle_table, handle, os)
#define cancel_osal_queue_handle(handle) \
	CANCEL_HANDLE_MACRO(&queue_handle_table, handle)
#define get_osal_queue(handle, os) \
	GET_OBJECT_MACRO(&queue_handle_table, handle, os, TMOSAL_ERR_HANDLE)
#define put_osal_queue(handle) \
	PUT_OBJECT_MACRO(&queue_handle_table, handle)
#define mark_dead_osal_queue(handle) \
	MARK_DEAD_OBJECT_MACRO(&queue_handle_table, handle)


/* Routines to manipulate with elements - all should be used with lock held */

static inline struct osal_queue_element *allocate_element(struct osal_queue *oq)
{
	struct list_head *h;
	
	if (list_empty(&oq->pool))
		return 0;
	
	h = oq->pool.next;
	list_del(h);
	return list_entry(h, struct osal_queue_element, list);
}

static inline void free_element(struct osal_queue *oq, struct osal_queue_element *e)
{
	list_add(&e->list, &oq->pool);
}

static inline void send_element(struct osal_queue *oq, struct osal_queue_element *e)
{
	list_add_tail(&e->list, &oq->queue);
}

static inline struct osal_queue_element *check_element(struct osal_queue *oq)
{
	if (list_empty(&oq->queue))
		return 0;
	else
		return list_entry(oq->queue.next, struct osal_queue_element, list);
}

static inline struct osal_queue_element *receive_element(struct osal_queue *oq)
{
	struct osal_queue_element *e;
	
	e = check_element(oq);
	if (e)
		list_del(&e->list);

	return e;
}


/* Queue API */

tmErrorCode_t tmosalQueueCreate(const UInt32 kMaxPackets,
		const UInt32 kMaxPacketSize,
		ptmosalQueueHandle_t pioQueueHandle,
		const tmosalQueueCreateFlags_t kFlags)
{
	u32 handle;
	struct osal_queue *oq;
	
	BUG_IF_INTERRUPT();
	BUG_IF_NOT_PREEMPTIBLE();
	
	if (unlikely(!kMaxPackets) || unlikely(!kMaxPacketSize) ||
			unlikely(!pioQueueHandle))
		return TMOSAL_ERR_BAD_PARAM;

	allocate_osal_queue_handle(handle);
	oq = init_osal_queue(kMaxPackets, kMaxPacketSize);
	if (unlikely(!oq)) {
		cancel_osal_queue_handle(handle);
		return TMOSAL_ERR_OUT_OF_MEMORY;
	}

	STAT_HANDLE_CREATE(curQueueNr, maxQueueNr);
	bind_osal_queue_to_handle(handle, oq);
	*pioQueueHandle = handle;
	return TM_OK;
}
EXPORT_SYMBOL(tmosalQueueCreate);


tmErrorCode_t tmosalQueueDestroy(const tmosalQueueHandle_t kQueueHandle)
{
	struct osal_queue *oq;
	unsigned long flags;
	int empty;
	tmErrorCode_t ret;
	
	get_osal_queue(kQueueHandle, oq);
	spin_lock_irqsave(&oq->wait.lock, flags);
	oq->dead = 1;
	empty = list_empty(&oq->wait.task_list);
	spin_unlock_irqrestore(&oq->wait.lock, flags);

	if (likely(empty))
		ret = TM_OK;
	else {
		wake_up(&oq->wait);
		if (!in_interrupt() && preemptible())
			schedule();
		/* queue will be deallocated by waiting task */
		ret = TMOSAL_ERR_QUE_TASK;
	}

	STAT_HANDLE_DESTROY(curQueueNr);
	mark_dead_osal_queue(kQueueHandle);
	put_osal_queue(kQueueHandle);
	return ret;
}
EXPORT_SYMBOL(tmosalQueueDestroy);


tmErrorCode_t tmosalQueueSend(const tmosalQueueHandle_t kQueueHandle,
		const Pointer pkBuffer, const UInt32 kPacketSize,
		const pInt32 pkTimeout, const tmosalQueueSendFlags_t kFlags)
{
	struct osal_queue *oq;
	struct osal_queue_element *e;
	unsigned long flags;
	int waiter_exists;
	
	/* check args */
	
	if (unlikely(!pkBuffer))
		return TMOSAL_ERR_QUE_BUFFER;
	
	get_osal_queue(kQueueHandle, oq);

	if (unlikely(kPacketSize > oq->size)) {
		put_osal_queue(kQueueHandle);
		return TMOSAL_ERR_BAD_PARAM;
	}
	
	/* add element to queue */
	
	spin_lock_irqsave(&oq->lock, flags);
	e = allocate_element(oq);
	spin_unlock_irqrestore(&oq->lock, flags);
	
	if (unlikely(!e)) {
		put_osal_queue(kQueueHandle);
		return TMOSAL_ERR_QUE_TIMEOUT;	/* not a typo - docs require TIMEOUT value here! */
	}
		
	e->size = kPacketSize;
	memcpy(e->data, pkBuffer, kPacketSize);

#if defined (CONFIG_TIME_DOCTOR)
    if (osal_debug_level > 0)
    {
        char name[9];
        sprintf(name, "%07XQ", (unsigned int)kQueueHandle);
        timeDoctor_Info(TDI_COMMAND(TDI_START, TDI_QUEUE),
                    CHAR2_ARRAY_TO_U32(name), CHAR_ARRAY_TO_U32(name));
    }
#endif
	
	spin_lock_irqsave(&oq->lock, flags);
	send_element(oq, e);
	spin_unlock_irqrestore(&oq->lock, flags);
	
	/* wake possible waiters */
	
	spin_lock_irqsave(&oq->wait.lock, flags);
	waiter_exists = !list_empty(&oq->wait.task_list);
	spin_unlock_irqrestore(&oq->wait.lock, flags);

	if (unlikely(waiter_exists)) {
		/* it could be better to use wake_up_locked() here, to avoid
		   races. Unfortunately, it is not exported. But there is no
		   much harm anyway, because extra call to wake_up() will just
		   wakeup threads, which will check situation and go sleep
		   again if needed */
		wake_up(&oq->wait);
		if (preemptible() && !in_interrupt())
			schedule();
	}

	put_osal_queue(kQueueHandle);
	return TM_OK;
}
EXPORT_SYMBOL(tmosalQueueSend);


tmErrorCode_t tmosalQueueReceive(const tmosalQueueHandle_t kQueueHandle,
		Pointer pioBuffer, pUInt32 pioPacketSize,
		const pInt32 pkTimeout, const tmosalQueueReceiveFlags_t kFlags)
{
	struct osal_queue *oq;
	unsigned long flags, flags2;
	struct osal_queue_element *e = 0;
	int jif;
	tmErrorCode_t ret;
	DECLARE_WAITQUEUE(wait, current);
			
	BUG_IF_SLEEP_IN_ATOMIC(pkTimeout);

	if (unlikely(!pioBuffer) || unlikely(!pioPacketSize))
		return TMOSAL_ERR_BAD_PARAM;

	get_osal_queue(kQueueHandle, oq);

	jif = (pkTimeout && *pkTimeout > 0) ? msecs_to_jiffies(*pkTimeout) : 0;
	
	spin_lock_irqsave(&oq->wait.lock, flags);
	
	wait.flags = 0;		/* non-exclusive */
	__add_wait_queue(&oq->wait, &wait);

	while (1) {
		/* check for dead */
		if (unlikely(oq->dead)) {
			ret = TMOSAL_ERR_QUE_DEL;
			break;
		}
		
		/* try to receive */
		spin_lock_irqsave(&oq->lock, flags2);
		e = receive_element(oq);
		spin_unlock_irqrestore(&oq->lock, flags2);
		if (e) {
			ret = TM_OK;
			break;
		}

		/* check for timeout condition */
		if (pkTimeout && !jif) {
			ret = TMOSAL_ERR_QUE_TIMEOUT;
			break;
		}
		
		/* will sleep */

		set_current_state(TASK_INTERRUPTIBLE);
		spin_unlock_irqrestore(&oq->wait.lock, flags);

#if defined (CONFIG_TIME_DOCTOR)
        if (osal_debug_level > 0)
        {
            char name[9];
            sprintf(name, "%07XQ", (unsigned int)kQueueHandle);
            timeDoctor_Info(TDI_COMMAND(TDI_STOP, TDI_QUEUE),
                        CHAR2_ARRAY_TO_U32(name), CHAR_ARRAY_TO_U32(name));
        }
#endif

		if (pkTimeout)
			jif = schedule_timeout(jif);
		else
			schedule();
			
		spin_lock_irqsave(&oq->wait.lock, flags);

		if(signal_pending(current))
		{
			// No really appropriate error to be found...send a timeout
			ret = TMOSAL_ERR_QUE_TIMEOUT;
			break;
		}
		/* will check the wakeup reason on new iteration */
	}

	__remove_wait_queue(&oq->wait, &wait);
	spin_unlock_irqrestore(&oq->wait.lock, flags);

	if (ret == TM_OK) {
		/* copy data to buffer */
		*pioPacketSize = e->size;
		memcpy(pioBuffer, e->data, e->size);
		
		/* return element to pool */
		spin_lock_irqsave(&oq->lock, flags);
		free_element(oq, e);
		spin_unlock_irqrestore(&oq->lock, flags);
	}

	put_osal_queue(kQueueHandle);
	return ret;
}
EXPORT_SYMBOL(tmosalQueueReceive);


tmErrorCode_t tmosalQueueGetPacketSize(const tmosalQueueHandle_t kQueueHandle,
		pUInt32 pioPacketSize)
{
	struct osal_queue *oq;
	struct osal_queue_element *e;
	unsigned long flags;
	
	if (unlikely(!pioPacketSize))
		return TMOSAL_ERR_BAD_PARAM;
	
	get_osal_queue(kQueueHandle, oq);
	spin_lock_irqsave(&oq->lock, flags);
	e = check_element(oq);
	if (!e) {
		spin_unlock_irqrestore(&oq->lock, flags);
		put_osal_queue(kQueueHandle);
		return TMOSAL_ERR_QUE_EMPTY;
	}
	*pioPacketSize = e->size;
	spin_unlock_irqrestore(&oq->lock, flags);
	put_osal_queue(kQueueHandle);
	return TM_OK;
}
EXPORT_SYMBOL(tmosalQueueGetPacketSize);


int osal_queue_submodule_init(void)
{
	int ret;
	
	ret = init_handle_table(&queue_handle_table, QUEUE_HANDLE_TABLE_INITIAL_SIZE, "queue");
	queue_handle_table.deallocator = (object_deallocator)free_osal_queue;
	return ret;
}

void osal_queue_submodule_uninit(void)
{
	uninit_handle_table(&queue_handle_table);
}
