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,
+ ¤t_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
--
To unsubscribe from this list: send the line "unsubscribe linux-iio" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
