Changelog
v1
-initial patch submission
v2
- formatting reworks, missing headers, code cleanup ...
- delete all debug traces
- add commentaries for better explanation of suspend/resume, timestamp, ...
- delete i2c/spi table ids keeping only of, and use I2C probe_new function
- switch calibbias to SI units and add calibias_available attribute
- use DMA-safe buffer for all regmap_bulk_* calls
- delete iio trigger usage and setup/handle interrupt in core module
- add open-drain interrupt support
- add FIFO on reference counter and buffer postenable/predisable to replace
iio trigger usage
- check that temperature data is present before copying in buffer
- add temperature sensor off when fifo is turned off
- delete timestamp channel reading
- move timestamp state in IIO device private data
- allow only 1 ODR change in a batch of data
- add driver-open-drain in devicetree YAML and delete spi options
This series add a new driver for managing InvenSense ICM-426xx 6-axis IMUs.
This next generation of chips includes new generations of 3-axis gyroscope
and 3-axis accelerometer, support of I3C in addition to I2C and SPI, and
intelligent MotionTracking features like pedometer, tilt detection, and
tap detection.
This series is delivering a driver supporting gyroscope, accelerometer and
temperature data, with polling and buffering using hwfifo and watermark,
on I2C and SPI busses.
Gyroscope and accelerometer sensors are completely independent and can have
different ODRs. Since there is only a single FIFO a specific value is used to
mark invalid data. For keeping the device standard we are de-multiplexing data
from the FIFO to 2 IIO devices with 2 buffers, 1 for the accelerometer and 1
for the gyroscope. This architecture also enables to easily turn each sensor
on/off without impacting the other. The device interrupt is used to read the
FIFO and launch parsing of accelerometer and gyroscope data. A complex
timestamping mechanism is added to handle correctly FIFO watermark and dynamic
changes of settings.
Jean-Baptiste Maneyrol (12):
iio: imu: inv_icm42600: add core of new inv_icm42600 driver
iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
iio: imu: inv_icm42600: add SPI driver for inv_icm42600 driver
iio: imu: inv_icm42600: add gyroscope IIO device
iio: imu: inv_icm42600: add accelerometer IIO device
iio: imu: inv_icm42600: add temperature sensor support
iio: imu: add Kconfig and Makefile for inv_icm42600 driver
iio: imu: inv_icm42600: add device interrupt
iio: imu: inv_icm42600: add buffer support in iio devices
iio: imu: inv_icm42600: add accurate timestamping
dt-bindings: iio: imu: Add inv_icm42600 documentation
MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor
.../bindings/iio/imu/invensense,icm42600.yaml | 86 ++
MAINTAINERS | 8 +
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 1 +
drivers/iio/imu/inv_icm42600/Kconfig | 29 +
drivers/iio/imu/inv_icm42600/Makefile | 15 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 395 +++++++++
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 784 +++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.c | 583 +++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 98 +++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 782 +++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 795 ++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 101 +++
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 100 +++
.../iio/imu/inv_icm42600/inv_icm42600_temp.c | 87 ++
.../iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 +
.../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 +++++
.../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++
18 files changed, 4175 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
create mode 100644 drivers/iio/imu/inv_icm42600/Kconfig
create mode 100644 drivers/iio/imu/inv_icm42600/Makefile
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
--
2.17.1
Add a timestamping mechanism for buffer that provides accurate
event timestamps when using watermark. This mechanism estimates
device internal clock by comparing FIFO interrupts delta time and
device elapsed time computed by parsing FIFO data.
Take interrupt timestamp in hard irq handler and add IIO device
specific timestamp structures in device private allocation.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 5 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 40 +++-
.../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 17 +-
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 40 +++-
.../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++++++++
8 files changed, 398 insertions(+), 13 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index 0f49f6df3647..291714d9aa54 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_buffer.o
+inv-icm42600-y += inv_icm42600_timestamp.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 4d5811562a61..2de0dd7675fb 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -126,6 +126,7 @@ struct inv_icm42600_suspended {
* @indio_accel: accelerometer IIO device.
* @buffer: data transfer buffer aligned for DMA.
* @fifo: FIFO management structure.
+ * @timestamp: interrupt timestamps.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -141,6 +142,10 @@ struct inv_icm42600_state {
struct iio_dev *indio_accel;
uint8_t buffer[2] ____cacheline_aligned;
struct inv_icm42600_fifo fifo;
+ struct {
+ int64_t gyro;
+ int64_t accel;
+ } timestamp;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index c73ce204efc6..ec1d124c1471 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -17,6 +17,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -50,6 +51,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[] = {
@@ -65,13 +67,15 @@ 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: 8 bytes */
+/* IIO buffer data: 16 bytes */
struct inv_icm42600_accel_buffer {
struct inv_icm42600_fifo_sensor_data accel;
int8_t temp;
uint8_t padding;
+ int64_t timestamp;
};
#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
@@ -92,6 +96,7 @@ 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_timestamp *ts = iio_priv(indio_dev);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0;
unsigned int sleep_temp = 0;
@@ -119,6 +124,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write */
+ inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -299,9 +305,11 @@ static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
return IIO_VAL_INT_PLUS_MICRO;
}
-static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
+static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
int val, int val2)
{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct device *dev = regmap_get_device(st->map);
unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
@@ -320,6 +328,11 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
pm_runtime_get_sync(dev);
mutex_lock(&st->lock);
+ ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
+ iio_buffer_enabled(indio_dev));
+ if (ret)
+ goto out_unlock;
+
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
if (ret)
goto out_unlock;
@@ -609,7 +622,7 @@ static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
iio_device_release_direct_mode(indio_dev);
return ret;
case IIO_CHAN_INFO_SAMP_FREQ:
- return inv_icm42600_accel_write_odr(st, val, val2);
+ return inv_icm42600_accel_write_odr(indio_dev, val, val2);
case IIO_CHAN_INFO_CALIBBIAS:
ret = iio_device_claim_direct_mode(indio_dev);
if (ret)
@@ -692,6 +705,8 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
{
struct device *dev = regmap_get_device(st->map);
const char *name;
+ struct inv_icm42600_timestamp *ts;
+ uint32_t period;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
@@ -699,7 +714,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!name)
return -ENOMEM;
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return -ENOMEM;
@@ -707,6 +722,10 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!buffer)
return -ENOMEM;
+ ts = iio_priv(indio_dev);
+ period = inv_icm42600_odr_to_period(st->conf.accel.odr);
+ inv_icm42600_timestamp_init(ts, period);
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->dev.parent = dev;
indio_dev->name = name;
@@ -726,16 +745,19 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *timestamp;
const int8_t *temp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_accel_buffer buffer = {
.padding = 0,
};
/* parse all fifo packets */
- for (i = 0; i < st->fifo.count; i += size) {
+ for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
&accel, &gyro, &temp, ×tamp, &odr);
/* quit if error or FIFO is empty */
@@ -746,10 +768,16 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
continue;
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_ACCEL)
+ inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
+ st->fifo.nb.total, no);
+
/* fill and push data buffer */
memcpy(&buffer.accel, accel, sizeof(buffer.accel));
buffer.temp = temp ? *temp : 0;
- iio_push_to_buffers(indio_dev, &buffer);
+ ts_val = inv_icm42600_timestamp_pop(ts);
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
}
return 0;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
index c91075f62231..3c8b1b19de15 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -14,6 +14,7 @@
#include <linux/iio/buffer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
#include "inv_icm42600_buffer.h"
/* FIFO header: 1 byte */
@@ -356,6 +357,7 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
struct device *dev = regmap_get_device(st->map);
unsigned int sensor;
unsigned int *watermark;
+ struct inv_icm42600_timestamp *ts;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int sleep_temp = 0;
unsigned int sleep_sensor = 0;
@@ -365,9 +367,11 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
if (indio_dev == st->indio_gyro) {
sensor = INV_ICM42600_SENSOR_GYRO;
watermark = &st->fifo.watermark.gyro;
+ ts = iio_priv(st->indio_gyro);
} else if (indio_dev == st->indio_accel) {
sensor = INV_ICM42600_SENSOR_ACCEL;
watermark = &st->fifo.watermark.accel;
+ ts = iio_priv(st->indio_accel);
} else {
return -EINVAL;
}
@@ -395,6 +399,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
if (!st->fifo.on)
ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
+ inv_icm42600_timestamp_reset(ts);
+
out_unlock:
mutex_unlock(&st->lock);
@@ -480,17 +486,26 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
{
+ struct inv_icm42600_timestamp *ts;
int ret;
if (st->fifo.nb.total == 0)
return 0;
+ /* handle gyroscope timestamp and FIFO data parsing */
+ ts = iio_priv(st->indio_gyro);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.gyro, st->timestamp.gyro);
if (st->fifo.nb.gyro > 0) {
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
+ /* handle accelerometer timestamp and FIFO data parsing */
+ ts = iio_priv(st->indio_accel);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.accel, st->timestamp.accel);
if (st->fifo.nb.accel > 0) {
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
@@ -503,8 +518,13 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
unsigned int count)
{
+ struct inv_icm42600_timestamp *ts;
+ int64_t gyro_ts, accel_ts;
int ret;
+ gyro_ts = iio_get_time_ns(st->indio_gyro);
+ accel_ts = iio_get_time_ns(st->indio_accel);
+
ret = inv_icm42600_buffer_fifo_read(st, count);
if (ret)
return ret;
@@ -513,12 +533,20 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
return 0;
if (st->fifo.nb.gyro > 0) {
+ ts = iio_priv(st->indio_gyro);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
+ st->fifo.nb.total, st->fifo.nb.gyro,
+ gyro_ts);
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
if (st->fifo.nb.accel > 0) {
+ ts = iio_priv(st->indio_accel);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
+ st->fifo.nb.total, st->fifo.nb.accel,
+ accel_ts);
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
return ret;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 6f1c1eb83953..c0d676219fc7 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -19,6 +19,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
{
@@ -413,6 +414,16 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st,
return inv_icm42600_set_conf(st, hw->conf);
}
+static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+
+ st->timestamp.gyro = iio_get_time_ns(st->indio_gyro);
+ st->timestamp.accel = iio_get_time_ns(st->indio_accel);
+
+ return IRQ_WAKE_THREAD;
+}
+
static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
{
struct inv_icm42600_state *st = _data;
@@ -493,7 +504,7 @@ static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq,
if (ret)
return ret;
- return devm_request_threaded_irq(dev, irq, NULL,
+ return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp,
inv_icm42600_irq_handler, irq_type,
"inv_icm42600", st);
}
@@ -613,6 +624,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ ret = inv_icm42600_timestamp_setup(st);
+ if (ret)
+ return ret;
+
ret = inv_icm42600_buffer_init(st);
if (ret)
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 b05c33876b8d..76d92a550278 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -17,6 +17,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -50,6 +51,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[] = {
@@ -65,13 +67,15 @@ 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: 8 bytes */
+/* IIO buffer data: 16 bytes */
struct inv_icm42600_gyro_buffer {
struct inv_icm42600_fifo_sensor_data gyro;
int8_t temp;
uint8_t padding;
+ int64_t timestamp;
};
#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
@@ -92,6 +96,7 @@ 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_timestamp *ts = iio_priv(indio_dev);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0;
unsigned int sleep_gyro = 0;
@@ -119,6 +124,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write */
+ inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -311,9 +317,11 @@ static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
return IIO_VAL_INT_PLUS_MICRO;
}
-static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
+static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
int val, int val2)
{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct device *dev = regmap_get_device(st->map);
unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
@@ -332,6 +340,11 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
pm_runtime_get_sync(dev);
mutex_lock(&st->lock);
+ ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
+ iio_buffer_enabled(indio_dev));
+ if (ret)
+ goto out_unlock;
+
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
if (ret)
goto out_unlock;
@@ -620,7 +633,7 @@ static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
iio_device_release_direct_mode(indio_dev);
return ret;
case IIO_CHAN_INFO_SAMP_FREQ:
- return inv_icm42600_gyro_write_odr(st, val, val2);
+ return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
case IIO_CHAN_INFO_CALIBBIAS:
ret = iio_device_claim_direct_mode(indio_dev);
if (ret)
@@ -703,6 +716,8 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
{
struct device *dev = regmap_get_device(st->map);
const char *name;
+ struct inv_icm42600_timestamp *ts;
+ uint32_t period;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
@@ -710,7 +725,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!name)
return -ENOMEM;
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return -ENOMEM;
@@ -718,6 +733,10 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!buffer)
return -ENOMEM;
+ ts = iio_priv(indio_dev);
+ period = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+ inv_icm42600_timestamp_init(ts, period);
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->dev.parent = dev;
indio_dev->name = name;
@@ -737,16 +756,19 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *timestamp;
const int8_t *temp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_gyro_buffer buffer = {
.padding = 0,
};
/* parse all fifo packets */
- for (i = 0; i < st->fifo.count; i += size) {
+ for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
&accel, &gyro, &temp, ×tamp, &odr);
/* quit if error or FIFO is empty */
@@ -757,10 +779,16 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
continue;
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_GYRO)
+ inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
+ st->fifo.nb.total, no);
+
/* fill and push data buffer */
memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
buffer.temp = temp ? *temp : 0;
- iio_push_to_buffers(indio_dev, &buffer);
+ ts_val = inv_icm42600_timestamp_pop(ts);
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
}
return 0;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
new file mode 100644
index 000000000000..7f2dc41f807b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
@@ -0,0 +1,195 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/regmap.h>
+#include <linux/math64.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
+
+/* internal chip period is 32kHz, 31250ns */
+#define INV_ICM42600_TIMESTAMP_PERIOD 31250
+/* allow a jitter of +/- 2% */
+#define INV_ICM42600_TIMESTAMP_JITTER 2
+/* compute min and max periods accepted */
+#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \
+ (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
+#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
+ (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
+
+/* Add a new value inside an accumulator and update the estimate value */
+static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
+{
+ uint64_t sum = 0;
+ size_t i;
+
+ acc->values[acc->idx++] = val;
+ if (acc->idx >= ARRAY_SIZE(acc->values))
+ acc->idx = 0;
+
+ /* compute the mean of all stored values, use 0 as empty slot */
+ for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
+ if (acc->values[i] == 0)
+ break;
+ sum += acc->values[i];
+ }
+
+ acc->val = div_u64(sum, i);
+}
+
+void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
+ uint32_t period)
+{
+ /* initial odr for sensor after reset is 1kHz */
+ const uint32_t default_period = 1000000;
+
+ /* current multiplier and period values after reset */
+ ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ ts->period = default_period;
+ /* new set multiplier is the one from chip initialization */
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+
+ /* use theoretical value for chip period */
+ inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
+}
+
+int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
+{
+ unsigned int val;
+
+ /* enable timestamp register */
+ val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
+ INV_ICM42600_TMST_CONFIG_TMST_EN;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
+ INV_ICM42600_TMST_CONFIG_MASK, val);
+}
+
+int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t period, bool fifo)
+{
+ /* when FIFO is on, prevent odr change if one is already pending */
+ if (fifo && ts->new_mult != 0)
+ return -EAGAIN;
+
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+
+ return 0;
+}
+
+static bool inv_validate_period(uint32_t period, uint32_t mult)
+{
+ const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
+ uint32_t period_min, period_max;
+
+ /* check that period is acceptable */
+ period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
+ period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
+ if (period > period_min && period < period_max)
+ return true;
+ else
+ return false;
+}
+
+static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
+ uint32_t mult, uint32_t period)
+{
+ uint32_t new_chip_period;
+
+ if (!inv_validate_period(period, mult))
+ return false;
+
+ /* update chip internal period estimation */
+ new_chip_period = period / mult;
+ inv_update_acc(&ts->chip_period, new_chip_period);
+
+ return true;
+}
+
+void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ size_t sensor_nb, int64_t timestamp)
+{
+ struct inv_icm42600_timestamp_interval *it;
+ int64_t delta, interval;
+ const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ uint32_t period = ts->period;
+ int32_t m;
+ bool valid = false;
+
+ if (fifo_nb == 0)
+ return;
+
+ /* update interrupt timestamp and compute chip and sensor periods */
+ it = &ts->it;
+ it->lo = it->up;
+ it->up = timestamp;
+ delta = it->up - it->lo;
+ if (it->lo != 0) {
+ /* compute period: delta time divided by number of samples */
+ period = div_s64(delta, fifo_nb);
+ valid = inv_compute_chip_period(ts, fifo_mult, period);
+ /* update sensor period if chip internal period is updated */
+ if (valid)
+ ts->period = ts->mult * ts->chip_period.val;
+ }
+
+ /* no previous data, compute theoritical value from interrupt */
+ if (ts->timestamp == 0) {
+ /* elapsed time: sensor period * sensor samples number */
+ interval = (int64_t)ts->period * (int64_t)sensor_nb;
+ ts->timestamp = it->up - interval;
+ return;
+ }
+
+ /* if interrupt interval is valid, sync with interrupt timestamp */
+ if (valid) {
+ /* compute measured fifo_period */
+ fifo_period = fifo_mult * ts->chip_period.val;
+ /* delta time between last sample and last interrupt */
+ delta = it->lo - ts->timestamp;
+ /* if there are multiple samples, go back to first one */
+ while (delta >= (fifo_period * 3 / 2))
+ delta -= fifo_period;
+ /* compute maximal adjustment value */
+ m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
+ if (delta > m)
+ delta = m;
+ else if (delta < -m)
+ delta = -m;
+ ts->timestamp += delta;
+ }
+}
+
+void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ unsigned int fifo_no)
+{
+ int64_t interval;
+ uint32_t fifo_mult;
+
+ if (ts->new_mult == 0)
+ return;
+
+ /* update to new multiplier and update period */
+ ts->mult = ts->new_mult;
+ ts->new_mult = 0;
+ ts->period = ts->mult * ts->chip_period.val;
+
+ /*
+ * After ODR change the time interval with the previous sample is
+ * undertermined (depends when the change occures). So we compute the
+ * timestamp from the current interrupt using the new FIFO period, the
+ * total number of samples and the current sample numero.
+ */
+ if (ts->timestamp != 0) {
+ /* compute measured fifo period */
+ fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ fifo_period = fifo_mult * ts->chip_period.val;
+ /* computes time interval between interrupt and this sample */
+ interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
+ ts->timestamp = ts->it.up - interval;
+ }
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
new file mode 100644
index 000000000000..4e4f331d4fe4
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_TIMESTAMP_H_
+#define INV_ICM42600_TIMESTAMP_H_
+
+#include <linux/kernel.h>
+
+struct inv_icm42600_state;
+
+/**
+ * struct inv_icm42600_timestamp_interval - timestamps interval
+ * @lo: interval lower bound
+ * @up: interval upper bound
+ */
+struct inv_icm42600_timestamp_interval {
+ int64_t lo;
+ int64_t up;
+};
+
+/**
+ * struct inv_icm42600_timestamp_acc - accumulator for computing an estimation
+ * @val: current estimation of the value, the mean of all values
+ * @idx: current index of the next free place in values table
+ * @values: table of all measured values, use for computing the mean
+ */
+struct inv_icm42600_timestamp_acc {
+ uint32_t val;
+ size_t idx;
+ uint32_t values[32];
+};
+
+/**
+ * struct inv_icm42600_timestamp - timestamp management states
+ * @it: interrupts interval timestamps
+ * @timestamp: store last timestamp for computing next data timestamp
+ * @mult: current internal period multiplier
+ * @new_mult: new set internal period multiplier (not yet effective)
+ * @period: measured current period of the sensor
+ * @chip_period: accumulator for computing internal chip period
+ */
+struct inv_icm42600_timestamp {
+ struct inv_icm42600_timestamp_interval it;
+ int64_t timestamp;
+ uint32_t mult;
+ uint32_t new_mult;
+ uint32_t period;
+ struct inv_icm42600_timestamp_acc chip_period;
+};
+
+void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
+ uint32_t period);
+
+int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st);
+
+int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t period, bool fifo);
+
+void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ size_t sensor_nb, int64_t timestamp);
+
+static inline int64_t
+inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts)
+{
+ ts->timestamp += ts->period;
+ return ts->timestamp;
+}
+
+void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ unsigned int fifo_no);
+
+static inline void
+inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts)
+{
+ const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL};
+
+ ts->it = interval_init;
+ ts->timestamp = 0;
+}
+
+#endif
--
2.17.1
Add all FIFO parsing and reading functions. Add accel and gyro
kfifo buffer and FIFO data parsing. Use device interrupt for
reading data FIFO and launching accel and gyro parsing.
Support hwfifo watermark by multiplexing gyro and accel settings.
Support hwfifo flush.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/Kconfig | 1 +
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 160 ++++-
.../imu/inv_icm42600/inv_icm42600_buffer.c | 555 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 98 ++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 30 +
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 160 ++++-
8 files changed, 1011 insertions(+), 2 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 22390a72f0a3..50cbcfcb6cf1 100644
--- a/drivers/iio/imu/inv_icm42600/Kconfig
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -2,6 +2,7 @@
config INV_ICM42600
tristate
+ select IIO_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 48965824f00c..0f49f6df3647 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -5,6 +5,7 @@ inv-icm42600-y += inv_icm42600_core.o
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_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 43749f56426c..4d5811562a61 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -14,6 +14,8 @@
#include <linux/pm.h>
#include <linux/iio/iio.h>
+#include "inv_icm42600_buffer.h"
+
enum inv_icm42600_chip {
INV_CHIP_ICM42600,
INV_CHIP_ICM42602,
@@ -123,6 +125,7 @@ struct inv_icm42600_suspended {
* @indio_gyro: gyroscope IIO device.
* @indio_accel: accelerometer IIO device.
* @buffer: data transfer buffer aligned for DMA.
+ * @fifo: FIFO management structure.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -137,6 +140,7 @@ struct inv_icm42600_state {
struct iio_dev *indio_gyro;
struct iio_dev *indio_accel;
uint8_t buffer[2] ____cacheline_aligned;
+ struct inv_icm42600_fifo fifo;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -377,6 +381,10 @@ 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);
+
int inv_icm42600_accel_init(struct inv_icm42600_state *st);
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 6a615d7ffb24..c73ce204efc6 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -11,9 +11,12 @@
#include <linux/delay.h>
#include <linux/math64.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
};
+/* IIO buffer data: 8 bytes */
+struct inv_icm42600_accel_buffer {
+ struct inv_icm42600_fifo_sensor_data accel;
+ int8_t temp;
+ uint8_t padding;
+};
+
+#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,
+};
+
+/* 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 */
+ 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)
@@ -248,7 +321,12 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ if (ret)
+ goto out_unlock;
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
+out_unlock:
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
pm_runtime_put_autosuspend(dev);
@@ -563,12 +641,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)
@@ -576,6 +693,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;
+ struct iio_buffer *buffer;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
if (!name)
@@ -585,14 +703,54 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!indio_dev)
return -ENOMEM;
+ buffer = devm_iio_kfifo_allocate(dev);
+ if (!buffer)
+ return -ENOMEM;
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->dev.parent = dev;
indio_dev->name = name;
indio_dev->info = &inv_icm42600_accel_info;
- indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
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;
+ indio_dev->setup_ops = &inv_icm42600_buffer_ops;
+
+ iio_device_attach_buffer(indio_dev, buffer);
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)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ ssize_t i, size;
+ const void *accel, *gyro, *timestamp;
+ const int8_t *temp;
+ unsigned int odr;
+ struct inv_icm42600_accel_buffer buffer = {
+ .padding = 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);
+ /* 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))
+ continue;
+
+ /* fill and push data buffer */
+ memcpy(&buffer.accel, accel, sizeof(buffer.accel));
+ buffer.temp = temp ? *temp : 0;
+ iio_push_to_buffers(indio_dev, &buffer);
+ }
+
+ 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..c91075f62231
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -0,0 +1,555 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#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 "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
+
+/* 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)
+
+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
+
+ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
+ const void **gyro, const int8_t **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;
+}
+
+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;
+ 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;
+
+ /* changing FIFO watermark requires to turn off watermark interrupt */
+ ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
+ 0, &restore);
+ if (ret)
+ return ret;
+
+ raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
+ memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
+ ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
+ st->buffer, sizeof(raw_wm));
+ if (ret)
+ return ret;
+
+ /* restore watermark interrupt */
+ if (restore) {
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
+ 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;
+}
+
+/*
+ * update_scan_mode callback is turning sensors on and setting data FIFO enable
+ * bits.
+ */
+static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ /* exit if FIFO is already on */
+ if (st->fifo.on) {
+ ret = 0;
+ goto out_on;
+ }
+
+ /* set FIFO threshold interrupt */
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
+ if (ret)
+ goto out_unlock;
+
+ /* flush FIFO data */
+ ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
+ INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
+ if (ret)
+ goto out_unlock;
+
+ /* set FIFO in streaming mode */
+ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+ INV_ICM42600_FIFO_CONFIG_STREAM);
+ if (ret)
+ goto out_unlock;
+
+ /* workaround: first read of FIFO count after reset is always 0 */
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2);
+ if (ret)
+ goto out_unlock;
+
+out_on:
+ /* increase FIFO on counter */
+ st->fifo.on++;
+out_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ /* exit if there are several sensors using the FIFO */
+ if (st->fifo.on > 1) {
+ ret = 0;
+ goto out_off;
+ }
+
+ /* 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;
+
+ /* flush FIFO data */
+ ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
+ INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
+ if (ret)
+ goto out_unlock;
+
+ /* disable FIFO threshold interrupt */
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
+ if (ret)
+ goto out_unlock;
+
+out_off:
+ /* decrease FIFO on counter */
+ st->fifo.on--;
+out_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+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_temp = 0;
+ unsigned int sleep_sensor = 0;
+ unsigned int sleep;
+ 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_sensor);
+ else
+ ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
+ if (ret)
+ goto out_unlock;
+
+ /* if FIFO is off, turn temperature off */
+ if (!st->fifo.on)
+ ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+
+ /* sleep maximum required time */
+ if (sleep_sensor > sleep_temp)
+ sleep = sleep_sensor;
+ else
+ sleep = sleep_temp;
+ 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 = inv_icm42600_buffer_postenable,
+ .predisable = inv_icm42600_buffer_predisable,
+ .postdisable = inv_icm42600_buffer_postdisable,
+};
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+ unsigned int max)
+{
+ size_t max_count;
+ __be16 *raw_fifo_count;
+ ssize_t i, size;
+ const void *accel, *gyro, *timestamp;
+ const int8_t *temp;
+ 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 */
+ raw_fifo_count = (__be16 *)st->buffer;
+ 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_cpup(raw_fifo_count);
+
+ /* check and clamp 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_fifo_parse(struct inv_icm42600_state *st)
+{
+ int ret;
+
+ if (st->fifo.nb.total == 0)
+ return 0;
+
+ if (st->fifo.nb.gyro > 0) {
+ ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
+ if (ret)
+ return ret;
+ }
+
+ if (st->fifo.nb.accel > 0) {
+ ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+ unsigned int count)
+{
+ int ret;
+
+ ret = inv_icm42600_buffer_fifo_read(st, count);
+ if (ret)
+ return ret;
+
+ if (st->fifo.nb.total == 0)
+ return 0;
+
+ if (st->fifo.nb.gyro > 0) {
+ ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
+ if (ret)
+ return ret;
+ }
+
+ if (st->fifo.nb.accel > 0) {
+ ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
+{
+ unsigned int val;
+ int ret;
+
+ /*
+ * Default FIFO configuration (bits 7 to 5)
+ * - use invalid value
+ * - FIFO count in bytes
+ * - FIFO count in big endian
+ */
+ val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ GENMASK(7, 5), val);
+ if (ret)
+ return ret;
+
+ /*
+ * Enable FIFO partial read and continuous watermark interrupt.
+ * Disable all FIFO EN bits.
+ */
+ 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,
+ GENMASK(6, 5) | GENMASK(3, 0), 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..de2a3949dcc7
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
@@ -0,0 +1,98 @@
+/* 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 - FIFO state variables
+ * @on: reference counter for FIFO on.
+ * @en: bits field of INV_ICM42600_SENSOR_* for FIFO EN bits.
+ * @period: FIFO internal period.
+ * @watermark: watermark configuration values for accel and gyro.
+ * @count: number of bytes in the FIFO data buffer.
+ * @nb: gyro, accel and total samples in the FIFO data buffer.
+ * @data: FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache).
+ */
+struct inv_icm42600_fifo {
+ unsigned int on;
+ 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] ____cacheline_aligned;
+};
+
+/* FIFO data packet */
+struct inv_icm42600_fifo_sensor_data {
+ __be16 x;
+ __be16 y;
+ __be16 z;
+} __packed;
+#define INV_ICM42600_FIFO_DATA_INVALID -32768
+
+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;
+}
+
+ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
+ const void **gyro, const int8_t **temp,
+ const void **timestamp, unsigned int *odr);
+
+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_fifo_parse(struct inv_icm42600_state *st);
+
+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 246c1eb52231..6f1c1eb83953 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -18,6 +18,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[] = {
{
@@ -429,6 +430,18 @@ static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
dev_warn(dev, "FIFO full data lost!\n");
+ /* FIFO threshold reached */
+ 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);
+ goto out_unlock;
+ }
+ ret = inv_icm42600_buffer_fifo_parse(st);
+ if (ret)
+ dev_err(dev, "FIFO parsing error %d\n", ret);
+ }
+
out_unlock:
mutex_unlock(&st->lock);
return IRQ_HANDLED;
@@ -600,6 +613,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ ret = inv_icm42600_buffer_init(st);
+ if (ret)
+ return ret;
+
ret = inv_icm42600_gyro_init(st);
if (ret)
return ret;
@@ -645,6 +662,14 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
goto out_unlock;
}
+ /* disable FIFO data streaming */
+ if (st->fifo.on) {
+ 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);
@@ -684,6 +709,11 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
if (ret)
goto out_unlock;
+ /* restore FIFO data streaming */
+ if (st->fifo.on)
+ 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 38654e0d217b..b05c33876b8d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -11,9 +11,12 @@
#include <linux/delay.h>
#include <linux/math64.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
};
+/* IIO buffer data: 8 bytes */
+struct inv_icm42600_gyro_buffer {
+ struct inv_icm42600_fifo_sensor_data gyro;
+ int8_t temp;
+ uint8_t padding;
+};
+
+#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,
+};
+
+/* 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 */
+ 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)
@@ -260,7 +333,12 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ if (ret)
+ goto out_unlock;
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
+out_unlock:
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
pm_runtime_put_autosuspend(dev);
@@ -574,12 +652,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)
@@ -587,6 +704,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;
+ struct iio_buffer *buffer;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
if (!name)
@@ -596,14 +714,54 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!indio_dev)
return -ENOMEM;
+ buffer = devm_iio_kfifo_allocate(dev);
+ if (!buffer)
+ return -ENOMEM;
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->dev.parent = dev;
indio_dev->name = name;
indio_dev->info = &inv_icm42600_gyro_info;
- indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
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;
+ indio_dev->setup_ops = &inv_icm42600_buffer_ops;
+
+ iio_device_attach_buffer(indio_dev, buffer);
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)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ ssize_t i, size;
+ const void *accel, *gyro, *timestamp;
+ const int8_t *temp;
+ unsigned int odr;
+ struct inv_icm42600_gyro_buffer buffer = {
+ .padding = 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);
+ /* 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))
+ continue;
+
+ /* fill and push data buffer */
+ memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
+ buffer.temp = temp ? *temp : 0;
+ iio_push_to_buffers(indio_dev, &buffer);
+ }
+
+ return 0;
+}
--
2.17.1
Hi Jean-Baptiste,
Thank you for the patch! Yet something to improve:
[auto build test ERROR on iio/togreg]
[also build test ERROR on robh/for-next linus/master v5.7-rc7 next-20200529]
[if your patch is applied to the wrong git tree, please drop us a note to help
improve the system. BTW, we also suggest to use '--base' option to specify the
base tree in git format-patch, please see https://stackoverflow.com/a/37406982]
url: https://github.com/0day-ci/linux/commits/Jean-Baptiste-Maneyrol/iio-imu-new-inv_icm42600-driver/20200528-030632
base: https://git.kernel.org/pub/scm/linux/kernel/git/jic23/iio.git togreg
config: i386-randconfig-c001-20200529 (attached as .config)
compiler: gcc-9 (Debian 9.3.0-13) 9.3.0
If you fix the issue, kindly add following tag as appropriate
Reported-by: kbuild test robot <[email protected]>
All errors (new ones prefixed by >>, old ones prefixed by <<):
ld: drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.o: in function `inv_icm42600_buffer_update_watermark':
drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c:215: undefined reference to `__moddi3'
>> ld: drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c:212: undefined reference to `__moddi3'
---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/[email protected]
On Wed, 27 May 2020 20:57:08 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Add all FIFO parsing and reading functions. Add accel and gyro
> kfifo buffer and FIFO data parsing. Use device interrupt for
> reading data FIFO and launching accel and gyro parsing.
>
> Support hwfifo watermark by multiplexing gyro and accel settings.
> Support hwfifo flush.
Both of these are complex given the interactions of the two sensors
types and to be honest I couldn't figure out exactly what the intent was.
Needs more docs!
Thanks,
Jonathan
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> ---
> drivers/iio/imu/inv_icm42600/Kconfig | 1 +
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 160 ++++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 555 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.h | 98 ++++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 30 +
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 160 ++++-
> 8 files changed, 1011 insertions(+), 2 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 22390a72f0a3..50cbcfcb6cf1 100644
> --- a/drivers/iio/imu/inv_icm42600/Kconfig
> +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> @@ -2,6 +2,7 @@
>
> config INV_ICM42600
> tristate
> + select IIO_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 48965824f00c..0f49f6df3647 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -5,6 +5,7 @@ inv-icm42600-y += inv_icm42600_core.o
> 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_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 43749f56426c..4d5811562a61 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -14,6 +14,8 @@
> #include <linux/pm.h>
> #include <linux/iio/iio.h>
>
> +#include "inv_icm42600_buffer.h"
> +
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> INV_CHIP_ICM42602,
> @@ -123,6 +125,7 @@ struct inv_icm42600_suspended {
> * @indio_gyro: gyroscope IIO device.
> * @indio_accel: accelerometer IIO device.
> * @buffer: data transfer buffer aligned for DMA.
> + * @fifo: FIFO management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -137,6 +140,7 @@ struct inv_icm42600_state {
> struct iio_dev *indio_gyro;
> struct iio_dev *indio_accel;
> uint8_t buffer[2] ____cacheline_aligned;
> + struct inv_icm42600_fifo fifo;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -377,6 +381,10 @@ 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);
> +
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
> +
> #endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 6a615d7ffb24..c73ce204efc6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -11,9 +11,12 @@
> #include <linux/delay.h>
> #include <linux/math64.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> };
>
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_accel_buffer {
> + struct inv_icm42600_fifo_sensor_data accel;
> + int8_t temp;
> + uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> + 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)
> @@ -248,7 +321,12 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
>
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + if (ret)
> + goto out_unlock;
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
>
> +out_unlock:
> mutex_unlock(&st->lock);
> pm_runtime_mark_last_busy(dev);
> pm_runtime_put_autosuspend(dev);
> @@ -563,12 +641,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)
> @@ -576,6 +693,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;
> + struct iio_buffer *buffer;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
> if (!name)
> @@ -585,14 +703,54 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!indio_dev)
> return -ENOMEM;
>
> + buffer = devm_iio_kfifo_allocate(dev);
> + if (!buffer)
> + return -ENOMEM;
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->dev.parent = dev;
> indio_dev->name = name;
> indio_dev->info = &inv_icm42600_accel_info;
> - indio_dev->modes = INDIO_DIRECT_MODE;
> + indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
> 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;
> + indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> + iio_device_attach_buffer(indio_dev, buffer);
>
> 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)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + ssize_t i, size;
> + const void *accel, *gyro, *timestamp;
> + const int8_t *temp;
> + unsigned int odr;
> + struct inv_icm42600_accel_buffer buffer = {
> + .padding = 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);
> + /* 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))
> + continue;
> +
> + /* fill and push data buffer */
> + memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> + buffer.temp = temp ? *temp : 0;
> + iio_push_to_buffers(indio_dev, &buffer);
> + }
> +
> + 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..c91075f62231
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,555 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/kernel.h>
> +#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 "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
> +
> +/* 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)
> +
> +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
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> + const void **gyro, const int8_t **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;
> +}
> +
> +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;
> +}
> +
I think some overview docs on how this is working would be good.
Set out the aims for the watermark selected and how it is achieved.
> +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;
> + 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;
> +
> + /* changing FIFO watermark requires to turn off watermark interrupt */
> + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> + 0, &restore);
> + if (ret)
> + return ret;
> +
> + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> + memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
> + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> + st->buffer, sizeof(raw_wm));
> + if (ret)
> + return ret;
> +
> + /* restore watermark interrupt */
> + if (restore) {
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> + 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;
> +}
> +
> +/*
> + * update_scan_mode callback is turning sensors on and setting data FIFO enable
> + * bits.
> + */
> +static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + /* exit if FIFO is already on */
> + if (st->fifo.on) {
> + ret = 0;
> + goto out_on;
> + }
> +
> + /* set FIFO threshold interrupt */
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> + if (ret)
> + goto out_unlock;
> +
> + /* flush FIFO data */
> + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> + if (ret)
> + goto out_unlock;
> +
> + /* set FIFO in streaming mode */
> + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> + INV_ICM42600_FIFO_CONFIG_STREAM);
> + if (ret)
> + goto out_unlock;
> +
> + /* workaround: first read of FIFO count after reset is always 0 */
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2);
> + if (ret)
> + goto out_unlock;
> +
> +out_on:
> + /* increase FIFO on counter */
> + st->fifo.on++;
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + /* exit if there are several sensors using the FIFO */
> + if (st->fifo.on > 1) {
> + ret = 0;
> + goto out_off;
> + }
> +
> + /* 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;
> +
> + /* flush FIFO data */
> + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> + if (ret)
> + goto out_unlock;
> +
> + /* disable FIFO threshold interrupt */
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
> + if (ret)
> + goto out_unlock;
> +
> +out_off:
> + /* decrease FIFO on counter */
> + st->fifo.on--;
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +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_temp = 0;
> + unsigned int sleep_sensor = 0;
> + unsigned int sleep;
> + 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_sensor);
> + else
> + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
> + if (ret)
> + goto out_unlock;
> +
> + /* if FIFO is off, turn temperature off */
> + if (!st->fifo.on)
> + ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> +
> + /* sleep maximum required time */
> + if (sleep_sensor > sleep_temp)
> + sleep = sleep_sensor;
> + else
> + sleep = sleep_temp;
> + 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 = inv_icm42600_buffer_postenable,
We've been slowly eroding the difference between preenable and posteenable.
Would be good to understand why you need to define both?
> + .predisable = inv_icm42600_buffer_predisable,
> + .postdisable = inv_icm42600_buffer_postdisable,
> +};
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> + unsigned int max)
> +{
> + size_t max_count;
> + __be16 *raw_fifo_count;
> + ssize_t i, size;
> + const void *accel, *gyro, *timestamp;
> + const int8_t *temp;
> + 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 */
> + raw_fifo_count = (__be16 *)st->buffer;
> + 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_cpup(raw_fifo_count);
> +
> + /* check and clamp 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_fifo_parse(struct inv_icm42600_state *st)
> +{
> + int ret;
> +
> + if (st->fifo.nb.total == 0)
> + return 0;
> +
> + if (st->fifo.nb.gyro > 0) {
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> + if (ret)
> + return ret;
> + }
> +
> + if (st->fifo.nb.accel > 0) {
> + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> + if (ret)
> + return ret;
> + }
> +
> + return 0;
> +}
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> + unsigned int count)
> +{
> + int ret;
> +
> + ret = inv_icm42600_buffer_fifo_read(st, count);
> + if (ret)
> + return ret;
Definitely searching my memory for how this works in the core, so
I may have it wrong.
This is a bit unusual (I think). The intent of the flush
is to read up to 'n' bytes because someone just did a read on the buffer
or select, and there was data in the hwfifo capable of satisfying the read
even though we haven't yet reached the watermark.
Given both sensor types are coming from one buffer, do we have a potential
issue here or under serving even though data is available?
The case I worry may be served late is when an poll / select
is waiting for sufficient data.
So what should we be doing? We want to guarantee to provide data
for each sensor type if it's in the hwfifo. As such we could keep reading
until we have enough, but that could cause some issues if the two data rates
are very different (overflow on the other kfifo)
Maybe what you have here is the best we can do.
I'm assuming the watermark level has a similar problem. One value represents
the sum of the two types of data.
> +
> + if (st->fifo.nb.total == 0)
> + return 0;
> +
> + if (st->fifo.nb.gyro > 0) {
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> + if (ret)
> + return ret;
> + }
> +
> + if (st->fifo.nb.accel > 0) {
> + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> + if (ret)
> + return ret;
> + }
> +
> + return 0;
> +}
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> +{
> + unsigned int val;
> + int ret;
> +
> + /*
> + * Default FIFO configuration (bits 7 to 5)
> + * - use invalid value
> + * - FIFO count in bytes
> + * - FIFO count in big endian
> + */
> + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + GENMASK(7, 5), val);
> + if (ret)
> + return ret;
> +
> + /*
> + * Enable FIFO partial read and continuous watermark interrupt.
> + * Disable all FIFO EN bits.
> + */
> + 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,
> + GENMASK(6, 5) | GENMASK(3, 0), 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..de2a3949dcc7
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> @@ -0,0 +1,98 @@
> +/* 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 - FIFO state variables
> + * @on: reference counter for FIFO on.
> + * @en: bits field of INV_ICM42600_SENSOR_* for FIFO EN bits.
> + * @period: FIFO internal period.
> + * @watermark: watermark configuration values for accel and gyro.
> + * @count: number of bytes in the FIFO data buffer.
> + * @nb: gyro, accel and total samples in the FIFO data buffer.
> + * @data: FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache).
> + */
> +struct inv_icm42600_fifo {
> + unsigned int on;
> + 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] ____cacheline_aligned;
> +};
> +
> +/* FIFO data packet */
> +struct inv_icm42600_fifo_sensor_data {
> + __be16 x;
> + __be16 y;
> + __be16 z;
> +} __packed;
Why packed? Should be anyway I think.
> +#define INV_ICM42600_FIFO_DATA_INVALID -32768
> +
> +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;
> +}
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> + const void **gyro, const int8_t **temp,
> + const void **timestamp, unsigned int *odr);
> +
> +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_fifo_parse(struct inv_icm42600_state *st);
> +
> +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 246c1eb52231..6f1c1eb83953 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -18,6 +18,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[] = {
> {
> @@ -429,6 +430,18 @@ static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
> if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
> dev_warn(dev, "FIFO full data lost!\n");
>
> + /* FIFO threshold reached */
> + 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);
> + goto out_unlock;
> + }
> + ret = inv_icm42600_buffer_fifo_parse(st);
> + if (ret)
> + dev_err(dev, "FIFO parsing error %d\n", ret);
> + }
> +
> out_unlock:
> mutex_unlock(&st->lock);
> return IRQ_HANDLED;
> @@ -600,6 +613,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + ret = inv_icm42600_buffer_init(st);
> + if (ret)
> + return ret;
> +
> ret = inv_icm42600_gyro_init(st);
> if (ret)
> return ret;
> @@ -645,6 +662,14 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
> goto out_unlock;
> }
>
> + /* disable FIFO data streaming */
> + if (st->fifo.on) {
> + 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);
> @@ -684,6 +709,11 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
> if (ret)
> goto out_unlock;
>
> + /* restore FIFO data streaming */
> + if (st->fifo.on)
> + 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 38654e0d217b..b05c33876b8d 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -11,9 +11,12 @@
> #include <linux/delay.h>
> #include <linux/math64.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> };
>
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_gyro_buffer {
> + struct inv_icm42600_fifo_sensor_data gyro;
> + int8_t temp;
> + uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> + 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)
> @@ -260,7 +333,12 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
>
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + if (ret)
> + goto out_unlock;
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
>
> +out_unlock:
> mutex_unlock(&st->lock);
> pm_runtime_mark_last_busy(dev);
> pm_runtime_put_autosuspend(dev);
> @@ -574,12 +652,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)
> +{
Nothing to do with this patch, but I realised reading this that we have
some 'unusual' use of the word flush here. It's a straight forward
read function so not sure why we called it flush.
> + 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)
> @@ -587,6 +704,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;
> + struct iio_buffer *buffer;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> if (!name)
> @@ -596,14 +714,54 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!indio_dev)
> return -ENOMEM;
>
> + buffer = devm_iio_kfifo_allocate(dev);
> + if (!buffer)
> + return -ENOMEM;
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->dev.parent = dev;
> indio_dev->name = name;
> indio_dev->info = &inv_icm42600_gyro_info;
> - indio_dev->modes = INDIO_DIRECT_MODE;
> + indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
> 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;
> + indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> + iio_device_attach_buffer(indio_dev, buffer);
>
> 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)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + ssize_t i, size;
> + const void *accel, *gyro, *timestamp;
> + const int8_t *temp;
> + unsigned int odr;
> + struct inv_icm42600_gyro_buffer buffer = {
> + .padding = 0,
Might be worth a comment here or where the structure is defined
on why we make padding explicit.
> + };
> +
> + /* 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);
> + /* 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))
> + continue;
> +
> + /* fill and push data buffer */
> + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> + buffer.temp = temp ? *temp : 0;
> + iio_push_to_buffers(indio_dev, &buffer);
> + }
> +
> + return 0;
> +}
On Wed, 27 May 2020 20:57:09 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Add a timestamping mechanism for buffer that provides accurate
> event timestamps when using watermark. This mechanism estimates
> device internal clock by comparing FIFO interrupts delta time and
> device elapsed time computed by parsing FIFO data.
>
> Take interrupt timestamp in hard irq handler and add IIO device
> specific timestamp structures in device private allocation.
I haven't checked the maths or anything, but basic principle seems
sound. I'm wondering if we want to document somewhere what the
various ways we do this sort of timestamp generation are. They
give me a headache normally and it would be good to point people
to a reference. Still that's a job for another day.
I commented on the (lack) of need for force 8 byte alignment inline.
Jonathan
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> ---
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 5 +
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 40 +++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 17 +-
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 40 +++-
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++++++++
> 8 files changed, 398 insertions(+), 13 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
>
> diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
> index 0f49f6df3647..291714d9aa54 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_buffer.o
> +inv-icm42600-y += inv_icm42600_timestamp.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 4d5811562a61..2de0dd7675fb 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -126,6 +126,7 @@ struct inv_icm42600_suspended {
> * @indio_accel: accelerometer IIO device.
> * @buffer: data transfer buffer aligned for DMA.
> * @fifo: FIFO management structure.
> + * @timestamp: interrupt timestamps.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -141,6 +142,10 @@ struct inv_icm42600_state {
> struct iio_dev *indio_accel;
> uint8_t buffer[2] ____cacheline_aligned;
> struct inv_icm42600_fifo fifo;
> + struct {
> + int64_t gyro;
> + int64_t accel;
> + } timestamp;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index c73ce204efc6..ec1d124c1471 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -17,6 +17,7 @@
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -50,6 +51,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[] = {
> @@ -65,13 +67,15 @@ 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: 8 bytes */
> +/* IIO buffer data: 16 bytes */
> struct inv_icm42600_accel_buffer {
> struct inv_icm42600_fifo_sensor_data accel;
> int8_t temp;
> uint8_t padding;
> + int64_t timestamp;
So this falls into the open question I have in the cleanup set on timestamp
alignment. What you have here guarantees that we have the correct spacing, but
it allows the alignment of the whole structure to be 4 bytes on x86_32
I don't 'think' that's a problem because the relevant unaligned 8 bytes write
and read should be fine at 4 byte alignment. Most other archs will
do 8 byte alignment anyway.
Though I'd explain that here to avoid confusion against the fix set where
I will probably be more paranoid and mark all of them __aligned(8) as it
makes it harder to get it wrong.
> };
>
> #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
> @@ -92,6 +96,7 @@ 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_timestamp *ts = iio_priv(indio_dev);
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int fifo_en = 0;
> unsigned int sleep_temp = 0;
> @@ -119,6 +124,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write */
> + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -299,9 +305,11 @@ static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
> return IIO_VAL_INT_PLUS_MICRO;
> }
>
> -static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> +static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
> int val, int val2)
> {
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
> struct device *dev = regmap_get_device(st->map);
> unsigned int idx;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> @@ -320,6 +328,11 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> pm_runtime_get_sync(dev);
> mutex_lock(&st->lock);
>
> + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
> + iio_buffer_enabled(indio_dev));
> + if (ret)
> + goto out_unlock;
> +
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> if (ret)
> goto out_unlock;
> @@ -609,7 +622,7 @@ static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
> iio_device_release_direct_mode(indio_dev);
> return ret;
> case IIO_CHAN_INFO_SAMP_FREQ:
> - return inv_icm42600_accel_write_odr(st, val, val2);
> + return inv_icm42600_accel_write_odr(indio_dev, val, val2);
> case IIO_CHAN_INFO_CALIBBIAS:
> ret = iio_device_claim_direct_mode(indio_dev);
> if (ret)
> @@ -692,6 +705,8 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> {
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> + struct inv_icm42600_timestamp *ts;
> + uint32_t period;
> struct iio_dev *indio_dev;
> struct iio_buffer *buffer;
>
> @@ -699,7 +714,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!name)
> return -ENOMEM;
>
> - indio_dev = devm_iio_device_alloc(dev, 0);
> + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
> if (!indio_dev)
> return -ENOMEM;
>
> @@ -707,6 +722,10 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!buffer)
> return -ENOMEM;
>
> + ts = iio_priv(indio_dev);
> + period = inv_icm42600_odr_to_period(st->conf.accel.odr);
> + inv_icm42600_timestamp_init(ts, period);
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->dev.parent = dev;
> indio_dev->name = name;
> @@ -726,16 +745,19 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
> {
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *timestamp;
> const int8_t *temp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_accel_buffer buffer = {
> .padding = 0,
> };
>
> /* parse all fifo packets */
> - for (i = 0; i < st->fifo.count; i += size) {
> + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
> size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> &accel, &gyro, &temp, ×tamp, &odr);
> /* quit if error or FIFO is empty */
> @@ -746,10 +768,16 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
> if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
> continue;
>
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_ACCEL)
> + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
> + st->fifo.nb.total, no);
> +
> /* fill and push data buffer */
> memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> buffer.temp = temp ? *temp : 0;
> - iio_push_to_buffers(indio_dev, &buffer);
> + ts_val = inv_icm42600_timestamp_pop(ts);
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
> }
>
> return 0;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> index c91075f62231..3c8b1b19de15 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -14,6 +14,7 @@
> #include <linux/iio/buffer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> #include "inv_icm42600_buffer.h"
>
> /* FIFO header: 1 byte */
> @@ -356,6 +357,7 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> struct device *dev = regmap_get_device(st->map);
> unsigned int sensor;
> unsigned int *watermark;
> + struct inv_icm42600_timestamp *ts;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int sleep_temp = 0;
> unsigned int sleep_sensor = 0;
> @@ -365,9 +367,11 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> if (indio_dev == st->indio_gyro) {
> sensor = INV_ICM42600_SENSOR_GYRO;
> watermark = &st->fifo.watermark.gyro;
> + ts = iio_priv(st->indio_gyro);
> } else if (indio_dev == st->indio_accel) {
> sensor = INV_ICM42600_SENSOR_ACCEL;
> watermark = &st->fifo.watermark.accel;
> + ts = iio_priv(st->indio_accel);
> } else {
> return -EINVAL;
> }
> @@ -395,6 +399,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> if (!st->fifo.on)
> ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
>
> + inv_icm42600_timestamp_reset(ts);
> +
> out_unlock:
> mutex_unlock(&st->lock);
>
> @@ -480,17 +486,26 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
>
> int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
> {
> + struct inv_icm42600_timestamp *ts;
> int ret;
>
> if (st->fifo.nb.total == 0)
> return 0;
>
> + /* handle gyroscope timestamp and FIFO data parsing */
> + ts = iio_priv(st->indio_gyro);
> + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.gyro, st->timestamp.gyro);
> if (st->fifo.nb.gyro > 0) {
> ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> if (ret)
> return ret;
> }
>
> + /* handle accelerometer timestamp and FIFO data parsing */
> + ts = iio_priv(st->indio_accel);
> + inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.accel, st->timestamp.accel);
> if (st->fifo.nb.accel > 0) {
> ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> if (ret)
> @@ -503,8 +518,13 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
> int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> unsigned int count)
> {
> + struct inv_icm42600_timestamp *ts;
> + int64_t gyro_ts, accel_ts;
> int ret;
>
> + gyro_ts = iio_get_time_ns(st->indio_gyro);
> + accel_ts = iio_get_time_ns(st->indio_accel);
> +
> ret = inv_icm42600_buffer_fifo_read(st, count);
> if (ret)
> return ret;
> @@ -513,12 +533,20 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> return 0;
>
> if (st->fifo.nb.gyro > 0) {
> + ts = iio_priv(st->indio_gyro);
> + inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
> + st->fifo.nb.total, st->fifo.nb.gyro,
> + gyro_ts);
> ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> if (ret)
> return ret;
> }
>
> if (st->fifo.nb.accel > 0) {
> + ts = iio_priv(st->indio_accel);
> + inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
> + st->fifo.nb.total, st->fifo.nb.accel,
> + accel_ts);
> ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> if (ret)
> return ret;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 6f1c1eb83953..c0d676219fc7 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -19,6 +19,7 @@
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
> {
> @@ -413,6 +414,16 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st,
> return inv_icm42600_set_conf(st, hw->conf);
> }
>
> +static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data)
> +{
> + struct inv_icm42600_state *st = _data;
> +
> + st->timestamp.gyro = iio_get_time_ns(st->indio_gyro);
> + st->timestamp.accel = iio_get_time_ns(st->indio_accel);
> +
> + return IRQ_WAKE_THREAD;
> +}
> +
> static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
> {
> struct inv_icm42600_state *st = _data;
> @@ -493,7 +504,7 @@ static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq,
> if (ret)
> return ret;
>
> - return devm_request_threaded_irq(dev, irq, NULL,
> + return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp,
> inv_icm42600_irq_handler, irq_type,
> "inv_icm42600", st);
> }
> @@ -613,6 +624,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + ret = inv_icm42600_timestamp_setup(st);
> + if (ret)
> + return ret;
> +
> ret = inv_icm42600_buffer_init(st);
> if (ret)
> 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 b05c33876b8d..76d92a550278 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -17,6 +17,7 @@
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -50,6 +51,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[] = {
> @@ -65,13 +67,15 @@ 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: 8 bytes */
> +/* IIO buffer data: 16 bytes */
> struct inv_icm42600_gyro_buffer {
> struct inv_icm42600_fifo_sensor_data gyro;
> int8_t temp;
> uint8_t padding;
> + int64_t timestamp;
> };
>
> #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
> @@ -92,6 +96,7 @@ 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_timestamp *ts = iio_priv(indio_dev);
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int fifo_en = 0;
> unsigned int sleep_gyro = 0;
> @@ -119,6 +124,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write */
> + inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -311,9 +317,11 @@ static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
> return IIO_VAL_INT_PLUS_MICRO;
> }
>
> -static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> +static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
> int val, int val2)
> {
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
> struct device *dev = regmap_get_device(st->map);
> unsigned int idx;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> @@ -332,6 +340,11 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> pm_runtime_get_sync(dev);
> mutex_lock(&st->lock);
>
> + ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
> + iio_buffer_enabled(indio_dev));
> + if (ret)
> + goto out_unlock;
> +
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> if (ret)
> goto out_unlock;
> @@ -620,7 +633,7 @@ static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
> iio_device_release_direct_mode(indio_dev);
> return ret;
> case IIO_CHAN_INFO_SAMP_FREQ:
> - return inv_icm42600_gyro_write_odr(st, val, val2);
> + return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
> case IIO_CHAN_INFO_CALIBBIAS:
> ret = iio_device_claim_direct_mode(indio_dev);
> if (ret)
> @@ -703,6 +716,8 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> {
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> + struct inv_icm42600_timestamp *ts;
> + uint32_t period;
> struct iio_dev *indio_dev;
> struct iio_buffer *buffer;
>
> @@ -710,7 +725,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!name)
> return -ENOMEM;
>
> - indio_dev = devm_iio_device_alloc(dev, 0);
> + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
> if (!indio_dev)
> return -ENOMEM;
>
> @@ -718,6 +733,10 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!buffer)
> return -ENOMEM;
>
> + ts = iio_priv(indio_dev);
> + period = inv_icm42600_odr_to_period(st->conf.gyro.odr);
> + inv_icm42600_timestamp_init(ts, period);
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->dev.parent = dev;
> indio_dev->name = name;
> @@ -737,16 +756,19 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
> {
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *timestamp;
> const int8_t *temp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_gyro_buffer buffer = {
> .padding = 0,
> };
>
> /* parse all fifo packets */
> - for (i = 0; i < st->fifo.count; i += size) {
> + for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
> size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> &accel, &gyro, &temp, ×tamp, &odr);
> /* quit if error or FIFO is empty */
> @@ -757,10 +779,16 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
> if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
> continue;
>
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_GYRO)
> + inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
> + st->fifo.nb.total, no);
> +
> /* fill and push data buffer */
> memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> buffer.temp = temp ? *temp : 0;
> - iio_push_to_buffers(indio_dev, &buffer);
> + ts_val = inv_icm42600_timestamp_pop(ts);
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
> }
>
> return 0;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> new file mode 100644
> index 000000000000..7f2dc41f807b
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> @@ -0,0 +1,195 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/kernel.h>
> +#include <linux/regmap.h>
> +#include <linux/math64.h>
> +
> +#include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> +
> +/* internal chip period is 32kHz, 31250ns */
> +#define INV_ICM42600_TIMESTAMP_PERIOD 31250
> +/* allow a jitter of +/- 2% */
> +#define INV_ICM42600_TIMESTAMP_JITTER 2
> +/* compute min and max periods accepted */
> +#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \
> + (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
> +#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
> + (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
> +
> +/* Add a new value inside an accumulator and update the estimate value */
> +static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
> +{
> + uint64_t sum = 0;
> + size_t i;
> +
> + acc->values[acc->idx++] = val;
> + if (acc->idx >= ARRAY_SIZE(acc->values))
> + acc->idx = 0;
> +
> + /* compute the mean of all stored values, use 0 as empty slot */
> + for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
> + if (acc->values[i] == 0)
> + break;
> + sum += acc->values[i];
> + }
> +
> + acc->val = div_u64(sum, i);
> +}
> +
> +void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
> + uint32_t period)
> +{
> + /* initial odr for sensor after reset is 1kHz */
> + const uint32_t default_period = 1000000;
> +
> + /* current multiplier and period values after reset */
> + ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->period = default_period;
> + /* new set multiplier is the one from chip initialization */
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> +
> + /* use theoretical value for chip period */
> + inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
> +}
> +
> +int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
> +{
> + unsigned int val;
> +
> + /* enable timestamp register */
> + val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
> + INV_ICM42600_TMST_CONFIG_TMST_EN;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
> + INV_ICM42600_TMST_CONFIG_MASK, val);
> +}
> +
> +int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + uint32_t period, bool fifo)
> +{
> + /* when FIFO is on, prevent odr change if one is already pending */
> + if (fifo && ts->new_mult != 0)
> + return -EAGAIN;
> +
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> +
> + return 0;
> +}
> +
> +static bool inv_validate_period(uint32_t period, uint32_t mult)
> +{
> + const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
> + uint32_t period_min, period_max;
> +
> + /* check that period is acceptable */
> + period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
> + period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
> + if (period > period_min && period < period_max)
> + return true;
> + else
> + return false;
> +}
> +
> +static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
> + uint32_t mult, uint32_t period)
> +{
> + uint32_t new_chip_period;
> +
> + if (!inv_validate_period(period, mult))
> + return false;
> +
> + /* update chip internal period estimation */
> + new_chip_period = period / mult;
> + inv_update_acc(&ts->chip_period, new_chip_period);
> +
> + return true;
> +}
> +
> +void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> + uint32_t fifo_period, size_t fifo_nb,
> + size_t sensor_nb, int64_t timestamp)
> +{
> + struct inv_icm42600_timestamp_interval *it;
> + int64_t delta, interval;
> + const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + uint32_t period = ts->period;
> + int32_t m;
> + bool valid = false;
> +
> + if (fifo_nb == 0)
> + return;
> +
> + /* update interrupt timestamp and compute chip and sensor periods */
> + it = &ts->it;
> + it->lo = it->up;
> + it->up = timestamp;
> + delta = it->up - it->lo;
> + if (it->lo != 0) {
> + /* compute period: delta time divided by number of samples */
> + period = div_s64(delta, fifo_nb);
> + valid = inv_compute_chip_period(ts, fifo_mult, period);
> + /* update sensor period if chip internal period is updated */
> + if (valid)
> + ts->period = ts->mult * ts->chip_period.val;
> + }
> +
> + /* no previous data, compute theoritical value from interrupt */
> + if (ts->timestamp == 0) {
> + /* elapsed time: sensor period * sensor samples number */
> + interval = (int64_t)ts->period * (int64_t)sensor_nb;
> + ts->timestamp = it->up - interval;
> + return;
> + }
> +
> + /* if interrupt interval is valid, sync with interrupt timestamp */
> + if (valid) {
> + /* compute measured fifo_period */
> + fifo_period = fifo_mult * ts->chip_period.val;
> + /* delta time between last sample and last interrupt */
> + delta = it->lo - ts->timestamp;
> + /* if there are multiple samples, go back to first one */
> + while (delta >= (fifo_period * 3 / 2))
> + delta -= fifo_period;
> + /* compute maximal adjustment value */
> + m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
> + if (delta > m)
> + delta = m;
> + else if (delta < -m)
> + delta = -m;
> + ts->timestamp += delta;
> + }
> +}
> +
> +void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
> + uint32_t fifo_period, size_t fifo_nb,
> + unsigned int fifo_no)
> +{
> + int64_t interval;
> + uint32_t fifo_mult;
> +
> + if (ts->new_mult == 0)
> + return;
> +
> + /* update to new multiplier and update period */
> + ts->mult = ts->new_mult;
> + ts->new_mult = 0;
> + ts->period = ts->mult * ts->chip_period.val;
> +
> + /*
> + * After ODR change the time interval with the previous sample is
> + * undertermined (depends when the change occures). So we compute the
> + * timestamp from the current interrupt using the new FIFO period, the
> + * total number of samples and the current sample numero.
> + */
> + if (ts->timestamp != 0) {
> + /* compute measured fifo period */
> + fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + fifo_period = fifo_mult * ts->chip_period.val;
> + /* computes time interval between interrupt and this sample */
> + interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
> + ts->timestamp = ts->it.up - interval;
> + }
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> new file mode 100644
> index 000000000000..4e4f331d4fe4
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> @@ -0,0 +1,85 @@
> +/* SPDX-License-Identifier: GPL-2.0-or-later */
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#ifndef INV_ICM42600_TIMESTAMP_H_
> +#define INV_ICM42600_TIMESTAMP_H_
> +
> +#include <linux/kernel.h>
> +
> +struct inv_icm42600_state;
> +
> +/**
> + * struct inv_icm42600_timestamp_interval - timestamps interval
> + * @lo: interval lower bound
> + * @up: interval upper bound
> + */
> +struct inv_icm42600_timestamp_interval {
> + int64_t lo;
> + int64_t up;
> +};
> +
> +/**
> + * struct inv_icm42600_timestamp_acc - accumulator for computing an estimation
> + * @val: current estimation of the value, the mean of all values
> + * @idx: current index of the next free place in values table
> + * @values: table of all measured values, use for computing the mean
> + */
> +struct inv_icm42600_timestamp_acc {
> + uint32_t val;
> + size_t idx;
> + uint32_t values[32];
> +};
> +
> +/**
> + * struct inv_icm42600_timestamp - timestamp management states
> + * @it: interrupts interval timestamps
> + * @timestamp: store last timestamp for computing next data timestamp
> + * @mult: current internal period multiplier
> + * @new_mult: new set internal period multiplier (not yet effective)
> + * @period: measured current period of the sensor
> + * @chip_period: accumulator for computing internal chip period
> + */
> +struct inv_icm42600_timestamp {
> + struct inv_icm42600_timestamp_interval it;
> + int64_t timestamp;
> + uint32_t mult;
> + uint32_t new_mult;
> + uint32_t period;
> + struct inv_icm42600_timestamp_acc chip_period;
> +};
> +
> +void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
> + uint32_t period);
> +
> +int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st);
> +
> +int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + uint32_t period, bool fifo);
> +
> +void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
> + uint32_t fifo_period, size_t fifo_nb,
> + size_t sensor_nb, int64_t timestamp);
> +
> +static inline int64_t
> +inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts)
> +{
> + ts->timestamp += ts->period;
> + return ts->timestamp;
> +}
> +
> +void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
> + uint32_t fifo_period, size_t fifo_nb,
> + unsigned int fifo_no);
> +
> +static inline void
> +inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts)
> +{
> + const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL};
> +
> + ts->it = interval_init;
> + ts->timestamp = 0;
> +}
> +
> +#endif
Hi Jonathan,
I agree that this multiplexed watermark computation value is anything except simple and clear to understand.
I will add more documentation about it. And it also triggered a kbuild robot issue, because it is using 64 bits modulo without using math64 macros.
For buffer preenable/postenable/..., the sequence I am using currently is:
- preenable -> turn chip on (pm_runtime_get)
- update_scan_mode -> set FIFO en bits configuration (which sensor data is going into the fifo)
- hwfifo_watermark -> compute and set watermark value
- postenable -> turn FIFO on (and multiplexed with a FIFO on counter since used by accel & gyro)
- predisable -> turn FIFO off (multiplexed with counter)
- postdisable -> turn chip off (pm_runtime_put)
This setting is working well. Good to note that if there is an error when enabling the buffer, postdisable will always get called after preenable. So it ensures pm_runtime reference counter to be always OK.
Another way would be to only store configuration in internal state with update_scan_mode and hwfifo_watermark, and do everything in postenable/predisable. This is a possibility, but makes things a little more complex.
For hwfifo flush, this is an interesting feature when there is a need to have data immediately. Or when there is a need to do a clean change of configuration. In Android systems, Android framework is mainly using FIFO flush to change the sensor configuration (ODR, watermark) in a clean way. For our case with the FIFO interleaved this is a not an issue. If there are samples from the 2 sensors, it means the 2 buffers are enabled. And if data is coming to the iio buffer sooner than expected, that should not be a problem. The limitation I see when the 2 sensors are runnings, is that we will return less data than should have been possible. I limit FIFO reading to the provided n bytes, so we could read less than n samples of 1 sensor.
Something I have in mind, that would be really interesting to be able to set/change watermark value when the buffer is enabled. Otherwise, we are always loosing events by turning sensor off when we want to change the value. Is there any limitation to work this way, or should it be possible to implement this feature in the future ?
Thanks,
JB
From: Jonathan Cameron <[email protected]>
Sent: Sunday, May 31, 2020 14:56
To: Jean-Baptiste Maneyrol <[email protected]>
Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
Subject: Re: [PATCH v2 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
?
?CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
On Wed, 27 May 2020 20:57:08 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Add all FIFO parsing and reading functions. Add accel and gyro
> kfifo buffer and FIFO data parsing. Use device interrupt for
> reading data FIFO and launching accel and gyro parsing.
>
> Support hwfifo watermark by multiplexing gyro and accel settings.
> Support hwfifo flush.
Both of these are complex given the interactions of the two sensors
types and to be honest I couldn't figure out exactly what the intent was.
Needs more docs!
Thanks,
Jonathan
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> ---
>? drivers/iio/imu/inv_icm42600/Kconfig????????? |?? 1 +
>? drivers/iio/imu/inv_icm42600/Makefile???????? |?? 1 +
>? drivers/iio/imu/inv_icm42600/inv_icm42600.h?? |?? 8 +
>? .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 160 ++++-
>? .../imu/inv_icm42600/inv_icm42600_buffer.c??? | 555 ++++++++++++++++++
>? .../imu/inv_icm42600/inv_icm42600_buffer.h??? |? 98 ++++
>? .../iio/imu/inv_icm42600/inv_icm42600_core.c? |? 30 +
>? .../iio/imu/inv_icm42600/inv_icm42600_gyro.c? | 160 ++++-
>? 8 files changed, 1011 insertions(+), 2 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 22390a72f0a3..50cbcfcb6cf1 100644
> --- a/drivers/iio/imu/inv_icm42600/Kconfig
> +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> @@ -2,6 +2,7 @@
>?
>? config INV_ICM42600
>??????? tristate
> +???? select IIO_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 48965824f00c..0f49f6df3647 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -5,6 +5,7 @@ inv-icm42600-y += inv_icm42600_core.o
>? 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_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 43749f56426c..4d5811562a61 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -14,6 +14,8 @@
>? #include <linux/pm.h>
>? #include <linux/iio/iio.h>
>?
> +#include "inv_icm42600_buffer.h"
> +
>? enum inv_icm42600_chip {
>??????? INV_CHIP_ICM42600,
>??????? INV_CHIP_ICM42602,
> @@ -123,6 +125,7 @@ struct inv_icm42600_suspended {
>?? *? @indio_gyro:???? gyroscope IIO device.
>?? *? @indio_accel:??? accelerometer IIO device.
>?? *? @buffer:???????? data transfer buffer aligned for DMA.
> + *? @fifo:?????????? FIFO management structure.
>?? */
>? struct inv_icm42600_state {
>??????? struct mutex lock;
> @@ -137,6 +140,7 @@ struct inv_icm42600_state {
>??????? struct iio_dev *indio_gyro;
>??????? struct iio_dev *indio_accel;
>??????? uint8_t buffer[2] ____cacheline_aligned;
> +???? struct inv_icm42600_fifo fifo;
>? };
>?
>? /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -377,6 +381,10 @@ 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);
> +
>? int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>?
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
> +
>? #endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 6a615d7ffb24..c73ce204efc6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -11,9 +11,12 @@
>? #include <linux/delay.h>
>? #include <linux/math64.h>
>? #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>?
>? #include "inv_icm42600.h"
>? #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>?
>? #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info)??????????????? \
>??????? {?????????????????????????????????????????????????????????????? \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
>??????? INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
>? };
>?
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_accel_buffer {
> +???? struct inv_icm42600_fifo_sensor_data accel;
> +???? int8_t temp;
> +???? uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> +???? 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)
> @@ -248,7 +321,12 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
>??????? mutex_lock(&st->lock);
>?
>??????? ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> +???? if (ret)
> +???????????? goto out_unlock;
> +???? inv_icm42600_buffer_update_fifo_period(st);
> +???? inv_icm42600_buffer_update_watermark(st);
>?
> +out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? pm_runtime_mark_last_busy(dev);
>??????? pm_runtime_put_autosuspend(dev);
> @@ -563,12 +641,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)
> @@ -576,6 +693,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;
> +???? struct iio_buffer *buffer;
>?
>??????? name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
>??????? if (!name)
> @@ -585,14 +703,54 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
>??????? if (!indio_dev)
>??????????????? return -ENOMEM;
>?
> +???? buffer = devm_iio_kfifo_allocate(dev);
> +???? if (!buffer)
> +???????????? return -ENOMEM;
> +
>??????? iio_device_set_drvdata(indio_dev, st);
>??????? indio_dev->dev.parent = dev;
>??????? indio_dev->name = name;
>??????? indio_dev->info = &inv_icm42600_accel_info;
> -???? indio_dev->modes = INDIO_DIRECT_MODE;
> +???? indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
>??????? 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;
> +???? indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> +???? iio_device_attach_buffer(indio_dev, buffer);
>?
>??????? 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)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? unsigned int odr;
> +???? struct inv_icm42600_accel_buffer buffer = {
> +???????????? .padding = 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);
> +???????????? /* 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))
> +???????????????????? continue;
> +
> +???????????? /* fill and push data buffer */
> +???????????? memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> +???????????? buffer.temp = temp ? *temp : 0;
> +???????????? iio_push_to_buffers(indio_dev, &buffer);
> +???? }
> +
> +???? 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..c91075f62231
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,555 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/kernel.h>
> +#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 "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
> +
> +/* 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)
> +
> +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
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> +???????????????????????????????????? const void **gyro, const int8_t **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;
> +}
> +
> +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;
> +}
> +
I think some overview docs on how this is working would be good.
Set out the aims for the watermark selected and how it is achieved.
> +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;
> +???? 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;
> +
> +???? /* changing FIFO watermark requires to turn off watermark interrupt */
> +???? ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +??????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +??????????????????????????????????? 0, &restore);
> +???? if (ret)
> +???????????? return ret;
> +
> +???? raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> +???? memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
> +???? ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> +???????????????????????????? st->buffer, sizeof(raw_wm));
> +???? if (ret)
> +???????????? return ret;
> +
> +???? /* restore watermark interrupt */
> +???? if (restore) {
> +???????????? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +????????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> +???????????? 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;
> +}
> +
> +/*
> + * update_scan_mode callback is turning sensors on and setting data FIFO enable
> + * bits.
> + */
> +static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? int ret;
> +
> +???? mutex_lock(&st->lock);
> +
> +???? /* exit if FIFO is already on */
> +???? if (st->fifo.on) {
> +???????????? ret = 0;
> +???????????? goto out_on;
> +???? }
> +
> +???? /* set FIFO threshold interrupt */
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* flush FIFO data */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> +??????????????????????? INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* set FIFO in streaming mode */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> +??????????????????????? INV_ICM42600_FIFO_CONFIG_STREAM);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* workaround: first read of FIFO count after reset is always 0 */
> +???? ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +out_on:
> +???? /* increase FIFO on counter */
> +???? st->fifo.on++;
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +???? return ret;
> +}
> +
> +static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? int ret;
> +
> +???? mutex_lock(&st->lock);
> +
> +???? /* exit if there are several sensors using the FIFO */
> +???? if (st->fifo.on > 1) {
> +???????????? ret = 0;
> +???????????? goto out_off;
> +???? }
> +
> +???? /* 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;
> +
> +???? /* flush FIFO data */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> +??????????????????????? INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* disable FIFO threshold interrupt */
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +out_off:
> +???? /* decrease FIFO on counter */
> +???? st->fifo.on--;
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +???? return ret;
> +}
> +
> +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_temp = 0;
> +???? unsigned int sleep_sensor = 0;
> +???? unsigned int sleep;
> +???? 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_sensor);
> +???? else
> +???????????? ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* if FIFO is off, turn temperature off */
> +???? if (!st->fifo.on)
> +???????????? ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
> +
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +
> +???? /* sleep maximum required time */
> +???? if (sleep_sensor > sleep_temp)
> +???????????? sleep = sleep_sensor;
> +???? else
> +???????????? sleep = sleep_temp;
> +???? 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 = inv_icm42600_buffer_postenable,
We've been slowly eroding the difference between preenable and posteenable.
Would be good to understand why you need to define both?
> +???? .predisable = inv_icm42600_buffer_predisable,
> +???? .postdisable = inv_icm42600_buffer_postdisable,
> +};
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> +?????????????????????????????? unsigned int max)
> +{
> +???? size_t max_count;
> +???? __be16 *raw_fifo_count;
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? 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 */
> +???? raw_fifo_count = (__be16 *)st->buffer;
> +???? 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_cpup(raw_fifo_count);
> +
> +???? /* check and clamp 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_fifo_parse(struct inv_icm42600_state *st)
> +{
> +???? int ret;
> +
> +???? if (st->fifo.nb.total == 0)
> +???????????? return 0;
> +
> +???? if (st->fifo.nb.gyro > 0) {
> +???????????? ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? if (st->fifo.nb.accel > 0) {
> +???????????? ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? return 0;
> +}
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> +????????????????????????????????? unsigned int count)
> +{
> +???? int ret;
> +
> +???? ret = inv_icm42600_buffer_fifo_read(st, count);
> +???? if (ret)
> +???????????? return ret;
Definitely searching my memory for how this works in the core, so
I may have it wrong.
This is a bit unusual (I think).? The intent of the flush
is to read up to 'n' bytes because someone just did a read on the buffer
or select, and there was data in the hwfifo capable of satisfying the read
even though we haven't yet reached the watermark.
Given both sensor types are coming from one buffer, do we have a potential
issue here or under serving even though data is available?
The case I worry may be served late is when an poll / select
is waiting for sufficient data.
So what should we be doing?? We want to guarantee to provide data
for each sensor type if it's in the hwfifo. As such we could keep reading
until we have enough, but that could cause some issues if the two data rates
are very different (overflow on the other kfifo)
Maybe what you have here is the best we can do.
I'm assuming the watermark level has a similar problem.? One value represents
the sum of the two types of data.
> +
> +???? if (st->fifo.nb.total == 0)
> +???????????? return 0;
> +
> +???? if (st->fifo.nb.gyro > 0) {
> +???????????? ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? if (st->fifo.nb.accel > 0) {
> +???????????? ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? return 0;
> +}
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> +{
> +???? unsigned int val;
> +???? int ret;
> +
> +???? /*
> +????? * Default FIFO configuration (bits 7 to 5)
> +????? * - use invalid value
> +????? * - FIFO count in bytes
> +????? * - FIFO count in big endian
> +????? */
> +???? val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> +????????????????????????????? GENMASK(7, 5), val);
> +???? if (ret)
> +???????????? return ret;
> +
> +???? /*
> +????? * Enable FIFO partial read and continuous watermark interrupt.
> +????? * Disable all FIFO EN bits.
> +????? */
> +???? 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,
> +?????????????????????????????? GENMASK(6, 5) | GENMASK(3, 0), 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..de2a3949dcc7
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> @@ -0,0 +1,98 @@
> +/* 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 - FIFO state variables
> + * @on:????????????? reference counter for FIFO on.
> + * @en:????????????? bits field of INV_ICM42600_SENSOR_* for FIFO EN bits.
> + * @period:? FIFO internal period.
> + * @watermark:?????? watermark configuration values for accel and gyro.
> + * @count:?? number of bytes in the FIFO data buffer.
> + * @nb:????????????? gyro, accel and total samples in the FIFO data buffer.
> + * @data:??? FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache).
> + */
> +struct inv_icm42600_fifo {
> +???? unsigned int on;
> +???? 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] ____cacheline_aligned;
> +};
> +
> +/* FIFO data packet */
> +struct inv_icm42600_fifo_sensor_data {
> +???? __be16 x;
> +???? __be16 y;
> +???? __be16 z;
> +} __packed;
Why packed?? Should be anyway I think.
> +#define INV_ICM42600_FIFO_DATA_INVALID?????????????? -32768
> +
> +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;
> +}
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> +???????????????????????????????????? const void **gyro, const int8_t **temp,
> +???????????????????????????????????? const void **timestamp, unsigned int *odr);
> +
> +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_fifo_parse(struct inv_icm42600_state *st);
> +
> +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 246c1eb52231..6f1c1eb83953 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -18,6 +18,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[] = {
>??????? {
> @@ -429,6 +430,18 @@ static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
>??????? if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
>??????????????? dev_warn(dev, "FIFO full data lost!\n");
>?
> +???? /* FIFO threshold reached */
> +???? 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);
> +???????????????????? goto out_unlock;
> +???????????? }
> +???????????? ret = inv_icm42600_buffer_fifo_parse(st);
> +???????????? if (ret)
> +???????????????????? dev_err(dev, "FIFO parsing error %d\n", ret);
> +???? }
> +
>? out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? return IRQ_HANDLED;
> @@ -600,6 +613,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>??????? if (ret)
>??????????????? return ret;
>?
> +???? ret = inv_icm42600_buffer_init(st);
> +???? if (ret)
> +???????????? return ret;
> +
>??????? ret = inv_icm42600_gyro_init(st);
>??????? if (ret)
>??????????????? return ret;
> @@ -645,6 +662,14 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
>??????????????? goto out_unlock;
>??????? }
>?
> +???? /* disable FIFO data streaming */
> +???? if (st->fifo.on) {
> +???????????? 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);
> @@ -684,6 +709,11 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
>??????? if (ret)
>??????????????? goto out_unlock;
>?
> +???? /* restore FIFO data streaming */
> +???? if (st->fifo.on)
> +???????????? 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 38654e0d217b..b05c33876b8d 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -11,9 +11,12 @@
>? #include <linux/delay.h>
>? #include <linux/math64.h>
>? #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>?
>? #include "inv_icm42600.h"
>? #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>?
>? #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info)???????? \
>??????? {?????????????????????????????????????????????????????????????? \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
>??????? INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
>? };
>?
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_gyro_buffer {
> +???? struct inv_icm42600_fifo_sensor_data gyro;
> +???? int8_t temp;
> +???? uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> +???? 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)
> @@ -260,7 +333,12 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
>??????? mutex_lock(&st->lock);
>?
>??????? ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> +???? if (ret)
> +???????????? goto out_unlock;
> +???? inv_icm42600_buffer_update_fifo_period(st);
> +???? inv_icm42600_buffer_update_watermark(st);
>?
> +out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? pm_runtime_mark_last_busy(dev);
>??????? pm_runtime_put_autosuspend(dev);
> @@ -574,12 +652,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)
> +{
Nothing to do with this patch, but I realised reading this that we have
some 'unusual' use of the word flush here.? It's a straight forward
read function so not sure why we called it flush.
> +???? 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)
> @@ -587,6 +704,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;
> +???? struct iio_buffer *buffer;
>?
>??????? name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
>??????? if (!name)
> @@ -596,14 +714,54 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>??????? if (!indio_dev)
>??????????????? return -ENOMEM;
>?
> +???? buffer = devm_iio_kfifo_allocate(dev);
> +???? if (!buffer)
> +???????????? return -ENOMEM;
> +
>??????? iio_device_set_drvdata(indio_dev, st);
>??????? indio_dev->dev.parent = dev;
>??????? indio_dev->name = name;
>??????? indio_dev->info = &inv_icm42600_gyro_info;
> -???? indio_dev->modes = INDIO_DIRECT_MODE;
> +???? indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
>??????? 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;
> +???? indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> +???? iio_device_attach_buffer(indio_dev, buffer);
>?
>??????? 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)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? unsigned int odr;
> +???? struct inv_icm42600_gyro_buffer buffer = {
> +???????????? .padding = 0,
Might be worth a comment here or where the structure is defined
on why we make padding explicit.
> +???? };
> +
> +???? /* 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);
> +???????????? /* 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))
> +???????????????????? continue;
> +
> +???????????? /* fill and push data buffer */
> +???????????? memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> +???????????? buffer.temp = temp ? *temp : 0;
> +???????????? iio_push_to_buffers(indio_dev, &buffer);
> +???? }
> +
> +???? return 0;
> +}
Hi Jonathan,
something I forgot, about the __packed attribute for the sensor data structure struct inv_icm42600_fifo_sensor_data located inside inv_icm42600_buffer.h.
I added it because this structure is used for decoding the FIFO data frame which can be unaligned. It is also used for storing data in other modules, but __packed attribute should not change anything here.
Thanks,
JB
From: [email protected] <[email protected]> on behalf of Jean-Baptiste Maneyrol <[email protected]>
Sent: Tuesday, June 2, 2020 14:57
To: Jonathan Cameron <[email protected]>
Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
Subject: --[[SPOOF or PHISHING]]--Re: [PATCH v2 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
?
?CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
Hi Jonathan,
I agree that this multiplexed watermark computation value is anything except simple and clear to understand.
I will add more documentation about it. And it also triggered a kbuild robot issue, because it is using 64 bits modulo without using math64 macros.
For buffer preenable/postenable/..., the sequence I am using currently is:
- preenable -> turn chip on (pm_runtime_get)
- update_scan_mode -> set FIFO en bits configuration (which sensor data is going into the fifo)
- hwfifo_watermark -> compute and set watermark value
- postenable -> turn FIFO on (and multiplexed with a FIFO on counter since used by accel & gyro)
- predisable -> turn FIFO off (multiplexed with counter)
- postdisable -> turn chip off (pm_runtime_put)
This setting is working well. Good to note that if there is an error when enabling the buffer, postdisable will always get called after preenable. So it ensures pm_runtime reference counter to be always OK.
Another way would be to only store configuration in internal state with update_scan_mode and hwfifo_watermark, and do everything in postenable/predisable. This is a possibility, but makes things a little more complex.
For hwfifo flush, this is an interesting feature when there is a need to have data immediately. Or when there is a need to do a clean change of configuration. In Android systems, Android framework is mainly using FIFO flush to change the sensor configuration (ODR, watermark) in a clean way. For our case with the FIFO interleaved this is a not an issue. If there are samples from the 2 sensors, it means the 2 buffers are enabled. And if data is coming to the iio buffer sooner than expected, that should not be a problem. The limitation I see when the 2 sensors are runnings, is that we will return less data than should have been possible. I limit FIFO reading to the provided n bytes, so we could read less than n samples of 1 sensor.
Something I have in mind, that would be really interesting to be able to set/change watermark value when the buffer is enabled. Otherwise, we are always loosing events by turning sensor off when we want to change the value. Is there any limitation to work this way, or should it be possible to implement this feature in the future ?
Thanks,
JB
From: Jonathan Cameron <[email protected]>
Sent: Sunday, May 31, 2020 14:56
To: Jean-Baptiste Maneyrol <[email protected]>
Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
Subject: Re: [PATCH v2 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
?
?CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
On Wed, 27 May 2020 20:57:08 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Add all FIFO parsing and reading functions. Add accel and gyro
> kfifo buffer and FIFO data parsing. Use device interrupt for
> reading data FIFO and launching accel and gyro parsing.
>
> Support hwfifo watermark by multiplexing gyro and accel settings.
> Support hwfifo flush.
Both of these are complex given the interactions of the two sensors
types and to be honest I couldn't figure out exactly what the intent was.
Needs more docs!
Thanks,
Jonathan
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> ---
>? drivers/iio/imu/inv_icm42600/Kconfig????????? |?? 1 +
>? drivers/iio/imu/inv_icm42600/Makefile???????? |?? 1 +
>? drivers/iio/imu/inv_icm42600/inv_icm42600.h?? |?? 8 +
>? .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 160 ++++-
>? .../imu/inv_icm42600/inv_icm42600_buffer.c??? | 555 ++++++++++++++++++
>? .../imu/inv_icm42600/inv_icm42600_buffer.h??? |? 98 ++++
>? .../iio/imu/inv_icm42600/inv_icm42600_core.c? |? 30 +
>? .../iio/imu/inv_icm42600/inv_icm42600_gyro.c? | 160 ++++-
>? 8 files changed, 1011 insertions(+), 2 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 22390a72f0a3..50cbcfcb6cf1 100644
> --- a/drivers/iio/imu/inv_icm42600/Kconfig
> +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> @@ -2,6 +2,7 @@
>?
>? config INV_ICM42600
>??????? tristate
> +???? select IIO_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 48965824f00c..0f49f6df3647 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -5,6 +5,7 @@ inv-icm42600-y += inv_icm42600_core.o
>? 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_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 43749f56426c..4d5811562a61 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -14,6 +14,8 @@
>? #include <linux/pm.h>
>? #include <linux/iio/iio.h>
>?
> +#include "inv_icm42600_buffer.h"
> +
>? enum inv_icm42600_chip {
>??????? INV_CHIP_ICM42600,
>??????? INV_CHIP_ICM42602,
> @@ -123,6 +125,7 @@ struct inv_icm42600_suspended {
>?? *? @indio_gyro:???? gyroscope IIO device.
>?? *? @indio_accel:??? accelerometer IIO device.
>?? *? @buffer:???????? data transfer buffer aligned for DMA.
> + *? @fifo:?????????? FIFO management structure.
>?? */
>? struct inv_icm42600_state {
>??????? struct mutex lock;
> @@ -137,6 +140,7 @@ struct inv_icm42600_state {
>??????? struct iio_dev *indio_gyro;
>??????? struct iio_dev *indio_accel;
>??????? uint8_t buffer[2] ____cacheline_aligned;
> +???? struct inv_icm42600_fifo fifo;
>? };
>?
>? /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -377,6 +381,10 @@ 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);
> +
>? int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>?
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
> +
>? #endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 6a615d7ffb24..c73ce204efc6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -11,9 +11,12 @@
>? #include <linux/delay.h>
>? #include <linux/math64.h>
>? #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>?
>? #include "inv_icm42600.h"
>? #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>?
>? #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info)??????????????? \
>??????? {?????????????????????????????????????????????????????????????? \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
>??????? INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
>? };
>?
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_accel_buffer {
> +???? struct inv_icm42600_fifo_sensor_data accel;
> +???? int8_t temp;
> +???? uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> +???? 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)
> @@ -248,7 +321,12 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
>??????? mutex_lock(&st->lock);
>?
>??????? ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> +???? if (ret)
> +???????????? goto out_unlock;
> +???? inv_icm42600_buffer_update_fifo_period(st);
> +???? inv_icm42600_buffer_update_watermark(st);
>?
> +out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? pm_runtime_mark_last_busy(dev);
>??????? pm_runtime_put_autosuspend(dev);
> @@ -563,12 +641,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)
> @@ -576,6 +693,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;
> +???? struct iio_buffer *buffer;
>?
>??????? name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
>??????? if (!name)
> @@ -585,14 +703,54 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
>??????? if (!indio_dev)
>??????????????? return -ENOMEM;
>?
> +???? buffer = devm_iio_kfifo_allocate(dev);
> +???? if (!buffer)
> +???????????? return -ENOMEM;
> +
>??????? iio_device_set_drvdata(indio_dev, st);
>??????? indio_dev->dev.parent = dev;
>??????? indio_dev->name = name;
>??????? indio_dev->info = &inv_icm42600_accel_info;
> -???? indio_dev->modes = INDIO_DIRECT_MODE;
> +???? indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
>??????? 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;
> +???? indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> +???? iio_device_attach_buffer(indio_dev, buffer);
>?
>??????? 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)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? unsigned int odr;
> +???? struct inv_icm42600_accel_buffer buffer = {
> +???????????? .padding = 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);
> +???????????? /* 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))
> +???????????????????? continue;
> +
> +???????????? /* fill and push data buffer */
> +???????????? memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> +???????????? buffer.temp = temp ? *temp : 0;
> +???????????? iio_push_to_buffers(indio_dev, &buffer);
> +???? }
> +
> +???? 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..c91075f62231
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,555 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/kernel.h>
> +#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 "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
> +
> +/* 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)
> +
> +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
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> +???????????????????????????????????? const void **gyro, const int8_t **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;
> +}
> +
> +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;
> +}
> +
I think some overview docs on how this is working would be good.
Set out the aims for the watermark selected and how it is achieved.
> +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;
> +???? 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;
> +
> +???? /* changing FIFO watermark requires to turn off watermark interrupt */
> +???? ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +??????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +??????????????????????????????????? 0, &restore);
> +???? if (ret)
> +???????????? return ret;
> +
> +???? raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> +???? memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
> +???? ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> +???????????????????????????? st->buffer, sizeof(raw_wm));
> +???? if (ret)
> +???????????? return ret;
> +
> +???? /* restore watermark interrupt */
> +???? if (restore) {
> +???????????? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +????????????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> +???????????? 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;
> +}
> +
> +/*
> + * update_scan_mode callback is turning sensors on and setting data FIFO enable
> + * bits.
> + */
> +static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? int ret;
> +
> +???? mutex_lock(&st->lock);
> +
> +???? /* exit if FIFO is already on */
> +???? if (st->fifo.on) {
> +???????????? ret = 0;
> +???????????? goto out_on;
> +???? }
> +
> +???? /* set FIFO threshold interrupt */
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* flush FIFO data */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> +??????????????????????? INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* set FIFO in streaming mode */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> +??????????????????????? INV_ICM42600_FIFO_CONFIG_STREAM);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* workaround: first read of FIFO count after reset is always 0 */
> +???? ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +out_on:
> +???? /* increase FIFO on counter */
> +???? st->fifo.on++;
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +???? return ret;
> +}
> +
> +static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? int ret;
> +
> +???? mutex_lock(&st->lock);
> +
> +???? /* exit if there are several sensors using the FIFO */
> +???? if (st->fifo.on > 1) {
> +???????????? ret = 0;
> +???????????? goto out_off;
> +???? }
> +
> +???? /* 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;
> +
> +???? /* flush FIFO data */
> +???? ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> +??????????????????????? INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* disable FIFO threshold interrupt */
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> +????????????????????????????? INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +out_off:
> +???? /* decrease FIFO on counter */
> +???? st->fifo.on--;
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +???? return ret;
> +}
> +
> +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_temp = 0;
> +???? unsigned int sleep_sensor = 0;
> +???? unsigned int sleep;
> +???? 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_sensor);
> +???? else
> +???????????? ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
> +???? if (ret)
> +???????????? goto out_unlock;
> +
> +???? /* if FIFO is off, turn temperature off */
> +???? if (!st->fifo.on)
> +???????????? ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
> +
> +out_unlock:
> +???? mutex_unlock(&st->lock);
> +
> +???? /* sleep maximum required time */
> +???? if (sleep_sensor > sleep_temp)
> +???????????? sleep = sleep_sensor;
> +???? else
> +???????????? sleep = sleep_temp;
> +???? 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 = inv_icm42600_buffer_postenable,
We've been slowly eroding the difference between preenable and posteenable.
Would be good to understand why you need to define both?
> +???? .predisable = inv_icm42600_buffer_predisable,
> +???? .postdisable = inv_icm42600_buffer_postdisable,
> +};
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> +?????????????????????????????? unsigned int max)
> +{
> +???? size_t max_count;
> +???? __be16 *raw_fifo_count;
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? 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 */
> +???? raw_fifo_count = (__be16 *)st->buffer;
> +???? 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_cpup(raw_fifo_count);
> +
> +???? /* check and clamp 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_fifo_parse(struct inv_icm42600_state *st)
> +{
> +???? int ret;
> +
> +???? if (st->fifo.nb.total == 0)
> +???????????? return 0;
> +
> +???? if (st->fifo.nb.gyro > 0) {
> +???????????? ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? if (st->fifo.nb.accel > 0) {
> +???????????? ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? return 0;
> +}
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> +????????????????????????????????? unsigned int count)
> +{
> +???? int ret;
> +
> +???? ret = inv_icm42600_buffer_fifo_read(st, count);
> +???? if (ret)
> +???????????? return ret;
Definitely searching my memory for how this works in the core, so
I may have it wrong.
This is a bit unusual (I think).? The intent of the flush
is to read up to 'n' bytes because someone just did a read on the buffer
or select, and there was data in the hwfifo capable of satisfying the read
even though we haven't yet reached the watermark.
Given both sensor types are coming from one buffer, do we have a potential
issue here or under serving even though data is available?
The case I worry may be served late is when an poll / select
is waiting for sufficient data.
So what should we be doing?? We want to guarantee to provide data
for each sensor type if it's in the hwfifo. As such we could keep reading
until we have enough, but that could cause some issues if the two data rates
are very different (overflow on the other kfifo)
Maybe what you have here is the best we can do.
I'm assuming the watermark level has a similar problem.? One value represents
the sum of the two types of data.
> +
> +???? if (st->fifo.nb.total == 0)
> +???????????? return 0;
> +
> +???? if (st->fifo.nb.gyro > 0) {
> +???????????? ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? if (st->fifo.nb.accel > 0) {
> +???????????? ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???? }
> +
> +???? return 0;
> +}
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> +{
> +???? unsigned int val;
> +???? int ret;
> +
> +???? /*
> +????? * Default FIFO configuration (bits 7 to 5)
> +????? * - use invalid value
> +????? * - FIFO count in bytes
> +????? * - FIFO count in big endian
> +????? */
> +???? val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> +???? ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> +????????????????????????????? GENMASK(7, 5), val);
> +???? if (ret)
> +???????????? return ret;
> +
> +???? /*
> +????? * Enable FIFO partial read and continuous watermark interrupt.
> +????? * Disable all FIFO EN bits.
> +????? */
> +???? 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,
> +?????????????????????????????? GENMASK(6, 5) | GENMASK(3, 0), 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..de2a3949dcc7
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> @@ -0,0 +1,98 @@
> +/* 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 - FIFO state variables
> + * @on:????????????? reference counter for FIFO on.
> + * @en:????????????? bits field of INV_ICM42600_SENSOR_* for FIFO EN bits.
> + * @period:? FIFO internal period.
> + * @watermark:?????? watermark configuration values for accel and gyro.
> + * @count:?? number of bytes in the FIFO data buffer.
> + * @nb:????????????? gyro, accel and total samples in the FIFO data buffer.
> + * @data:??? FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache).
> + */
> +struct inv_icm42600_fifo {
> +???? unsigned int on;
> +???? 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] ____cacheline_aligned;
> +};
> +
> +/* FIFO data packet */
> +struct inv_icm42600_fifo_sensor_data {
> +???? __be16 x;
> +???? __be16 y;
> +???? __be16 z;
> +} __packed;
Why packed?? Should be anyway I think.
> +#define INV_ICM42600_FIFO_DATA_INVALID?????????????? -32768
> +
> +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;
> +}
> +
> +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> +???????????????????????????????????? const void **gyro, const int8_t **temp,
> +???????????????????????????????????? const void **timestamp, unsigned int *odr);
> +
> +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_fifo_parse(struct inv_icm42600_state *st);
> +
> +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 246c1eb52231..6f1c1eb83953 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -18,6 +18,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[] = {
>??????? {
> @@ -429,6 +430,18 @@ static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
>??????? if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
>??????????????? dev_warn(dev, "FIFO full data lost!\n");
>?
> +???? /* FIFO threshold reached */
> +???? 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);
> +???????????????????? goto out_unlock;
> +???????????? }
> +???????????? ret = inv_icm42600_buffer_fifo_parse(st);
> +???????????? if (ret)
> +???????????????????? dev_err(dev, "FIFO parsing error %d\n", ret);
> +???? }
> +
>? out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? return IRQ_HANDLED;
> @@ -600,6 +613,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>??????? if (ret)
>??????????????? return ret;
>?
> +???? ret = inv_icm42600_buffer_init(st);
> +???? if (ret)
> +???????????? return ret;
> +
>??????? ret = inv_icm42600_gyro_init(st);
>??????? if (ret)
>??????????????? return ret;
> @@ -645,6 +662,14 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
>??????????????? goto out_unlock;
>??????? }
>?
> +???? /* disable FIFO data streaming */
> +???? if (st->fifo.on) {
> +???????????? 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);
> @@ -684,6 +709,11 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
>??????? if (ret)
>??????????????? goto out_unlock;
>?
> +???? /* restore FIFO data streaming */
> +???? if (st->fifo.on)
> +???????????? 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 38654e0d217b..b05c33876b8d 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -11,9 +11,12 @@
>? #include <linux/delay.h>
>? #include <linux/math64.h>
>? #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/kfifo_buf.h>
>?
>? #include "inv_icm42600.h"
>? #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>?
>? #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info)???????? \
>??????? {?????????????????????????????????????????????????????????????? \
> @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
>??????? INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
>? };
>?
> +/* IIO buffer data: 8 bytes */
> +struct inv_icm42600_gyro_buffer {
> +???? struct inv_icm42600_fifo_sensor_data gyro;
> +???? int8_t temp;
> +???? uint8_t padding;
> +};
> +
> +#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,
> +};
> +
> +/* 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 */
> +???? 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)
> @@ -260,7 +333,12 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
>??????? mutex_lock(&st->lock);
>?
>??????? ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> +???? if (ret)
> +???????????? goto out_unlock;
> +???? inv_icm42600_buffer_update_fifo_period(st);
> +???? inv_icm42600_buffer_update_watermark(st);
>?
> +out_unlock:
>??????? mutex_unlock(&st->lock);
>??????? pm_runtime_mark_last_busy(dev);
>??????? pm_runtime_put_autosuspend(dev);
> @@ -574,12 +652,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)
> +{
Nothing to do with this patch, but I realised reading this that we have
some 'unusual' use of the word flush here.? It's a straight forward
read function so not sure why we called it flush.
> +???? 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)
> @@ -587,6 +704,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;
> +???? struct iio_buffer *buffer;
>?
>??????? name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
>??????? if (!name)
> @@ -596,14 +714,54 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>??????? if (!indio_dev)
>??????????????? return -ENOMEM;
>?
> +???? buffer = devm_iio_kfifo_allocate(dev);
> +???? if (!buffer)
> +???????????? return -ENOMEM;
> +
>??????? iio_device_set_drvdata(indio_dev, st);
>??????? indio_dev->dev.parent = dev;
>??????? indio_dev->name = name;
>??????? indio_dev->info = &inv_icm42600_gyro_info;
> -???? indio_dev->modes = INDIO_DIRECT_MODE;
> +???? indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
>??????? 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;
> +???? indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> +
> +???? iio_device_attach_buffer(indio_dev, buffer);
>?
>??????? 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)
> +{
> +???? struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> +???? ssize_t i, size;
> +???? const void *accel, *gyro, *timestamp;
> +???? const int8_t *temp;
> +???? unsigned int odr;
> +???? struct inv_icm42600_gyro_buffer buffer = {
> +???????????? .padding = 0,
Might be worth a comment here or where the structure is defined
on why we make padding explicit.
> +???? };
> +
> +???? /* 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);
> +???????????? /* 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))
> +???????????????????? continue;
> +
> +???????????? /* fill and push data buffer */
> +???????????? memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> +???????????? buffer.temp = temp ? *temp : 0;
> +???????????? iio_push_to_buffers(indio_dev, &buffer);
> +???? }
> +
> +???? return 0;
> +}
On Tue, 2 Jun 2020 12:57:26 +0000
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Hi Jonathan,
>
> I agree that this multiplexed watermark computation value is anything except simple and clear to understand.
> I will add more documentation about it. And it also triggered a
> kbuild robot issue, because it is using 64 bits modulo without using
> math64 macros.
>
> For buffer preenable/postenable/..., the sequence I am using
> currently is:
> - preenable -> turn chip on (pm_runtime_get)
> - update_scan_mode -> set FIFO en bits configuration (which sensor
> data is going into the fifo)
> - hwfifo_watermark -> compute and set watermark value
> - postenable -> turn FIFO on (and multiplexed with a FIFO on counter
> since used by accel & gyro)
> - predisable -> turn FIFO off (multiplexed with counter)
> - postdisable -> turn chip off (pm_runtime_put)
>
> This setting is working well. Good to note that if there is an error
> when enabling the buffer, postdisable will always get called after
> preenable. So it ensures pm_runtime reference counter to be always OK.
>
> Another way would be to only store configuration in internal state
> with update_scan_mode and hwfifo_watermark, and do everything in
> postenable/predisable. This is a possibility, but makes things a
> little more complex.
What you have here is fine. Thanks for the explanation.
>
> For hwfifo flush, this is an interesting feature when there is a need
> to have data immediately. Or when there is a need to do a clean
> change of configuration. In Android systems, Android framework is
> mainly using FIFO flush to change the sensor configuration (ODR,
> watermark) in a clean way. For our case with the FIFO interleaved
> this is a not an issue. If there are samples from the 2 sensors, it
> means the 2 buffers are enabled. And if data is coming to the iio
> buffer sooner than expected, that should not be a problem. The
> limitation I see when the 2 sensors are runnings, is that we will
> return less data than should have been possible. I limit FIFO reading
> to the provided n bytes, so we could read less than n samples of 1
> sensor.
Yes. I'm a little nervous about unexpected impacts of this last
case. As you say early data rarely matters but late data can
potentially be an issue as software will assume it didn't get the
data because it didn't yet exist.
>
> Something I have in mind, that would be really interesting to be able
> to set/change watermark value when the buffer is enabled. Otherwise,
> we are always loosing events by turning sensor off when we want to
> change the value. Is there any limitation to work this way, or should
> it be possible to implement this feature in the future ?
I can't see why it would particularly matter in general if we did this
whilst 'live'. However, potentially we have sensors where reducing the
value could result in not getting an interrupt from a hw fifo.
(unwise design perhaps but we all know those exist :)
So care would be needed - perhaps some kind of opt in?
>
> Thanks,
> JB
Please remember to wrap our email if possible.
Thanks,
Jonathan
>
>
> From: Jonathan Cameron <[email protected]>
> Sent: Sunday, May 31, 2020 14:56
> To: Jean-Baptiste Maneyrol <[email protected]>
> Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
> Subject: Re: [PATCH v2 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
>
> CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
>
> On Wed, 27 May 2020 20:57:08 +0200
> Jean-Baptiste Maneyrol <[email protected]> wrote:
>
> > Add all FIFO parsing and reading functions. Add accel and gyro
> > kfifo buffer and FIFO data parsing. Use device interrupt for
> > reading data FIFO and launching accel and gyro parsing.
> >
> > Support hwfifo watermark by multiplexing gyro and accel settings.
> > Support hwfifo flush.
>
> Both of these are complex given the interactions of the two sensors
> types and to be honest I couldn't figure out exactly what the intent was.
> Needs more docs!
>
> Thanks,
>
> Jonathan
>
> >
> > Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> > ---
> > drivers/iio/imu/inv_icm42600/Kconfig | 1 +
> > drivers/iio/imu/inv_icm42600/Makefile | 1 +
> > drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
> > .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 160 ++++-
> > .../imu/inv_icm42600/inv_icm42600_buffer.c | 555 ++++++++++++++++++
> > .../imu/inv_icm42600/inv_icm42600_buffer.h | 98 ++++
> > .../iio/imu/inv_icm42600/inv_icm42600_core.c | 30 +
> > .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 160 ++++-
> > 8 files changed, 1011 insertions(+), 2 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 22390a72f0a3..50cbcfcb6cf1 100644
> > --- a/drivers/iio/imu/inv_icm42600/Kconfig
> > +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> > @@ -2,6 +2,7 @@
> >
> > config INV_ICM42600
> > tristate
> > + select IIO_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 48965824f00c..0f49f6df3647 100644
> > --- a/drivers/iio/imu/inv_icm42600/Makefile
> > +++ b/drivers/iio/imu/inv_icm42600/Makefile
> > @@ -5,6 +5,7 @@ inv-icm42600-y += inv_icm42600_core.o
> > 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_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 43749f56426c..4d5811562a61 100644
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> > @@ -14,6 +14,8 @@
> > #include <linux/pm.h>
> > #include <linux/iio/iio.h>
> >
> > +#include "inv_icm42600_buffer.h"
> > +
> > enum inv_icm42600_chip {
> > INV_CHIP_ICM42600,
> > INV_CHIP_ICM42602,
> > @@ -123,6 +125,7 @@ struct inv_icm42600_suspended {
> > * @indio_gyro: gyroscope IIO device.
> > * @indio_accel: accelerometer IIO device.
> > * @buffer: data transfer buffer aligned for DMA.
> > + * @fifo: FIFO management structure.
> > */
> > struct inv_icm42600_state {
> > struct mutex lock;
> > @@ -137,6 +140,7 @@ struct inv_icm42600_state {
> > struct iio_dev *indio_gyro;
> > struct iio_dev *indio_accel;
> > uint8_t buffer[2] ____cacheline_aligned;
> > + struct inv_icm42600_fifo fifo;
> > };
> >
> > /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> > @@ -377,6 +381,10 @@ 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);
> > +
> > int inv_icm42600_accel_init(struct inv_icm42600_state *st);
> >
> > +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
> > +
> > #endif
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> > index 6a615d7ffb24..c73ce204efc6 100644
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> > @@ -11,9 +11,12 @@
> > #include <linux/delay.h>
> > #include <linux/math64.h>
> > #include <linux/iio/iio.h>
> > +#include <linux/iio/buffer.h>
> > +#include <linux/iio/kfifo_buf.h>
> >
> > #include "inv_icm42600.h"
> > #include "inv_icm42600_temp.h"
> > +#include "inv_icm42600_buffer.h"
> >
> > #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
> > { \
> > @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
> > INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> > };
> >
> > +/* IIO buffer data: 8 bytes */
> > +struct inv_icm42600_accel_buffer {
> > + struct inv_icm42600_fifo_sensor_data accel;
> > + int8_t temp;
> > + uint8_t padding;
> > +};
> > +
> > +#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,
> > +};
> > +
> > +/* 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 */
> > + 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)
> > @@ -248,7 +321,12 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> > mutex_lock(&st->lock);
> >
> > ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> > + if (ret)
> > + goto out_unlock;
> > + inv_icm42600_buffer_update_fifo_period(st);
> > + inv_icm42600_buffer_update_watermark(st);
> >
> > +out_unlock:
> > mutex_unlock(&st->lock);
> > pm_runtime_mark_last_busy(dev);
> > pm_runtime_put_autosuspend(dev);
> > @@ -563,12 +641,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)
> > @@ -576,6 +693,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;
> > + struct iio_buffer *buffer;
> >
> > name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
> > if (!name)
> > @@ -585,14 +703,54 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> > if (!indio_dev)
> > return -ENOMEM;
> >
> > + buffer = devm_iio_kfifo_allocate(dev);
> > + if (!buffer)
> > + return -ENOMEM;
> > +
> > iio_device_set_drvdata(indio_dev, st);
> > indio_dev->dev.parent = dev;
> > indio_dev->name = name;
> > indio_dev->info = &inv_icm42600_accel_info;
> > - indio_dev->modes = INDIO_DIRECT_MODE;
> > + indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
> > 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;
> > + indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> > +
> > + iio_device_attach_buffer(indio_dev, buffer);
> >
> > 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)
> > +{
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> > + ssize_t i, size;
> > + const void *accel, *gyro, *timestamp;
> > + const int8_t *temp;
> > + unsigned int odr;
> > + struct inv_icm42600_accel_buffer buffer = {
> > + .padding = 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);
> > + /* 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))
> > + continue;
> > +
> > + /* fill and push data buffer */
> > + memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> > + buffer.temp = temp ? *temp : 0;
> > + iio_push_to_buffers(indio_dev, &buffer);
> > + }
> > +
> > + 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..c91075f62231
> > --- /dev/null
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> > @@ -0,0 +1,555 @@
> > +// SPDX-License-Identifier: GPL-2.0-or-later
> > +/*
> > + * Copyright (C) 2020 Invensense, Inc.
> > + */
> > +
> > +#include <linux/kernel.h>
> > +#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 "inv_icm42600.h"
> > +#include "inv_icm42600_buffer.h"
> > +
> > +/* 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)
> > +
> > +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
> > +
> > +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> > + const void **gyro, const int8_t **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;
> > +}
> > +
> > +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;
> > +}
> > +
>
> I think some overview docs on how this is working would be good.
> Set out the aims for the watermark selected and how it is achieved.
>
> > +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;
> > + 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;
> > +
> > + /* changing FIFO watermark requires to turn off watermark interrupt */
> > + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> > + 0, &restore);
> > + if (ret)
> > + return ret;
> > +
> > + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> > + memcpy(st->buffer, &raw_wm, sizeof(raw_wm));
> > + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> > + st->buffer, sizeof(raw_wm));
> > + if (ret)
> > + return ret;
> > +
> > + /* restore watermark interrupt */
> > + if (restore) {
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> > + 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;
> > +}
> > +
> > +/*
> > + * update_scan_mode callback is turning sensors on and setting data FIFO enable
> > + * bits.
> > + */
> > +static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev)
> > +{
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> > + int ret;
> > +
> > + mutex_lock(&st->lock);
> > +
> > + /* exit if FIFO is already on */
> > + if (st->fifo.on) {
> > + ret = 0;
> > + goto out_on;
> > + }
> > +
> > + /* set FIFO threshold interrupt */
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
> > + if (ret)
> > + goto out_unlock;
> > +
> > + /* flush FIFO data */
> > + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> > + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> > + if (ret)
> > + goto out_unlock;
> > +
> > + /* set FIFO in streaming mode */
> > + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> > + INV_ICM42600_FIFO_CONFIG_STREAM);
> > + if (ret)
> > + goto out_unlock;
> > +
> > + /* workaround: first read of FIFO count after reset is always 0 */
> > + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2);
> > + if (ret)
> > + goto out_unlock;
> > +
> > +out_on:
> > + /* increase FIFO on counter */
> > + st->fifo.on++;
> > +out_unlock:
> > + mutex_unlock(&st->lock);
> > + return ret;
> > +}
> > +
> > +static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev)
> > +{
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> > + int ret;
> > +
> > + mutex_lock(&st->lock);
> > +
> > + /* exit if there are several sensors using the FIFO */
> > + if (st->fifo.on > 1) {
> > + ret = 0;
> > + goto out_off;
> > + }
> > +
> > + /* 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;
> > +
> > + /* flush FIFO data */
> > + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> > + INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH);
> > + if (ret)
> > + goto out_unlock;
> > +
> > + /* disable FIFO threshold interrupt */
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> > + INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
> > + if (ret)
> > + goto out_unlock;
> > +
> > +out_off:
> > + /* decrease FIFO on counter */
> > + st->fifo.on--;
> > +out_unlock:
> > + mutex_unlock(&st->lock);
> > + return ret;
> > +}
> > +
> > +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_temp = 0;
> > + unsigned int sleep_sensor = 0;
> > + unsigned int sleep;
> > + 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_sensor);
> > + else
> > + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
> > + if (ret)
> > + goto out_unlock;
> > +
> > + /* if FIFO is off, turn temperature off */
> > + if (!st->fifo.on)
> > + ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
> > +
> > +out_unlock:
> > + mutex_unlock(&st->lock);
> > +
> > + /* sleep maximum required time */
> > + if (sleep_sensor > sleep_temp)
> > + sleep = sleep_sensor;
> > + else
> > + sleep = sleep_temp;
> > + 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 = inv_icm42600_buffer_postenable,
>
> We've been slowly eroding the difference between preenable and posteenable.
> Would be good to understand why you need to define both?
>
> > + .predisable = inv_icm42600_buffer_predisable,
> > + .postdisable = inv_icm42600_buffer_postdisable,
> > +};
> > +
> > +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> > + unsigned int max)
> > +{
> > + size_t max_count;
> > + __be16 *raw_fifo_count;
> > + ssize_t i, size;
> > + const void *accel, *gyro, *timestamp;
> > + const int8_t *temp;
> > + 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 */
> > + raw_fifo_count = (__be16 *)st->buffer;
> > + 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_cpup(raw_fifo_count);
> > +
> > + /* check and clamp 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_fifo_parse(struct inv_icm42600_state *st)
> > +{
> > + int ret;
> > +
> > + if (st->fifo.nb.total == 0)
> > + return 0;
> > +
> > + if (st->fifo.nb.gyro > 0) {
> > + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> > + if (ret)
> > + return ret;
> > + }
> > +
> > + if (st->fifo.nb.accel > 0) {
> > + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> > + if (ret)
> > + return ret;
> > + }
> > +
> > + return 0;
> > +}
> > +
> > +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> > + unsigned int count)
> > +{
> > + int ret;
> > +
> > + ret = inv_icm42600_buffer_fifo_read(st, count);
> > + if (ret)
> > + return ret;
> Definitely searching my memory for how this works in the core, so
> I may have it wrong.
>
> This is a bit unusual (I think). The intent of the flush
> is to read up to 'n' bytes because someone just did a read on the buffer
> or select, and there was data in the hwfifo capable of satisfying the read
> even though we haven't yet reached the watermark.
>
> Given both sensor types are coming from one buffer, do we have a potential
> issue here or under serving even though data is available?
>
> The case I worry may be served late is when an poll / select
> is waiting for sufficient data.
>
> So what should we be doing? We want to guarantee to provide data
> for each sensor type if it's in the hwfifo. As such we could keep reading
> until we have enough, but that could cause some issues if the two data rates
> are very different (overflow on the other kfifo)
>
> Maybe what you have here is the best we can do.
>
> I'm assuming the watermark level has a similar problem. One value represents
> the sum of the two types of data.
>
> > +
> > + if (st->fifo.nb.total == 0)
> > + return 0;
> > +
> > + if (st->fifo.nb.gyro > 0) {
> > + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> > + if (ret)
> > + return ret;
> > + }
> > +
> > + if (st->fifo.nb.accel > 0) {
> > + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> > + if (ret)
> > + return ret;
> > + }
> > +
> > + return 0;
> > +}
> > +
> > +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> > +{
> > + unsigned int val;
> > + int ret;
> > +
> > + /*
> > + * Default FIFO configuration (bits 7 to 5)
> > + * - use invalid value
> > + * - FIFO count in bytes
> > + * - FIFO count in big endian
> > + */
> > + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> > + GENMASK(7, 5), val);
> > + if (ret)
> > + return ret;
> > +
> > + /*
> > + * Enable FIFO partial read and continuous watermark interrupt.
> > + * Disable all FIFO EN bits.
> > + */
> > + 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,
> > + GENMASK(6, 5) | GENMASK(3, 0), 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..de2a3949dcc7
> > --- /dev/null
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> > @@ -0,0 +1,98 @@
> > +/* 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 - FIFO state variables
> > + * @on: reference counter for FIFO on.
> > + * @en: bits field of INV_ICM42600_SENSOR_* for FIFO EN bits.
> > + * @period: FIFO internal period.
> > + * @watermark: watermark configuration values for accel and gyro.
> > + * @count: number of bytes in the FIFO data buffer.
> > + * @nb: gyro, accel and total samples in the FIFO data buffer.
> > + * @data: FIFO data buffer aligned for DMA (2kB + 32 bytes of read cache).
> > + */
> > +struct inv_icm42600_fifo {
> > + unsigned int on;
> > + 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] ____cacheline_aligned;
> > +};
> > +
> > +/* FIFO data packet */
> > +struct inv_icm42600_fifo_sensor_data {
> > + __be16 x;
> > + __be16 y;
> > + __be16 z;
> > +} __packed;
>
> Why packed? Should be anyway I think.
>
> > +#define INV_ICM42600_FIFO_DATA_INVALID -32768
> > +
> > +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;
> > +}
> > +
> > +ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
> > + const void **gyro, const int8_t **temp,
> > + const void **timestamp, unsigned int *odr);
> > +
> > +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_fifo_parse(struct inv_icm42600_state *st);
> > +
> > +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 246c1eb52231..6f1c1eb83953 100644
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> > @@ -18,6 +18,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[] = {
> > {
> > @@ -429,6 +430,18 @@ static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
> > if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
> > dev_warn(dev, "FIFO full data lost!\n");
> >
> > + /* FIFO threshold reached */
> > + 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);
> > + goto out_unlock;
> > + }
> > + ret = inv_icm42600_buffer_fifo_parse(st);
> > + if (ret)
> > + dev_err(dev, "FIFO parsing error %d\n", ret);
> > + }
> > +
> > out_unlock:
> > mutex_unlock(&st->lock);
> > return IRQ_HANDLED;
> > @@ -600,6 +613,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> > if (ret)
> > return ret;
> >
> > + ret = inv_icm42600_buffer_init(st);
> > + if (ret)
> > + return ret;
> > +
> > ret = inv_icm42600_gyro_init(st);
> > if (ret)
> > return ret;
> > @@ -645,6 +662,14 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
> > goto out_unlock;
> > }
> >
> > + /* disable FIFO data streaming */
> > + if (st->fifo.on) {
> > + 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);
> > @@ -684,6 +709,11 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
> > if (ret)
> > goto out_unlock;
> >
> > + /* restore FIFO data streaming */
> > + if (st->fifo.on)
> > + 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 38654e0d217b..b05c33876b8d 100644
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> > @@ -11,9 +11,12 @@
> > #include <linux/delay.h>
> > #include <linux/math64.h>
> > #include <linux/iio/iio.h>
> > +#include <linux/iio/buffer.h>
> > +#include <linux/iio/kfifo_buf.h>
> >
> > #include "inv_icm42600.h"
> > #include "inv_icm42600_temp.h"
> > +#include "inv_icm42600_buffer.h"
> >
> > #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
> > { \
> > @@ -64,6 +67,76 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
> > INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> > };
> >
> > +/* IIO buffer data: 8 bytes */
> > +struct inv_icm42600_gyro_buffer {
> > + struct inv_icm42600_fifo_sensor_data gyro;
> > + int8_t temp;
> > + uint8_t padding;
> > +};
> > +
> > +#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,
> > +};
> > +
> > +/* 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 */
> > + 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)
> > @@ -260,7 +333,12 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> > mutex_lock(&st->lock);
> >
> > ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> > + if (ret)
> > + goto out_unlock;
> > + inv_icm42600_buffer_update_fifo_period(st);
> > + inv_icm42600_buffer_update_watermark(st);
> >
> > +out_unlock:
> > mutex_unlock(&st->lock);
> > pm_runtime_mark_last_busy(dev);
> > pm_runtime_put_autosuspend(dev);
> > @@ -574,12 +652,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)
> > +{
>
> Nothing to do with this patch, but I realised reading this that we have
> some 'unusual' use of the word flush here. It's a straight forward
> read function so not sure why we called it flush.
>
> > + 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)
> > @@ -587,6 +704,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;
> > + struct iio_buffer *buffer;
> >
> > name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> > if (!name)
> > @@ -596,14 +714,54 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> > if (!indio_dev)
> > return -ENOMEM;
> >
> > + buffer = devm_iio_kfifo_allocate(dev);
> > + if (!buffer)
> > + return -ENOMEM;
> > +
> > iio_device_set_drvdata(indio_dev, st);
> > indio_dev->dev.parent = dev;
> > indio_dev->name = name;
> > indio_dev->info = &inv_icm42600_gyro_info;
> > - indio_dev->modes = INDIO_DIRECT_MODE;
> > + indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
> > 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;
> > + indio_dev->setup_ops = &inv_icm42600_buffer_ops;
> > +
> > + iio_device_attach_buffer(indio_dev, buffer);
> >
> > 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)
> > +{
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> > + ssize_t i, size;
> > + const void *accel, *gyro, *timestamp;
> > + const int8_t *temp;
> > + unsigned int odr;
> > + struct inv_icm42600_gyro_buffer buffer = {
> > + .padding = 0,
>
> Might be worth a comment here or where the structure is defined
> on why we make padding explicit.
>
> > + };
> > +
> > + /* 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);
> > + /* 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))
> > + continue;
> > +
> > + /* fill and push data buffer */
> > + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> > + buffer.temp = temp ? *temp : 0;
> > + iio_push_to_buffers(indio_dev, &buffer);
> > + }
> > +
> > + return 0;
> > +}