Merge branch '3.0-mvf' into colibri_vf

6 files changed, 182 insertions, 72 deletions

diff --git a/arch/arm/mach-mvf/mm.c b/arch/arm/mach-mvf/mm.c
index 957398ccb8f5..e81970a232f9 100644
--- a/arch/arm/mach-mvf/mm.c
+++ b/arch/arm/mach-mvf/mm.c
@@ -69,19 +69,34 @@ int mxc_init_l2x0(void)
 {
 	unsigned int val;
 
-	writel(0x132, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_TAG_LATENCY_CTRL));
-	writel(0x132, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_DATA_LATENCY_CTRL));
+	//Read the fuse bit in MSCM_CPxCFG1 register to determine if L2 cache present.
+	//For the Cortex-A5 core in Vybrid,
+	//if L2 is present, then L2WY = 0x08 (8-way set-associative)
 
-	val = readl(MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_PREFETCH_CTRL));
-	val |= 0x40800000;
-	writel(val, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_PREFETCH_CTRL));
-	val = readl(MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_POWER_CTRL));
-	val |= L2X0_DYNAMIC_CLK_GATING_EN;
-	val |= L2X0_STNDBY_MODE_EN;
-	writel(val, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_POWER_CTRL));
+	val = readl(MVF_IO_ADDRESS(MVF_MSCM_BASE_ADDR + MVF_MSCM_CPxCFG1));
 
-	l2x0_init(MVF_IO_ADDRESS(L2_BASE_ADDR), 0x0, ~0x00000000);
-	return 0;
+	if(((val & MVF_MSCM_L2WY) >> 16) != 0x00)
+	{
+		writel(0x132, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_TAG_LATENCY_CTRL));
+		writel(0x132, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_DATA_LATENCY_CTRL));
+
+		val = readl(MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_PREFETCH_CTRL));
+		val |= 0x40800000;
+		writel(val, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_PREFETCH_CTRL));
+		val = readl(MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_POWER_CTRL));
+		val |= L2X0_DYNAMIC_CLK_GATING_EN;
+		val |= L2X0_STNDBY_MODE_EN;
+		writel(val, MVF_IO_ADDRESS(L2_BASE_ADDR + L2X0_POWER_CTRL));
+
+		l2x0_init(MVF_IO_ADDRESS(L2_BASE_ADDR), 0x0, ~0x00000000);
+		return 0;
+	}
+	else
+	{
+		//No L2 cache present, return no such device / address.
+		printk("L2x0: L2 cache not present");
+		return -ENXIO;
+	}
 }
 
 
diff --git a/arch/arm/mach-mvf/mvf_sema4.c b/arch/arm/mach-mvf/mvf_sema4.c
index 311b56a69d73..4521d124f1b7 100644
--- a/arch/arm/mach-mvf/mvf_sema4.c
+++ b/arch/arm/mach-mvf/mvf_sema4.c
@@ -35,6 +35,8 @@
 #include <linux/time.h>
 //#endif
 
+#include <linux/debugfs.h>
+
 #include <linux/mvf_sema4.h>
 
 // ************************************ Local Data *************************************************
@@ -46,6 +48,9 @@ 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};
 
+// debugfs
+#define DEBUGFS_DIR "mvf_sema4"
+static struct dentry *debugfs_dir;
 
 // ************************************ Interrupt handler *************************************************
 
@@ -66,9 +71,12 @@ static irqreturn_t sema4_irq_handler(int irq, void *dev_id)
 			// make sure there's a gate assigned
 			if(gates[gate_num])
 			{
-				if(gates[gate_num]->use_interrupts)
+				//if(gates[gate_num]->use_interrupts) {
 					// wake up whoever was aiting
 					wake_up_interruptible(&(gates[gate_num]->wait_queue));
+					// bump stats
+					gates[gate_num]->interrupts++;
+				//}
 			}
 		}
 	}
@@ -78,19 +86,6 @@ static irqreturn_t sema4_irq_handler(int irq, void *dev_id)
 
 // ************************************ 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;
@@ -109,15 +104,20 @@ static int initialize(void)
 		return -EIO;
 	}
 
+	// debugfs
+	debugfs_dir = debugfs_create_dir(DEBUGFS_DIR, NULL);
+
 	initialized = true;
 	return 0;
 }
 
-int mvf_sema4_assign(int gate_num, bool use_interrupts, MVF_SEMA4** sema4_p)
+int mvf_sema4_assign(int gate_num, MVF_SEMA4** sema4_p)
 {
 	int retval;
 	u32 cp0ine;
 	unsigned long irq_flags;
+	char debugfs_gatedir_name[4];
+	struct dentry *debugfs_gate_dir;
 
 	// take the opportunity to initialize the whole sub-system
 	if(!initialized)
@@ -136,74 +136,122 @@ int mvf_sema4_assign(int gate_num, bool use_interrupts, MVF_SEMA4** sema4_p)
 	*sema4_p = (MVF_SEMA4 *)kmalloc(sizeof(MVF_SEMA4), GFP_KERNEL);
 	if(*sema4_p == NULL)
 		return -ENOMEM;
+	memset(*sema4_p, 0, sizeof(MVF_SEMA4));
 
 	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);
-	}
+	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);
+
+	// debugfs
+	sprintf(debugfs_gatedir_name, "%d", gate_num);
+	debugfs_gate_dir = debugfs_create_dir(debugfs_gatedir_name, debugfs_dir);
+	debugfs_create_u32("attempts", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->attempts);
+	debugfs_create_u32("interrupts", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->interrupts);
+	debugfs_create_u32("failures", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->failures);
+	debugfs_create_u64("total_latency_us", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->total_latency_us);
+	debugfs_create_u32("worst_latency_us", S_IRUGO | S_IWUGO, debugfs_gate_dir, &(*sema4_p)->worst_latency_us);
 
 	return 0;
 }
+EXPORT_SYMBOL(mvf_sema4_assign);
 
 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;
+	int gate_num;
+	if(!sema4)
+		return -EINVAL;
+	gate_num = sema4->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);
-	}
+	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;
 }
+EXPORT_SYMBOL(mvf_sema4_deassign);
+
+static long delta_time(struct timeval *start) {
+
+	struct timeval now;
+	long now_us, start_us;
+
+	do_gettimeofday(&now);
+
+	now_us = (now.tv_sec * 1000000) + now.tv_usec;
+	start_us = (start->tv_sec * 1000000) + start->tv_usec;
+
+	return now_us > start_us ? now_us - start_us : 0;
+}
+
+static void add_latency_stat(MVF_SEMA4 *sema4) {
+
+	long latency = delta_time(&sema4->request_time);
 
-int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us)
+	sema4->total_latency_us += latency;
+	sema4->worst_latency_us = sema4->worst_latency_us < latency ? latency : sema4->worst_latency_us;
+}
+
+int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us, bool use_interrupts)
 {
 	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)
+	int gate_num;
+	if(!sema4)
 		return -EINVAL;
+	gate_num = sema4->gate_num;
+
+	// bump stats
+	gates[gate_num]->attempts++;
+	do_gettimeofday(&gates[gate_num]->request_time);
 
 	// 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)
+	if(readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) == LOCK_VALUE) {
+		add_latency_stat(gates[gate_num]);
 		return 0;
+	}
 
 	// no timeout, fail
-	if(!timeout_us)
+	if(!timeout_us) {
+		gates[gate_num]->failures++;
 		return -EBUSY;
+	}
+
+	// spin lock?
+	if(!use_interrupts) {
+		while(readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) != LOCK_VALUE) {
+
+			if((timeout_us != 0xffffffff) && (delta_time(&gates[gate_num]->request_time) > timeout_us)) {
+				gates[gate_num]->failures++;
+				return -EBUSY;
+			}
+
+			writeb(LOCK_VALUE, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num);
+		}
+		add_latency_stat(gates[gate_num]);
+		return 0;
+	}
 
 	// 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)))
+		if(wait_event_interruptible(sema4->wait_queue, (readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num) == LOCK_VALUE))) {
+			gates[gate_num]->failures++;
 			return -ERESTARTSYS;
+		}
 	}
 	else
 	{
@@ -211,24 +259,40 @@ int mvf_sema4_lock(MVF_SEMA4 *sema4, unsigned int timeout_us)
 		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)
+		if(retval == 0) {
+			gates[gate_num]->failures++;
 			return -ETIME;
-		else if(retval < 0)
+		}
+		else if(retval < 0) {
+			gates[gate_num]->failures++;
 			return retval;
+		}
 	}
 
+	add_latency_stat(gates[gate_num]);
 	return 0;
 }
+EXPORT_SYMBOL(mvf_sema4_lock);
 
 int mvf_sema4_unlock(MVF_SEMA4 *sema4)
 {
-	int gate_num = find_sema4(sema4);
-	if(gate_num < 0)
-		return gate_num;
+	if(!sema4)
+		return -EINVAL;
 
 	// unlock it
-	writeb(0, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + gate_num);
+	writeb(0, MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + sema4->gate_num);
 
 	return 0;
 }
+EXPORT_SYMBOL(mvf_sema4_unlock);
+
+// return 0 on success (meaning it is set to us)
+int mvf_sema4_test(MVF_SEMA4 *sema4)
+{
+	if(!sema4)
+		return -EINVAL;
+
+	return (readb(MVF_IO_ADDRESS(MVF_SEMA4_BASE_ADDR) + sema4->gate_num)) == LOCK_VALUE ? 0 : 1;
+}
+EXPORT_SYMBOL(mvf_sema4_test);
 
diff --git a/arch/arm/plat-mxc/include/mach/mvf.h b/arch/arm/plat-mxc/include/mach/mvf.h
index 838eef400e95..498a08cbe88f 100644
--- a/arch/arm/plat-mxc/include/mach/mvf.h
+++ b/arch/arm/plat-mxc/include/mach/mvf.h
@@ -190,7 +190,7 @@
 #define MVF_USBC0_BASE_ADDR		0x40034000
 #define MVF_USBC1_BASE_ADDR		0x400B4000
 #define MVF_USBPHY0_BASE_ADDR		0x40050800
-#define MVF_USBPHY1_BASE_ADDR		0x40050B00
+#define MVF_USBPHY1_BASE_ADDR		0x40050C00
 
 #define MVF_MSCM_INT_ROUTER_BASE	(MVF_MSCM_BASE_ADDR + 0x800)
 
@@ -294,6 +294,18 @@
 #define MVF_WKPU_BASE		(MVF_IO_ADDRESS(MVF_WKPU_BASE_ADDR))
 
 /*
+ * defines for MSCM - Misc system control module
+ */
+#define MVF_MSCM_CPxTYPE 	0x0
+#define MVF_MSCM_CPxNUM 	0x04
+#define MVF_MSCM_CPxMASTER 0x8
+#define MVF_MSCM_CPxCOUNT 	0x0c
+#define MVF_MSCM_CPxCFG0 	0x10
+#define MVF_MSCM_CPxCFG1 	0x14
+
+#define MVF_MSCM_L2WY 		0xFF0000
+
+/*
  * defines for SPBA modules
  */
 #define MVF_SPBA_SDHC1	0x04
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 36bad1eaa428..e13e020cab2e 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -439,7 +439,7 @@ 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, 10000000);
+	result = mvf_sema4_lock(sema4, 10000000, true);
 	if(result)
 		return result;
 #endif
@@ -620,9 +620,9 @@ static int __init i2c_imx_probe(struct platform_device *pdev)
 	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");
+	// for makeing sure not in use by MQX concurrently
+	if(mvf_sema4_assign(MVF_I2C_SEMAPHORE_NUMBER, &sema4)) {
+		dev_err(&pdev->dev, "could not assign MQX semaphore %d\n", MVF_I2C_SEMAPHORE_NUMBER);
 		goto fail5;
 	}
 #endif
diff --git a/include/linux/Kbuild b/include/linux/Kbuild
index 947d36f054e5..d2505e354e0c 100644
--- a/include/linux/Kbuild
+++ b/include/linux/Kbuild
@@ -36,6 +36,7 @@ header-y += mxc_sim_interface.h
 header-y += mxc_v4l2.h
 header-y += mxcfb.h
 header-y += mvf-fb.h
+header-y += mvf_sema4.h
 header-y += pmic_adc.h
 header-y += pmic_battery.h
 header-y += pmic_external.h
diff --git a/include/linux/mvf_sema4.h b/include/linux/mvf_sema4.h
index 898065afb23f..4e94276396de 100644
--- a/include/linux/mvf_sema4.h
+++ b/include/linux/mvf_sema4.h
@@ -1,15 +1,33 @@
 #ifndef __MVF_SEMA4__
 #define __MVF_SEMA4__
 
+#include <linux/sched.h>
+
+#define MVF_SHMEM_SEMAPHORE_NUMBER      (1)
+#define MVF_PRINTF_SEMAPHORE_NUMBER     (2)
+#define MVF_I2C_SEMAPHORE_NUMBER        (3)
+#define MVF_RESERVED1_SEMAPHORE_NUMBER  (4)
+#define MVF_RESERVED2_SEMAPHORE_NUMBER  (5)
+
+#ifdef __KERNEL__
+
 typedef struct mvf_sema4_handle_struct {
 	int gate_num;
-	int use_interrupts;
 	wait_queue_head_t wait_queue;
+	// stats
+	u32 attempts;
+	u32 interrupts;
+	u32 failures;
+	struct timeval request_time;
+	u64 total_latency_us;
+	u32 worst_latency_us;
 } MVF_SEMA4;
 
-int mvf_sema4_assign(int gate_num, bool use_interrupts, MVF_SEMA4** sema4_p);
+int mvf_sema4_assign(int gate_num, 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_lock(MVF_SEMA4 *sema4, unsigned int timeout_us, bool use_interrupts);
 int mvf_sema4_unlock(MVF_SEMA4 *sema4);
+int mvf_sema4_test(MVF_SEMA4 *sema4);
 
 #endif
+#endif