char: vf610_sema4: Add support for semaphore module on Vybrid

Add support for semaphore module on Vybrid.

Signed-off-by: Sanchayan Maity <maitysanchayan@gmail.com>
Signed-off-by: Stefan Agner <stefan.agner@toradex.com>

4 files changed, 412 insertions, 0 deletions

diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index a043107da2af..1654733cf429 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -24,6 +24,12 @@ config DEVKMEM
 	  kind of kernel debugging operations.
 	  When in doubt, say "N".
 
+config VF610_SEMA4
+	bool "VF610 SEMA4 driver"
+	depends on SOC_VF610
+	help
+	  Support for VF610 SEMA4 driver, most people should say N here.
+
 config SGI_SNSC
 	bool "SGI Altix system controller communication support"
 	depends on (IA64_SGI_SN2 || IA64_GENERIC)
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index d8a7579300d2..fef56bfad9db 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -59,4 +59,5 @@ obj-$(CONFIG_JS_RTC)		+= js-rtc.o
 js-rtc-y = rtc.o
 
 obj-$(CONFIG_TILE_SROM)		+= tile-srom.o
+obj-$(CONFIG_VF610_SEMA4)	+= vf610_sema4.o
 obj-$(CONFIG_XILLYBUS)		+= xillybus/
diff --git a/drivers/char/vf610_sema4.c b/drivers/char/vf610_sema4.c
new file mode 100644
index 000000000000..352676a307ca
--- /dev/null
+++ b/drivers/char/vf610_sema4.c
@@ -0,0 +1,322 @@
+/*
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ * Copyright (C) 2016 Toradex AG.
+ *
+ * Taken from the 4.1.15 kernel release for i.MX7 by Freescale.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vf610_sema4.h>
+
+static struct vf610_sema4_mutex_device *vf610_sema4;
+
+struct vf610_sema4_mutex *
+vf610_sema4_mutex_create(u32 dev_num, u32 mutex_num)
+{
+	struct vf610_sema4_mutex *mutex_ptr = NULL;
+
+	if (mutex_num >= SEMA4_NUM_GATES || dev_num >= SEMA4_NUM_DEVICES)
+		goto out;
+
+	if (vf610_sema4->cpine_val & (1 < mutex_num)) {
+		pr_err("Error: requiring a allocated sema4.\n");
+		pr_err("mutex_num %d cpine_val 0x%08x.\n",
+				mutex_num, vf610_sema4->cpine_val);
+	}
+	mutex_ptr = kzalloc(sizeof(*mutex_ptr), GFP_KERNEL);
+	if (!mutex_ptr)
+		goto out;
+	vf610_sema4->mutex_ptr[mutex_num] = mutex_ptr;
+	vf610_sema4->alloced |= 1 < mutex_num;
+	vf610_sema4->cpine_val |= idx_sema4[mutex_num];
+	writew(vf610_sema4->cpine_val, vf610_sema4->ioaddr + SEMA4_CP0INE);
+
+	mutex_ptr->valid = CORE_MUTEX_VALID;
+	mutex_ptr->gate_num = mutex_num;
+	init_waitqueue_head(&mutex_ptr->wait_q);
+
+out:
+	return mutex_ptr;
+}
+EXPORT_SYMBOL(vf610_sema4_mutex_create);
+
+int vf610_sema4_mutex_destroy(struct vf610_sema4_mutex *mutex_ptr)
+{
+	u32 mutex_num;
+
+	if ((mutex_ptr == NULL) || (mutex_ptr->valid != CORE_MUTEX_VALID))
+		return -EINVAL;
+
+	mutex_num = mutex_ptr->gate_num;
+	if ((vf610_sema4->cpine_val & idx_sema4[mutex_num]) == 0) {
+		pr_err("Error: trying to destroy a un-allocated sema4.\n");
+		pr_err("mutex_num %d cpine_val 0x%08x.\n",
+				mutex_num, vf610_sema4->cpine_val);
+	}
+	vf610_sema4->mutex_ptr[mutex_num] = NULL;
+	vf610_sema4->alloced &= ~(1 << mutex_num);
+	vf610_sema4->cpine_val &= ~(idx_sema4[mutex_num]);
+	writew(vf610_sema4->cpine_val, vf610_sema4->ioaddr + SEMA4_CP0INE);
+
+	kfree(mutex_ptr);
+
+	return 0;
+}
+EXPORT_SYMBOL(vf610_sema4_mutex_destroy);
+
+int _vf610_sema4_mutex_lock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	int ret = 0, i = 0;
+
+	if ((mutex_ptr == NULL) || (mutex_ptr->valid != CORE_MUTEX_VALID))
+		return -EINVAL;
+
+	i = mutex_ptr->gate_num;
+	mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+	mutex_ptr->gate_val &= SEMA4_GATE_MASK;
+	/* Check to see if this core already own it */
+	if (mutex_ptr->gate_val == SEMA4_A5_LOCK) {
+		/* return -EBUSY, invoker should be in sleep, and re-lock ag */
+		pr_err("%s -> %s %d already locked, wait! num %d val %d.\n",
+				__FILE__, __func__, __LINE__,
+				i, mutex_ptr->gate_val);
+		ret = -EBUSY;
+		goto out;
+	} else {
+		/* try to lock the mutex */
+		mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+		mutex_ptr->gate_val &= (~SEMA4_GATE_MASK);
+		mutex_ptr->gate_val |= SEMA4_A5_LOCK;
+		writeb(mutex_ptr->gate_val, vf610_sema4->ioaddr + i);
+		mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+		mutex_ptr->gate_val &= SEMA4_GATE_MASK;
+		/* double check the mutex is locked, otherwise, return -EBUSY */
+		if (mutex_ptr->gate_val != SEMA4_A5_LOCK) {
+			pr_debug("wait-locked num %d val %d.\n",
+					i, mutex_ptr->gate_val);
+			ret = -EBUSY;
+		}
+	}
+out:
+	return ret;
+}
+
+int vf610_sema4_mutex_trylock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	int ret = 0;
+
+	ret = _vf610_sema4_mutex_lock(mutex_ptr);
+	if (ret == 0)
+		return SEMA4_A5_LOCK;
+	else
+		return ret;
+}
+EXPORT_SYMBOL(vf610_sema4_mutex_trylock);
+
+int vf610_sema4_mutex_lock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vf610_sema4->lock, flags);
+	ret = _vf610_sema4_mutex_lock(mutex_ptr);
+	spin_unlock_irqrestore(&vf610_sema4->lock, flags);
+	while (-EBUSY == ret) {
+		spin_lock_irqsave(&vf610_sema4->lock, flags);
+		ret = _vf610_sema4_mutex_lock(mutex_ptr);
+		spin_unlock_irqrestore(&vf610_sema4->lock, flags);
+		if (ret == 0)
+			break;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(vf610_sema4_mutex_lock);
+
+int vf610_sema4_mutex_unlock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	int ret = 0, i = 0;
+
+	if ((mutex_ptr == NULL) || (mutex_ptr->valid != CORE_MUTEX_VALID))
+		return -EINVAL;
+
+	i = mutex_ptr->gate_num;
+	mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+	mutex_ptr->gate_val &= SEMA4_GATE_MASK;
+	/* make sure it is locked by this core */
+	if (mutex_ptr->gate_val != SEMA4_A5_LOCK) {
+		pr_err("%d Trying to unlock an unlock mutex.\n", __LINE__);
+		ret = -EINVAL;
+		goto out;
+	}
+	/* unlock it */
+	mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+	mutex_ptr->gate_val &= (~SEMA4_GATE_MASK);
+	writeb(mutex_ptr->gate_val | SEMA4_UNLOCK, vf610_sema4->ioaddr + i);
+	mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+	mutex_ptr->gate_val &= SEMA4_GATE_MASK;
+	/* make sure it is locked by this core */
+	if (mutex_ptr->gate_val == SEMA4_A5_LOCK)
+		pr_err("%d ERROR, failed to unlock the mutex.\n", __LINE__);
+
+out:
+	return ret;
+}
+EXPORT_SYMBOL(vf610_sema4_mutex_unlock);
+
+static irqreturn_t vf610_sema4_isr(int irq, void *dev_id)
+{
+	int i;
+	struct vf610_sema4_mutex *mutex_ptr;
+	unsigned int mask;
+	struct vf610_sema4_mutex_device *vf610_sema4 = dev_id;
+
+	vf610_sema4->cpntf_val = readw(vf610_sema4->ioaddr + SEMA4_CP0NTF);
+	for (i = 0; i < SEMA4_NUM_GATES; i++) {
+		mask = idx_sema4[i];
+		if ((vf610_sema4->cpntf_val) & mask) {
+			mutex_ptr = vf610_sema4->mutex_ptr[i];
+			/*
+			 * An interrupt is pending on this mutex, the only way
+			 * to clear it is to lock it (either by this core or
+			 * another).
+			 */
+			mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+			mutex_ptr->gate_val &= (~SEMA4_GATE_MASK);
+			mutex_ptr->gate_val |= SEMA4_A5_LOCK;
+			writeb(mutex_ptr->gate_val, vf610_sema4->ioaddr + i);
+			mutex_ptr->gate_val = readb(vf610_sema4->ioaddr + i);
+			mutex_ptr->gate_val &= SEMA4_GATE_MASK;
+			if (mutex_ptr->gate_val == SEMA4_A5_LOCK) {
+				/*
+				 * wake up the wait queue, whatever there
+				 * are wait task or not.
+				 * NOTE: check gate is locted or not in
+				 * sema4_lock func by wait task.
+				 */
+				mutex_ptr->gate_val =
+					readb(vf610_sema4->ioaddr + i);
+				mutex_ptr->gate_val &= (~SEMA4_GATE_MASK);
+				mutex_ptr->gate_val |= SEMA4_UNLOCK;
+
+				writeb(mutex_ptr->gate_val,
+						vf610_sema4->ioaddr + i);
+				wake_up(&mutex_ptr->wait_q);
+			} else {
+				pr_debug("can't lock gate%d %s retry!\n", i,
+						mutex_ptr->gate_val ?
+						"locked by m4" : "");
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct of_device_id vf610_sema4_dt_ids[] = {
+	{ .compatible = "fsl,vf610-sema4", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, vf610_sema4_dt_ids);
+
+static int vf610_sema4_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	vf610_sema4 = devm_kzalloc(&pdev->dev, sizeof(*vf610_sema4), GFP_KERNEL);
+	if (!vf610_sema4)
+		return -ENOMEM;
+
+	vf610_sema4->dev = &pdev->dev;
+	vf610_sema4->cpine_val = 0;
+	spin_lock_init(&vf610_sema4->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (IS_ERR(res)) {
+		dev_err(&pdev->dev, "unable to get vf610 sema4 resource 0\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	vf610_sema4->ioaddr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(vf610_sema4->ioaddr)) {
+		ret = PTR_ERR(vf610_sema4->ioaddr);
+		goto err;
+	}
+
+	vf610_sema4->irq = platform_get_irq(pdev, 0);
+	if (!vf610_sema4->irq) {
+		dev_err(&pdev->dev, "failed to get irq\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	ret = devm_request_irq(&pdev->dev, vf610_sema4->irq, vf610_sema4_isr,
+				IRQF_SHARED, "vf610-sema4", vf610_sema4);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request vf610 sema4 irq\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	platform_set_drvdata(pdev, vf610_sema4);
+
+err:
+	return ret;
+}
+
+static int vf610_sema4_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver vf610_sema4_driver = {
+	.driver = {
+		   .owner = THIS_MODULE,
+		   .name = "vf610-sema4",
+		   .of_match_table = vf610_sema4_dt_ids,
+		   },
+	.probe = vf610_sema4_probe,
+	.remove = vf610_sema4_remove,
+};
+
+static int __init vf610_sema4_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&vf610_sema4_driver);
+	if (ret)
+		pr_err("Unable to initialize sema4 driver\n");
+	else
+		pr_info("vf610 sema4 driver is registered.\n");
+
+	return ret;
+}
+
+static void __exit vf610_sema4_exit(void)
+{
+	pr_info("vf610 sema4 driver is unregistered.\n");
+	platform_driver_unregister(&vf610_sema4_driver);
+}
+
+module_exit(vf610_sema4_exit);
+module_init(vf610_sema4_init);
+
+MODULE_DESCRIPTION("VF610 SEMA4 driver");
+MODULE_LICENSE("GPL");
diff --git a/include/linux/vf610_sema4.h b/include/linux/vf610_sema4.h
new file mode 100644
index 000000000000..a57de2303e9a
--- /dev/null
+++ b/include/linux/vf610_sema4.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2016 Toradex AG.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __LINUX_VF610_SEMA4_H__
+#define __LINUX_VF610_SEMA4_H__
+
+#define SEMA4_NUM_DEVICES	1
+#define SEMA4_NUM_GATES		16
+
+#define SEMA4_UNLOCK	0x00
+#define SEMA4_A5_LOCK	0x01
+#define SEMA4_GATE_MASK	0x03
+
+#define CORE_MUTEX_VALID	(('c'<<24)|('m'<<24)|('t'<<24)|'x')
+
+/*
+ * The enumerates
+ */
+enum {
+	/* sema4 registers offset */
+	SEMA4_CP0INE	= 0x42,
+	SEMA4_CP1INE	= 0x4A,
+	SEMA4_CP0NTF	= 0x82,
+	SEMA4_CP1NTF	= 0x8A,
+};
+
+static const unsigned int idx_sema4[SEMA4_NUM_GATES] = {
+	1 << 15, 1 << 14, 1 << 13, 1 << 12,
+	1 << 11, 1 << 10, 1 << 9, 1 << 8,
+	1 << 7, 1 << 6, 1 << 5, 1 << 4,
+	1 << 3, 1 << 2, 1 << 1, 1 << 0,
+};
+
+struct vf610_sema4_mutex {
+	u32			valid;
+	u32			gate_num;
+	unsigned char		gate_val;
+	wait_queue_head_t       wait_q;
+};
+
+struct vf610_sema4_mutex_device {
+	struct device		*dev;
+	u16			cpntf_val;
+	u16			cpine_val;
+	void __iomem		*ioaddr;	/* Mapped address */
+	spinlock_t		lock;		/* Mutex */
+	int			irq;
+
+	u16			alloced;
+	struct vf610_sema4_mutex	*mutex_ptr[SEMA4_NUM_GATES];
+};
+
+struct vf610_sema4_mutex *
+	vf610_sema4_mutex_create(u32 dev_num, u32 mutex_num);
+#ifdef CONFIG_VF610_SEMA4
+int vf610_sema4_mutex_destroy(struct vf610_sema4_mutex *mutex_ptr);
+int vf610_sema4_mutex_trylock(struct vf610_sema4_mutex *mutex_ptr);
+int vf610_sema4_mutex_lock(struct vf610_sema4_mutex *mutex_ptr);
+int vf610_sema4_mutex_unlock(struct vf610_sema4_mutex *mutex_ptr);
+#else
+static inline int vf610_sema4_mutex_destroy(struct vf610_sema4_mutex *mutex_ptr)
+{
+	return 0;
+}
+static inline int vf610_sema4_mutex_trylock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	return 0;
+}
+static inline int vf610_sema4_mutex_lock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	return 0;
+}
+static inline int vf610_sema4_mutex_unlock(struct vf610_sema4_mutex *mutex_ptr)
+{
+	return 0;
+}
+#endif
+#endif /* __LINUX_VF610_SEMA4_H__ */