add semaphore protection with MQX of I2C bus

4 files changed, 272 insertions, 1 deletions

diff --git a/arch/arm/mach-mvf/Makefile b/arch/arm/mach-mvf/Makefile
index e25fb28b7216..fdbd22479609 100644
--- a/arch/arm/mach-mvf/Makefile
+++ b/arch/arm/mach-mvf/Makefile
@@ -3,7 +3,7 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o mvf_fec.o usb_dr.o usb_dr2.o pm.o
+obj-y   := cpu.o mm.o system.o devices.o dummy_gpio.o irq.o bus_freq.o mvf_fec.o usb_dr.o usb_dr2.o pm.o mvf_sema4.o
 obj-y += l2switch.o
 
 obj-$(CONFIG_ARCH_MVFA5) += clock.o mvf_suspend.o
diff --git a/arch/arm/mach-mvf/mvf_sema4.c b/arch/arm/mach-mvf/mvf_sema4.c
new file mode 100644
index 000000000000..311b56a69d73
--- /dev/null
+++ b/arch/arm/mach-mvf/mvf_sema4.c
@@ -0,0 +1,234 @@
+/*
+* header goes here
+*/
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+
+#include <mach/mvf.h>
+
+#define NUM_GATES 16
+#define MASK_FROM_GATE(gate_num) ((u32)(1 << (NUM_GATES*2 - 1 - idx[gate_num])))
+
+#define THIS_CORE (0)
+#define LOCK_VALUE (THIS_CORE + 1)
+
+#define SEMA4_CP0INE (MVF_SEMA4_BASE_ADDR + 0x40)
+#define SEMA4_CP0NTF (MVF_SEMA4_BASE_ADDR + 0x80)
+
+//#if 0
+#include <linux/mm.h>
+#include <linux/version.h>
+#include <linux/kdev_t.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <mach/hardware.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/time.h>
+//#endif
+
+#include <linux/mvf_sema4.h>
+
+// ************************************ Local Data *************************************************
+
+static MVF_SEMA4* gates[NUM_GATES];
+
+static bool initialized = false;
+
+// account for the way the bits are set / returned in CP0INE and CP0NTF
+static const int idx[16] = {3, 2, 1, 0, 7, 6, 5, 4, 11, 10, 9, 8, 15, 14, 13, 12};
+
+
+// ************************************ Interrupt handler *************************************************
+
+static irqreturn_t sema4_irq_handler(int irq, void *dev_id)
+{
+	int gate_num;
+
+	u32 cp0ntf = readl(MVF_IO_ADDRESS(SEMA4_CP0NTF));
+
+	for(gate_num=0; gate_num<NUM_GATES; gate_num++)
+	{
+		// interrupt from this gate?
+		if(cp0ntf & MASK_FROM_GATE(gate_num))
+		{
+			// grab the gate to stop the interrupts
+			writeb(LOCK_VALUE, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num);
+
+			// make sure there's a gate assigned
+			if(gates[gate_num])
+			{
+				if(gates[gate_num]->use_interrupts)
+					// wake up whoever was aiting
+					wake_up_interruptible(&(gates[gate_num]->wait_queue));
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+// ************************************ Utility functions *************************************************
+
+static int find_sema4(MVF_SEMA4 *sema4)
+{
+	int i;
+
+	for(i=0; i<NUM_GATES; i++)
+	{
+		if(gates[i] == sema4)
+			return i;
+	}
+
+	return -EINVAL;
+}
+
+static int initialize(void)
+{
+	int i;
+
+	// clear the gates table
+	for(i=0; i<NUM_GATES; i++)
+		gates[i] = NULL;
+
+	// clear out all notification requests
+	writel(0, MVF_IO_ADDRESS(SEMA4_CP0INE));
+
+	//Register the interrupt handler
+	if (request_irq(MVF_INT_SEMA4, sema4_irq_handler, 0, "mvf_sema4_handler", NULL) != 0)
+	{
+		printk(KERN_ERR "Failed to register MVF_INT_SEMA4 interrupt.\n");
+		return -EIO;
+	}
+
+	initialized = true;
+	return 0;
+}
+
+int mvf_sema4_assign(int gate_num, bool use_interrupts, MVF_SEMA4** sema4_p)
+{
+	int retval;
+	u32 cp0ine;
+	unsigned long irq_flags;
+
+	// take the opportunity to initialize the whole sub-system
+	if(!initialized)
+	{
+		retval = initialize();
+		if(retval)
+			return retval;
+	}
+
+	if((gate_num < 0) || (gate_num >= NUM_GATES))
+		return -EINVAL;
+
+	if(gates[gate_num])
+		return -EBUSY;
+
+	*sema4_p = (MVF_SEMA4 *)kmalloc(sizeof(MVF_SEMA4), GFP_KERNEL);
+	if(*sema4_p == NULL)
+		return -ENOMEM;
+
+	gates[gate_num] = *sema4_p;
+	(*sema4_p)->gate_num = gate_num;
+	(*sema4_p)->use_interrupts = use_interrupts;
+
+	if(use_interrupts)
+	{
+		init_waitqueue_head(&((*sema4_p)->wait_queue));
+		local_irq_save(irq_flags);
+		cp0ine = readl(MVF_IO_ADDRESS(SEMA4_CP0INE));
+		cp0ine |= MASK_FROM_GATE(gate_num);
+		writel(cp0ine, MVF_IO_ADDRESS(SEMA4_CP0INE));
+		local_irq_restore(irq_flags);
+	}
+
+	return 0;
+}
+
+int mvf_sema4_deassign(MVF_SEMA4 *sema4)
+{
+	u32 cp0ine;
+	unsigned long irq_flags;
+
+	int gate_num = find_sema4(sema4);
+	if(gate_num < 0)
+		return gate_num;
+
+	if(sema4->use_interrupts)
+	{
+		local_irq_save(irq_flags);
+		cp0ine = readl(MVF_IO_ADDRESS(SEMA4_CP0INE));
+		cp0ine &= ~MASK_FROM_GATE(gate_num);
+		writel(cp0ine, MVF_IO_ADDRESS(SEMA4_CP0INE));
+		local_irq_restore(irq_flags);
+	}
+
+	kfree(sema4);
+	gates[gate_num] = NULL;
+
+	return 0;
+}
+
+int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us)
+{
+	int retval;
+	int gate_num = find_sema4(sema4);
+	if(gate_num < 0)
+		return gate_num;
+
+	// cant use timeouts if not using interruppts
+	// TODO use spin lock if not using interrupts
+	if((!sema4->use_interrupts) && timeout_us)
+		return -EINVAL;
+
+	// try to grab it
+	writeb(LOCK_VALUE, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num);
+	if(readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) == LOCK_VALUE)
+		return 0;
+
+	// no timeout, fail
+	if(!timeout_us)
+		return -EBUSY;
+
+	// wait forever?
+	if(timeout_us == 0xffffffff)
+	{
+		if(wait_event_interruptible(sema4->wait_queue, (readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) == LOCK_VALUE)))
+			return -ERESTARTSYS;
+	}
+	else
+	{
+		// return: 0 = timeout, >0 = woke up with that many jiffies left, <0 = error
+		retval = wait_event_interruptible_timeout(sema4->wait_queue,
+							(readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) == LOCK_VALUE),
+							usecs_to_jiffies(timeout_us));
+		if(retval == 0)
+			return -ETIME;
+		else if(retval < 0)
+			return retval;
+	}
+
+	return 0;
+}
+
+int mvf_sema4_unlock(MVF_SEMA4 *sema4)
+{
+	int gate_num = find_sema4(sema4);
+	if(gate_num < 0)
+		return gate_num;
+
+	// unlock it
+	writeb(0, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num);
+
+	return 0;
+}
+
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 71b8cd4f8a3e..7741e8036997 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -52,6 +52,7 @@
 #include <mach/irqs.h>
 #include <mach/hardware.h>
 #include <mach/i2c.h>
+#include <linux/mvf_sema4.h>
 
 /** Defines ********************************************************************
 *******************************************************************************/
@@ -122,6 +123,8 @@ static u16 __initdata i2c_clk_div[60][2] = {
 	{ 2304,	0x3C },	{ 2560,	0x3D },	{ 3072,	0x3E }, { 3584,	0x7A },
 	{ 3840,	0x3F }, { 4096,	0x7B }, { 5120,	0x7D },	{ 6144,	0x7E },
 };
+
+static MVF_SEMA4* sema4;
 #else
 static u16 __initdata i2c_clk_div[50][2] = {
 	{ 22,	0x20 }, { 24,	0x21 }, { 26,	0x22 }, { 28,	0x23 },
@@ -152,6 +155,7 @@ struct imx_i2c_struct {
 	unsigned int		ifdr; /* IMX_I2C_IFDR */
 };
 
+
 /** Functions for IMX I2C adapter driver ***************************************
 *******************************************************************************/
 
@@ -434,6 +438,12 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
+#ifdef CONFIG_ARCH_MVF
+	result = mvf_sema4_lock(sema4, 1000000);
+	if(result)
+		return result;
+#endif
+
 	/* Start I2C transfer */
 	result = i2c_imx_start(i2c_imx);
 	if (result)
@@ -482,6 +492,10 @@ fail0:
 	/* Stop I2C transfer */
 	i2c_imx_stop(i2c_imx);
 
+#ifdef CONFIG_ARCH_MVF
+	mvf_sema4_unlock(sema4);
+#endif
+
 	dev_dbg(&i2c_imx->adapter.dev, "<%s> exit with: %s: %d\n", __func__,
 		(result < 0) ? "error" : "success msg",
 			(result < 0) ? result : num);
@@ -605,6 +619,14 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	/* Set up platform driver data */
 	platform_set_drvdata(pdev, i2c_imx);
 
+#ifdef CONFIG_ARCH_MVF
+	// make sure not in use by MQX
+	if(mvf_sema4_assign(3, true, &sema4)) {
+		dev_err(&pdev->dev, "could not grab MQX semaphore\n");
+		goto fail5;
+	}
+#endif
+
 	dev_dbg(&i2c_imx->adapter.dev, "claimed irq %d\n", i2c_imx->irq);
 	dev_dbg(&i2c_imx->adapter.dev, "device resources from 0x%x to 0x%x\n",
 		i2c_imx->res->start, i2c_imx->res->end);
diff --git a/include/linux/mvf_sema4.h b/include/linux/mvf_sema4.h
new file mode 100644
index 000000000000..898065afb23f
--- /dev/null
+++ b/include/linux/mvf_sema4.h
@@ -0,0 +1,15 @@
+#ifndef __MVF_SEMA4__
+#define __MVF_SEMA4__
+
+typedef struct mvf_sema4_handle_struct {
+	int gate_num;
+	int use_interrupts;
+	wait_queue_head_t wait_queue;
+} MVF_SEMA4;
+
+int mvf_sema4_assign(int gate_num, bool use_interrupts, MVF_SEMA4** sema4_p);
+int mvf_sema4_deassign(MVF_SEMA4 *sema4);
+int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us);
+int mvf_sema4_unlock(MVF_SEMA4 *sema4);
+
+#endif