On Thu, 7 May 2020 16:42:19 +0200
Jean-Baptiste Maneyrol <jmaney...@invensense.com> wrote:
> 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 <jmaney...@invensense.com>
> ---
> 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,
Bit big to be in the header..
> + 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;
blank line
> + 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);
if (sleep_gyro > sleep_temp)
msleep(sleep_gyro);
else
msleep(sleep_temp);
> + 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;
blank line here.
> + /* 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");
Very noisy logging. I'd drop it for the final version of the driver.
> + continue;
> + }
> + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
buffer.temp = 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);
