Use triggered buffer by parsing FIFO data read in device trigger.
Support hwfifo watermark by multiplexing gyro and accel settings.
Support hwfifo flush.
Simply use interrupt timestamp first.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@xxxxxxxxxxxxxx>
---
drivers/iio/imu/inv_icm42600/Kconfig | 3 +-
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.c | 353 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 162 ++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 23 ++
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 183 +++++++++
.../imu/inv_icm42600/inv_icm42600_trigger.c | 15 +-
9 files changed, 928 insertions(+), 3 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
index 7b3eaeb2aa4a..8c0969319c49 100644
--- a/drivers/iio/imu/inv_icm42600/Kconfig
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -2,7 +2,8 @@
config INV_ICM42600
tristate
- select IIO_TRIGGER
+ select IIO_BUFFER
+ select IIO_TRIGGERED_BUFFER
config INV_ICM42600_I2C
tristate "InvenSense ICM-426xx I2C driver"
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index e1f2aacbe888..d6732118010c 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
inv-icm42600-y += inv_icm42600_accel.o
inv-icm42600-y += inv_icm42600_temp.o
inv-icm42600-y += inv_icm42600_trigger.o
+inv-icm42600-y += inv_icm42600_buffer.o
obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
inv-icm42600-i2c-y += inv_icm42600_i2c.o
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 175c1f67faee..947ca4dd245b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -15,6 +15,8 @@
#include <linux/iio/iio.h>
#include <linux/iio/trigger.h>
+#include "inv_icm42600_buffer.h"
+
enum inv_icm42600_chip {
INV_CHIP_ICM42600,
INV_CHIP_ICM42602,
@@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
* @indio_gyro: gyroscope IIO device.
* @indio_accel: accelerometer IIO device.
* @trigger: device internal interrupt trigger
+ * @fifo: FIFO management structure.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -138,6 +141,7 @@ struct inv_icm42600_state {
struct iio_dev *indio_gyro;
struct iio_dev *indio_accel;
struct iio_trigger *trigger;
+ struct inv_icm42600_fifo fifo;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -378,8 +382,12 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+
int inv_icm42600_accel_init(struct inv_icm42600_state *st);
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+
int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
int irq_type);
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 74dac5f283d4..4206be54d057 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -10,9 +10,13 @@
#include <linux/regmap.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -46,6 +50,7 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_Y,
INV_ICM42600_ACCEL_SCAN_Z,
INV_ICM42600_ACCEL_SCAN_TEMP,
+ INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
@@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
inv_icm42600_accel_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
};
+/* IIO buffer data */
+struct inv_icm42600_accel_buffer {
+ struct inv_icm42600_fifo_sensor_data accel;
+ int8_t temp;
+ int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
+ (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
+ BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
+ BIT(INV_ICM42600_ACCEL_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_accel_scan_masks[] = {
+ /* 3-axis accel + temperature */
+ INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+ 0,
+};
+
+static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
+{
+ struct iio_poll_func *pf = _data;
+ struct iio_dev *indio_dev = pf->indio_dev;
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t fifo_nb = st->fifo.nb.total;
+ int ret;
+
+ /* exit if no sample */
+ if (fifo_nb == 0)
+ goto out;
+
+ ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
+ if (ret)
+ dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
+ ret);
+
+out:
+ iio_trigger_notify_done(indio_dev->trig);
+ return IRQ_HANDLED;
+}
+
+/* enable accelerometer sensor and FIFO write */
+static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
+ const unsigned long *scan_mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int fifo_en = 0;
+ unsigned int sleep_temp = 0;
+ unsigned int sleep_accel = 0;
+ unsigned int sleep;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+ /* enable temp sensor */
+ ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_TEMP;
+ }
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
+ /* enable accel sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_ACCEL;
+ }
+
+ /* update data FIFO write and FIFO watermark */
+ ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+ if (ret)
+ goto out_unlock;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ /* sleep maximum required time */
+ if (sleep_accel > sleep_temp)
+ sleep = sleep_accel;
+ else
+ sleep = sleep_temp;
+ if (sleep)
+ msleep(sleep);
+ return ret;
+}
+
static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
struct iio_chan_spec const *chan,
int16_t *val)
@@ -250,6 +347,8 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
@@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
}
}
+static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
+ unsigned int val)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ st->fifo.watermark.accel = val;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
+static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
+ unsigned int count)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (count == 0)
+ return 0;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+ if (!ret)
+ ret = st->fifo.nb.accel;
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
static const struct iio_info inv_icm42600_accel_info = {
.read_raw = inv_icm42600_accel_read_raw,
.read_avail = inv_icm42600_accel_read_avail,
.write_raw = inv_icm42600_accel_write_raw,
.write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
.debugfs_reg_access = inv_icm42600_debugfs_reg,
+ .update_scan_mode = inv_icm42600_accel_update_scan_mode,
+ .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
+ .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
};
int inv_icm42600_accel_init(struct inv_icm42600_state *st)
@@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
struct device *dev = regmap_get_device(st->map);
const char *name;
struct iio_dev *indio_dev;
+ int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
if (!name)
@@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = inv_icm42600_accel_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+ indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
+
+ ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+ inv_icm42600_accel_handler,
+ &inv_icm42600_buffer_ops);
+ if (ret)
+ return ret;
+
+ indio_dev->trig = iio_trigger_get(st->trigger);
st->indio_accel = indio_dev;
return devm_iio_device_register(dev, st->indio_accel);
}
+
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t accel_nb = st->fifo.nb.accel;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ struct inv_icm42600_accel_buffer buffer;
+
+ /* exit if no accel sample */
+ if (accel_nb == 0)
+ return 0;
+
+ /* parse all fifo packets */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
+ size);
+ /* quit if error or FIFO is empty */
+ if (size <= 0)
+ return size;
+ /* skip packet if no accel data or data is invalid */
+ if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
+ dev_dbg(regmap_get_device(st->map), "skip accel data\n");
+ continue;
+ }
+ memcpy(&buffer.accel, accel, sizeof(buffer.accel));
+ memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ }
+
+ return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
new file mode 100644
index 000000000000..b428abdc92ee
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -0,0 +1,353 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
+{
+ uint32_t period_gyro, period_accel, period;
+
+ if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
+ period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+ else
+ period_gyro = U32_MAX;
+
+ if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
+ period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+ else
+ period_accel = U32_MAX;
+
+ if (period_gyro <= period_accel)
+ period = period_gyro;
+ else
+ period = period_accel;
+
+ st->fifo.period = period;
+}
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+ unsigned int fifo_en)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /* update only FIFO EN bits */
+ mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
+ INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
+ INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
+ INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+
+ val = 0;
+ if (fifo_en & INV_ICM42600_SENSOR_GYRO)
+ val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
+ if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
+ val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+ if (fifo_en & INV_ICM42600_SENSOR_TEMP)
+ val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
+
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+ mask, val);
+ if (ret)
+ return ret;
+
+ st->fifo.en = fifo_en;
+ inv_icm42600_buffer_update_fifo_period(st);
+
+ return 0;
+}
+
+static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
+{
+ size_t packet_size;
+
+ if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
+ (fifo_en & INV_ICM42600_SENSOR_ACCEL))
+ packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+ else
+ packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+
+ return packet_size;
+}
+
+static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
+ size_t packet_size)
+{
+ size_t wm_size;
+ unsigned int wm;
+
+ wm_size = watermark * packet_size;
+ if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
+ wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
+
+ wm = wm_size / packet_size;
+
+ return wm;
+}
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
+{
+ size_t packet_size, wm_size;
+ unsigned int wm_gyro, wm_accel, watermark;
+ uint32_t period_gyro, period_accel, period;
+ int64_t latency_gyro, latency_accel, latency;
+ unsigned int mask, val;
+ bool restore;
+ __le16 raw_wm;
+ int ret;
+
+ packet_size = inv_icm42600_get_packet_size(st->fifo.en);
+
+ /* get minimal latency, depending on sensor watermark and odr */
+ wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
+ packet_size);
+ wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
+ packet_size);
+ period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+ period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+ latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
+ latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
+ if (latency_gyro == 0) {
+ latency = latency_accel;
+ watermark = wm_accel;
+ } else if (latency_accel == 0) {
+ latency = latency_gyro;
+ watermark = wm_gyro;
+ } else {
+ /* compute the smallest latency that is a multiple of both */
+ if (latency_gyro <= latency_accel) {
+ latency = latency_gyro;
+ latency -= latency_accel % latency_gyro;
+ } else {
+ latency = latency_accel;
+ latency -= latency_gyro % latency_accel;
+ }
+ /* use the shortest period */
+ if (period_gyro <= period_accel)
+ period = period_gyro;
+ else
+ period = period_accel;
+ /* all this works because periods are multiple of each others */
+ watermark = div_s64(latency, period);
+ if (watermark < 1)
+ watermark = 1;
+ }
+ wm_size = watermark * packet_size;
+ dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
+ watermark, wm_size);
+
+ /* changing FIFO watermark requires to turn off watermark interrupt */
+ mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ val = 0;
+ ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ mask, val, &restore);
+ if (ret)
+ return ret;
+
+ raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
+ ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
+ &raw_wm, sizeof(raw_wm));
+ if (ret)
+ return ret;
+
+ /* restore watermark interrupt */
+ if (restore) {
+ mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ mask, val);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct device *dev = regmap_get_device(st->map);
+
+ pm_runtime_get_sync(dev);
+
+ return 0;
+}
+
+static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int sensor;
+ unsigned int *watermark;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int sleep = 0;
+ int ret;
+
+ if (indio_dev == st->indio_gyro) {
+ sensor = INV_ICM42600_SENSOR_GYRO;
+ watermark = &st->fifo.watermark.gyro;
+ } else if (indio_dev == st->indio_accel) {
+ sensor = INV_ICM42600_SENSOR_ACCEL;
+ watermark = &st->fifo.watermark.accel;
+ } else {
+ return -EINVAL;
+ }
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
+ if (ret)
+ goto out_unlock;
+
+ *watermark = 0;
+ ret = inv_icm42600_buffer_update_watermark(st);
+ if (ret)
+ goto out_unlock;
+
+ conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
+ if (sensor == INV_ICM42600_SENSOR_GYRO)
+ ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
+ else
+ ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ if (sleep)
+ msleep(sleep);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
+ .preenable = inv_icm42600_buffer_preenable,
+ .postenable = iio_triggered_buffer_postenable,
+ .predisable = iio_triggered_buffer_predisable,
+ .postdisable = inv_icm42600_buffer_postdisable,
+};
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+ unsigned int max)
+{
+ struct device *dev = regmap_get_device(st->map);
+ __be16 raw_fifo_count;
+ size_t max_count;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ int ret;
+
+ /* reset all samples counters */
+ st->fifo.count = 0;
+ st->fifo.nb.gyro = 0;
+ st->fifo.nb.accel = 0;
+ st->fifo.nb.total = 0;
+
+ /* compute maximum FIFO read size */
+ if (max == 0)
+ max_count = sizeof(st->fifo.data);
+ else
+ max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
+
+ /* read FIFO count value */
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
+ &raw_fifo_count, sizeof(raw_fifo_count));
+ if (ret)
+ return ret;
+ st->fifo.count = be16_to_cpu(raw_fifo_count);
+ dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
+
+ /* check and sanitize FIFO count value */
+ if (st->fifo.count == 0)
+ return 0;
+ if (st->fifo.count > max_count)
+ st->fifo.count = max_count;
+
+ /* read all FIFO data in internal buffer */
+ ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
+ st->fifo.data, st->fifo.count);
+ if (ret)
+ return ret;
+
+ /* compute number of samples for each sensor */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ if (size <= 0)
+ break;
+ if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
+ st->fifo.nb.gyro++;
+ if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
+ st->fifo.nb.accel++;
+ st->fifo.nb.total++;
+ }
+
+ return 0;
+}
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+ unsigned int count)
+{
+ int64_t ts_gyro, ts_accel;
+ int ret;
+
+ dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
+
+ ts_gyro = iio_get_time_ns(st->indio_gyro);
+ ts_accel = iio_get_time_ns(st->indio_accel);
+ ret = inv_icm42600_buffer_fifo_read(st, count);
+ if (ret)
+ return ret;
+
+ if (st->fifo.nb.total == 0)
+ return 0;
+
+ ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
+ if (ret)
+ return ret;
+
+ return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
+}
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /*
+ * Default FIFO configuration (bits 7 to 5)
+ * - use invalid value
+ * - FIFO count in bytes
+ * - FIFO count in big endian
+ */
+ mask = GENMASK(7, 5);
+ val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ mask, val);
+ if (ret)
+ return ret;
+
+ /*
+ * Enable FIFO partial read and continuous watermark interrupt.
+ * Disable all FIFO EN bits.
+ */
+ mask = GENMASK(6, 5) | GENMASK(3, 0);
+ val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
+ INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+ mask, val);
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
new file mode 100644
index 000000000000..74b91c0e664b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_BUFFER_H_
+#define INV_ICM42600_BUFFER_H_
+
+#include <linux/kernel.h>
+#include <linux/bits.h>
+
+struct inv_icm42600_state;
+
+#define INV_ICM42600_SENSOR_GYRO BIT(0)
+#define INV_ICM42600_SENSOR_ACCEL BIT(1)
+#define INV_ICM42600_SENSOR_TEMP BIT(2)
+
+struct inv_icm42600_fifo {
+ unsigned int en;
+ uint32_t period;
+ struct {
+ unsigned int gyro;
+ unsigned int accel;
+ } watermark;
+ size_t count;
+ struct {
+ size_t gyro;
+ size_t accel;
+ size_t total;
+ } nb;
+ uint8_t data[2080];
+};
+
+/* FIFO header: 1 byte */
+#define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
+#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
+#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
+#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
+#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
+#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
+
+/* FIFO data packet */
+struct inv_icm42600_fifo_sensor_data {
+ __be16 x;
+ __be16 y;
+ __be16 z;
+} __packed;
+#define INV_ICM42600_FIFO_DATA_INVALID -32768
+
+struct inv_icm42600_fifo_1sensor_packet {
+ uint8_t header;
+ struct inv_icm42600_fifo_sensor_data data;
+ int8_t temp;
+} __packed;
+#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
+
+struct inv_icm42600_fifo_2sensors_packet {
+ uint8_t header;
+ struct inv_icm42600_fifo_sensor_data accel;
+ struct inv_icm42600_fifo_sensor_data gyro;
+ int8_t temp;
+ __be16 timestamp;
+} __packed;
+#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
+
+static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
+{
+ return be16_to_cpu(d);
+}
+
+static inline bool
+inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
+{
+ int16_t x, y, z;
+
+ x = inv_icm42600_fifo_get_sensor_data(s->x);
+ y = inv_icm42600_fifo_get_sensor_data(s->y);
+ z = inv_icm42600_fifo_get_sensor_data(s->z);
+
+ if (x == INV_ICM42600_FIFO_DATA_INVALID &&
+ y == INV_ICM42600_FIFO_DATA_INVALID &&
+ z == INV_ICM42600_FIFO_DATA_INVALID)
+ return false;
+
+ return true;
+}
+
+static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
+ const void **accel, const void **gyro, const void **temp,
+ const void **timestamp, unsigned int *odr)
+{
+ const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
+ const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
+ uint8_t header = *((const uint8_t *)packet);
+
+ /* FIFO empty */
+ if (header & INV_ICM42600_FIFO_HEADER_MSG) {
+ *accel = NULL;
+ *gyro = NULL;
+ *temp = NULL;
+ *timestamp = NULL;
+ *odr = 0;
+ return 0;
+ }
+
+ /* handle odr flags */
+ *odr = 0;
+ if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
+ *odr |= INV_ICM42600_SENSOR_GYRO;
+ if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
+ *odr |= INV_ICM42600_SENSOR_ACCEL;
+
+ /* accel + gyro */
+ if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
+ (header & INV_ICM42600_FIFO_HEADER_GYRO)) {
+ *accel = &pack2->accel;
+ *gyro = &pack2->gyro;
+ *temp = &pack2->temp;
+ *timestamp = &pack2->timestamp;
+ return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+ }
+
+ /* accel only */
+ if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
+ *accel = &pack1->data;
+ *gyro = NULL;
+ *temp = &pack1->temp;
+ *timestamp = NULL;
+ return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+ }
+
+ /* gyro only */
+ if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
+ *accel = NULL;
+ *gyro = &pack1->data;
+ *temp = &pack1->temp;
+ *timestamp = NULL;
+ return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+ }
+
+ /* invalid packet if here */
+ return -EINVAL;
+}
+
+extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+ unsigned int fifo_en);
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+ unsigned int max);
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+ unsigned int count);
+
+#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 1102c54396e3..689089065ff9 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -14,6 +14,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
{
@@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ /* setup FIFO buffer */
+ ret = inv_icm42600_buffer_init(st);
+ if (ret)
+ return ret;
+
/* setup interrupt trigger */
ret = inv_icm42600_trigger_init(st, irq, irq_type);
if (ret)
@@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
goto out_unlock;
}
+ /* disable FIFO data streaming */
+ if (iio_buffer_enabled(st->indio_gyro) ||
+ iio_buffer_enabled(st->indio_accel)) {
+ /* set FIFO in bypass mode */
+ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+ INV_ICM42600_FIFO_CONFIG_BYPASS);
+ if (ret)
+ goto out_unlock;
+ }
+
ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
INV_ICM42600_SENSOR_MODE_OFF, false,
NULL);
@@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
if (ret)
goto out_unlock;
+ /* restore FIFO data streaming */
+ if (iio_buffer_enabled(st->indio_gyro) ||
+ iio_buffer_enabled(st->indio_accel)) {
+ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+ INV_ICM42600_FIFO_CONFIG_STREAM);
+ }
+
out_unlock:
mutex_unlock(&st->lock);
return ret;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index c0164ab2830e..dafb104abc77 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -10,9 +10,13 @@
#include <linux/regmap.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -46,6 +50,7 @@ enum inv_icm42600_gyro_scan {
INV_ICM42600_GYRO_SCAN_Y,
INV_ICM42600_GYRO_SCAN_Z,
INV_ICM42600_GYRO_SCAN_TEMP,
+ INV_ICM42600_GYRO_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
@@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
inv_icm42600_gyro_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
};
+/* IIO buffer data */
+struct inv_icm42600_gyro_buffer {
+ struct inv_icm42600_fifo_sensor_data gyro;
+ int8_t temp;
+ int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
+ (BIT(INV_ICM42600_GYRO_SCAN_X) | \
+ BIT(INV_ICM42600_GYRO_SCAN_Y) | \
+ BIT(INV_ICM42600_GYRO_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_gyro_scan_masks[] = {
+ /* 3-axis gyro + temperature */
+ INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+ 0,
+};
+
+static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
+{
+ struct iio_poll_func *pf = _data;
+ struct iio_dev *indio_dev = pf->indio_dev;
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t fifo_nb = st->fifo.nb.total;
+ int ret;
+
+ /* exit if no sample */
+ if (fifo_nb == 0)
+ goto out;
+
+ ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
+ if (ret)
+ dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
+ ret);
+
+out:
+ iio_trigger_notify_done(indio_dev->trig);
+ return IRQ_HANDLED;
+}
+
+/* enable gyroscope sensor and FIFO write */
+static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
+ const unsigned long *scan_mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int fifo_en = 0;
+ unsigned int sleep_gyro = 0;
+ unsigned int sleep_temp = 0;
+ unsigned int sleep;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+ /* enable temp sensor */
+ ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_TEMP;
+ }
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
+ /* enable gyro sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_GYRO;
+ }
+
+ /* update data FIFO write and FIFO watermark */
+ ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+ if (ret)
+ goto out_unlock;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ /* sleep maximum required time */
+ if (sleep_gyro > sleep_temp)
+ sleep = sleep_gyro;
+ else
+ sleep = sleep_temp;
+ if (sleep)
+ msleep(sleep);
+ return ret;
+}
+
static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
struct iio_chan_spec const *chan,
int16_t *val)
@@ -262,6 +359,8 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
@@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
}
}
+static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
+ unsigned int val)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ st->fifo.watermark.gyro = val;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
+static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
+ unsigned int count)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (count == 0)
+ return 0;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+ if (!ret)
+ ret = st->fifo.nb.gyro;
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
static const struct iio_info inv_icm42600_gyro_info = {
.read_raw = inv_icm42600_gyro_read_raw,
.read_avail = inv_icm42600_gyro_read_avail,
.write_raw = inv_icm42600_gyro_write_raw,
.write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
.debugfs_reg_access = inv_icm42600_debugfs_reg,
+ .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
+ .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
+ .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
};
int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
@@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
struct device *dev = regmap_get_device(st->map);
const char *name;
struct iio_dev *indio_dev;
+ int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
if (!name)
@@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = inv_icm42600_gyro_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+ indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
+
+ ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+ inv_icm42600_gyro_handler,
+ &inv_icm42600_buffer_ops);
+ if (ret)
+ return ret;
+
+ indio_dev->trig = iio_trigger_get(st->trigger);
st->indio_gyro = indio_dev;
return devm_iio_device_register(dev, st->indio_gyro);
}
+
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t gyro_nb = st->fifo.nb.gyro;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ struct inv_icm42600_gyro_buffer buffer;
+
+ /* exit if no gyro sample */
+ if (gyro_nb == 0)
+ return 0;
+
+ /* parse all fifo packets */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
+ size);
+ /* quit if error or FIFO is empty */
+ if (size <= 0)
+ return size;
+ /* skip packet if no gyro data or data is invalid */
+ if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
+ dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
+ continue;
+ }
+ memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
+ memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ }
+
+ return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
index 7a5e76305f0b..5667e0204722 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
@@ -13,6 +13,7 @@
#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
{
@@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
dev_warn(dev, "FIFO full data lost!\n");
/* FIFO threshold reached */
- if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
- iio_trigger_poll_chained(st->trigger);
+ if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
+ ret = inv_icm42600_buffer_fifo_read(st, 0);
+ if (ret)
+ dev_err(dev, "FIFO read error %d\n", ret);
+ } else {
+ st->fifo.count = 0;
+ st->fifo.nb.gyro = 0;
+ st->fifo.nb.accel = 0;
+ st->fifo.nb.total = 0;
+ }
+
+ iio_trigger_poll_chained(st->trigger);
out_unlock:
mutex_unlock(&st->lock);
--
2.17.1
