#include <linux/device.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/firmware.h>
#include <linux/platform_device.h>
#include <linux/miscdevice.h>
#include <linux/delay.h>
#include <linux/completion.h>
#include <linux/freezer.h>
#include <linux/uaccess.h>
#include <linux/io.h>
#include <linux/poll.h>
#include <linux/dspdl-coff.h>

#include <mach/irqs.h>
#include <mach/hardware.h>
#include <mach/common.h>
#include <mach/time.h>
#include <mach/mux.h>

#include <asm/mach-types.h>
#include <asm/gpio.h>
#include <asm/io.h>

#ifdef CONFIG_PROC_FS
#include <linux/proc_fs.h>
#include <linux/seq_file.h>
#include <asm/uaccess.h>
#endif /* CONFIG_PROC_FS */

#define MODULE_NAME 	"msgq"

#define MSGQ_DSP_INTERRUPT	1

/* TODO:
	* use lib/crc32 code
 */

static const char msgq_driver_version[] = "v2.0";

static char *msgq_fw_name = "femtobts.out";
module_param_named(fw_name, msgq_fw_name, charp, 0644);

static int msgq_arm_irq = 47;
module_param_named(arm_irq, msgq_arm_irq, int, S_IRUGO);
MODULE_PARM_DESC(arm_irq, "ARM-side MSGQ interrupt number. Default: 47");

static int msgq_check_crc = 0;
module_param_named(check_crc, msgq_check_crc, int, S_IRUGO);
MODULE_PARM_DESC(arm_irq, "Valide/Generate a CRC-32 for each messages. Default: 0");


struct msgq_t {
	u32	u32MagicId;    
	u32	u32QueueSize;
	u32	u32MsgSize;  
	u32	reserved[2];
	u32	u32Latency;    
	u32	u32Head;    
	u32	u32Tail;    
	u8	u8Dir;      
	u8	u8ArmIntReq;  
	u8	u8ArmIntFlag; 
	u8	u8DspIntReq;  
	u8	u8DspIntFlag;   
	char	szName[27];   
	u8	pu8Mem[0];     
};

struct msgq_mdev_t {
	char devName[128];
 	struct mutex rd_lock; 
 	struct mutex wr_lock; 
	struct miscdevice dev;

	u32 u32BadCrcCnt;
  	spinlock_t lock;
	wait_queue_head_t waitq;
	volatile struct msgq_t __iomem *msgq;
};

struct msgq_dev_t {
	int irq;
	int wake;
	
	u32 mdev_count;	
	struct msgq_mdev_t *mdev;

    	volatile void * __iomem *mmio;
};

static struct msgq_dev_t *msgq_dev;

static u32 msgq_GetTimestamp(void)
{
	void __iomem *timer0 = IO_ADDRESS(DAVINCI_TIMER0_BASE + 0x14);

	return __raw_readl(timer0);
}

static const u32 crc32_tab[] = {
	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
	0xe963a535, 0x9e6495a3,	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
	0xf3b97148, 0x84be41de,	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,	0x14015c4f, 0x63066cd9,
	0xfa0f3d63, 0x8d080df5,	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,	0x35b5a8fa, 0x42b2986c,
	0xdbbbc9d6, 0xacbcf940,	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
	0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,	0x76dc4190, 0x01db7106,
	0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
	0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
	0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
	0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
	0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
	0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
	0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
	0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
	0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
	0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
	0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
	0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
	0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
	0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
	0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
	0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};

static u32 msgq_Crc32(u32 crc, const void *buf, size_t size)
{
	const uint8_t *p;

	p = buf;
	crc = crc ^ ~0U;

	while (size--)
		crc = crc32_tab[(crc ^ *p++) & 0xFF] ^ (crc >> 8);

	return crc ^ ~0U;
}

static int msgq_TrigDspIsr(int intNum)
{
	void __iomem *intgen = IO_ADDRESS(DAVINCI_SYSTEM_MODULE_BASE + 0x10);

	__raw_writel(0x30F10000 | (1 << (intNum + 4)), intgen);
	
	return 0;
}

static irqreturn_t msgq_isr(int irq, void *dev_id)
{
	int i;
	struct msgq_dev_t *dev = dev_id;
	
	for (i = 0; i < dev->mdev_count; i++) {
		if (dev->mdev[i].msgq->u8ArmIntFlag) {
			dev->mdev[i].msgq->u8ArmIntReq = 0;
			dev->mdev[i].msgq->u8ArmIntFlag = 0;
			wake_up_interruptible(&dev->mdev[i].waitq);
		}
    	}
	return IRQ_HANDLED;
}

#ifdef CONFIG_PROC_FS
static void *msgq_seq_start(struct seq_file *m, loff_t *pos)
{
	return *pos < 1 ? (void *)1 : NULL;
}

static void *msgq_seq_next(struct seq_file *m, void *v, loff_t *pos)
{
	++*pos;
	return NULL;
}

static void msgq_seq_stop(struct seq_file *m, void *v)
{
}

static int msgq_proc_show(struct seq_file *m, void *v)
{
	int i;
	struct msgq_dev_t *dev = m->private;

	for (i = 0; i < dev->mdev_count; i++) {

		seq_printf(m, "\nMSGQ #%d/%d\n===========\n", i+1, dev->mdev_count); 
		seq_printf(m, "\tname       : %s\n", dev->mdev[i].msgq->szName);
		seq_printf(m, "\tdir        : %s\n", dev->mdev[i].msgq->u8Dir ? "ARM -> DSP" : "DSP -> ARM");
		seq_printf(m, "\tmsg size   : %d bytes\n", dev->mdev[i].msgq->u32MsgSize);
		seq_printf(m, "\tqueue size : %d messages\n", dev->mdev[i].msgq->u32QueueSize);
		seq_printf(m, "\thead       : %d\n", dev->mdev[i].msgq->u32Head);
		seq_printf(m, "\ttail       : %d\n", dev->mdev[i].msgq->u32Tail);
		seq_printf(m, "\tirq        : ARM %d:%d  DSP %d:%d\n", dev->mdev[i].msgq->u8ArmIntReq,
			dev->mdev[i].msgq->u8ArmIntFlag, dev->mdev[i].msgq->u8DspIntReq, 
			dev->mdev[i].msgq->u8DspIntFlag );
		seq_printf(m, "\tlatency    : < %d us\n", dev->mdev[i].msgq->u32Latency );
		seq_printf(m, "\tcrc error  : %d\n", dev->mdev[i].u32BadCrcCnt );
		dev->mdev[i].msgq->u32Latency = 0;
	}	
	return 0;
}

static const struct seq_operations msgq_proc_op = {
	.start	= msgq_seq_start,
	.next	= msgq_seq_next,
	.stop	= msgq_seq_stop,
	.show	= msgq_proc_show
};

static int msgq_proc_open(struct inode *inode, struct file *file)
{
	int ret;
	struct seq_file *m;
	struct msgq_dev_t *dev;

	dev = PDE(inode)->data;
	if (!dev)
		return -ENOENT;

	ret = seq_open(file, &msgq_proc_op);
	if (ret < 0)
		return ret;

	m = file->private_data;
	m->private = dev;

	return 0;
}

static ssize_t msgq_proc_write(struct file *file, const char __user *buf,
			       size_t size, loff_t *ppos)
{
	char *kbuf;
	int ret = 0;

	if (size <= 1 || size >= PAGE_SIZE)
		return -EINVAL;

	kbuf = kmalloc(size + 1, GFP_KERNEL);
	if (!kbuf)
		return -ENOMEM;

	if (copy_from_user(kbuf, buf, size) != 0) {
		kfree(kbuf);
		return -EFAULT;
	}
	kbuf[size] = 0;

	if (0 == 1) {
	}
	else {
		printk(KERN_ERR "msgq: Invalid Command (%s)\n", kbuf);
		kfree(kbuf);
		return -EINVAL;
	}

	ret = size;

	kfree(kbuf);
	return ret;
}

static const struct file_operations msgq_proc_fops = {
	.open		= msgq_proc_open,
	.write		= msgq_proc_write,
	.read		= seq_read,
	.llseek		= seq_lseek,
	.release	= seq_release,
	.owner		= THIS_MODULE,
};

#endif /* CONFIG_PROC_FS */

static int msgq_open(struct inode *inode, struct file *filp)
{
	int i;
	struct msgq_mdev_t *mdev = NULL;

	for (i = 0; i < msgq_dev->mdev_count; i++) {

		if (msgq_dev->mdev[i].dev.minor == MINOR(inode->i_rdev)) {
			mdev = &msgq_dev->mdev[i];
			break;
		}
	}
	if (mdev == NULL) {
		printk(KERN_ERR "msgq: Invalid minor device\n");
		return -ENODEV;
	}

	// Reset stats
	mdev->msgq->u32Latency = 0;

	filp->private_data = mdev;
	return 0;
}

static int msgq_release(struct inode *inode, struct file *filp)
{
	//struct msgq_mdev_t *mdev = filp->private_data;

	return 0;
}

static int msgq_read(struct file *filp, char *buf, size_t count, loff_t *ppos)
{
	int ret;
	u32 crc;
	u8 *pSrc;
	u32 length;
	u32 latency;
	u32 timestamp;
	wait_queue_t wq;
	struct msgq_mdev_t *mdev = filp->private_data;

	mutex_lock(&mdev->rd_lock);
	while (mdev->msgq->u32Tail == mdev->msgq->u32Head) {

		if (filp->f_flags & O_NONBLOCK) {
			mutex_unlock(&mdev->rd_lock);
			return -EAGAIN;  
		}

		init_waitqueue_entry( &wq, current );

		/* Ask the DSP to be interrupted */	
		mdev->msgq->u8ArmIntReq = 1;

		ret = wait_event_interruptible(mdev->waitq,
					       mdev->msgq->u32Tail != mdev->msgq->u32Head);
		if (ret < 0) {
			mutex_unlock(&mdev->rd_lock);
			return ret;
		}
	}
	pSrc = (u8 *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * mdev->msgq->u32Tail];

	/* Read timestamp */	
	memcpy(&timestamp, &pSrc[0], sizeof(u32));

	/* Read length */	
	memcpy(&length, &pSrc[4], sizeof(u32));

	/* Verify count */
	if (length > count) {
		mutex_unlock(&mdev->rd_lock);
		return -EINVAL;
	}

	/* Read message */	
	ret = copy_to_user(buf, &pSrc[8], length);
    	if (ret) {  
		mutex_unlock(&mdev->rd_lock);
		return ret;  
	}	

	/* Validate the CRC */
	if (msgq_check_crc != 0) {
		memcpy(&crc, &pSrc[8 + length], sizeof(u32));
		if (crc != 0) {
			crc = msgq_Crc32(0, &pSrc[4], sizeof(u32) + length + sizeof(u32));
			if ( crc != 0xFFFFFFFF ) {
				// Invalid CRC, drop the primitive
				mdev->u32BadCrcCnt++;
				printk( KERN_INFO "msgq: RX a primitive with a bad CRC !! (%08X)\n", crc );
		
				//return -EILSEQ;
				length = -EILSEQ;
			}
		}
	}	

	/* Update tail pointer */
	mdev->msgq->u32Tail = ((mdev->msgq->u32Tail+1) >= mdev->msgq->u32QueueSize) ?
				0 : (mdev->msgq->u32Tail+1);

	/* Compute round-trip delay in usec */
	latency = (msgq_GetTimestamp() - timestamp) / 27;
	if (latency > mdev->msgq->u32Latency) {
		mdev->msgq->u32Latency = latency;
	}

	mutex_unlock(&mdev->rd_lock);
    
	/* Verify if the DSP wants to be interrupted */                             
	if (mdev->msgq->u8DspIntReq) {

		/* Interrupt the DSP */
		mdev->msgq->u8DspIntFlag = 1;
		msgq_TrigDspIsr(MSGQ_DSP_INTERRUPT);
	}    

	return length;
}

static unsigned int msgq_poll(struct file *filp, struct poll_table_struct *wait)
{
	struct msgq_mdev_t *mdev = filp->private_data;
	int rc = 0;

	if (mdev->msgq->u8Dir) /* ARM2DSP */ {
		u32 nextHead;
		nextHead = ((mdev->msgq->u32Head+1) >= mdev->msgq->u32QueueSize) ? 0 : (mdev->msgq->u32Head+1);
		if (nextHead != mdev->msgq->u32Tail) {
			rc = POLLOUT | POLLWRNORM;
			return rc;		
		}
	} else {
		if (mdev->msgq->u32Tail != mdev->msgq->u32Head) {
			rc = POLLIN | POLLRDNORM;
			return rc;
		}
	}

	/* Ask the DSP to be interrupted */
	mdev->msgq->u8ArmIntReq = 1;

	/* Add ourselves to the wait queue */
	poll_wait(filp, &mdev->waitq, wait);

	if (mdev->msgq->u8Dir) /* ARM2DSP */ {
		u32 nextHead;
		nextHead = ((mdev->msgq->u32Head+1) >= mdev->msgq->u32QueueSize) ? 0 : (mdev->msgq->u32Head+1);
		if (nextHead != mdev->msgq->u32Tail)
			rc = POLLOUT | POLLWRNORM;
	} else {
		if (mdev->msgq->u32Tail != mdev->msgq->u32Head)
			rc = POLLIN | POLLRDNORM;
	}

	return rc;
}
 
static int msgq_write(struct file *filp, const char *buf, size_t count, loff_t *ppos)
{
	int ret;
	u32 crc;
	u32 nextHead;
	wait_queue_t wq;
	u32 timestamp = msgq_GetTimestamp();
	struct msgq_mdev_t *mdev = filp->private_data;

	if (count>mdev->msgq->u32MsgSize) {
		printk(KERN_ERR "msgq: Message is too large (%d > %d)\n", count, mdev->msgq->u32MsgSize );
		return -EINVAL;
	}

	mutex_lock(&mdev->wr_lock);

	nextHead = ((mdev->msgq->u32Head+1) >= mdev->msgq->u32QueueSize) ? 0 : (mdev->msgq->u32Head+1);
    
	// Look if there is room available
	while (nextHead == mdev->msgq->u32Tail) {
		if (filp->f_flags & O_NONBLOCK) {
			mutex_unlock(&mdev->wr_lock);
			return -EAGAIN;  
		}

		init_waitqueue_entry(&wq, current);

		/* Ask the DSP to be interrupted */	
		mdev->msgq->u8ArmIntReq = 1;

		ret = wait_event_interruptible(mdev->waitq,
					       nextHead != mdev->msgq->u32Tail);
		if (ret < 0) {
			printk(KERN_ERR "msgq: Falied to wait for event (%d)\n", ret );
			mutex_unlock(&mdev->wr_lock);
			return ret;
		}
	}
	
	/* Write timestamp */	
	memcpy((void *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * 
			mdev->msgq->u32Head + 0 * sizeof(u32)], &timestamp, sizeof(u32));

	/* Write length */	
	memcpy((void *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * 
			mdev->msgq->u32Head + 1 * sizeof(u32)], &count, sizeof(u32));

	/* Write message */	
	ret = copy_from_user((void *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * 
			mdev->msgq->u32Head + 2 * sizeof(u32)], buf, count);
    	if (ret) {  
		printk(KERN_ERR "msgq: Copy from user failed (%d)\n", ret );
		mutex_unlock(&mdev->wr_lock);
		return ret;  
	}	

	/* Compute and add the CRC */
	if (msgq_check_crc != 0) {
		crc = msgq_Crc32(0, (void *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * 
				mdev->msgq->u32Head + 1 * sizeof(u32)], sizeof(count) + count); 
		crc ^= 0xFFFFFFFF;
	}
	else {
		crc = 0;
	}
	memcpy((void *)&mdev->msgq->pu8Mem[(mdev->msgq->u32MsgSize + 12) * 
			mdev->msgq->u32Head + 2 * sizeof(u32) + count], &crc, sizeof(u32));

	/* Update head pointer */
	mdev->msgq->u32Head = nextHead;

   	mutex_unlock(&mdev->wr_lock);
    
	/* Verify if the DSP wants to be interrupted */                            
	if (mdev->msgq->u8DspIntReq) {

		mdev->msgq->u8DspIntFlag = 1;
		msgq_TrigDspIsr(MSGQ_DSP_INTERRUPT);
	}    

	return count;
}

static const struct file_operations msgq_fops = {
	.open		= msgq_open,
	.release	= msgq_release,
	.read		= msgq_read,
	.write		= msgq_write,
	.poll		= msgq_poll,
	.owner		= THIS_MODULE,
};

static int __devinit msgq_probe(struct platform_device *pdev)
{
	int i;
	int ret = -ENODEV;
	struct msgq_t *pMsgq;
	struct file_hdr_t *file_hdr;
	struct opt_hdr_t *opt_hdr;
	struct section_hdr_t *section_hdr;
	const struct firmware *fw_entry;
	struct msgq_dev_t *dev = NULL;
	struct proc_dir_entry *ent;

	ret = request_firmware( &fw_entry, msgq_fw_name, &pdev->dev);
	if (ret < 0) {
		printk( KERN_ERR "msgq: firmware <%s> not available\n", msgq_fw_name);
		goto error_fw_request;
	}

	/* The memory is set to zero by kzalloc */
	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
	if (!dev) {
		printk(KERN_ERR "msgq: Failed to allocate device structure\n");
		ret = -ENOMEM;
		goto error_alloc_dev;
	}

	/* Extract header infos. */
	file_hdr = (struct file_hdr_t *)fw_entry->data;
	if (file_hdr->magic != 0xC2) {
		printk(KERN_ERR "msgq: Invalid magic number in header (0x%02X)\n",
			file_hdr->magic);
		ret = -EFAULT;
		goto error_fw;
	}
	if (file_hdr->target_id != 0x0099) {
		printk(KERN_ERR "msgq: Invalid target ID in header (0x%04X)\n",
			file_hdr->target_id);
		ret = -EFAULT;
		goto error_fw;
	}

	/* Make sure file is a valid COFF. */
	if (!(file_hdr->flags & F_EXEC)) {
		printk(KERN_ERR "msgq: Not an executable");
		ret = -EFAULT;
		goto error_fw;
	}

	/* Points to first Section */
	opt_hdr = (struct opt_hdr_t *) (fw_entry->data + sizeof(struct file_hdr_t));
	section_hdr = (struct section_hdr_t *)((void *) opt_hdr + file_hdr->opthdr);

	/* Try to locate the MSGQ section */
	for (i = 0; i < file_hdr->nscns; i++) {
		if ((section_hdr->s_size != 0) &&
		    (section_hdr->s_scnptr != 0) &&
		    !(section_hdr->s_flags & STYP_COPY) &&
		    (strncmp(section_hdr->s_name, "MSGQUEUE", 8) == 0)) {
			/* Found */
			break;
		}
		section_hdr++;
	}
	if (i == file_hdr->nscns) {
		printk(KERN_ERR "msgq: No \"MSGQUEUE\" section found in COFF file\n");
		ret = -EFAULT;
		goto error_fw;
	}

	/* Map MSGQ memory section */
    	dev->mmio = ioremap(section_hdr->s_paddr, section_hdr->s_size);
    	if (!dev->mmio) {        
		printk(KERN_ERR "msgq: Failed to I/O remap MSGQ memory\n");
        	ret = -ENOMEM;
        	goto error_ioremap;
    	}

	/* Count the number of queues */
	pMsgq = (struct msgq_t *)dev->mmio;
	for (i = sizeof(struct msgq_t); i < section_hdr->s_size; ) {
		if ((pMsgq->u32MagicId != 0x4D534751/*'MSGQ'*/) || 
		    (i + pMsgq->u32QueueSize * (pMsgq->u32MsgSize + 12) > section_hdr->s_size)) {
			break;
		}
		dev->mdev_count++;
		i += (sizeof(struct msgq_t) + pMsgq->u32QueueSize * (pMsgq->u32MsgSize + 12) + 0x3F) & ~0x3F;
		pMsgq = (struct msgq_t *)((u32)pMsgq + ((sizeof(struct msgq_t) + 
				pMsgq->u32QueueSize * (pMsgq->u32MsgSize + 12) + 0x3F) & ~0x3F));
	}
	if (dev->mdev_count == 0) {
		printk(KERN_ERR "msgq: No valid msgq found\n");
		ret = -EFAULT;
		goto error_noqueue;
	}
	release_firmware(fw_entry);

	/* Alloc memory for each queue misc device */
	dev->mdev = kzalloc(dev->mdev_count * sizeof(struct msgq_mdev_t), GFP_KERNEL);
    	if (!dev->mdev) {
		printk(KERN_ERR "msgq: Failed to allocate misc device structure\n");
		ret = -ENOMEM;
		goto error_alloc_mdev;
	}

	/* Init each queue device */
	pMsgq = (struct msgq_t *)dev->mmio;
	for (i = 0; i < dev->mdev_count; i++) {
		sprintf(dev->mdev[i].devName, "msgq!%s", pMsgq->szName );

		dev->mdev[i].dev.name  = dev->mdev[i].devName;
		dev->mdev[i].dev.minor = MISC_DYNAMIC_MINOR;
		dev->mdev[i].dev.fops  = &msgq_fops;
		ret = misc_register(&dev->mdev[i].dev);
		if (ret < 0) {
			printk(KERN_ERR "msgq: Error registering misc driver\n");
			goto error_miscdev;
		}

		dev->mdev[i].u32BadCrcCnt = 0;
   		mutex_init(&dev->mdev[i].wr_lock);
    		mutex_init(&dev->mdev[i].rd_lock);
    		spin_lock_init(&dev->mdev[i].lock);
		init_waitqueue_head(&dev->mdev[i].waitq);

		dev->mdev[i].msgq = pMsgq;
		pMsgq = (struct msgq_t *)((u32)pMsgq + (((sizeof(struct msgq_t) + 
				pMsgq->u32QueueSize * (pMsgq->u32MsgSize + 12)) + 0x3F) & ~0x3F));
	}
	
	/* Set the global device structure pointer */
	msgq_dev = dev;

	/* Register IRQ */
    	if (request_irq(msgq_arm_irq, msgq_isr, IRQF_TRIGGER_RISING, MODULE_NAME, dev) < 0) {
        	printk(KERN_ERR "msgq: failed to acquire irq %d\n", dev->irq);
        	goto error_irq;
    	}

	/* Create PROC Entry */
	#ifdef CONFIG_PROC_FS
	{
		ent = create_proc_entry(MODULE_NAME, 0, NULL);
		if (!ent) {
			printk(KERN_ERR "msgq: failed to create proc entry\n");
		}
		ent->proc_fops = &msgq_proc_fops;
		ent->data = dev;
	}	
	#endif

	printk(KERN_INFO "msgq: probed\n");
	return 0;

error_irq:
	msgq_dev = NULL;
error_miscdev:
	for (i = 0; i < dev->mdev_count; i++) {
		if (dev->mdev[i].msgq != NULL) {
			misc_deregister(&dev->mdev[i].dev);
		}
	}
	kfree(dev->mdev);
error_alloc_mdev:
error_noqueue:
    	iounmap(dev->mmio);
error_ioremap:
error_fw:
	kfree(dev);
error_alloc_dev:
	release_firmware(fw_entry);
error_fw_request:
	printk(KERN_ERR "msgq: probe failed\n");
	return ret;
}

static int __devexit msgq_remove(struct platform_device *pdev)
{
	int i;
	struct msgq_dev_t *dev = msgq_dev;

	#ifdef CONFIG_PROC_FS
	{
		remove_proc_entry(MODULE_NAME, NULL);
	}
	#endif

	disable_irq(msgq_arm_irq);
	free_irq(msgq_arm_irq, dev);

	for (i = 0; i < dev->mdev_count; i++) {
		if (dev->mdev[i].msgq != NULL) {
			misc_deregister(&dev->mdev[i].dev);
		}
	}
	kfree(dev->mdev);
    	iounmap(dev->mmio);
	kfree(dev);
	msgq_dev = NULL;
	return 0;
}

static void msgq_platform_release(struct device *device)
{
}

static struct platform_driver msgq_driver = {
	.probe = msgq_probe,
	.remove = __devexit_p(msgq_remove),
	.driver = {
		.name = MODULE_NAME,
		.owner = THIS_MODULE,
	},
};

static struct platform_device msgq_pdevice = {
	.name 	= MODULE_NAME,
	.id 	= 1,
	.dev = {
		.release = msgq_platform_release,
	}
};

static int __init msgq_init(void)
{
	int err = 0;

	err =  platform_driver_register(&msgq_driver);
	if ( err <  0 ) {
		printk( KERN_ERR "Failed to register msgq driver\n" );
		return err;
	}

	err = platform_device_register( &msgq_pdevice );
	if ( err < 0 ) {
		platform_driver_unregister( &msgq_driver );
		printk( KERN_ERR "Failed to register msgq driver\n" );
		return err;
	}
	return err;
}

static void __exit msgq_cleanup(void)
{
	platform_device_unregister( &msgq_pdevice );
	platform_driver_unregister( &msgq_driver );
}

module_init(msgq_init);
module_exit(msgq_cleanup);

MODULE_DESCRIPTION("Message Queue Driver");
MODULE_AUTHOR("Lyrtech RD Inc. <www.lyrtech.com>");
MODULE_LICENSE("GPL");
MODULE_ALIAS("platform:" MODULE_NAME);