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