[PATCH 08/10] iio: cros_ec_sensors_ring: add ChromeOS EC Sensors Ring

[Enric Balletbo i Serra]

Add support for handling sensor events FIFO produced by the sensor
hub. A single device with a buffer will collect all samples produced
by the sensors managed by the CrosEC sensor hub.

Signed-off-by: Guenter Roeck <groeck@xxxxxxxxxxxx>
Signed-off-by: Enric Balletbo i Serra <enric.balletbo@xxxxxxxxxxxxx>
---
 drivers/iio/common/cros_ec_sensors/Kconfig         |   9 +
 drivers/iio/common/cros_ec_sensors/Makefile        |   1 +
 .../common/cros_ec_sensors/cros_ec_sensors_ring.c  | 541 +++++++++++++++++++++
 3 files changed, 551 insertions(+)
 create mode 100644 drivers/iio/common/cros_ec_sensors/cros_ec_sensors_ring.c

diff --git a/drivers/iio/common/cros_ec_sensors/Kconfig b/drivers/iio/common/cros_ec_sensors/Kconfig
index 22b4211..778c3bf 100644
--- a/drivers/iio/common/cros_ec_sensors/Kconfig
+++ b/drivers/iio/common/cros_ec_sensors/Kconfig
@@ -39,3 +39,12 @@ config IIO_CROS_EC_ACTIVITY
 	  Activities can be simple (low/no motion) or more complex (riding train).
 	  They are being reported by physical devices or the EC itself.
 	  Creates an IIO device to manage all activities.
+
+config IIO_CROS_EC_SENSORS_RING
+	tristate "ChromeOS EC Sensors Ring"
+	depends on IIO_CROS_EC_SENSORS || IIO_CROS_EC_LIGHT_PROX
+	help
+	  Add support for handling sensor events FIFO produced by
+	  the sensor hub.
+	  A single device with a buffer will collect all samples produced
+	  by the sensors managed by the CrosEC sensor hub
diff --git a/drivers/iio/common/cros_ec_sensors/Makefile b/drivers/iio/common/cros_ec_sensors/Makefile
index 8f54f1e..0eb3fc5 100644
--- a/drivers/iio/common/cros_ec_sensors/Makefile
+++ b/drivers/iio/common/cros_ec_sensors/Makefile
@@ -6,3 +6,4 @@ obj-$(CONFIG_IIO_CROS_EC_SENSORS_CORE) += cros_ec_sensors_core.o
 obj-$(CONFIG_IIO_CROS_EC_SENSORS) += cros_ec_sensors.o
 obj-$(CONFIG_IIO_CROS_EC_LIGHT_PROX) += cros_ec_light_prox.o
 obj-$(CONFIG_IIO_CROS_EC_ACTIVITY) += cros_ec_activity.o
+obj-$(CONFIG_IIO_CROS_EC_SENSORS_RING) += cros_ec_sensors_ring.o
diff --git a/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_ring.c b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_ring.c
new file mode 100644
index 0000000..1c74df9
--- /dev/null
+++ b/drivers/iio/common/cros_ec_sensors/cros_ec_sensors_ring.c
@@ -0,0 +1,541 @@
+/*
+ * cros_ec_sensors_ring - Driver for Chrome OS EC Sensor hub FIFO.
+ *
+ * Copyright (C) 2015 Google, Inc
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * 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.
+ *
+ * This driver uses the cros-ec interface to communicate with the Chrome OS
+ * EC about accelerometer data. Accelerometer access is presented through
+ * iio sysfs.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/kernel.h>
+#include <linux/mfd/cros_ec.h>
+#include <linux/mfd/cros_ec_commands.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/platform_device.h>
+
+#include "cros_ec_sensors_core.h"
+
+/* The ring is a FIFO that return sensor information from
+ * the single EC FIFO.
+ * There are always 5 channels returned:
+* | ID | FLAG | X | Y | Z | Timestamp |
+ * ID is the EC sensor id
+ * FLAG is for meta data, only flush bit is defined.
+ */
+#define CROS_EC_FLUSH_BIT 1
+
+enum {
+	CHANNEL_SENSOR_ID,
+	CHANNEL_SENSOR_FLAG,
+	CHANNEL_X,
+	CHANNEL_Y,
+	CHANNEL_Z,
+	CHANNEL_TIMESTAMP,
+	MAX_CHANNEL,
+};
+
+enum {
+	LAST_TS,
+	NEW_TS,
+	ALL_TS
+};
+
+#define CROS_EC_SENSOR_MAX 16
+
+struct cros_ec_fifo_info {
+	struct ec_response_motion_sense_fifo_info info;
+	uint16_t lost[CROS_EC_SENSOR_MAX];
+};
+
+
+struct cros_ec_sensors_ring_sample {
+	uint8_t sensor_id;
+	uint8_t flag;
+	int16_t  vector[MAX_AXIS];
+	s64      timestamp;
+} __packed;
+
+/* State data for ec_sensors iio driver. */
+struct cros_ec_sensors_ring_state {
+	/* Shared by all sensors */
+	struct cros_ec_sensors_core_state core;
+
+	/* Notifier to kick to the interrupt */
+	struct notifier_block notifier;
+
+	/* Preprocessed ring to send to kfifo */
+	struct cros_ec_sensors_ring_sample *ring;
+
+	struct iio_trigger *trig;
+	s64    fifo_timestamp[ALL_TS];
+	struct ec_response_motion_sense_fifo_info fifo_info;
+};
+
+static const struct iio_info ec_sensors_info = {
+	.driver_module = THIS_MODULE,
+};
+
+static s64 cros_ec_get_time_ns(void)
+{
+	struct timespec ts;
+
+	get_monotonic_boottime(&ts);
+	return timespec_to_ns(&ts);
+}
+
+/*
+ * cros_ec_ring_process_event: process one EC FIFO event
+ *
+ * Process one EC event, add it in the ring if necessary.
+ *
+ * Return true if out event has been populated.
+ *
+ * fifo_info: fifo information from the EC.
+ * fifo_timestamp: timestamp at time of fifo_info collection.
+ * current_timestamp: estimated current timestamp.
+ * in: incoming FIFO event from EC
+ * out: outgoing event to user space.
+ */
+bool cros_ec_ring_process_event(const struct cros_ec_fifo_info *fifo_info,
+				const s64 fifo_timestamp,
+				s64 *current_timestamp,
+				struct ec_response_motion_sensor_data *in,
+				struct cros_ec_sensors_ring_sample *out)
+{
+	int axis;
+	s64 new_timestamp;
+
+	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP) {
+		new_timestamp = fifo_timestamp -
+			((s64)fifo_info->info.timestamp * 1000) +
+			((s64)in->timestamp * 1000);
+		/*
+		 * The timestamp can be stale if we had to use the fifo
+		 * info timestamp.
+		 */
+		if (new_timestamp - *current_timestamp > 0)
+			*current_timestamp = new_timestamp;
+	}
+
+	if (in->flags & MOTIONSENSE_SENSOR_FLAG_FLUSH) {
+		out->sensor_id = in->sensor_num;
+		out->timestamp = *current_timestamp;
+		out->flag = CROS_EC_FLUSH_BIT;
+		return true;
+	}
+	if (in->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP)
+		/* If we just have a timestamp, skip this entry. */
+		return false;
+
+	/* Regular sample */
+	out->sensor_id = in->sensor_num;
+	out->timestamp = *current_timestamp;
+	out->flag = 0;
+	for (axis = X; axis < MAX_AXIS; axis++)
+		out->vector[axis] = in->data[axis];
+	return true;
+}
+/*
+ * cros_ec_ring_handler - the trigger handler function
+ *
+ * @irq: the interrupt number
+ * @p: private data - always a pointer to the poll func.
+ *
+ * On a trigger event occurring, if the pollfunc is attached then this
+ * handler is called as a threaded interrupt (and hence may sleep). It
+ * is responsible for grabbing data from the device and pushing it into
+ * the associated buffer.
+ */
+static irqreturn_t cros_ec_ring_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct cros_ec_sensors_ring_state *state = iio_priv(indio_dev);
+	struct cros_ec_fifo_info fifo_info;
+	s64    fifo_timestamp, current_timestamp;
+	int    i, j, number_data, ret;
+	unsigned long sensor_mask = 0;
+	struct ec_response_motion_sensor_data *in;
+	struct cros_ec_sensors_ring_sample *out, *last_out;
+
+
+	/* Get FIFO information */
+	mutex_lock(&state->core.cmd_lock);
+	memcpy(&fifo_info, &state->fifo_info, sizeof(state->fifo_info));
+	fifo_timestamp = state->fifo_timestamp[NEW_TS];
+	mutex_unlock(&state->core.cmd_lock);
+
+	/* Copy elements in the main fifo */
+	if (fifo_info.info.total_lost) {
+		/* Need to retrieve the number of lost vectors per sensor */
+		state->core.param.cmd = MOTIONSENSE_CMD_FIFO_INFO;
+		if (cros_ec_motion_send_host_cmd(&state->core, 0))
+			goto ring_handler_end;
+		memcpy(&fifo_info, &state->core.resp->fifo_info,
+		       sizeof(fifo_info));
+		fifo_timestamp = cros_ec_get_time_ns();
+	}
+
+	current_timestamp = state->fifo_timestamp[LAST_TS];
+	out = state->ring;
+	for (i = 0; i < fifo_info.info.count; i += number_data) {
+		state->core.param.cmd = MOTIONSENSE_CMD_FIFO_READ;
+		state->core.param.fifo_read.max_data_vector =
+			fifo_info.info.count - i;
+		ret = cros_ec_motion_send_host_cmd(&state->core,
+			       sizeof(state->core.resp->fifo_read) +
+			       state->core.param.fifo_read.max_data_vector *
+			       sizeof(struct ec_response_motion_sensor_data));
+		if (ret != EC_RES_SUCCESS) {
+			dev_warn(&indio_dev->dev, "Fifo error: %d\n", ret);
+			break;
+		}
+		number_data =
+			state->core.resp->fifo_read.number_data;
+		if (number_data == 0) {
+			dev_dbg(&indio_dev->dev, "Unexpected empty FIFO\n");
+			break;
+		}
+
+		for (in = state->core.resp->fifo_read.data, j = 0;
+		     j < number_data; j++, in++) {
+			BUG_ON(out >= state->ring + fifo_info.info.size);
+			if (cros_ec_ring_process_event(
+					&fifo_info, fifo_timestamp,
+					&current_timestamp, in, out)) {
+				sensor_mask |= (1 << in->sensor_num);
+				out++;
+			}
+		}
+	}
+	last_out = out;
+
+	if (out == state->ring)
+		/* Unexpected empty FIFO. */
+		goto ring_handler_end;
+
+	/*
+	 * Check if current_timestamp is ahead of the last sample.
+	 * Normally, the EC appends a timestamp after the last sample, but if
+	 * the AP is slow to respond to the IRQ, the EC may have added new
+	 * samples. Use the FIFO info timestamp as last timestamp then.
+	 */
+	if ((last_out-1)->timestamp == current_timestamp)
+		current_timestamp = fifo_timestamp;
+
+	/* check if buffer is set properly */
+	if (!indio_dev->active_scan_mask ||
+	    (bitmap_empty(indio_dev->active_scan_mask,
+			  indio_dev->masklength)))
+		goto ring_handler_end;
+
+	/*
+	 * calculate proper timestamps
+	 *
+	 * If there is a sample with a proper timestamp
+	 *                        timestamp | count
+	 * older_unprocess_out --> TS1      | 1
+	 *                         TS1      | 2
+	 * out -->                 TS1      | 3
+	 * next_out -->            TS2      |
+	 * We spread time for the samples [older_unprocess_out .. out]
+	 * between TS1 and TS2: [TS1+1/4, TS1+2/4, TS1+3/4, TS2].
+	 *
+	 * If we reach the end of the samples, we compare with the
+	 * current timestamp:
+	 *
+	 * older_unprocess_out --> TS1      | 1
+	 *                         TS1      | 2
+	 * out -->                 TS1      | 3
+	 * We know have [TS1+1/3, TS1+2/3, current timestamp]
+	 */
+	for_each_set_bit(i, &sensor_mask, BITS_PER_LONG) {
+		s64 older_timestamp;
+		s64 timestamp;
+		struct cros_ec_sensors_ring_sample *older_unprocess_out =
+			state->ring;
+		struct cros_ec_sensors_ring_sample *next_out;
+		int count = 1;
+
+		if (fifo_info.info.total_lost) {
+			int lost = fifo_info.lost[i];
+
+			if (lost)
+				dev_warn(&indio_dev->dev,
+					"Sensor %d: lost: %d out of %d\n", i,
+					lost, fifo_info.info.total_lost);
+		}
+
+		for (out = state->ring; out < last_out; out = next_out) {
+			s64 time_period;
+
+			next_out = out + 1;
+			if (out->sensor_id != i)
+				continue;
+
+			/* Timestamp to start with */
+			older_timestamp = out->timestamp;
+
+			/* find next sample */
+			while (next_out < last_out && next_out->sensor_id != i)
+				next_out++;
+
+			if (next_out >= last_out) {
+				timestamp = current_timestamp;
+			} else {
+				timestamp = next_out->timestamp;
+				if (timestamp == older_timestamp) {
+					count++;
+					continue;
+				}
+			}
+
+			/* The next sample has a new timestamp,
+			 * spread the unprocessed samples
+			 */
+			if (next_out < last_out)
+				count++;
+			time_period = div_s64(timestamp - older_timestamp,
+					      count);
+
+			for (; older_unprocess_out <= out;
+					older_unprocess_out++) {
+				if (older_unprocess_out->sensor_id != i)
+					continue;
+				older_timestamp += time_period;
+				older_unprocess_out->timestamp =
+					older_timestamp;
+			}
+			count = 1;
+			/* The next_out sample has a valid timestamp, skip. */
+			next_out++;
+			older_unprocess_out = next_out;
+		}
+	}
+
+	/* push the event into the kfifo */
+	for (out = state->ring; out < last_out; out++)
+		iio_push_to_buffers(indio_dev, (u8 *)out);
+
+ring_handler_end:
+	state->fifo_timestamp[LAST_TS] = current_timestamp;
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+static int cros_ec_ring_event(struct notifier_block *nb,
+	unsigned long queued_during_suspend, void *_notify)
+{
+	struct cros_ec_sensors_ring_state *state;
+	struct cros_ec_device *ec;
+
+	state = container_of(nb, struct cros_ec_sensors_ring_state, notifier);
+	ec = state->core.ec;
+
+	if (ec->event_data.event_type != EC_MKBP_EVENT_SENSOR_FIFO)
+		return NOTIFY_DONE;
+
+	if (ec->event_size != sizeof(ec->event_data.data.sensor_fifo)) {
+		dev_warn(ec->dev, "Invalid fifo info size\n");
+		return NOTIFY_DONE;
+	}
+
+	if (queued_during_suspend)
+		return NOTIFY_OK;
+
+	mutex_lock(&state->core.cmd_lock);
+	memcpy(&state->fifo_info, &ec->event_data.data.sensor_fifo.info,
+			ec->event_size);
+	state->fifo_timestamp[NEW_TS] = cros_ec_get_time_ns();
+	mutex_unlock(&state->core.cmd_lock);
+
+	/*
+	 * We are not in a low level interrupt,
+	 * we can not call iio_trigger_poll().
+	 */
+	iio_trigger_poll_chained(state->trig);
+	return NOTIFY_OK;
+}
+
+#define CROS_EC_RING_ID(_id, _name)		\
+{						\
+	.type = IIO_ACCEL,			\
+	.modified = 1,				\
+	.channel2 = IIO_NO_MOD,			\
+	.scan_index = _id,			\
+	.scan_type = {				\
+		.sign = 'u',			\
+		.realbits = 8,			\
+		.storagebits = 8,		\
+	},					\
+	.extend_name = _name,			\
+}
+
+#define CROS_EC_RING_AXIS(_axis)		\
+{						\
+	.type = IIO_ACCEL,			\
+	.modified = 1,				\
+	.channel2 = IIO_MOD_##_axis,		\
+	.scan_index = CHANNEL_##_axis,		\
+	.scan_type = {				\
+		.sign = 's',			\
+		.realbits = 16,			\
+		.storagebits = 16,		\
+	},					\
+	.extend_name = "ring",			\
+}
+
+static const struct iio_chan_spec cros_ec_ring_channels[] = {
+	CROS_EC_RING_ID(CHANNEL_SENSOR_ID, "id"),
+	CROS_EC_RING_ID(CHANNEL_SENSOR_FLAG, "flag"),
+	CROS_EC_RING_AXIS(X),
+	CROS_EC_RING_AXIS(Y),
+	CROS_EC_RING_AXIS(Z),
+	IIO_CHAN_SOFT_TIMESTAMP(CHANNEL_TIMESTAMP)
+};
+
+static int cros_ec_ring_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct cros_ec_dev *ec_dev = dev_get_drvdata(dev->parent);
+	struct cros_ec_device *ec_device;
+	struct iio_dev *indio_dev;
+	struct cros_ec_sensors_ring_state *state;
+	int ret;
+
+	if (!ec_dev || !ec_dev->ec_dev) {
+		dev_warn(&pdev->dev, "No CROS EC device found.\n");
+		return -EINVAL;
+	}
+	ec_device = ec_dev->ec_dev;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*state));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	ret = cros_ec_sensors_core_init(pdev, indio_dev, false);
+	if (ret)
+		return ret;
+
+	state = iio_priv(indio_dev);
+	/* Retrieve FIFO information */
+	state->core.param.cmd = MOTIONSENSE_CMD_FIFO_INFO;
+	/* If it fails, just assume the FIFO is not supported.
+	 * For other errors, the other sensor drivers would have noticed
+	 * already.
+	 */
+	if (cros_ec_motion_send_host_cmd(&state->core, 0))
+		return -ENODEV;
+
+	/* Allocate the full fifo.
+	 * We need to copy the whole FIFO to set timestamps properly *
+	 */
+	state->ring = devm_kcalloc(&pdev->dev,
+			state->core.resp->fifo_info.size,
+			sizeof(*state->ring), GFP_KERNEL);
+	if (!state->ring)
+		return -ENOMEM;
+
+	state->fifo_timestamp[LAST_TS] = cros_ec_get_time_ns();
+
+	indio_dev->channels = cros_ec_ring_channels;
+	indio_dev->num_channels = ARRAY_SIZE(cros_ec_ring_channels);
+
+	indio_dev->info = &ec_sensors_info;
+
+	state->trig = devm_iio_trigger_alloc(&pdev->dev,
+			"%s-trigger%d", indio_dev->name, indio_dev->id);
+	if (!state->trig)
+		return -ENOMEM;
+	state->trig->dev.parent = &pdev->dev;
+	iio_trigger_set_drvdata(state->trig, indio_dev);
+
+	ret = iio_trigger_register(state->trig);
+	if (ret)
+		return ret;
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 cros_ec_ring_handler, NULL);
+	if (ret < 0)
+		goto err_trigger_unregister;
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto err_buffer_cleanup;
+
+	/* register the notifier that will act as a top half interrupt. */
+	state->notifier.notifier_call = cros_ec_ring_event;
+	ret = blocking_notifier_chain_register(&ec_device->event_notifier,
+					       &state->notifier);
+	if (ret < 0) {
+		dev_warn(&indio_dev->dev, "failed to register notifier\n");
+		goto err_device_unregister;
+	}
+	return ret;
+
+err_device_unregister:
+	iio_device_unregister(indio_dev);
+err_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
+err_trigger_unregister:
+	iio_trigger_unregister(state->trig);
+	return ret;
+}
+
+static int cros_ec_ring_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct cros_ec_sensors_ring_state *state = iio_priv(indio_dev);
+	struct cros_ec_device *ec = state->core.ec;
+
+	blocking_notifier_chain_unregister(&ec->event_notifier,
+					   &state->notifier);
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+	iio_trigger_unregister(state->trig);
+	return 0;
+}
+
+static const struct platform_device_id cros_ec_ring_ids[] = {
+	{
+		.name = "cros-ec-ring",
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, cros_ec_ring_ids);
+
+static struct platform_driver cros_ec_ring_platform_driver = {
+	.driver = {
+		.name	= "cros-ec-ring",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= cros_ec_ring_probe,
+	.remove		= cros_ec_ring_remove,
+	.id_table	= cros_ec_ring_ids,
+};
+module_platform_driver(cros_ec_ring_platform_driver);
+
+MODULE_DESCRIPTION("ChromeOS EC sensor hub ring driver");
+MODULE_LICENSE("GPL v2");
-- 
2.1.0