IKXCLOCK-870 Add M4 Fusion Driver

Added a separate IIO fusion driver to handle M4 fusion data.
Removed duplicate mutex_destroy() calls.
Deleted unused pressure sensor header.

Change-Id: I2f24a30dbba5b84f590618e3b506d04c92d2a136
Signed-off-by: Eric Tashakkor <w36098@motorola.com>

6 files changed, 442 insertions, 8 deletions

diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 47438c892e3..2f037745c9b 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -63,6 +63,7 @@ obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_display.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_passive.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_als.o
 obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_heartrate.o
+obj-$(CONFIG_MFD_M4SENSORHUB)   += m4sensorhub_fusion.o
 obj-$(CONFIG_VIB_GPIO)          += vib-gpio.o
 obj-$(CONFIG_C55_CTRL)          += c55_ctrl.o
 obj-$(CONFIG_MMI_FACTORY)       += mmi-factory.o
diff --git a/drivers/misc/m4sensorhub_fusion.c b/drivers/misc/m4sensorhub_fusion.c
new file mode 100644
index 00000000000..bd3d426fd78
--- /dev/null
+++ b/drivers/misc/m4sensorhub_fusion.c
@@ -0,0 +1,441 @@
+/*
+ *  Copyright (C) 2014 Motorola, Inc.
+ *
+ *  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; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  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
+ *
+ *  Adds ability to program periodic interrupts from user space that
+ *  can wake the phone out of low power modes.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/fs.h>
+#include <linux/m4sensorhub.h>
+#include <linux/slab.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/types.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/events.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/m4sensorhub/m4sensorhub_fusion.h>
+
+#define m4fus_err(format, args...)  KDEBUG(M4SH_ERROR, format, ## args)
+
+#define M4FUS_IRQ_ENABLED_BIT       0
+
+struct m4fus_driver_data {
+	struct platform_device      *pdev;
+	struct m4sensorhub_data     *m4;
+	struct mutex                mutex; /* controls driver entry points */
+
+	struct m4sensorhub_fusion_iio_data   iiodat;
+	int16_t         samplerate;
+	uint16_t        status;
+};
+
+
+static void m4fus_isr(enum m4sensorhub_irqs int_event, void *handle)
+{
+	int err = 0;
+	struct iio_dev *iio = handle;
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	int size = 0;
+
+	mutex_lock(&(dd->mutex));
+	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_ROTATIONVECTOR);
+	if (size > (sizeof(int32_t) * ARRAY_SIZE(dd->iiodat.values))) {
+		m4fus_err("%s: M4 register is too large.\n", __func__);
+		err = -EOVERFLOW;
+		goto m4fus_isr_fail;
+	}
+
+	err = m4sensorhub_reg_read(dd->m4, M4SH_REG_FUSION_ROTATIONVECTOR,
+		(char *)&(dd->iiodat.values[0]));
+	if (err < 0) {
+		m4fus_err("%s: Failed to read rotation data.\n", __func__);
+		goto m4fus_isr_fail;
+	} else if (err != size) {
+		m4fus_err("%s: Read %d bytes instead of %d for %s.\n",
+			  __func__, err, size, "rotation");
+		err = -EBADE;
+		goto m4fus_isr_fail;
+	}
+
+	dd->iiodat.type = FUSION_TYPE_ROTATION;
+	dd->iiodat.timestamp = iio_get_time_ns();
+	iio_push_to_buffers(iio, (unsigned char *)&(dd->iiodat));
+
+m4fus_isr_fail:
+	if (err < 0)
+		m4fus_err("%s: Failed with error code %d.\n", __func__, err);
+
+	mutex_unlock(&(dd->mutex));
+
+	return;
+}
+
+static int m4fus_set_samplerate(struct iio_dev *iio, int16_t rate)
+{
+	int err = 0;
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	int size = 0;
+
+	if (rate == dd->samplerate)
+		goto m4fus_set_samplerate_irq_check;
+
+	size = m4sensorhub_reg_getsize(dd->m4, M4SH_REG_FUSION_SAMPLERATE);
+	err = m4sensorhub_reg_write(dd->m4, M4SH_REG_FUSION_SAMPLERATE,
+		(char *)&rate, m4sh_no_mask);
+	if (err < 0) {
+		m4fus_err("%s: Failed to set sample rate.\n", __func__);
+		goto m4fus_set_samplerate_fail;
+	} else if (err != size) {
+		m4fus_err("%s:  Wrote %d bytes instead of %d.\n",
+			  __func__, err, size);
+		err = -EBADE;
+		goto m4fus_set_samplerate_fail;
+	}
+	dd->samplerate = rate;
+
+m4fus_set_samplerate_irq_check:
+	if (rate >= 0) {
+		/* Enable the IRQ if necessary */
+		if (!(dd->status & (1 << M4FUS_IRQ_ENABLED_BIT))) {
+			err = m4sensorhub_irq_enable(dd->m4,
+				M4SH_IRQ_FUSION_DATA_READY);
+			if (err < 0) {
+				m4fus_err("%s: Failed to enable irq.\n",
+					  __func__);
+				goto m4fus_set_samplerate_fail;
+			}
+			dd->status = dd->status | (1 << M4FUS_IRQ_ENABLED_BIT);
+		}
+	} else {
+		/* Disable the IRQ if necessary */
+		if (dd->status & (1 << M4FUS_IRQ_ENABLED_BIT)) {
+			err = m4sensorhub_irq_disable(dd->m4,
+				M4SH_IRQ_FUSION_DATA_READY);
+			if (err < 0) {
+				m4fus_err("%s: Failed to disable irq.\n",
+					  __func__);
+				goto m4fus_set_samplerate_fail;
+			}
+			dd->status = dd->status & ~(1 << M4FUS_IRQ_ENABLED_BIT);
+		}
+	}
+
+m4fus_set_samplerate_fail:
+	return err;
+}
+
+static ssize_t m4fus_setrate_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct iio_dev *iio = platform_get_drvdata(pdev);
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	ssize_t size = 0;
+
+	mutex_lock(&(dd->mutex));
+	size = snprintf(buf, PAGE_SIZE, "Current rate: %hd\n", dd->samplerate);
+	mutex_unlock(&(dd->mutex));
+	return size;
+}
+static ssize_t m4fus_setrate_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	int err = 0;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct iio_dev *iio = platform_get_drvdata(pdev);
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	int value = 0;
+
+	mutex_lock(&(dd->mutex));
+
+	err = kstrtoint(buf, 10, &value);
+	if (err < 0) {
+		m4fus_err("%s: Failed to convert value.\n", __func__);
+		goto m4fus_enable_store_exit;
+	}
+
+	if ((value < -1) || (value > 32767)) {
+		m4fus_err("%s: Invalid samplerate %d passed.\n",
+			  __func__, value);
+		err = -EINVAL;
+		goto m4fus_enable_store_exit;
+	}
+
+	err = m4fus_set_samplerate(iio, value);
+	if (err < 0) {
+		m4fus_err("%s: Failed to set sample rate.\n", __func__);
+		goto m4fus_enable_store_exit;
+	}
+
+m4fus_enable_store_exit:
+	if (err < 0) {
+		m4fus_err("%s: Failed with error code %d.\n", __func__, err);
+		size = err;
+	}
+
+	mutex_unlock(&(dd->mutex));
+
+	return size;
+}
+static IIO_DEVICE_ATTR(setrate, S_IRUSR | S_IWUSR,
+		m4fus_setrate_show, m4fus_setrate_store, 0);
+
+static ssize_t m4fus_iiodata_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct iio_dev *iio = platform_get_drvdata(pdev);
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	ssize_t size = 0;
+
+	mutex_lock(&(dd->mutex));
+	size = snprintf(buf, PAGE_SIZE, "%s%d\n%s%d\n%s%d\n%s%d\n",
+		"rotation[0]: ", dd->iiodat.values[0],
+		"rotation[1]: ", dd->iiodat.values[1],
+		"rotation[2]: ", dd->iiodat.values[2],
+		"rotation[3]: ", dd->iiodat.values[3]);
+	mutex_unlock(&(dd->mutex));
+	return size;
+}
+static IIO_DEVICE_ATTR(iiodata, S_IRUGO, m4fus_iiodata_show, NULL, 0);
+
+static struct attribute *m4fus_iio_attributes[] = {
+	&iio_dev_attr_setrate.dev_attr.attr,
+	&iio_dev_attr_iiodata.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group m4fus_iio_attr_group = {
+	.attrs = m4fus_iio_attributes,
+};
+
+static const struct iio_info m4fus_iio_info = {
+	.driver_module = THIS_MODULE,
+	.attrs = &m4fus_iio_attr_group,
+};
+
+static const struct iio_chan_spec m4fus_iio_channels[] = {
+	{
+		.type = IIO_FUSION,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = M4FUS_DATA_STRUCT_SIZE_BITS,
+			.storagebits = M4FUS_DATA_STRUCT_SIZE_BITS,
+			.shift = 0,
+		},
+	},
+};
+
+static void m4fus_remove_iiodev(struct iio_dev *iio)
+{
+	struct m4fus_driver_data *dd = iio_priv(iio);
+
+	/* Remember, only call when dd->mutex is locked */
+	iio_kfifo_free(iio->buffer);
+	iio_buffer_unregister(iio);
+	iio_device_unregister(iio);
+	mutex_destroy(&(dd->mutex));
+	iio_device_free(iio); /* dd is freed here */
+	return;
+}
+
+static int m4fus_create_iiodev(struct iio_dev *iio)
+{
+	int err = 0;
+	struct m4fus_driver_data *dd = iio_priv(iio);
+
+	iio->name = M4FUS_DRIVER_NAME;
+	iio->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_HARDWARE;
+	iio->num_channels = 1;
+	iio->info = &m4fus_iio_info;
+	iio->channels = m4fus_iio_channels;
+
+	iio->buffer = iio_kfifo_allocate(iio);
+	if (iio->buffer == NULL) {
+		m4fus_err("%s: Failed to allocate IIO buffer.\n", __func__);
+		err = -ENOMEM;
+		goto m4fus_create_iiodev_kfifo_fail;
+	}
+
+	iio->buffer->scan_timestamp = true;
+	iio->buffer->access->set_bytes_per_datum(iio->buffer,
+		sizeof(dd->iiodat));
+	err = iio_buffer_register(iio, iio->channels, iio->num_channels);
+	if (err < 0) {
+		m4fus_err("%s: Failed to register IIO buffer.\n", __func__);
+		goto m4fus_create_iiodev_buffer_fail;
+	}
+
+	err = iio_device_register(iio);
+	if (err < 0) {
+		m4fus_err("%s: Failed to register IIO device.\n", __func__);
+		goto m4fus_create_iiodev_iioreg_fail;
+	}
+
+	goto m4fus_create_iiodev_exit;
+
+m4fus_create_iiodev_iioreg_fail:
+	iio_buffer_unregister(iio);
+m4fus_create_iiodev_buffer_fail:
+	iio_kfifo_free(iio->buffer);
+m4fus_create_iiodev_kfifo_fail:
+	iio_device_free(iio); /* dd is freed here */
+m4fus_create_iiodev_exit:
+	return err;
+}
+
+static int m4fus_driver_init(struct init_calldata *p_arg)
+{
+	struct iio_dev *iio = p_arg->p_data;
+	struct m4fus_driver_data *dd = iio_priv(iio);
+	int err = 0;
+
+	mutex_lock(&(dd->mutex));
+
+	dd->m4 = p_arg->p_m4sensorhub_data;
+	if (dd->m4 == NULL) {
+		m4fus_err("%s: M4 sensor data is NULL.\n", __func__);
+		err = -ENODATA;
+		goto m4fus_driver_init_fail;
+	}
+
+	err = m4sensorhub_irq_register(dd->m4,
+		M4SH_IRQ_FUSION_DATA_READY, m4fus_isr, iio);
+	if (err < 0) {
+		m4fus_err("%s: Failed to register M4 IRQ.\n", __func__);
+		goto m4fus_driver_init_fail;
+	}
+
+	goto m4fus_driver_init_exit;
+
+m4fus_driver_init_fail:
+	m4fus_err("%s: Init failed with error code %d.\n", __func__, err);
+m4fus_driver_init_exit:
+	mutex_unlock(&(dd->mutex));
+	return err;
+}
+
+static int m4fus_probe(struct platform_device *pdev)
+{
+	struct m4fus_driver_data *dd = NULL;
+	struct iio_dev *iio = NULL;
+	int err = 0;
+
+	iio = iio_device_alloc(sizeof(dd));
+	if (iio == NULL) {
+		m4fus_err("%s: Failed to allocate IIO data.\n", __func__);
+		err = -ENOMEM;
+		goto m4fus_probe_fail_noiio;
+	}
+
+	dd = iio_priv(iio);
+	dd->pdev = pdev;
+	mutex_init(&(dd->mutex));
+	platform_set_drvdata(pdev, iio);
+	dd->samplerate = -1; /* We always start disabled */
+
+	err = m4fus_create_iiodev(iio); /* iio and dd are freed on fail */
+	if (err < 0) {
+		m4fus_err("%s: Failed to create IIO device.\n", __func__);
+		goto m4fus_probe_fail_noiio;
+	}
+
+	err = m4sensorhub_register_initcall(m4fus_driver_init, iio);
+	if (err < 0) {
+		m4fus_err("%s: Failed to register initcall.\n", __func__);
+		goto m4fus_probe_fail;
+	}
+
+	return 0;
+
+m4fus_probe_fail:
+	m4fus_remove_iiodev(iio); /* iio and dd are freed here */
+m4fus_probe_fail_noiio:
+	m4fus_err("%s: Probe failed with error code %d.\n", __func__, err);
+	return err;
+}
+
+static int __exit m4fus_remove(struct platform_device *pdev)
+{
+	struct iio_dev *iio = platform_get_drvdata(pdev);
+	struct m4fus_driver_data *dd = NULL;
+
+	if (iio == NULL)
+		goto m4fus_remove_exit;
+
+	dd = iio_priv(iio);
+	if (dd == NULL)
+		goto m4fus_remove_exit;
+
+	mutex_lock(&(dd->mutex));
+	if (dd->status & (1 << M4FUS_IRQ_ENABLED_BIT)) {
+		m4sensorhub_irq_disable(dd->m4, M4SH_IRQ_FUSION_DATA_READY);
+		dd->status = dd->status & ~(1 << M4FUS_IRQ_ENABLED_BIT);
+	}
+	m4sensorhub_irq_unregister(dd->m4, M4SH_IRQ_FUSION_DATA_READY);
+	m4sensorhub_unregister_initcall(m4fus_driver_init);
+	m4fus_remove_iiodev(iio);  /* dd is freed here */
+
+m4fus_remove_exit:
+	return 0;
+}
+
+static struct of_device_id m4fusion_match_tbl[] = {
+	{ .compatible = "mot,m4fusion" },
+	{},
+};
+
+static struct platform_driver m4fus_driver = {
+	.probe		= m4fus_probe,
+	.remove		= __exit_p(m4fus_remove),
+	.shutdown	= NULL,
+	.suspend	= NULL,
+	.resume		= NULL,
+	.driver		= {
+		.name	= M4FUS_DRIVER_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(m4fusion_match_tbl),
+	},
+};
+
+static int __init m4fus_init(void)
+{
+	return platform_driver_register(&m4fus_driver);
+}
+
+static void __exit m4fus_exit(void)
+{
+	platform_driver_unregister(&m4fus_driver);
+}
+
+module_init(m4fus_init);
+module_exit(m4fus_exit);
+
+MODULE_ALIAS("platform:m4fus");
+MODULE_DESCRIPTION("M4 Sensor Hub Fusion client driver");
+MODULE_AUTHOR("Motorola");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/m4sensorhub_gesture.c b/drivers/misc/m4sensorhub_gesture.c
index 2686a0f9501..22a972f3dd9 100644
--- a/drivers/misc/m4sensorhub_gesture.c
+++ b/drivers/misc/m4sensorhub_gesture.c
@@ -381,7 +381,6 @@ static int m4ges_probe(struct platform_device *pdev)
 	return 0;
 
 m4ges_probe_fail:
-	mutex_destroy(&(dd->mutex));
 	m4ges_remove_iiodev(iio); /* iio and dd are freed here */
 m4ges_probe_fail_noiio:
 	m4ges_err("%s: Probe failed with error code %d.\n", __func__, err);
@@ -409,7 +408,6 @@ static int __exit m4ges_remove(struct platform_device *pdev)
 	m4sensorhub_irq_unregister(dd->m4,
 				   M4SH_IRQ_GESTURE_DETECTED);
 	m4sensorhub_unregister_initcall(m4ges_driver_init);
-	mutex_destroy(&(dd->mutex));
 	m4ges_remove_iiodev(iio);  /* dd is freed here */
 
 m4ges_remove_exit:
diff --git a/drivers/misc/m4sensorhub_heartrate.c b/drivers/misc/m4sensorhub_heartrate.c
index feb9ab0b54a..fc207b32834 100644
--- a/drivers/misc/m4sensorhub_heartrate.c
+++ b/drivers/misc/m4sensorhub_heartrate.c
@@ -531,7 +531,6 @@ static int m4hrt_probe(struct platform_device *pdev)
 	return 0;
 
 m4hrt_probe_fail:
-	mutex_destroy(&(dd->mutex));
 	m4hrt_remove_iiodev(iio); /* iio and dd are freed here */
 m4hrt_probe_fail_noiio:
 	m4hrt_err("%s: Probe failed with error code %d.\n", __func__, err);
@@ -559,7 +558,6 @@ static int __exit m4hrt_remove(struct platform_device *pdev)
 	m4sensorhub_irq_unregister(dd->m4,
 				   M4SH_IRQ_HRSENSOR_DATA_READY);
 	m4sensorhub_unregister_initcall(m4hrt_driver_init);
-	mutex_destroy(&(dd->mutex));
 	m4hrt_remove_iiodev(iio);  /* dd is freed here */
 
 m4hrt_remove_exit:
diff --git a/drivers/misc/m4sensorhub_passive.c b/drivers/misc/m4sensorhub_passive.c
index e072d8b0a5e..c62ae7fac97 100644
--- a/drivers/misc/m4sensorhub_passive.c
+++ b/drivers/misc/m4sensorhub_passive.c
@@ -412,7 +412,6 @@ static int m4pas_probe(struct platform_device *pdev)
 	return 0;
 
 m4pas_probe_fail:
-	mutex_destroy(&(dd->mutex));
 	m4pas_remove_iiodev(iio); /* iio and dd are freed here */
 m4pas_probe_fail_noiio:
 	m4pas_err("%s: Probe failed with error code %d.\n", __func__, err);
@@ -440,7 +439,6 @@ static int __exit m4pas_remove(struct platform_device *pdev)
 	m4sensorhub_irq_unregister(dd->m4,
 				   M4SH_IRQ_PASSIVE_BUFFER_FULL);
 	m4sensorhub_unregister_initcall(m4pas_driver_init);
-	mutex_destroy(&(dd->mutex));
 	m4pas_remove_iiodev(iio);  /* dd is freed here */
 
 m4pas_remove_exit:
diff --git a/drivers/misc/m4sensorhub_pedometer.c b/drivers/misc/m4sensorhub_pedometer.c
index ad05e8dba8c..ce932cc4df9 100644
--- a/drivers/misc/m4sensorhub_pedometer.c
+++ b/drivers/misc/m4sensorhub_pedometer.c
@@ -476,7 +476,6 @@ static int m4ped_probe(struct platform_device *pdev)
 	return 0;
 
 m4ped_probe_fail:
-	mutex_destroy(&(dd->mutex));
 	m4ped_remove_iiodev(iio); /* iio and dd are freed here */
 m4ped_probe_fail_noiio:
 	m4ped_err("%s: Probe failed with error code %d.\n", __func__, err);
@@ -508,7 +507,6 @@ static int __exit m4ped_remove(struct platform_device *pdev)
 	m4sensorhub_irq_unregister(dd->m4,
 				   M4SH_IRQ_ACTIVITY_CHANGE);
 	m4sensorhub_unregister_initcall(m4ped_driver_init);
-	mutex_destroy(&(dd->mutex));
 	m4ped_remove_iiodev(iio);  /* dd is freed here */
 
 m4ped_remove_exit: