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
v3
- delete const pointer cast for iio_device_get_drvdata
- change gyro and accel init to return the allocated iio_dev structure
- delete manual parent device assignment
- correct style and improve readability
- add commentaries about IIO buffer and watermark complex computation
- add timestamp alignment in IIO buffer structure
- wrap lines 80 columns for dt bindings
- add ABI documentation for calibbias values in SI units
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 (13):
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
Documentation: ABI: add specific icm42600 documentation
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
.../ABI/testing/sysfs-bus-iio-icm42600 | 20 +
.../bindings/iio/imu/invensense,icm42600.yaml | 90 ++
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 | 789 +++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.c | 601 +++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 98 +++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 786 +++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 800 ++++++++++++++++++
.../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 ++
19 files changed, 4231 insertions(+)
create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
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 I2C driver for InvenSense ICM-426xxx devices.
Configure bus signal slew rates as indicated in the datasheet.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 100 ++++++++++++++++++
1 file changed, 100 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
new file mode 100644
index 000000000000..4789cead23b3
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
@@ -0,0 +1,100 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 InvenSense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/property.h>
+
+#include "inv_icm42600.h"
+
+static int inv_icm42600_i2c_bus_setup(struct inv_icm42600_state *st)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /* setup interface registers */
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
+ INV_ICM42600_INTF_CONFIG6_MASK,
+ INV_ICM42600_INTF_CONFIG6_I3C_EN);
+ if (ret)
+ return ret;
+
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
+ INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY, 0);
+ if (ret)
+ return ret;
+
+ /* set slew rates for I2C and SPI */
+ mask = INV_ICM42600_DRIVE_CONFIG_I2C_MASK |
+ INV_ICM42600_DRIVE_CONFIG_SPI_MASK;
+ val = INV_ICM42600_DRIVE_CONFIG_I2C(INV_ICM42600_SLEW_RATE_12_36NS) |
+ INV_ICM42600_DRIVE_CONFIG_SPI(INV_ICM42600_SLEW_RATE_12_36NS);
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_DRIVE_CONFIG,
+ mask, val);
+ if (ret)
+ return ret;
+
+ /* disable SPI bus */
+ return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK,
+ INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS);
+}
+
+static int inv_icm42600_probe(struct i2c_client *client)
+{
+ const void *match;
+ enum inv_icm42600_chip chip;
+ struct regmap *regmap;
+
+ if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_I2C_BLOCK))
+ return -ENOTSUPP;
+
+ match = device_get_match_data(&client->dev);
+ if (!match)
+ return -EINVAL;
+ chip = (enum inv_icm42600_chip)match;
+
+ regmap = devm_regmap_init_i2c(client, &inv_icm42600_regmap_config);
+ if (IS_ERR(regmap))
+ return PTR_ERR(regmap);
+
+ return inv_icm42600_core_probe(regmap, chip, inv_icm42600_i2c_bus_setup);
+}
+
+static const struct of_device_id inv_icm42600_of_matches[] = {
+ {
+ .compatible = "invensense,icm42600",
+ .data = (void *)INV_CHIP_ICM42600,
+ }, {
+ .compatible = "invensense,icm42602",
+ .data = (void *)INV_CHIP_ICM42602,
+ }, {
+ .compatible = "invensense,icm42605",
+ .data = (void *)INV_CHIP_ICM42605,
+ }, {
+ .compatible = "invensense,icm42622",
+ .data = (void *)INV_CHIP_ICM42622,
+ },
+ {}
+};
+MODULE_DEVICE_TABLE(of, inv_icm42600_of_matches);
+
+static struct i2c_driver inv_icm42600_driver = {
+ .driver = {
+ .name = "inv-icm42600-i2c",
+ .of_match_table = inv_icm42600_of_matches,
+ .pm = &inv_icm42600_pm_ops,
+ },
+ .probe_new = inv_icm42600_probe,
+};
+module_i2c_driver(inv_icm42600_driver);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver");
+MODULE_LICENSE("GPL");
--
2.17.1
Core component of a new driver for InvenSense ICM-426xx devices.
It includes registers definition, main probe/setup, and device
utility functions.
ICM-426xx devices are latest generation of 6-axis IMU,
gyroscope+accelerometer and temperature sensor. This device
includes a 2K FIFO, supports I2C/I3C/SPI, and provides
intelligent motion features like pedometer, tilt detection,
and tap detection.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 372 ++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 634 ++++++++++++++++++
2 files changed, 1006 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
new file mode 100644
index 000000000000..14c8ef152418
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -0,0 +1,372 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_H_
+#define INV_ICM42600_H_
+
+#include <linux/bits.h>
+#include <linux/bitfield.h>
+#include <linux/regmap.h>
+#include <linux/mutex.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm.h>
+#include <linux/iio/iio.h>
+
+enum inv_icm42600_chip {
+ INV_CHIP_ICM42600,
+ INV_CHIP_ICM42602,
+ INV_CHIP_ICM42605,
+ INV_CHIP_ICM42622,
+ INV_CHIP_NB,
+};
+
+/* serial bus slew rates */
+enum inv_icm42600_slew_rate {
+ INV_ICM42600_SLEW_RATE_20_60NS,
+ INV_ICM42600_SLEW_RATE_12_36NS,
+ INV_ICM42600_SLEW_RATE_6_18NS,
+ INV_ICM42600_SLEW_RATE_4_12NS,
+ INV_ICM42600_SLEW_RATE_2_6NS,
+ INV_ICM42600_SLEW_RATE_INF_2NS,
+};
+
+enum inv_icm42600_sensor_mode {
+ INV_ICM42600_SENSOR_MODE_OFF,
+ INV_ICM42600_SENSOR_MODE_STANDBY,
+ INV_ICM42600_SENSOR_MODE_LOW_POWER,
+ INV_ICM42600_SENSOR_MODE_LOW_NOISE,
+ INV_ICM42600_SENSOR_MODE_NB,
+};
+
+/* gyroscope fullscale values */
+enum inv_icm42600_gyro_fs {
+ INV_ICM42600_GYRO_FS_2000DPS,
+ INV_ICM42600_GYRO_FS_1000DPS,
+ INV_ICM42600_GYRO_FS_500DPS,
+ INV_ICM42600_GYRO_FS_250DPS,
+ INV_ICM42600_GYRO_FS_125DPS,
+ INV_ICM42600_GYRO_FS_62_5DPS,
+ INV_ICM42600_GYRO_FS_31_25DPS,
+ INV_ICM42600_GYRO_FS_15_625DPS,
+ INV_ICM42600_GYRO_FS_NB,
+};
+
+/* accelerometer fullscale values */
+enum inv_icm42600_accel_fs {
+ INV_ICM42600_ACCEL_FS_16G,
+ INV_ICM42600_ACCEL_FS_8G,
+ INV_ICM42600_ACCEL_FS_4G,
+ INV_ICM42600_ACCEL_FS_2G,
+ INV_ICM42600_ACCEL_FS_NB,
+};
+
+/* ODR suffixed by LN or LP are Low-Noise or Low-Power mode only */
+enum inv_icm42600_odr {
+ INV_ICM42600_ODR_8KHZ_LN = 3,
+ INV_ICM42600_ODR_4KHZ_LN,
+ INV_ICM42600_ODR_2KHZ_LN,
+ INV_ICM42600_ODR_1KHZ_LN,
+ INV_ICM42600_ODR_200HZ,
+ INV_ICM42600_ODR_100HZ,
+ INV_ICM42600_ODR_50HZ,
+ INV_ICM42600_ODR_25HZ,
+ INV_ICM42600_ODR_12_5HZ,
+ INV_ICM42600_ODR_6_25HZ_LP,
+ INV_ICM42600_ODR_3_125HZ_LP,
+ INV_ICM42600_ODR_1_5625HZ_LP,
+ INV_ICM42600_ODR_500HZ,
+ INV_ICM42600_ODR_NB,
+};
+
+enum inv_icm42600_filter {
+ /* Low-Noise mode sensor data filter (3rd order filter by default) */
+ INV_ICM42600_FILTER_BW_ODR_DIV_2,
+
+ /* Low-Power mode sensor data filter (averaging) */
+ INV_ICM42600_FILTER_AVG_1X = 1,
+ INV_ICM42600_FILTER_AVG_16X = 6,
+};
+
+struct inv_icm42600_sensor_conf {
+ int mode;
+ int fs;
+ int odr;
+ int filter;
+};
+#define INV_ICM42600_SENSOR_CONF_INIT {-1, -1, -1, -1}
+
+struct inv_icm42600_conf {
+ struct inv_icm42600_sensor_conf gyro;
+ struct inv_icm42600_sensor_conf accel;
+ bool temp_en;
+};
+
+struct inv_icm42600_suspended {
+ enum inv_icm42600_sensor_mode gyro;
+ enum inv_icm42600_sensor_mode accel;
+ bool temp;
+};
+
+/**
+ * struct inv_icm42600_state - driver state variables
+ * @lock: lock for serializing multiple registers access.
+ * @chip: chip identifier.
+ * @name: chip name.
+ * @map: regmap pointer.
+ * @vdd_supply: VDD voltage regulator for the chip.
+ * @vddio_supply: I/O voltage regulator for the chip.
+ * @orientation: sensor chip orientation relative to main hardware.
+ * @conf: chip sensors configurations.
+ * @suspended: suspended sensors configuration.
+ */
+struct inv_icm42600_state {
+ struct mutex lock;
+ enum inv_icm42600_chip chip;
+ const char *name;
+ struct regmap *map;
+ struct regulator *vdd_supply;
+ struct regulator *vddio_supply;
+ struct iio_mount_matrix orientation;
+ struct inv_icm42600_conf conf;
+ struct inv_icm42600_suspended suspended;
+};
+
+/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
+
+/* Bank selection register, available in all banks */
+#define INV_ICM42600_REG_BANK_SEL 0x76
+#define INV_ICM42600_BANK_SEL_MASK GENMASK(2, 0)
+
+/* User bank 0 (MSB 0x00) */
+#define INV_ICM42600_REG_DEVICE_CONFIG 0x0011
+#define INV_ICM42600_DEVICE_CONFIG_SOFT_RESET BIT(0)
+
+#define INV_ICM42600_REG_DRIVE_CONFIG 0x0013
+#define INV_ICM42600_DRIVE_CONFIG_I2C_MASK GENMASK(5, 3)
+#define INV_ICM42600_DRIVE_CONFIG_I2C(_rate) \
+ FIELD_PREP(INV_ICM42600_DRIVE_CONFIG_I2C_MASK, (_rate))
+#define INV_ICM42600_DRIVE_CONFIG_SPI_MASK GENMASK(2, 0)
+#define INV_ICM42600_DRIVE_CONFIG_SPI(_rate) \
+ FIELD_PREP(INV_ICM42600_DRIVE_CONFIG_SPI_MASK, (_rate))
+
+#define INV_ICM42600_REG_INT_CONFIG 0x0014
+#define INV_ICM42600_INT_CONFIG_INT2_LATCHED BIT(5)
+#define INV_ICM42600_INT_CONFIG_INT2_PUSH_PULL BIT(4)
+#define INV_ICM42600_INT_CONFIG_INT2_ACTIVE_HIGH BIT(3)
+#define INV_ICM42600_INT_CONFIG_INT2_ACTIVE_LOW 0x00
+#define INV_ICM42600_INT_CONFIG_INT1_LATCHED BIT(2)
+#define INV_ICM42600_INT_CONFIG_INT1_PUSH_PULL BIT(1)
+#define INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH BIT(0)
+#define INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW 0x00
+
+#define INV_ICM42600_REG_FIFO_CONFIG 0x0016
+#define INV_ICM42600_FIFO_CONFIG_MASK GENMASK(7, 6)
+#define INV_ICM42600_FIFO_CONFIG_BYPASS \
+ FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 0)
+#define INV_ICM42600_FIFO_CONFIG_STREAM \
+ FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 1)
+#define INV_ICM42600_FIFO_CONFIG_STOP_ON_FULL \
+ FIELD_PREP(INV_ICM42600_FIFO_CONFIG_MASK, 2)
+
+/* all sensor data are 16 bits (2 registers wide) in big-endian */
+#define INV_ICM42600_REG_TEMP_DATA 0x001D
+#define INV_ICM42600_REG_ACCEL_DATA_X 0x001F
+#define INV_ICM42600_REG_ACCEL_DATA_Y 0x0021
+#define INV_ICM42600_REG_ACCEL_DATA_Z 0x0023
+#define INV_ICM42600_REG_GYRO_DATA_X 0x0025
+#define INV_ICM42600_REG_GYRO_DATA_Y 0x0027
+#define INV_ICM42600_REG_GYRO_DATA_Z 0x0029
+#define INV_ICM42600_DATA_INVALID -32768
+
+#define INV_ICM42600_REG_INT_STATUS 0x002D
+#define INV_ICM42600_INT_STATUS_UI_FSYNC BIT(6)
+#define INV_ICM42600_INT_STATUS_PLL_RDY BIT(5)
+#define INV_ICM42600_INT_STATUS_RESET_DONE BIT(4)
+#define INV_ICM42600_INT_STATUS_DATA_RDY BIT(3)
+#define INV_ICM42600_INT_STATUS_FIFO_THS BIT(2)
+#define INV_ICM42600_INT_STATUS_FIFO_FULL BIT(1)
+#define INV_ICM42600_INT_STATUS_AGC_RDY BIT(0)
+
+/*
+ * FIFO access registers
+ * FIFO count is 16 bits (2 registers) big-endian
+ * FIFO data is a continuous read register to read FIFO content
+ */
+#define INV_ICM42600_REG_FIFO_COUNT 0x002E
+#define INV_ICM42600_REG_FIFO_DATA 0x0030
+
+#define INV_ICM42600_REG_SIGNAL_PATH_RESET 0x004B
+#define INV_ICM42600_SIGNAL_PATH_RESET_DMP_INIT_EN BIT(6)
+#define INV_ICM42600_SIGNAL_PATH_RESET_DMP_MEM_RESET BIT(5)
+#define INV_ICM42600_SIGNAL_PATH_RESET_RESET BIT(3)
+#define INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE BIT(2)
+#define INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH BIT(1)
+
+/* default configuration: all data big-endian and fifo count in bytes */
+#define INV_ICM42600_REG_INTF_CONFIG0 0x004C
+#define INV_ICM42600_INTF_CONFIG0_FIFO_HOLD_LAST_DATA BIT(7)
+#define INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_REC BIT(6)
+#define INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN BIT(5)
+#define INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN BIT(4)
+#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK GENMASK(1, 0)
+#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS \
+ FIELD_PREP(INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, 2)
+#define INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_I2C_DIS \
+ FIELD_PREP(INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK, 3)
+
+#define INV_ICM42600_REG_INTF_CONFIG1 0x004D
+#define INV_ICM42600_INTF_CONFIG1_ACCEL_LP_CLK_RC BIT(3)
+
+#define INV_ICM42600_REG_PWR_MGMT0 0x004E
+#define INV_ICM42600_PWR_MGMT0_TEMP_DIS BIT(5)
+#define INV_ICM42600_PWR_MGMT0_IDLE BIT(4)
+#define INV_ICM42600_PWR_MGMT0_GYRO(_mode) \
+ FIELD_PREP(GENMASK(3, 2), (_mode))
+#define INV_ICM42600_PWR_MGMT0_ACCEL(_mode) \
+ FIELD_PREP(GENMASK(1, 0), (_mode))
+
+#define INV_ICM42600_REG_GYRO_CONFIG0 0x004F
+#define INV_ICM42600_GYRO_CONFIG0_FS(_fs) \
+ FIELD_PREP(GENMASK(7, 5), (_fs))
+#define INV_ICM42600_GYRO_CONFIG0_ODR(_odr) \
+ FIELD_PREP(GENMASK(3, 0), (_odr))
+
+#define INV_ICM42600_REG_ACCEL_CONFIG0 0x0050
+#define INV_ICM42600_ACCEL_CONFIG0_FS(_fs) \
+ FIELD_PREP(GENMASK(7, 5), (_fs))
+#define INV_ICM42600_ACCEL_CONFIG0_ODR(_odr) \
+ FIELD_PREP(GENMASK(3, 0), (_odr))
+
+#define INV_ICM42600_REG_GYRO_ACCEL_CONFIG0 0x0052
+#define INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(_f) \
+ FIELD_PREP(GENMASK(7, 4), (_f))
+#define INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(_f) \
+ FIELD_PREP(GENMASK(3, 0), (_f))
+
+#define INV_ICM42600_REG_TMST_CONFIG 0x0054
+#define INV_ICM42600_TMST_CONFIG_MASK GENMASK(4, 0)
+#define INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN BIT(4)
+#define INV_ICM42600_TMST_CONFIG_TMST_RES_16US BIT(3)
+#define INV_ICM42600_TMST_CONFIG_TMST_DELTA_EN BIT(2)
+#define INV_ICM42600_TMST_CONFIG_TMST_FSYNC_EN BIT(1)
+#define INV_ICM42600_TMST_CONFIG_TMST_EN BIT(0)
+
+#define INV_ICM42600_REG_FIFO_CONFIG1 0x005F
+#define INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD BIT(6)
+#define INV_ICM42600_FIFO_CONFIG1_WM_GT_TH BIT(5)
+#define INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN BIT(3)
+#define INV_ICM42600_FIFO_CONFIG1_TEMP_EN BIT(2)
+#define INV_ICM42600_FIFO_CONFIG1_GYRO_EN BIT(1)
+#define INV_ICM42600_FIFO_CONFIG1_ACCEL_EN BIT(0)
+
+/* FIFO watermark is 16 bits (2 registers wide) in little-endian */
+#define INV_ICM42600_REG_FIFO_WATERMARK 0x0060
+#define INV_ICM42600_FIFO_WATERMARK_VAL(_wm) \
+ cpu_to_le16((_wm) & GENMASK(11, 0))
+/* FIFO is 2048 bytes, let 12 samples for reading latency */
+#define INV_ICM42600_FIFO_WATERMARK_MAX (2048 - 12 * 16)
+
+#define INV_ICM42600_REG_INT_CONFIG1 0x0064
+#define INV_ICM42600_INT_CONFIG1_TPULSE_DURATION BIT(6)
+#define INV_ICM42600_INT_CONFIG1_TDEASSERT_DISABLE BIT(5)
+#define INV_ICM42600_INT_CONFIG1_ASYNC_RESET BIT(4)
+
+#define INV_ICM42600_REG_INT_SOURCE0 0x0065
+#define INV_ICM42600_INT_SOURCE0_UI_FSYNC_INT1_EN BIT(6)
+#define INV_ICM42600_INT_SOURCE0_PLL_RDY_INT1_EN BIT(5)
+#define INV_ICM42600_INT_SOURCE0_RESET_DONE_INT1_EN BIT(4)
+#define INV_ICM42600_INT_SOURCE0_UI_DRDY_INT1_EN BIT(3)
+#define INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN BIT(2)
+#define INV_ICM42600_INT_SOURCE0_FIFO_FULL_INT1_EN BIT(1)
+#define INV_ICM42600_INT_SOURCE0_UI_AGC_RDY_INT1_EN BIT(0)
+
+#define INV_ICM42600_REG_WHOAMI 0x0075
+#define INV_ICM42600_WHOAMI_ICM42600 0x40
+#define INV_ICM42600_WHOAMI_ICM42602 0x41
+#define INV_ICM42600_WHOAMI_ICM42605 0x42
+#define INV_ICM42600_WHOAMI_ICM42622 0x46
+
+/* User bank 1 (MSB 0x10) */
+#define INV_ICM42600_REG_SENSOR_CONFIG0 0x1003
+#define INV_ICM42600_SENSOR_CONFIG0_ZG_DISABLE BIT(5)
+#define INV_ICM42600_SENSOR_CONFIG0_YG_DISABLE BIT(4)
+#define INV_ICM42600_SENSOR_CONFIG0_XG_DISABLE BIT(3)
+#define INV_ICM42600_SENSOR_CONFIG0_ZA_DISABLE BIT(2)
+#define INV_ICM42600_SENSOR_CONFIG0_YA_DISABLE BIT(1)
+#define INV_ICM42600_SENSOR_CONFIG0_XA_DISABLE BIT(0)
+
+/* Timestamp value is 20 bits (3 registers) in little-endian */
+#define INV_ICM42600_REG_TMSTVAL 0x1062
+#define INV_ICM42600_TMSTVAL_MASK GENMASK(19, 0)
+
+#define INV_ICM42600_REG_INTF_CONFIG4 0x107A
+#define INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY BIT(6)
+#define INV_ICM42600_INTF_CONFIG4_SPI_AP_4WIRE BIT(1)
+
+#define INV_ICM42600_REG_INTF_CONFIG6 0x107C
+#define INV_ICM42600_INTF_CONFIG6_MASK GENMASK(4, 0)
+#define INV_ICM42600_INTF_CONFIG6_I3C_EN BIT(4)
+#define INV_ICM42600_INTF_CONFIG6_I3C_IBI_BYTE_EN BIT(3)
+#define INV_ICM42600_INTF_CONFIG6_I3C_IBI_EN BIT(2)
+#define INV_ICM42600_INTF_CONFIG6_I3C_DDR_EN BIT(1)
+#define INV_ICM42600_INTF_CONFIG6_I3C_SDR_EN BIT(0)
+
+/* User bank 4 (MSB 0x40) */
+#define INV_ICM42600_REG_INT_SOURCE8 0x404F
+#define INV_ICM42600_INT_SOURCE8_FSYNC_IBI_EN BIT(5)
+#define INV_ICM42600_INT_SOURCE8_PLL_RDY_IBI_EN BIT(4)
+#define INV_ICM42600_INT_SOURCE8_UI_DRDY_IBI_EN BIT(3)
+#define INV_ICM42600_INT_SOURCE8_FIFO_THS_IBI_EN BIT(2)
+#define INV_ICM42600_INT_SOURCE8_FIFO_FULL_IBI_EN BIT(1)
+#define INV_ICM42600_INT_SOURCE8_AGC_RDY_IBI_EN BIT(0)
+
+#define INV_ICM42600_REG_OFFSET_USER0 0x4077
+#define INV_ICM42600_REG_OFFSET_USER1 0x4078
+#define INV_ICM42600_REG_OFFSET_USER2 0x4079
+#define INV_ICM42600_REG_OFFSET_USER3 0x407A
+#define INV_ICM42600_REG_OFFSET_USER4 0x407B
+#define INV_ICM42600_REG_OFFSET_USER5 0x407C
+#define INV_ICM42600_REG_OFFSET_USER6 0x407D
+#define INV_ICM42600_REG_OFFSET_USER7 0x407E
+#define INV_ICM42600_REG_OFFSET_USER8 0x407F
+
+/* Sleep times required by the driver */
+#define INV_ICM42600_POWER_UP_TIME_MS 100
+#define INV_ICM42600_RESET_TIME_MS 1
+#define INV_ICM42600_ACCEL_STARTUP_TIME_MS 20
+#define INV_ICM42600_GYRO_STARTUP_TIME_MS 60
+#define INV_ICM42600_GYRO_STOP_TIME_MS 150
+#define INV_ICM42600_TEMP_STARTUP_TIME_MS 14
+#define INV_ICM42600_SUSPEND_DELAY_MS 2000
+
+typedef int (*inv_icm42600_bus_setup)(struct inv_icm42600_state *);
+
+extern const struct regmap_config inv_icm42600_regmap_config;
+extern const struct dev_pm_ops inv_icm42600_pm_ops;
+
+const struct iio_mount_matrix *
+inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
+ const struct iio_chan_spec *chan);
+
+uint32_t inv_icm42600_odr_to_period(enum inv_icm42600_odr odr);
+
+int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep_ms);
+
+int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep_ms);
+
+int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
+ unsigned int *sleep_ms);
+
+int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
+ unsigned int writeval, unsigned int *readval);
+
+int inv_icm42600_core_probe(struct regmap *regmap, int chip,
+ inv_icm42600_bus_setup bus_setup);
+
+#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
new file mode 100644
index 000000000000..2eb25c5f77f8
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -0,0 +1,634 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42600.h"
+
+static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
+ {
+ .name = "user banks",
+ .range_min = 0x0000,
+ .range_max = 0x4FFF,
+ .selector_reg = INV_ICM42600_REG_BANK_SEL,
+ .selector_mask = INV_ICM42600_BANK_SEL_MASK,
+ .selector_shift = 0,
+ .window_start = 0,
+ .window_len = 0x1000,
+ },
+};
+
+const struct regmap_config inv_icm42600_regmap_config = {
+ .reg_bits = 8,
+ .val_bits = 8,
+ .max_register = 0x4FFF,
+ .ranges = inv_icm42600_regmap_ranges,
+ .num_ranges = ARRAY_SIZE(inv_icm42600_regmap_ranges),
+};
+EXPORT_SYMBOL_GPL(inv_icm42600_regmap_config);
+
+struct inv_icm42600_hw {
+ uint8_t whoami;
+ const char *name;
+ const struct inv_icm42600_conf *conf;
+};
+
+/* chip initial default configuration */
+static const struct inv_icm42600_conf inv_icm42600_default_conf = {
+ .gyro = {
+ .mode = INV_ICM42600_SENSOR_MODE_OFF,
+ .fs = INV_ICM42600_GYRO_FS_2000DPS,
+ .odr = INV_ICM42600_ODR_50HZ,
+ .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2,
+ },
+ .accel = {
+ .mode = INV_ICM42600_SENSOR_MODE_OFF,
+ .fs = INV_ICM42600_ACCEL_FS_16G,
+ .odr = INV_ICM42600_ODR_50HZ,
+ .filter = INV_ICM42600_FILTER_BW_ODR_DIV_2,
+ },
+ .temp_en = false,
+};
+
+static const struct inv_icm42600_hw inv_icm42600_hw[INV_CHIP_NB] = {
+ [INV_CHIP_ICM42600] = {
+ .whoami = INV_ICM42600_WHOAMI_ICM42600,
+ .name = "icm42600",
+ .conf = &inv_icm42600_default_conf,
+ },
+ [INV_CHIP_ICM42602] = {
+ .whoami = INV_ICM42600_WHOAMI_ICM42602,
+ .name = "icm42602",
+ .conf = &inv_icm42600_default_conf,
+ },
+ [INV_CHIP_ICM42605] = {
+ .whoami = INV_ICM42600_WHOAMI_ICM42605,
+ .name = "icm42605",
+ .conf = &inv_icm42600_default_conf,
+ },
+ [INV_CHIP_ICM42622] = {
+ .whoami = INV_ICM42600_WHOAMI_ICM42622,
+ .name = "icm42622",
+ .conf = &inv_icm42600_default_conf,
+ },
+};
+
+const struct iio_mount_matrix *
+inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
+ const struct iio_chan_spec *chan)
+{
+ const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+
+ return &st->orientation;
+}
+
+uint32_t inv_icm42600_odr_to_period(enum inv_icm42600_odr odr)
+{
+ static uint32_t odr_periods[INV_ICM42600_ODR_NB] = {
+ /* reserved values */
+ 0, 0, 0,
+ /* 8kHz */
+ 125000,
+ /* 4kHz */
+ 250000,
+ /* 2kHz */
+ 500000,
+ /* 1kHz */
+ 1000000,
+ /* 200Hz */
+ 5000000,
+ /* 100Hz */
+ 10000000,
+ /* 50Hz */
+ 20000000,
+ /* 25Hz */
+ 40000000,
+ /* 12.5Hz */
+ 80000000,
+ /* 6.25Hz */
+ 160000000,
+ /* 3.125Hz */
+ 320000000,
+ /* 1.5625Hz */
+ 640000000,
+ /* 500Hz */
+ 2000000,
+ };
+
+ return odr_periods[odr];
+}
+
+static int inv_icm42600_set_pwr_mgmt0(struct inv_icm42600_state *st,
+ enum inv_icm42600_sensor_mode gyro,
+ enum inv_icm42600_sensor_mode accel,
+ bool temp, unsigned int *sleep_ms)
+{
+ enum inv_icm42600_sensor_mode oldgyro = st->conf.gyro.mode;
+ enum inv_icm42600_sensor_mode oldaccel = st->conf.accel.mode;
+ bool oldtemp = st->conf.temp_en;
+ unsigned int sleepval;
+ unsigned int val;
+ int ret;
+
+ /* if nothing changed, exit */
+ if (gyro == oldgyro && accel == oldaccel && temp == oldtemp)
+ return 0;
+
+ val = INV_ICM42600_PWR_MGMT0_GYRO(gyro) |
+ INV_ICM42600_PWR_MGMT0_ACCEL(accel);
+ if (!temp)
+ val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS;
+ ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val);
+ if (ret)
+ return ret;
+
+ st->conf.gyro.mode = gyro;
+ st->conf.accel.mode = accel;
+ st->conf.temp_en = temp;
+
+ /* compute required wait time for sensors to stabilize */
+ sleepval = 0;
+ /* temperature stabilization time */
+ if (temp && !oldtemp) {
+ if (sleepval < INV_ICM42600_TEMP_STARTUP_TIME_MS)
+ sleepval = INV_ICM42600_TEMP_STARTUP_TIME_MS;
+ }
+ /* accel startup time */
+ if (accel != oldaccel && oldaccel == INV_ICM42600_SENSOR_MODE_OFF) {
+ /* block any register write for at least 200 µs */
+ usleep_range(200, 300);
+ if (sleepval < INV_ICM42600_ACCEL_STARTUP_TIME_MS)
+ sleepval = INV_ICM42600_ACCEL_STARTUP_TIME_MS;
+ }
+ if (gyro != oldgyro) {
+ /* gyro startup time */
+ if (oldgyro == INV_ICM42600_SENSOR_MODE_OFF) {
+ /* block any register write for at least 200 µs */
+ usleep_range(200, 300);
+ if (sleepval < INV_ICM42600_GYRO_STARTUP_TIME_MS)
+ sleepval = INV_ICM42600_GYRO_STARTUP_TIME_MS;
+ /* gyro stop time */
+ } else if (gyro == INV_ICM42600_SENSOR_MODE_OFF) {
+ if (sleepval < INV_ICM42600_GYRO_STOP_TIME_MS)
+ sleepval = INV_ICM42600_GYRO_STOP_TIME_MS;
+ }
+ }
+
+ /* deferred sleep value if sleep pointer is provided or direct sleep */
+ if (sleep_ms)
+ *sleep_ms = sleepval;
+ else if (sleepval)
+ msleep(sleepval);
+
+ return 0;
+}
+
+int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep_ms)
+{
+ struct inv_icm42600_sensor_conf *oldconf = &st->conf.accel;
+ unsigned int val;
+ int ret;
+
+ /* Sanitize missing values with current values */
+ if (conf->mode < 0)
+ conf->mode = oldconf->mode;
+ if (conf->fs < 0)
+ conf->fs = oldconf->fs;
+ if (conf->odr < 0)
+ conf->odr = oldconf->odr;
+ if (conf->filter < 0)
+ conf->filter = oldconf->filter;
+
+ /* set ACCEL_CONFIG0 register (accel fullscale & odr) */
+ if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) {
+ val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->fs) |
+ INV_ICM42600_ACCEL_CONFIG0_ODR(conf->odr);
+ ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val);
+ if (ret)
+ return ret;
+ oldconf->fs = conf->fs;
+ oldconf->odr = conf->odr;
+ }
+
+ /* set GYRO_ACCEL_CONFIG0 register (accel filter) */
+ if (conf->filter != oldconf->filter) {
+ val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->filter) |
+ INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(st->conf.gyro.filter);
+ ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
+ if (ret)
+ return ret;
+ oldconf->filter = conf->filter;
+ }
+
+ /* set PWR_MGMT0 register (accel sensor mode) */
+ return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode, conf->mode,
+ st->conf.temp_en, sleep_ms);
+}
+
+int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep_ms)
+{
+ struct inv_icm42600_sensor_conf *oldconf = &st->conf.gyro;
+ unsigned int val;
+ int ret;
+
+ /* sanitize missing values with current values */
+ if (conf->mode < 0)
+ conf->mode = oldconf->mode;
+ if (conf->fs < 0)
+ conf->fs = oldconf->fs;
+ if (conf->odr < 0)
+ conf->odr = oldconf->odr;
+ if (conf->filter < 0)
+ conf->filter = oldconf->filter;
+
+ /* set GYRO_CONFIG0 register (gyro fullscale & odr) */
+ if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) {
+ val = INV_ICM42600_GYRO_CONFIG0_FS(conf->fs) |
+ INV_ICM42600_GYRO_CONFIG0_ODR(conf->odr);
+ ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val);
+ if (ret)
+ return ret;
+ oldconf->fs = conf->fs;
+ oldconf->odr = conf->odr;
+ }
+
+ /* set GYRO_ACCEL_CONFIG0 register (gyro filter) */
+ if (conf->filter != oldconf->filter) {
+ val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(st->conf.accel.filter) |
+ INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->filter);
+ ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
+ if (ret)
+ return ret;
+ oldconf->filter = conf->filter;
+ }
+
+ /* set PWR_MGMT0 register (gyro sensor mode) */
+ return inv_icm42600_set_pwr_mgmt0(st, conf->mode, st->conf.accel.mode,
+ st->conf.temp_en, sleep_ms);
+
+ return 0;
+}
+
+int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
+ unsigned int *sleep_ms)
+{
+ return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
+ st->conf.accel.mode, enable,
+ sleep_ms);
+}
+
+int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
+ unsigned int writeval, unsigned int *readval)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ if (readval)
+ ret = regmap_read(st->map, reg, readval);
+ else
+ ret = regmap_write(st->map, reg, writeval);
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
+static int inv_icm42600_set_conf(struct inv_icm42600_state *st,
+ const struct inv_icm42600_conf *conf)
+{
+ unsigned int val;
+ int ret;
+
+ /* set PWR_MGMT0 register (gyro & accel sensor mode, temp enabled) */
+ val = INV_ICM42600_PWR_MGMT0_GYRO(conf->gyro.mode) |
+ INV_ICM42600_PWR_MGMT0_ACCEL(conf->accel.mode);
+ if (!conf->temp_en)
+ val |= INV_ICM42600_PWR_MGMT0_TEMP_DIS;
+ ret = regmap_write(st->map, INV_ICM42600_REG_PWR_MGMT0, val);
+ if (ret)
+ return ret;
+
+ /* set GYRO_CONFIG0 register (gyro fullscale & odr) */
+ val = INV_ICM42600_GYRO_CONFIG0_FS(conf->gyro.fs) |
+ INV_ICM42600_GYRO_CONFIG0_ODR(conf->gyro.odr);
+ ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_CONFIG0, val);
+ if (ret)
+ return ret;
+
+ /* set ACCEL_CONFIG0 register (accel fullscale & odr) */
+ val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->accel.fs) |
+ INV_ICM42600_ACCEL_CONFIG0_ODR(conf->accel.odr);
+ ret = regmap_write(st->map, INV_ICM42600_REG_ACCEL_CONFIG0, val);
+ if (ret)
+ return ret;
+
+ /* set GYRO_ACCEL_CONFIG0 register (gyro & accel filters) */
+ val = INV_ICM42600_GYRO_ACCEL_CONFIG0_ACCEL_FILT(conf->accel.filter) |
+ INV_ICM42600_GYRO_ACCEL_CONFIG0_GYRO_FILT(conf->gyro.filter);
+ ret = regmap_write(st->map, INV_ICM42600_REG_GYRO_ACCEL_CONFIG0, val);
+ if (ret)
+ return ret;
+
+ /* update internal conf */
+ st->conf = *conf;
+
+ return 0;
+}
+
+/**
+ * inv_icm42600_setup() - check and setup chip
+ * @st: driver internal state
+ * @bus_setup: callback for setting up bus specific registers
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+static int inv_icm42600_setup(struct inv_icm42600_state *st,
+ inv_icm42600_bus_setup bus_setup)
+{
+ const struct inv_icm42600_hw *hw = &inv_icm42600_hw[st->chip];
+ const struct device *dev = regmap_get_device(st->map);
+ unsigned int val;
+ int ret;
+
+ /* check chip self-identification value */
+ ret = regmap_read(st->map, INV_ICM42600_REG_WHOAMI, &val);
+ if (ret)
+ return ret;
+ if (val != hw->whoami) {
+ dev_err(dev, "invalid whoami %#02x expected %#02x (%s)\n",
+ val, hw->whoami, hw->name);
+ return -ENODEV;
+ }
+ st->name = hw->name;
+
+ /* reset to make sure previous state are not there */
+ ret = regmap_write(st->map, INV_ICM42600_REG_DEVICE_CONFIG,
+ INV_ICM42600_DEVICE_CONFIG_SOFT_RESET);
+ if (ret)
+ return ret;
+ msleep(INV_ICM42600_RESET_TIME_MS);
+
+ ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &val);
+ if (ret)
+ return ret;
+ if (!(val & INV_ICM42600_INT_STATUS_RESET_DONE)) {
+ dev_err(dev, "reset error, reset done bit not set\n");
+ return -ENODEV;
+ }
+
+ /* set chip bus configuration */
+ ret = bus_setup(st);
+ if (ret)
+ return ret;
+
+ /* sensor data in big-endian (default) */
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN,
+ INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN);
+ if (ret)
+ return ret;
+
+ return inv_icm42600_set_conf(st, hw->conf);
+}
+
+static int inv_icm42600_enable_regulator_vddio(struct inv_icm42600_state *st)
+{
+ int ret;
+
+ ret = regulator_enable(st->vddio_supply);
+ if (ret)
+ return ret;
+
+ /* wait a little for supply ramp */
+ usleep_range(3000, 4000);
+
+ return 0;
+}
+
+static void inv_icm42600_disable_vdd_reg(void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+ const struct device *dev = regmap_get_device(st->map);
+ int ret;
+
+ ret = regulator_disable(st->vdd_supply);
+ if (ret)
+ dev_err(dev, "failed to disable vdd error %d\n", ret);
+}
+
+static void inv_icm42600_disable_vddio_reg(void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+ const struct device *dev = regmap_get_device(st->map);
+ int ret;
+
+ ret = regulator_disable(st->vddio_supply);
+ if (ret)
+ dev_err(dev, "failed to disable vddio error %d\n", ret);
+}
+
+static void inv_icm42600_disable_pm(void *_data)
+{
+ struct device *dev = _data;
+
+ pm_runtime_put_sync(dev);
+ pm_runtime_disable(dev);
+}
+
+int inv_icm42600_core_probe(struct regmap *regmap, int chip,
+ inv_icm42600_bus_setup bus_setup)
+{
+ struct device *dev = regmap_get_device(regmap);
+ struct inv_icm42600_state *st;
+ int ret;
+
+ if (chip < 0 || chip >= INV_CHIP_NB) {
+ dev_err(dev, "invalid chip = %d\n", chip);
+ return -ENODEV;
+ }
+
+ st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
+ if (!st)
+ return -ENOMEM;
+
+ dev_set_drvdata(dev, st);
+ mutex_init(&st->lock);
+ st->chip = chip;
+ st->map = regmap;
+
+ ret = iio_read_mount_matrix(dev, "mount-matrix", &st->orientation);
+ if (ret) {
+ dev_err(dev, "failed to retrieve mounting matrix %d\n", ret);
+ return ret;
+ }
+
+ st->vdd_supply = devm_regulator_get(dev, "vdd");
+ if (IS_ERR(st->vdd_supply))
+ return PTR_ERR(st->vdd_supply);
+
+ st->vddio_supply = devm_regulator_get(dev, "vddio");
+ if (IS_ERR(st->vddio_supply))
+ return PTR_ERR(st->vddio_supply);
+
+ ret = regulator_enable(st->vdd_supply);
+ if (ret)
+ return ret;
+ msleep(INV_ICM42600_POWER_UP_TIME_MS);
+
+ ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vdd_reg, st);
+ if (ret)
+ return ret;
+
+ ret = inv_icm42600_enable_regulator_vddio(st);
+ if (ret)
+ return ret;
+
+ ret = devm_add_action_or_reset(dev, inv_icm42600_disable_vddio_reg, st);
+ if (ret)
+ return ret;
+
+ /* setup chip registers */
+ ret = inv_icm42600_setup(st, bus_setup);
+ if (ret)
+ return ret;
+
+ /* setup runtime power management */
+ ret = pm_runtime_set_active(dev);
+ if (ret)
+ return ret;
+ pm_runtime_get_noresume(dev);
+ pm_runtime_enable(dev);
+ pm_runtime_set_autosuspend_delay(dev, INV_ICM42600_SUSPEND_DELAY_MS);
+ pm_runtime_use_autosuspend(dev);
+ pm_runtime_put(dev);
+
+ return devm_add_action_or_reset(dev, inv_icm42600_disable_pm, dev);
+}
+EXPORT_SYMBOL_GPL(inv_icm42600_core_probe);
+
+/*
+ * Suspend saves sensors state and turns everything off.
+ * Check first if runtime suspend has not already done the job.
+ */
+static int __maybe_unused inv_icm42600_suspend(struct device *dev)
+{
+ struct inv_icm42600_state *st = dev_get_drvdata(dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ st->suspended.gyro = st->conf.gyro.mode;
+ st->suspended.accel = st->conf.accel.mode;
+ st->suspended.temp = st->conf.temp_en;
+ if (pm_runtime_suspended(dev)) {
+ ret = 0;
+ goto out_unlock;
+ }
+
+ ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
+ INV_ICM42600_SENSOR_MODE_OFF, false,
+ NULL);
+ if (ret)
+ goto out_unlock;
+
+ regulator_disable(st->vddio_supply);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+/*
+ * System resume gets the system back on and restores the sensors state.
+ * Manually put runtime power management in system active state.
+ */
+static int __maybe_unused inv_icm42600_resume(struct device *dev)
+{
+ struct inv_icm42600_state *st = dev_get_drvdata(dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_enable_regulator_vddio(st);
+ if (ret)
+ goto out_unlock;
+
+ pm_runtime_disable(dev);
+ pm_runtime_set_active(dev);
+ pm_runtime_enable(dev);
+
+ /* restore sensors state */
+ ret = inv_icm42600_set_pwr_mgmt0(st, st->suspended.gyro,
+ st->suspended.accel,
+ st->suspended.temp, NULL);
+ if (ret)
+ goto out_unlock;
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+/* Runtime suspend will turn off sensors that are enabled by iio devices. */
+static int __maybe_unused inv_icm42600_runtime_suspend(struct device *dev)
+{
+ struct inv_icm42600_state *st = dev_get_drvdata(dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ /* disable all sensors */
+ ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
+ INV_ICM42600_SENSOR_MODE_OFF, false,
+ NULL);
+ if (ret)
+ goto error_unlock;
+
+ regulator_disable(st->vddio_supply);
+
+error_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+/* Sensors are enabled by iio devices, no need to turn them back on here. */
+static int __maybe_unused inv_icm42600_runtime_resume(struct device *dev)
+{
+ struct inv_icm42600_state *st = dev_get_drvdata(dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_enable_regulator_vddio(st);
+
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+const struct dev_pm_ops inv_icm42600_pm_ops = {
+ SET_SYSTEM_SLEEP_PM_OPS(inv_icm42600_suspend, inv_icm42600_resume)
+ SET_RUNTIME_PM_OPS(inv_icm42600_runtime_suspend,
+ inv_icm42600_runtime_resume, NULL)
+};
+EXPORT_SYMBOL_GPL(inv_icm42600_pm_ops);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-426xx device driver");
+MODULE_LICENSE("GPL");
--
2.17.1
Add 3 modules: inv-icm42600, inv-icm42600-i2c, inv-icm42600-spi.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 1 +
drivers/iio/imu/inv_icm42600/Kconfig | 28 +++++++++++++++++++++++++++
drivers/iio/imu/inv_icm42600/Makefile | 13 +++++++++++++
4 files changed, 43 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/Kconfig
create mode 100644 drivers/iio/imu/inv_icm42600/Makefile
diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig
index fc4123d518bc..f02883b08480 100644
--- a/drivers/iio/imu/Kconfig
+++ b/drivers/iio/imu/Kconfig
@@ -91,6 +91,7 @@ config KMX61
To compile this driver as module, choose M here: the module will
be called kmx61.
+source "drivers/iio/imu/inv_icm42600/Kconfig"
source "drivers/iio/imu/inv_mpu6050/Kconfig"
source "drivers/iio/imu/st_lsm6dsx/Kconfig"
diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile
index 88b2c4555230..13e9ff442b11 100644
--- a/drivers/iio/imu/Makefile
+++ b/drivers/iio/imu/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_FXOS8700) += fxos8700_core.o
obj-$(CONFIG_FXOS8700_I2C) += fxos8700_i2c.o
obj-$(CONFIG_FXOS8700_SPI) += fxos8700_spi.o
+obj-y += inv_icm42600/
obj-y += inv_mpu6050/
obj-$(CONFIG_KMX61) += kmx61.o
diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
new file mode 100644
index 000000000000..22390a72f0a3
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -0,0 +1,28 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+config INV_ICM42600
+ tristate
+
+config INV_ICM42600_I2C
+ tristate "InvenSense ICM-426xx I2C driver"
+ depends on I2C
+ select INV_ICM42600
+ select REGMAP_I2C
+ help
+ This driver supports the InvenSense ICM-426xx motion tracking
+ devices over I2C.
+
+ This driver can be built as a module. The module will be called
+ inv-icm42600-i2c.
+
+config INV_ICM42600_SPI
+ tristate "InvenSense ICM-426xx SPI driver"
+ depends on SPI_MASTER
+ select INV_ICM42600
+ select REGMAP_SPI
+ help
+ This driver supports the InvenSense ICM-426xx motion tracking
+ devices over SPI.
+
+ This driver can be built as a module. The module will be called
+ inv-icm42600-spi.
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
new file mode 100644
index 000000000000..48965824f00c
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+obj-$(CONFIG_INV_ICM42600) += inv-icm42600.o
+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
+
+obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
+inv-icm42600-i2c-y += inv_icm42600_i2c.o
+
+obj-$(CONFIG_INV_ICM42600_SPI) += inv-icm42600-spi.o
+inv-icm42600-spi-y += inv_icm42600_spi.o
--
2.17.1
Hardware offset available as calibscale sysfs attributes are real
physical values exprimed in SI units.
calibscale_available sysfs attributes represents the range of
acceptable values.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../ABI/testing/sysfs-bus-iio-icm42600 | 20 +++++++++++++++++++
1 file changed, 20 insertions(+)
create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
diff --git a/Documentation/ABI/testing/sysfs-bus-iio-icm42600 b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
new file mode 100644
index 000000000000..0bf1fd4f5bf1
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
@@ -0,0 +1,20 @@
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_x_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_y_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_z_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibbias
+KernelVersion: 5.8
+Contact: [email protected]
+Description:
+ Hardware applied calibration offset (assumed to fix production
+ inaccuracies). Values represent a real physical offset expressed
+ in SI units (m/s^2 for accelerometer and rad/s for gyroscope).
+
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_calibbias_available
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_calibbias_available
+KernelVersion: 5.8
+Contact: [email protected]
+Description:
+ Range of available values for hardware offset. Values in SI
+ units (m/s^2 for accelerometer and rad/s for gyroscope).
--
2.17.1
Add IIO device for accelerometer sensor with data polling
interface.
Attributes: raw, scale, sampling_frequency, calibbias.
Accelerometer in low noise mode.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 592 ++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 4 +
3 files changed, 600 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index d155470d770a..3b190461a2b6 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -121,6 +121,7 @@ struct inv_icm42600_suspended {
* @conf: chip sensors configurations.
* @suspended: suspended sensors configuration.
* @indio_gyro: gyroscope IIO device.
+ * @indio_accel: accelerometer IIO device.
* @buffer: data transfer buffer aligned for DMA.
*/
struct inv_icm42600_state {
@@ -134,6 +135,7 @@ struct inv_icm42600_state {
struct inv_icm42600_conf conf;
struct inv_icm42600_suspended suspended;
struct iio_dev *indio_gyro;
+ struct iio_dev *indio_accel;
uint8_t buffer[2] ____cacheline_aligned;
};
@@ -375,4 +377,6 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
new file mode 100644
index 000000000000..717c6b0869fc
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -0,0 +1,592 @@
+// 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 "inv_icm42600.h"
+
+#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
+ { \
+ .type = IIO_ACCEL, \
+ .modified = 1, \
+ .channel2 = _modifier, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_RAW) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_type = \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .info_mask_shared_by_type_available = \
+ BIT(IIO_CHAN_INFO_SCALE) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_all = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .info_mask_shared_by_all_available = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_BE, \
+ }, \
+ .ext_info = _ext_info, \
+ }
+
+enum inv_icm42600_accel_scan {
+ INV_ICM42600_ACCEL_SCAN_X,
+ INV_ICM42600_ACCEL_SCAN_Y,
+ INV_ICM42600_ACCEL_SCAN_Z,
+};
+
+static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
+ IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
+ {},
+};
+
+static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
+ inv_icm42600_accel_ext_infos),
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
+ inv_icm42600_accel_ext_infos),
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
+ inv_icm42600_accel_ext_infos),
+};
+
+static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int reg;
+ __be16 *data;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_ACCEL_DATA_X;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_ACCEL_DATA_Y;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_ACCEL_DATA_Z;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ /* enable accel sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ if (ret)
+ goto exit;
+
+ /* read accel register data */
+ data = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpup(data);
+ if (*val == INV_ICM42600_DATA_INVALID)
+ ret = -EINVAL;
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+/* IIO format int + nano */
+static const int inv_icm42600_accel_scale[] = {
+ /* +/- 16G => 0.004788403 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
+ /* +/- 8G => 0.002394202 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
+ /* +/- 4G => 0.001197101 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
+ /* +/- 2G => 0.000598550 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
+};
+
+static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int idx;
+
+ idx = st->conf.accel.fs;
+
+ *val = inv_icm42600_accel_scale[2 * idx];
+ *val2 = inv_icm42600_accel_scale[2 * idx + 1];
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) {
+ if (val == inv_icm42600_accel_scale[idx] &&
+ val2 == inv_icm42600_accel_scale[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale))
+ return -EINVAL;
+
+ conf.fs = idx / 2;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/* IIO format int + micro */
+static const int inv_icm42600_accel_odr[] = {
+ /* 12.5Hz */
+ 12, 500000,
+ /* 25Hz */
+ 25, 0,
+ /* 50Hz */
+ 50, 0,
+ /* 100Hz */
+ 100, 0,
+ /* 200Hz */
+ 200, 0,
+ /* 1kHz */
+ 1000, 0,
+ /* 2kHz */
+ 2000, 0,
+ /* 4kHz */
+ 4000, 0,
+};
+
+static const int inv_icm42600_accel_odr_conv[] = {
+ INV_ICM42600_ODR_12_5HZ,
+ INV_ICM42600_ODR_25HZ,
+ INV_ICM42600_ODR_50HZ,
+ INV_ICM42600_ODR_100HZ,
+ INV_ICM42600_ODR_200HZ,
+ INV_ICM42600_ODR_1KHZ_LN,
+ INV_ICM42600_ODR_2KHZ_LN,
+ INV_ICM42600_ODR_4KHZ_LN,
+};
+
+static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int odr;
+ unsigned int i;
+
+ odr = st->conf.accel.odr;
+
+ for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
+ if (inv_icm42600_accel_odr_conv[i] == odr)
+ break;
+ }
+ if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
+ return -EINVAL;
+
+ *val = inv_icm42600_accel_odr[2 * i];
+ *val2 = inv_icm42600_accel_odr[2 * i + 1];
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
+ if (val == inv_icm42600_accel_odr[idx] &&
+ val2 == inv_icm42600_accel_odr[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
+ return -EINVAL;
+
+ conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/*
+ * Calibration bias values, IIO range format int + micro.
+ * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
+ */
+static int inv_icm42600_accel_calibbias[] = {
+ -10, 42010, /* min: -10.042010 m/s² */
+ 0, 4903, /* step: 0.004903 m/s² */
+ 10, 37106, /* max: 10.037106 m/s² */
+};
+
+static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t bias;
+ unsigned int reg;
+ int16_t offset;
+ uint8_t data[2];
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER4;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER6;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER7;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
+ memcpy(data, st->buffer, sizeof(data));
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ if (ret)
+ return ret;
+
+ /* 12 bits signed value */
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ case IIO_MOD_Y:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ case IIO_MOD_Z:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /*
+ * convert raw offset to g then to m/s²
+ * 12 bits signed raw step 0.5mg to g: 5 / 10000
+ * g to m/s²: 9.806650
+ * result in micro (1000000)
+ * (offset * 5 * 9.806650 * 1000000) / 10000
+ */
+ val64 = (int64_t)offset * 5LL * 9806650LL;
+ /* for rounding, add + or - divisor (10000) divided by 2 */
+ if (val64 >= 0)
+ val64 += 10000LL / 2LL;
+ else
+ val64 -= 10000LL / 2LL;
+ bias = div_s64(val64, 10000L);
+ *val = bias / 1000000L;
+ *val2 = bias % 1000000L;
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t min, max;
+ unsigned int reg, regval;
+ int16_t offset;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER4;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER6;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER7;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* inv_icm42600_accel_calibbias: min - step - max in micro */
+ min = inv_icm42600_accel_calibbias[0] * 1000000L +
+ inv_icm42600_accel_calibbias[1];
+ max = inv_icm42600_accel_calibbias[4] * 1000000L +
+ inv_icm42600_accel_calibbias[5];
+ val64 = (int64_t)val * 1000000LL + (int64_t)val2;
+ if (val64 < min || val64 > max)
+ return -EINVAL;
+
+ /*
+ * convert m/s² to g then to raw value
+ * m/s² to g: 1 / 9.806650
+ * g to raw 12 bits signed, step 0.5mg: 10000 / 5
+ * val in micro (1000000)
+ * val * 10000 / (9.806650 * 1000000 * 5)
+ */
+ val64 = val64 * 10000LL;
+ /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
+ if (val64 >= 0)
+ val64 += 9806650 * 5 / 2;
+ else
+ val64 -= 9806650 * 5 / 2;
+ offset = div_s64(val64, 9806650 * 5);
+
+ /* clamp value limited to 12 bits signed */
+ if (offset < -2048)
+ offset = -2048;
+ else if (offset > 2047)
+ offset = 2047;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ /* OFFSET_USER4 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ case IIO_MOD_Y:
+ /* OFFSET_USER7 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
+ break;
+ case IIO_MOD_Z:
+ /* OFFSET_USER7 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int16_t data;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_read_sensor(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ case IIO_CHAN_INFO_SCALE:
+ return inv_icm42600_accel_read_scale(st, val, val2);
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_accel_read_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42600_accel_read_offset(st, chan, val, val2);
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ const int **vals,
+ int *type, int *length, long mask)
+{
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ *vals = inv_icm42600_accel_scale;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ *length = ARRAY_SIZE(inv_icm42600_accel_scale);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ *vals = inv_icm42600_accel_odr;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ *length = ARRAY_SIZE(inv_icm42600_accel_odr);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ *vals = inv_icm42600_accel_calibbias;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ return IIO_AVAIL_RANGE;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_write_scale(st, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_accel_write_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ long mask)
+{
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ return IIO_VAL_INT_PLUS_NANO;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return IIO_VAL_INT_PLUS_MICRO;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return IIO_VAL_INT_PLUS_MICRO;
+ default:
+ return -EINVAL;
+ }
+}
+
+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,
+};
+
+struct iio_dev *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)
+ return ERR_PTR(-ENOMEM);
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return ERR_PTR(-ENOMEM);
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_accel_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_accel_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+
+ ret = devm_iio_device_register(dev, indio_dev);
+ if (ret)
+ return ERR_PTR(ret);
+
+ return indio_dev;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 84e9ff320b3b..a35ff21f50bb 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -513,6 +513,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (IS_ERR(st->indio_gyro))
return PTR_ERR(st->indio_gyro);
+ st->indio_accel = inv_icm42600_accel_init(st);
+ if (IS_ERR(st->indio_accel))
+ return PTR_ERR(st->indio_accel);
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
--
2.17.1
Add MAINTAINERS entry for InvenSense ICM-426xx IMU device.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
MAINTAINERS | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/MAINTAINERS b/MAINTAINERS
index 60ed2963efaa..cd8b5fece94d 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -8861,6 +8861,14 @@ F: include/dt-bindings/interconnect/
F: include/linux/interconnect-provider.h
F: include/linux/interconnect.h
+INVENSENSE ICM-426xx IMU DRIVER
+M: Jean-Baptiste Maneyrol <[email protected]>
+L: [email protected]
+S: Maintained
+W https://invensense.tdk.com/
+F: Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
+F: drivers/iio/imu/inv_icm42600/
+
INVENSENSE MPU-3050 GYROSCOPE DRIVER
M: Linus Walleij <[email protected]>
L: [email protected]
--
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, 396 insertions(+), 15 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 7b52d92739c3..c0f5059b13b3 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 77cdad99de91..e51e15029116 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,16 +67,18 @@ 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: size must be a power of 2
- * 8 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding
+ * IIO buffer data: size must be a power of 2 and timestamp aligned
+ * 16 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding + 8 bytes timestamp
*/
struct inv_icm42600_accel_buffer {
struct inv_icm42600_fifo_sensor_data accel;
int8_t temp;
uint8_t padding;
+ int64_t timestamp __aligned(8);
};
#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
@@ -95,6 +99,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;
@@ -122,6 +127,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;
@@ -302,9 +308,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;
@@ -323,6 +331,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;
@@ -612,7 +625,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)
@@ -695,6 +708,7 @@ struct iio_dev *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;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
int ret;
@@ -703,7 +717,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!name)
return ERR_PTR(-ENOMEM);
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return ERR_PTR(-ENOMEM);
@@ -711,6 +725,9 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!buffer)
return ERR_PTR(-ENOMEM);
+ ts = iio_priv(indio_dev);
+ inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name;
indio_dev->info = &inv_icm42600_accel_info;
@@ -732,17 +749,20 @@ struct iio_dev *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;
/* buffer is copied to userspace, set padding to 0 to avoid data leak */
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 */
@@ -753,10 +773,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 e58e6f0c5698..99576b2c171f 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -13,6 +13,7 @@
#include <linux/iio/buffer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
#include "inv_icm42600_buffer.h"
/* FIFO header: 1 byte */
@@ -374,6 +375,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;
@@ -383,9 +385,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;
}
@@ -413,6 +417,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);
@@ -498,17 +504,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)
@@ -521,8 +536,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;
@@ -531,12 +551,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 9e1f6e65fd45..8bd77185ccb7 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[] = {
{
@@ -412,6 +413,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;
@@ -495,7 +506,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);
}
@@ -617,6 +628,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 d53c604ae1de..1924b0b528df 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,16 +67,18 @@ 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: size must be a power of 2
- * 8 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding
+ * IIO buffer data: size must be a power of 2 and timestamp aligned
+ * 16 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding + 8 bytes timestamp
*/
struct inv_icm42600_gyro_buffer {
struct inv_icm42600_fifo_sensor_data gyro;
int8_t temp;
uint8_t padding;
+ int64_t timestamp __aligned(8);
};
#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
@@ -95,6 +99,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;
@@ -122,6 +127,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;
@@ -314,9 +320,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;
@@ -335,6 +343,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;
@@ -623,7 +636,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)
@@ -706,6 +719,7 @@ struct iio_dev *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;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
int ret;
@@ -714,7 +728,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!name)
return ERR_PTR(-ENOMEM);
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return ERR_PTR(-ENOMEM);
@@ -722,6 +736,9 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!buffer)
return ERR_PTR(-ENOMEM);
+ ts = iio_priv(indio_dev);
+ inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name;
indio_dev->info = &inv_icm42600_gyro_info;
@@ -743,17 +760,20 @@ struct iio_dev *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;
/* buffer is copied to userspace, set padding to 0 to avoid data leak */
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 */
@@ -764,10 +784,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 temperature channel in gyroscope and accelerometer devices.
Temperature is available in full 16 bits resolution as a processed
channel. Scale and offset attributes are also provided for the low
8 bits resolution raw temperature found in the FIFO.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 11 ++-
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 11 ++-
.../iio/imu/inv_icm42600/inv_icm42600_temp.c | 87 +++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 +++++++
4 files changed, 137 insertions(+), 2 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 717c6b0869fc..3f214df44093 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -13,6 +13,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -45,6 +46,7 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_X,
INV_ICM42600_ACCEL_SCAN_Y,
INV_ICM42600_ACCEL_SCAN_Z,
+ INV_ICM42600_ACCEL_SCAN_TEMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
@@ -59,6 +61,7 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
inv_icm42600_accel_ext_infos),
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),
};
static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
@@ -450,8 +453,14 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
int16_t data;
int ret;
- if (chan->type != IIO_ACCEL)
+ switch (chan->type) {
+ case IIO_ACCEL:
+ break;
+ case IIO_TEMP:
+ return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
+ default:
return -EINVAL;
+ }
switch (mask) {
case IIO_CHAN_INFO_RAW:
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index 3875ecbee67e..6a0e7661fa48 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -13,6 +13,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -45,6 +46,7 @@ enum inv_icm42600_gyro_scan {
INV_ICM42600_GYRO_SCAN_X,
INV_ICM42600_GYRO_SCAN_Y,
INV_ICM42600_GYRO_SCAN_Z,
+ INV_ICM42600_GYRO_SCAN_TEMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
@@ -59,6 +61,7 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
inv_icm42600_gyro_ext_infos),
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),
};
static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
@@ -461,8 +464,14 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
int16_t data;
int ret;
- if (chan->type != IIO_ANGL_VEL)
+ switch (chan->type) {
+ case IIO_ANGL_VEL:
+ break;
+ case IIO_TEMP:
+ return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
+ default:
return -EINVAL;
+ }
switch (mask) {
case IIO_CHAN_INFO_RAW:
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
new file mode 100644
index 000000000000..b0871352fe39
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
@@ -0,0 +1,87 @@
+// 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/math64.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
+
+static int inv_icm42600_temp_read(struct inv_icm42600_state *st, int32_t *temp)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t data;
+ __be16 *raw;
+ int16_t val;
+ int ret;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_temp_conf(st, true, NULL);
+ if (ret)
+ goto exit;
+
+ raw = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TEMP_DATA, raw, sizeof(*raw));
+ if (ret)
+ goto exit;
+
+ val = (int16_t)be16_to_cpup(raw);
+ if (val == INV_ICM42600_DATA_INVALID) {
+ ret = -EINVAL;
+ goto exit;
+ }
+ /*
+ * T°C = (val / 132.48) + 25 = ((val * 100) / 13248) + 25
+ * Tm°C = (val * 100 * 1000) / 13248 + 25000
+ */
+ data = (int64_t)(val) * 100LL * 1000LL;
+ *temp = div_s64(data, 13248) + 25000;
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+int inv_icm42600_temp_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int32_t temp;
+ int ret;
+
+ if (chan->type != IIO_TEMP)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_PROCESSED:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_temp_read(st, &temp);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = temp;
+ return IIO_VAL_INT;
+ case IIO_CHAN_INFO_SCALE:
+ *val = 483;
+ *val2 = 91787;
+ return IIO_VAL_INT_PLUS_MICRO;
+ case IIO_CHAN_INFO_OFFSET:
+ *val = 25000;
+ return IIO_VAL_INT;
+ default:
+ return -EINVAL;
+ }
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
new file mode 100644
index 000000000000..2c3a932faa94
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_TEMP_H_
+#define INV_ICM42600_TEMP_H_
+
+#include <linux/iio/iio.h>
+
+#define INV_ICM42600_TEMP_CHAN(_index) \
+ { \
+ .type = IIO_TEMP, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_PROCESSED) | \
+ BIT(IIO_CHAN_INFO_OFFSET) | \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 8, \
+ .storagebits = 8, \
+ }, \
+ }
+
+int inv_icm42600_temp_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask);
+
+#endif
--
2.17.1
Add INT1 interrupt support. Support interrupt edge and level,
active high or low. Push-pull or open-drain configurations.
Interrupt will be used to read data from the FIFO.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 2 +-
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 101 +++++++++++++++++-
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 3 +-
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 3 +-
4 files changed, 105 insertions(+), 4 deletions(-)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 3b190461a2b6..148894c888cc 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -372,7 +372,7 @@ int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
unsigned int writeval, unsigned int *readval);
-int inv_icm42600_core_probe(struct regmap *regmap, int chip,
+int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
inv_icm42600_bus_setup bus_setup);
struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index a35ff21f50bb..95b2a6d91e5b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -9,8 +9,11 @@
#include <linux/slab.h>
#include <linux/delay.h>
#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
#include <linux/regulator/consumer.h>
#include <linux/pm_runtime.h>
+#include <linux/property.h>
#include <linux/regmap.h>
#include <linux/iio/iio.h>
@@ -408,6 +411,82 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st,
return inv_icm42600_set_conf(st, hw->conf);
}
+static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int status;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ ret = regmap_read(st->map, INV_ICM42600_REG_INT_STATUS, &status);
+ if (ret)
+ goto out_unlock;
+
+ /* FIFO full */
+ if (status & INV_ICM42600_INT_STATUS_FIFO_FULL)
+ dev_warn(dev, "FIFO full data lost!\n");
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ return IRQ_HANDLED;
+}
+
+/**
+ * inv_icm42600_irq_init() - initialize int pin and interrupt handler
+ * @st: driver internal state
+ * @irq: irq number
+ * @irq_type: irq trigger type
+ * @open_drain: true if irq is open drain, false for push-pull
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq,
+ int irq_type, bool open_drain)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int val;
+ int ret;
+
+ /* configure INT1 interrupt: default is active low on edge */
+ switch (irq_type) {
+ case IRQF_TRIGGER_RISING:
+ case IRQF_TRIGGER_HIGH:
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH;
+ break;
+ default:
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW;
+ break;
+ }
+
+ switch (irq_type) {
+ case IRQF_TRIGGER_LOW:
+ case IRQF_TRIGGER_HIGH:
+ val |= INV_ICM42600_INT_CONFIG_INT1_LATCHED;
+ break;
+ default:
+ break;
+ }
+
+ if (!open_drain)
+ val |= INV_ICM42600_INT_CONFIG_INT1_PUSH_PULL;
+
+ ret = regmap_write(st->map, INV_ICM42600_REG_INT_CONFIG, val);
+ if (ret)
+ return ret;
+
+ /* Deassert async reset for proper INT pin operation (cf datasheet) */
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_CONFIG1,
+ INV_ICM42600_INT_CONFIG1_ASYNC_RESET, 0);
+ if (ret)
+ return ret;
+
+ return devm_request_threaded_irq(dev, irq, NULL,
+ inv_icm42600_irq_handler, irq_type,
+ "inv_icm42600", st);
+}
+
static int inv_icm42600_enable_regulator_vddio(struct inv_icm42600_state *st)
{
int ret;
@@ -452,11 +531,14 @@ static void inv_icm42600_disable_pm(void *_data)
pm_runtime_disable(dev);
}
-int inv_icm42600_core_probe(struct regmap *regmap, int chip,
+int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
inv_icm42600_bus_setup bus_setup)
{
struct device *dev = regmap_get_device(regmap);
struct inv_icm42600_state *st;
+ struct irq_data *irq_desc;
+ int irq_type;
+ bool open_drain;
int ret;
if (chip < 0 || chip >= INV_CHIP_NB) {
@@ -464,6 +546,19 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
return -ENODEV;
}
+ /* get irq properties, set trigger falling by default */
+ irq_desc = irq_get_irq_data(irq);
+ if (!irq_desc) {
+ dev_err(dev, "could not find IRQ %d\n", irq);
+ return -EINVAL;
+ }
+
+ irq_type = irqd_get_trigger_type(irq_desc);
+ if (!irq_type)
+ irq_type = IRQF_TRIGGER_FALLING;
+
+ open_drain = device_property_read_bool(dev, "drive-open-drain");
+
st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
if (!st)
return -ENOMEM;
@@ -517,6 +612,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (IS_ERR(st->indio_accel))
return PTR_ERR(st->indio_accel);
+ ret = inv_icm42600_irq_init(st, irq, irq_type, open_drain);
+ if (ret)
+ return ret;
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
index 4789cead23b3..85b1934cec60 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
@@ -64,7 +64,8 @@ static int inv_icm42600_probe(struct i2c_client *client)
if (IS_ERR(regmap))
return PTR_ERR(regmap);
- return inv_icm42600_core_probe(regmap, chip, inv_icm42600_i2c_bus_setup);
+ return inv_icm42600_core_probe(regmap, chip, client->irq,
+ inv_icm42600_i2c_bus_setup);
}
static const struct of_device_id inv_icm42600_of_matches[] = {
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
index a9c5e2fdbe2a..323789697a08 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
@@ -63,7 +63,8 @@ static int inv_icm42600_probe(struct spi_device *spi)
if (IS_ERR(regmap))
return PTR_ERR(regmap);
- return inv_icm42600_core_probe(regmap, chip, inv_icm42600_spi_bus_setup);
+ return inv_icm42600_core_probe(regmap, chip, spi->irq,
+ inv_icm42600_spi_bus_setup);
}
static const struct of_device_id inv_icm42600_of_matches[] = {
--
2.17.1
Add SPI driver for InvenSense ICM-426xxx devices.
Configure bus signal slew rates as indicated in the datasheet.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 99 +++++++++++++++++++
1 file changed, 99 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
new file mode 100644
index 000000000000..a9c5e2fdbe2a
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
@@ -0,0 +1,99 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 InvenSense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/spi/spi.h>
+#include <linux/regmap.h>
+#include <linux/property.h>
+
+#include "inv_icm42600.h"
+
+static int inv_icm42600_spi_bus_setup(struct inv_icm42600_state *st)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /* setup interface registers */
+ val = INV_ICM42600_INTF_CONFIG6_I3C_EN |
+ INV_ICM42600_INTF_CONFIG6_I3C_SDR_EN |
+ INV_ICM42600_INTF_CONFIG6_I3C_DDR_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
+ INV_ICM42600_INTF_CONFIG6_MASK, val);
+ if (ret)
+ return ret;
+
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
+ INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY, 0);
+ if (ret)
+ return ret;
+
+ /* set slew rates for I2C and SPI */
+ mask = INV_ICM42600_DRIVE_CONFIG_I2C_MASK |
+ INV_ICM42600_DRIVE_CONFIG_SPI_MASK;
+ val = INV_ICM42600_DRIVE_CONFIG_I2C(INV_ICM42600_SLEW_RATE_20_60NS) |
+ INV_ICM42600_DRIVE_CONFIG_SPI(INV_ICM42600_SLEW_RATE_INF_2NS);
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_DRIVE_CONFIG,
+ mask, val);
+ if (ret)
+ return ret;
+
+ /* disable i2c bus */
+ return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK,
+ INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_I2C_DIS);
+}
+
+static int inv_icm42600_probe(struct spi_device *spi)
+{
+ const void *match;
+ enum inv_icm42600_chip chip;
+ struct regmap *regmap;
+
+ match = device_get_match_data(&spi->dev);
+ if (!match)
+ return -EINVAL;
+ chip = (enum inv_icm42600_chip)match;
+
+ regmap = devm_regmap_init_spi(spi, &inv_icm42600_regmap_config);
+ if (IS_ERR(regmap))
+ return PTR_ERR(regmap);
+
+ return inv_icm42600_core_probe(regmap, chip, inv_icm42600_spi_bus_setup);
+}
+
+static const struct of_device_id inv_icm42600_of_matches[] = {
+ {
+ .compatible = "invensense,icm42600",
+ .data = (void *)INV_CHIP_ICM42600,
+ }, {
+ .compatible = "invensense,icm42602",
+ .data = (void *)INV_CHIP_ICM42602,
+ }, {
+ .compatible = "invensense,icm42605",
+ .data = (void *)INV_CHIP_ICM42605,
+ }, {
+ .compatible = "invensense,icm42622",
+ .data = (void *)INV_CHIP_ICM42622,
+ },
+ {}
+};
+MODULE_DEVICE_TABLE(of, inv_icm42600_of_matches);
+
+static struct spi_driver inv_icm42600_driver = {
+ .driver = {
+ .name = "inv-icm42600-spi",
+ .of_match_table = inv_icm42600_of_matches,
+ .pm = &inv_icm42600_pm_ops,
+ },
+ .probe = inv_icm42600_probe,
+};
+module_spi_driver(inv_icm42600_driver);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-426xx SPI driver");
+MODULE_LICENSE("GPL");
--
2.17.1
Add IIO device for gyroscope sensor with data polling interface.
Attributes: raw, scale, sampling_frequency, calibbias.
Gyroscope in low noise mode.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 6 +
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 603 ++++++++++++++++++
3 files changed, 613 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 14c8ef152418..d155470d770a 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -120,6 +120,8 @@ struct inv_icm42600_suspended {
* @orientation: sensor chip orientation relative to main hardware.
* @conf: chip sensors configurations.
* @suspended: suspended sensors configuration.
+ * @indio_gyro: gyroscope IIO device.
+ * @buffer: data transfer buffer aligned for DMA.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -131,6 +133,8 @@ struct inv_icm42600_state {
struct iio_mount_matrix orientation;
struct inv_icm42600_conf conf;
struct inv_icm42600_suspended suspended;
+ struct iio_dev *indio_gyro;
+ uint8_t buffer[2] ____cacheline_aligned;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -369,4 +373,6 @@ int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
int inv_icm42600_core_probe(struct regmap *regmap, int chip,
inv_icm42600_bus_setup bus_setup);
+struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 2eb25c5f77f8..84e9ff320b3b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -509,6 +509,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (ret)
return ret;
+ st->indio_gyro = inv_icm42600_gyro_init(st);
+ if (IS_ERR(st->indio_gyro))
+ return PTR_ERR(st->indio_gyro);
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
new file mode 100644
index 000000000000..3875ecbee67e
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -0,0 +1,603 @@
+// 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 "inv_icm42600.h"
+
+#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
+ { \
+ .type = IIO_ANGL_VEL, \
+ .modified = 1, \
+ .channel2 = _modifier, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_RAW) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_type = \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .info_mask_shared_by_type_available = \
+ BIT(IIO_CHAN_INFO_SCALE) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_all = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .info_mask_shared_by_all_available = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_BE, \
+ }, \
+ .ext_info = _ext_info, \
+ }
+
+enum inv_icm42600_gyro_scan {
+ INV_ICM42600_GYRO_SCAN_X,
+ INV_ICM42600_GYRO_SCAN_Y,
+ INV_ICM42600_GYRO_SCAN_Z,
+};
+
+static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
+ IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
+ {},
+};
+
+static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X,
+ inv_icm42600_gyro_ext_infos),
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y,
+ inv_icm42600_gyro_ext_infos),
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
+ inv_icm42600_gyro_ext_infos),
+};
+
+static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int reg;
+ __be16 *data;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_GYRO_DATA_X;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_GYRO_DATA_Y;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_GYRO_DATA_Z;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ /* enable gyro sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ if (ret)
+ goto exit;
+
+ /* read gyro register data */
+ data = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpup(data);
+ if (*val == INV_ICM42600_DATA_INVALID)
+ ret = -EINVAL;
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+/* IIO format int + nano */
+static const int inv_icm42600_gyro_scale[] = {
+ /* +/- 2000dps => 0.001065264 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
+ /* +/- 1000dps => 0.000532632 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
+ /* +/- 500dps => 0.000266316 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
+ /* +/- 250dps => 0.000133158 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
+ /* +/- 125dps => 0.000066579 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
+ /* +/- 62.5dps => 0.000033290 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
+ /* +/- 31.25dps => 0.000016645 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
+ /* +/- 15.625dps => 0.000008322 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
+};
+
+static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int idx;
+
+ idx = st->conf.gyro.fs;
+
+ *val = inv_icm42600_gyro_scale[2 * idx];
+ *val2 = inv_icm42600_gyro_scale[2 * idx + 1];
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) {
+ if (val == inv_icm42600_gyro_scale[idx] &&
+ val2 == inv_icm42600_gyro_scale[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
+ return -EINVAL;
+
+ conf.fs = idx / 2;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/* IIO format int + micro */
+static const int inv_icm42600_gyro_odr[] = {
+ /* 12.5Hz */
+ 12, 500000,
+ /* 25Hz */
+ 25, 0,
+ /* 50Hz */
+ 50, 0,
+ /* 100Hz */
+ 100, 0,
+ /* 200Hz */
+ 200, 0,
+ /* 1kHz */
+ 1000, 0,
+ /* 2kHz */
+ 2000, 0,
+ /* 4kHz */
+ 4000, 0,
+};
+
+static const int inv_icm42600_gyro_odr_conv[] = {
+ INV_ICM42600_ODR_12_5HZ,
+ INV_ICM42600_ODR_25HZ,
+ INV_ICM42600_ODR_50HZ,
+ INV_ICM42600_ODR_100HZ,
+ INV_ICM42600_ODR_200HZ,
+ INV_ICM42600_ODR_1KHZ_LN,
+ INV_ICM42600_ODR_2KHZ_LN,
+ INV_ICM42600_ODR_4KHZ_LN,
+};
+
+static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int odr;
+ unsigned int i;
+
+ odr = st->conf.gyro.odr;
+
+ for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
+ if (inv_icm42600_gyro_odr_conv[i] == odr)
+ break;
+ }
+ if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
+ return -EINVAL;
+
+ *val = inv_icm42600_gyro_odr[2 * i];
+ *val2 = inv_icm42600_gyro_odr[2 * i + 1];
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) {
+ if (val == inv_icm42600_gyro_odr[idx] &&
+ val2 == inv_icm42600_gyro_odr[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
+ return -EINVAL;
+
+ conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/*
+ * Calibration bias values, IIO range format int + nano.
+ * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
+ */
+static int inv_icm42600_gyro_calibbias[] = {
+ -1, 117010721, /* min: -1.117010721 rad/s */
+ 0, 545415, /* step: 0.000545415 rad/s */
+ 1, 116465306, /* max: 1.116465306 rad/s */
+};
+
+static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t bias;
+ unsigned int reg;
+ int16_t offset;
+ uint8_t data[2];
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER0;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER1;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER3;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
+ memcpy(data, st->buffer, sizeof(data));
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ if (ret)
+ return ret;
+
+ /* 12 bits signed value */
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ case IIO_MOD_Y:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ case IIO_MOD_Z:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /*
+ * convert raw offset to dps then to rad/s
+ * 12 bits signed raw max 64 to dps: 64 / 2048
+ * dps to rad: Pi / 180
+ * result in nano (1000000000)
+ * (offset * 64 * Pi * 1000000000) / (2048 * 180)
+ */
+ val64 = (int64_t)offset * 64LL * 3141592653LL;
+ /* for rounding, add + or - divisor (2048 * 180) divided by 2 */
+ if (val64 >= 0)
+ val64 += 2048 * 180 / 2;
+ else
+ val64 -= 2048 * 180 / 2;
+ bias = div_s64(val64, 2048 * 180);
+ *val = bias / 1000000000L;
+ *val2 = bias % 1000000000L;
+
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64, min, max;
+ unsigned int reg, regval;
+ int16_t offset;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER0;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER1;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER3;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* inv_icm42600_gyro_calibbias: min - step - max in nano */
+ min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL +
+ (int64_t)inv_icm42600_gyro_calibbias[1];
+ max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL +
+ (int64_t)inv_icm42600_gyro_calibbias[5];
+ val64 = (int64_t)val * 1000000000LL + (int64_t)val2;
+ if (val64 < min || val64 > max)
+ return -EINVAL;
+
+ /*
+ * convert rad/s to dps then to raw value
+ * rad to dps: 180 / Pi
+ * dps to raw 12 bits signed, max 64: 2048 / 64
+ * val in nano (1000000000)
+ * val * 180 * 2048 / (Pi * 1000000000 * 64)
+ */
+ val64 = val64 * 180LL * 2048LL;
+ /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
+ if (val64 >= 0)
+ val64 += 3141592653LL * 64LL / 2LL;
+ else
+ val64 -= 3141592653LL * 64LL / 2LL;
+ offset = div64_s64(val64, 3141592653LL * 64LL);
+
+ /* clamp value limited to 12 bits signed */
+ if (offset < -2048)
+ offset = -2048;
+ else if (offset > 2047)
+ offset = 2047;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ /* OFFSET_USER1 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
+ break;
+ case IIO_MOD_Y:
+ /* OFFSET_USER1 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ case IIO_MOD_Z:
+ /* OFFSET_USER4 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int16_t data;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ case IIO_CHAN_INFO_SCALE:
+ return inv_icm42600_gyro_read_scale(st, val, val2);
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_gyro_read_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42600_gyro_read_offset(st, chan, val, val2);
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ const int **vals,
+ int *type, int *length, long mask)
+{
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ *vals = inv_icm42600_gyro_scale;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ *length = ARRAY_SIZE(inv_icm42600_gyro_scale);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ *vals = inv_icm42600_gyro_odr;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ *length = ARRAY_SIZE(inv_icm42600_gyro_odr);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ *vals = inv_icm42600_gyro_calibbias;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ return IIO_AVAIL_RANGE;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_write_scale(st, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_gyro_write_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ long mask)
+{
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ return IIO_VAL_INT_PLUS_NANO;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return IIO_VAL_INT_PLUS_MICRO;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return IIO_VAL_INT_PLUS_NANO;
+ default:
+ return -EINVAL;
+ }
+}
+
+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,
+};
+
+struct iio_dev *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)
+ return ERR_PTR(-ENOMEM);
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return ERR_PTR(-ENOMEM);
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_gyro_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_gyro_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+
+ ret = devm_iio_device_register(dev, indio_dev);
+ if (ret)
+ return ERR_PTR(ret);
+
+ return indio_dev;
+}
--
2.17.1
Document the ICM-426xxx devices devicetree bindings.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../bindings/iio/imu/invensense,icm42600.yaml | 90 +++++++++++++++++++
1 file changed, 90 insertions(+)
create mode 100644 Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
diff --git a/Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml b/Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
new file mode 100644
index 000000000000..abd8d25e1136
--- /dev/null
+++ b/Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
@@ -0,0 +1,90 @@
+# SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause)
+%YAML 1.2
+---
+$id: http://devicetree.org/schemas/iio/imu/invensense,icm42600.yaml#
+$schema: http://devicetree.org/meta-schemas/core.yaml#
+
+title: InvenSense ICM-426xx Inertial Measurement Unit
+
+maintainers:
+ - Jean-Baptiste Maneyrol <[email protected]>
+
+description: |
+ 6-axis MotionTracking device that combines a 3-axis gyroscope and a 3-axis
+ accelerometer.
+
+ It has a configurable host interface that supports I3C, I2C and SPI serial
+ communication, features a 2kB FIFO and 2 programmable interrupts with
+ ultra-low-power wake-on-motion support to minimize system power consumption.
+
+ Other industry-leading features include InvenSense on-chip APEX Motion
+ Processing engine for gesture recognition, activity classification, and
+ pedometer, along with programmable digital filters, and an embedded
+ temperature sensor.
+
+ https://invensense.tdk.com/wp-content/uploads/2020/03/DS-000292-ICM-42605-v1.4.pdf
+
+properties:
+ compatible:
+ enum:
+ - invensense,icm42600
+ - invensense,icm42602
+ - invensense,icm42605
+ - invensense,icm42622
+
+ reg:
+ maxItems: 1
+
+ interrupts:
+ maxItems: 1
+
+ drive-open-drain:
+ type: boolean
+
+ vdd-supply:
+ description: Regulator that provides power to the sensor
+
+ vddio-supply:
+ description: Regulator that provides power to the bus
+
+required:
+ - compatible
+ - reg
+ - interrupts
+
+examples:
+ - |
+ #include <dt-bindings/gpio/gpio.h>
+ #include <dt-bindings/interrupt-controller/irq.h>
+ i2c0 {
+ #address-cells = <1>;
+ #size-cells = <0>;
+
+ icm42605@68 {
+ compatible = "invensense,icm42605";
+ reg = <0x68>;
+ interrupt-parent = <&gpio2>;
+ interrupts = <7 IRQ_TYPE_EDGE_FALLING>;
+ vdd-supply = <&vdd>;
+ vddio-supply = <&vddio>;
+ };
+ };
+ - |
+ #include <dt-bindings/gpio/gpio.h>
+ #include <dt-bindings/interrupt-controller/irq.h>
+ spi0 {
+ #address-cells = <1>;
+ #size-cells = <0>;
+
+ icm42602@0 {
+ compatible = "invensense,icm42602";
+ reg = <0>;
+ spi-max-frequency = <24000000>;
+ spi-cpha;
+ spi-cpol;
+ interrupt-parent = <&gpio1>;
+ interrupts = <2 IRQ_TYPE_EDGE_FALLING>;
+ vdd-supply = <&vdd>;
+ vddio-supply = <&vddio>;
+ };
+ };
--
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 | 164 ++++-
.../imu/inv_icm42600/inv_icm42600_buffer.c | 573 ++++++++++++++++++
.../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 | 164 ++++-
8 files changed, 1037 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 148894c888cc..7b52d92739c3 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,
struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
+
struct iio_dev *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 3f214df44093..77cdad99de91 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,79 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
};
+/*
+ * IIO buffer data: size must be a power of 2
+ * 8 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding
+ */
+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 +324,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 +644,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,
};
struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
@@ -576,6 +696,7 @@ struct iio_dev *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;
int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
@@ -586,12 +707,20 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!indio_dev)
return ERR_PTR(-ENOMEM);
+ buffer = devm_iio_kfifo_allocate(dev);
+ if (!buffer)
+ return ERR_PTR(-ENOMEM);
+
iio_device_set_drvdata(indio_dev, st);
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);
ret = devm_iio_device_register(dev, indio_dev);
if (ret)
@@ -599,3 +728,36 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
return indio_dev;
}
+
+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;
+ /* buffer is copied to userspace, set padding to 0 to avoid data leak */
+ 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..e58e6f0c5698
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -0,0 +1,573 @@
+// 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/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;
+}
+
+/**
+ * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold
+ * @st: driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ *
+ * FIFO watermark threshold is computed based on the required watermark values
+ * set for gyro and accel sensors. Since watermark is all about acceptable data
+ * latency, use the smallest setting between the 2. It means choosing the
+ * smallest latency but this is not as simple as choosing the smallest watermark
+ * value. Latency depends on watermark and ODR. It requires several steps:
+ * 1) compute gyro and accel latencies and choose the smallest value.
+ * 2) adapt the choosen latency so that it is a multiple of both gyro and accel
+ * ones. Otherwise it is possible that you don't meet a requirement. (for
+ * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the
+ * value of 4 will not meet accel latency requirement because 6 is not a
+ * multiple of 4. You need to use the value 2.)
+ * 3) Since all periods are multiple of each others, watermark is computed by
+ * dividing this computed latency by the smallest period, which corresponds
+ * to the FIFO frequency. Beware that this is only true because we are not
+ * using 500Hz frequency which is not a multiple of the others.
+ */
+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;
+ uint32_t latency_gyro, latency_accel, latency;
+ bool restore;
+ __le16 raw_wm;
+ int ret;
+
+ packet_size = inv_icm42600_get_packet_size(st->fifo.en);
+
+ /* compute sensors 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);
+ /* use us for odr to avoid overflow using 32 bits values */
+ period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr) / 1000UL;
+ period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr) / 1000UL;
+ latency_gyro = period_gyro * wm_gyro;
+ latency_accel = period_accel * wm_accel;
+
+ /* 0 value for watermark means that the sensor is turned off */
+ if (latency_gyro == 0) {
+ watermark = wm_accel;
+ } else if (latency_accel == 0) {
+ watermark = wm_gyro;
+ } else {
+ /* compute the smallest latency that is a multiple of both */
+ if (latency_gyro <= latency_accel)
+ latency = latency_gyro - (latency_accel % latency_gyro);
+ else
+ latency = latency_accel - (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 = latency / period;
+ if (watermark < 1)
+ watermark = 1;
+ }
+
+ /* compute watermark value in bytes */
+ 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 95b2a6d91e5b..9e1f6e65fd45 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[] = {
{
@@ -428,6 +429,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;
@@ -604,6 +617,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;
+
st->indio_gyro = inv_icm42600_gyro_init(st);
if (IS_ERR(st->indio_gyro))
return PTR_ERR(st->indio_gyro);
@@ -649,6 +666,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);
@@ -688,6 +713,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 6a0e7661fa48..d53c604ae1de 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,79 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
};
+/*
+ * IIO buffer data: size must be a power of 2
+ * 8 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding
+ */
+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 +336,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 +655,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,
};
struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
@@ -587,6 +707,7 @@ struct iio_dev *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;
int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
@@ -597,12 +718,20 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!indio_dev)
return ERR_PTR(-ENOMEM);
+ buffer = devm_iio_kfifo_allocate(dev);
+ if (!buffer)
+ return ERR_PTR(-ENOMEM);
+
iio_device_set_drvdata(indio_dev, st);
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);
ret = devm_iio_device_register(dev, indio_dev);
if (ret)
@@ -610,3 +739,36 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
return indio_dev;
}
+
+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;
+ /* buffer is copied to userspace, set padding to 0 to avoid data leak */
+ 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! Perhaps something to improve:
[auto build test WARNING on iio/togreg]
[also build test WARNING on robh/for-next linus/master v5.7 next-20200608]
[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/20200609-044917
base: https://git.kernel.org/pub/scm/linux/kernel/git/jic23/iio.git togreg
config: ia64-allmodconfig (attached as .config)
compiler: ia64-linux-gcc (GCC) 9.3.0
reproduce (this is a W=1 build):
wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
chmod +x ~/bin/make.cross
# save the attached .config to linux build tree
COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-9.3.0 make.cross ARCH=ia64
If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <[email protected]>
All warnings (new ones prefixed by >>, old ones prefixed by <<):
drivers/iio/imu/inv_icm42600/inv_icm42600_core.c: In function 'inv_icm42600_get_mount_matrix':
>> drivers/iio/imu/inv_icm42600/inv_icm42600_core.c:91:63: warning: passing argument 1 of 'iio_device_get_drvdata' discards 'const' qualifier from pointer target type [-Wdiscarded-qualifiers]
91 | const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
| ^~~~~~~~~
In file included from drivers/iio/imu/inv_icm42600/inv_icm42600_core.c:15:
include/linux/iio/iio.h:672:60: note: expected 'struct iio_dev *' but argument is of type 'const struct iio_dev *'
672 | static inline void *iio_device_get_drvdata(struct iio_dev *indio_dev)
| ~~~~~~~~~~~~~~~~^~~~~~~~~
vim +91 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 86
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 87 const struct iio_mount_matrix *
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 88 inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 89 const struct iio_chan_spec *chan)
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 90 {
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 @91 const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 92
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 93 return &st->orientation;
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 94 }
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 95
:::::: The code at line 91 was first introduced by commit
:::::: 632e967c33fb32bb0a2a2f85a2a8d2e6d2b125f3 iio: imu: inv_icm42600: add core of new inv_icm42600 driver
:::::: TO: Jean-Baptiste Maneyrol <[email protected]>
:::::: CC: 0day robot <[email protected]>
---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/[email protected]
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 next-20200608]
[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/20200609-044917
base: https://git.kernel.org/pub/scm/linux/kernel/git/jic23/iio.git togreg
config: x86_64-allyesconfig (attached as .config)
compiler: clang version 11.0.0 (https://github.com/llvm/llvm-project e429cffd4f228f70c1d9df0e5d77c08590dd9766)
reproduce (this is a W=1 build):
wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
chmod +x ~/bin/make.cross
# install x86_64 cross compiling tool for clang build
# apt-get install binutils-x86-64-linux-gnu
# save the attached .config to linux build tree
COMPILER_INSTALL_PATH=$HOME/0day COMPILER=clang make.cross ARCH=x86_64
If you fix the issue, kindly add following tag as appropriate
Reported-by: kernel test robot <[email protected]>
All errors (new ones prefixed by >>, old ones prefixed by <<):
>> drivers/iio/imu/inv_icm42600/inv_icm42600_core.c:91:63: error: passing 'const struct iio_dev *' to parameter of type 'struct iio_dev *' discards qualifiers [-Werror,-Wincompatible-pointer-types-discards-qualifiers]
const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
^~~~~~~~~
include/linux/iio/iio.h:672:60: note: passing argument to parameter 'indio_dev' here
static inline void *iio_device_get_drvdata(struct iio_dev *indio_dev)
^
1 error generated.
vim +91 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 86
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 87 const struct iio_mount_matrix *
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 88 inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 89 const struct iio_chan_spec *chan)
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 90 {
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 @91 const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 92
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 93 return &st->orientation;
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 94 }
632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 95
:::::: The code at line 91 was first introduced by commit
:::::: 632e967c33fb32bb0a2a2f85a2a8d2e6d2b125f3 iio: imu: inv_icm42600: add core of new inv_icm42600 driver
:::::: TO: Jean-Baptiste Maneyrol <[email protected]>
:::::: CC: 0day robot <[email protected]>
---
0-DAY CI Kernel Test Service, Intel Corporation
https://lists.01.org/hyperkitty/list/[email protected]
For anyone curious, this is an artefact of me failing to push the iio tree
out last weekend which included a patch to make iio_device_get_drvdata
take a const.
Sorry for the noise
Jonathan
On Tue, 9 Jun 2020 07:03:53 +0800
kernel test robot <[email protected]> wrote:
> Hi Jean-Baptiste,
>
> Thank you for the patch! Perhaps something to improve:
>
> [auto build test WARNING on iio/togreg]
> [also build test WARNING on robh/for-next linus/master v5.7 next-20200608]
> [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/20200609-044917
> base: https://git.kernel.org/pub/scm/linux/kernel/git/jic23/iio.git togreg
> config: ia64-allmodconfig (attached as .config)
> compiler: ia64-linux-gcc (GCC) 9.3.0
> reproduce (this is a W=1 build):
> wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
> chmod +x ~/bin/make.cross
> # save the attached .config to linux build tree
> COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-9.3.0 make.cross ARCH=ia64
>
> If you fix the issue, kindly add following tag as appropriate
> Reported-by: kernel test robot <[email protected]>
>
> All warnings (new ones prefixed by >>, old ones prefixed by <<):
>
> drivers/iio/imu/inv_icm42600/inv_icm42600_core.c: In function 'inv_icm42600_get_mount_matrix':
> >> drivers/iio/imu/inv_icm42600/inv_icm42600_core.c:91:63: warning: passing argument 1 of 'iio_device_get_drvdata' discards 'const' qualifier from pointer target type [-Wdiscarded-qualifiers]
> 91 | const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> | ^~~~~~~~~
> In file included from drivers/iio/imu/inv_icm42600/inv_icm42600_core.c:15:
> include/linux/iio/iio.h:672:60: note: expected 'struct iio_dev *' but argument is of type 'const struct iio_dev *'
> 672 | static inline void *iio_device_get_drvdata(struct iio_dev *indio_dev)
> | ~~~~~~~~~~~~~~~~^~~~~~~~~
>
> vim +91 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
>
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 86
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 87 const struct iio_mount_matrix *
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 88 inv_icm42600_get_mount_matrix(const struct iio_dev *indio_dev,
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 89 const struct iio_chan_spec *chan)
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 90 {
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 @91 const struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 92
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 93 return &st->orientation;
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 94 }
> 632e967c33fb32 Jean-Baptiste Maneyrol 2020-06-08 95
>
> :::::: The code at line 91 was first introduced by commit
> :::::: 632e967c33fb32bb0a2a2f85a2a8d2e6d2b125f3 iio: imu: inv_icm42600: add core of new inv_icm42600 driver
>
> :::::: TO: Jean-Baptiste Maneyrol <[email protected]>
> :::::: CC: 0day robot <[email protected]>
>
> ---
> 0-DAY CI Kernel Test Service, Intel Corporation
> https://lists.01.org/hyperkitty/list/[email protected]
On Mon, 8 Jun 2020 22:42:47 +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.
Thanks for the added docs. I'm not 'totally' convinced that
people only use watermarks to control expected latency. Sometimes
they also do it because they are doing some batch based algorithm
and need a certain amount of data to feed into it. Still latency
is probably more common, so your explanation is fine.
All patches so far look good to me.
>
> 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 | 164 ++++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 573 ++++++++++++++++++
> .../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 | 164 ++++-
> 8 files changed, 1037 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 148894c888cc..7b52d92739c3 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,
>
> struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
> +
> struct iio_dev *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 3f214df44093..77cdad99de91 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,79 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> };
>
> +/*
> + * IIO buffer data: size must be a power of 2
> + * 8 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding
> + */
> +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 +324,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 +644,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,
> };
>
> struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
> @@ -576,6 +696,7 @@ struct iio_dev *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;
> int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
> @@ -586,12 +707,20 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!indio_dev)
> return ERR_PTR(-ENOMEM);
>
> + buffer = devm_iio_kfifo_allocate(dev);
> + if (!buffer)
> + return ERR_PTR(-ENOMEM);
> +
> iio_device_set_drvdata(indio_dev, st);
> 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);
>
> ret = devm_iio_device_register(dev, indio_dev);
> if (ret)
> @@ -599,3 +728,36 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
>
> return indio_dev;
> }
> +
> +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;
> + /* buffer is copied to userspace, set padding to 0 to avoid data leak */
> + 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..e58e6f0c5698
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,573 @@
> +// 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/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;
> +}
> +
> +/**
> + * inv_icm42600_buffer_update_watermark - update watermark FIFO threshold
> + * @st: driver internal state
> + *
> + * Returns 0 on success, a negative error code otherwise.
> + *
> + * FIFO watermark threshold is computed based on the required watermark values
> + * set for gyro and accel sensors. Since watermark is all about acceptable data
> + * latency, use the smallest setting between the 2. It means choosing the
> + * smallest latency but this is not as simple as choosing the smallest watermark
> + * value. Latency depends on watermark and ODR. It requires several steps:
> + * 1) compute gyro and accel latencies and choose the smallest value.
> + * 2) adapt the choosen latency so that it is a multiple of both gyro and accel
> + * ones. Otherwise it is possible that you don't meet a requirement. (for
> + * example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the
> + * value of 4 will not meet accel latency requirement because 6 is not a
> + * multiple of 4. You need to use the value 2.)
> + * 3) Since all periods are multiple of each others, watermark is computed by
> + * dividing this computed latency by the smallest period, which corresponds
> + * to the FIFO frequency. Beware that this is only true because we are not
> + * using 500Hz frequency which is not a multiple of the others.
> + */
> +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;
> + uint32_t latency_gyro, latency_accel, latency;
> + bool restore;
> + __le16 raw_wm;
> + int ret;
> +
> + packet_size = inv_icm42600_get_packet_size(st->fifo.en);
> +
> + /* compute sensors 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);
> + /* use us for odr to avoid overflow using 32 bits values */
> + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr) / 1000UL;
> + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr) / 1000UL;
> + latency_gyro = period_gyro * wm_gyro;
> + latency_accel = period_accel * wm_accel;
> +
> + /* 0 value for watermark means that the sensor is turned off */
> + if (latency_gyro == 0) {
> + watermark = wm_accel;
> + } else if (latency_accel == 0) {
> + watermark = wm_gyro;
> + } else {
> + /* compute the smallest latency that is a multiple of both */
> + if (latency_gyro <= latency_accel)
> + latency = latency_gyro - (latency_accel % latency_gyro);
> + else
> + latency = latency_accel - (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 = latency / period;
> + if (watermark < 1)
> + watermark = 1;
> + }
> +
> + /* compute watermark value in bytes */
> + 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 95b2a6d91e5b..9e1f6e65fd45 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[] = {
> {
> @@ -428,6 +429,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;
> @@ -604,6 +617,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;
> +
> st->indio_gyro = inv_icm42600_gyro_init(st);
> if (IS_ERR(st->indio_gyro))
> return PTR_ERR(st->indio_gyro);
> @@ -649,6 +666,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);
> @@ -688,6 +713,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 6a0e7661fa48..d53c604ae1de 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,79 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> };
>
> +/*
> + * IIO buffer data: size must be a power of 2
> + * 8 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding
> + */
> +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 +336,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 +655,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,
> };
>
> struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> @@ -587,6 +707,7 @@ struct iio_dev *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;
> int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> @@ -597,12 +718,20 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!indio_dev)
> return ERR_PTR(-ENOMEM);
>
> + buffer = devm_iio_kfifo_allocate(dev);
> + if (!buffer)
> + return ERR_PTR(-ENOMEM);
> +
> iio_device_set_drvdata(indio_dev, st);
> 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);
>
> ret = devm_iio_device_register(dev, indio_dev);
> if (ret)
> @@ -610,3 +739,36 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>
> return indio_dev;
> }
> +
> +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;
> + /* buffer is copied to userspace, set padding to 0 to avoid data leak */
> + 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;
> +}
On Mon, 8 Jun 2020 22:42:48 +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.
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
As normal I'm going to wuss out on checking the maths in timestamp
calcs - it normally gives me a headache :)
So only remaining thing I think here is to leave it a bit longer
to give time for others to take a look + ideally a sanity check
/ review of the dt bindings.
Thanks
Jonathan
> ---
> 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, 396 insertions(+), 15 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 7b52d92739c3..c0f5059b13b3 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 77cdad99de91..e51e15029116 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,16 +67,18 @@ 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: size must be a power of 2
> - * 8 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding
> + * IIO buffer data: size must be a power of 2 and timestamp aligned
> + * 16 bytes: 7 bytes data (accel 6 + temp 1) + 1 byte padding + 8 bytes timestamp
> */
> struct inv_icm42600_accel_buffer {
> struct inv_icm42600_fifo_sensor_data accel;
> int8_t temp;
> uint8_t padding;
> + int64_t timestamp __aligned(8);
> };
>
> #define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
> @@ -95,6 +99,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;
> @@ -122,6 +127,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;
> @@ -302,9 +308,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;
> @@ -323,6 +331,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;
> @@ -612,7 +625,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)
> @@ -695,6 +708,7 @@ struct iio_dev *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;
> struct iio_dev *indio_dev;
> struct iio_buffer *buffer;
> int ret;
> @@ -703,7 +717,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!name)
> return ERR_PTR(-ENOMEM);
>
> - indio_dev = devm_iio_device_alloc(dev, 0);
> + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
> if (!indio_dev)
> return ERR_PTR(-ENOMEM);
>
> @@ -711,6 +725,9 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
> if (!buffer)
> return ERR_PTR(-ENOMEM);
>
> + ts = iio_priv(indio_dev);
> + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->name = name;
> indio_dev->info = &inv_icm42600_accel_info;
> @@ -732,17 +749,20 @@ struct iio_dev *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;
> /* buffer is copied to userspace, set padding to 0 to avoid data leak */
> 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 */
> @@ -753,10 +773,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 e58e6f0c5698..99576b2c171f 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -13,6 +13,7 @@
> #include <linux/iio/buffer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> #include "inv_icm42600_buffer.h"
>
> /* FIFO header: 1 byte */
> @@ -374,6 +375,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;
> @@ -383,9 +385,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;
> }
> @@ -413,6 +417,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);
>
> @@ -498,17 +504,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)
> @@ -521,8 +536,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;
> @@ -531,12 +551,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 9e1f6e65fd45..8bd77185ccb7 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[] = {
> {
> @@ -412,6 +413,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;
> @@ -495,7 +506,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);
> }
> @@ -617,6 +628,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 d53c604ae1de..1924b0b528df 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,16 +67,18 @@ 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: size must be a power of 2
> - * 8 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding
> + * IIO buffer data: size must be a power of 2 and timestamp aligned
> + * 16 bytes: 7 bytes data (gyro 6 + temp 1) + 1 byte padding + 8 bytes timestamp
> */
> struct inv_icm42600_gyro_buffer {
> struct inv_icm42600_fifo_sensor_data gyro;
> int8_t temp;
> uint8_t padding;
> + int64_t timestamp __aligned(8);
> };
>
> #define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
> @@ -95,6 +99,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;
> @@ -122,6 +127,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;
> @@ -314,9 +320,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;
> @@ -335,6 +343,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;
> @@ -623,7 +636,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)
> @@ -706,6 +719,7 @@ struct iio_dev *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;
> struct iio_dev *indio_dev;
> struct iio_buffer *buffer;
> int ret;
> @@ -714,7 +728,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!name)
> return ERR_PTR(-ENOMEM);
>
> - indio_dev = devm_iio_device_alloc(dev, 0);
> + indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
> if (!indio_dev)
> return ERR_PTR(-ENOMEM);
>
> @@ -722,6 +736,9 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> if (!buffer)
> return ERR_PTR(-ENOMEM);
>
> + ts = iio_priv(indio_dev);
> + inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
> +
> iio_device_set_drvdata(indio_dev, st);
> indio_dev->name = name;
> indio_dev->info = &inv_icm42600_gyro_info;
> @@ -743,17 +760,20 @@ struct iio_dev *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;
> /* buffer is copied to userspace, set padding to 0 to avoid data leak */
> 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 */
> @@ -764,10 +784,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
On 6/8/20 10:42 PM, Jean-Baptiste Maneyrol wrote:
> + case IIO_CHAN_INFO_PROCESSED:
> + ret = iio_device_claim_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + ret = inv_icm42600_temp_read(st, &temp);
> + iio_device_release_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + *val = temp;
> + return IIO_VAL_INT;
> + case IIO_CHAN_INFO_SCALE:
> + *val = 483;
> + *val2 = 91787;
> + return IIO_VAL_INT_PLUS_MICRO;
> + case IIO_CHAN_INFO_OFFSET:
> + *val = 25000;
> + return IIO_VAL_INT;
If the data is returned processed there is no need to specify scale and
offset.
But since the transformation to turn the data into standard units is a
simple linear transform the preferred way to handle this is to return
RAW data and specify scale and offset.
Hello Lars,
for the temperature data, the problem is that temperature in the FIFO (used in buffer) is not in the same format than when reading the register.
Reading the temperature register return a full precision value on 16 bits. I am using a PROCESSED attribute for it.
Temperature data in buffer (coming from the FIFO) is on 8 bits in lower precision. It is reported as raw data, thus the need of the offset and scale values.
So offset and scale values are only for transforming the temperature data from the buffer, and direct read is a full precision already processed in m?C.
Thanks for the review,
JB
From: Lars-Peter Clausen <[email protected]>
Sent: Sunday, June 14, 2020 17:10
To: Jean-Baptiste Maneyrol <[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]>
Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
Subject: Re: [PATCH v3 06/13] iio: imu: inv_icm42600: add temperature sensor support
?
?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 6/8/20 10:42 PM, Jean-Baptiste Maneyrol wrote:
> +???? case IIO_CHAN_INFO_PROCESSED:
> +???????????? ret = iio_device_claim_direct_mode(indio_dev);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???????????? ret = inv_icm42600_temp_read(st, &temp);
> +???????????? iio_device_release_direct_mode(indio_dev);
> +???????????? if (ret)
> +???????????????????? return ret;
> +???????????? *val = temp;
> +???????????? return IIO_VAL_INT;
> +???? case IIO_CHAN_INFO_SCALE:
> +???????????? *val = 483;
> +???????????? *val2 = 91787;
> +???????????? return IIO_VAL_INT_PLUS_MICRO;
> +???? case IIO_CHAN_INFO_OFFSET:
> +???????????? *val = 25000;
> +???????????? return IIO_VAL_INT;
If the data is returned processed there is no need to specify scale and
offset.
But since the transformation to turn the data into standard units is a
simple linear transform the preferred way to handle this is to return
RAW data and specify scale and offset.
On Mon, 08 Jun 2020 22:42:49 +0200, Jean-Baptiste Maneyrol wrote:
> Document the ICM-426xxx devices devicetree bindings.
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
> ---
> .../bindings/iio/imu/invensense,icm42600.yaml | 90 +++++++++++++++++++
> 1 file changed, 90 insertions(+)
> create mode 100644 Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
>
Reviewed-by: Rob Herring <[email protected]>
On Sun, 14 Jun 2020 20:35:13 +0000
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Hello Lars,
>
> for the temperature data, the problem is that temperature in the FIFO (used in buffer) is not in the same format than when reading the register.
>
> Reading the temperature register return a full precision value on 16 bits. I am using a PROCESSED attribute for it.
> Temperature data in buffer (coming from the FIFO) is on 8 bits in lower precision. It is reported as raw data, thus the need of the offset and scale values.
>
> So offset and scale values are only for transforming the temperature data from the buffer, and direct read is a full precision already processed in m°C.
That is a problem. We have no means of describing it in IIO.
If the channel is processed via sysfs the assumption would normally
be that it is processed in the buffer as well. We don't really
have any means of describing the two separately.
In cases where we've seen this before the way around it was to
change the data in the fifo so that the scaling was the same as the
sysfs channel (usually it's just a bit of padding).
Can we do that here? Looks like the ratio is 1:64 so should
be possible.
>
> Thanks for the review,
> JB
>
> From: Lars-Peter Clausen <[email protected]>
> Sent: Sunday, June 14, 2020 17:10
> To: Jean-Baptiste Maneyrol <[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]>
> Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
> Subject: Re: [PATCH v3 06/13] iio: imu: inv_icm42600: add temperature sensor support
>
> 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 6/8/20 10:42 PM, Jean-Baptiste Maneyrol wrote:
> > + case IIO_CHAN_INFO_PROCESSED:
> > + ret = iio_device_claim_direct_mode(indio_dev);
> > + if (ret)
> > + return ret;
> > + ret = inv_icm42600_temp_read(st, &temp);
> > + iio_device_release_direct_mode(indio_dev);
> > + if (ret)
> > + return ret;
> > + *val = temp;
> > + return IIO_VAL_INT;
> > + case IIO_CHAN_INFO_SCALE:
> > + *val = 483;
> > + *val2 = 91787;
> > + return IIO_VAL_INT_PLUS_MICRO;
> > + case IIO_CHAN_INFO_OFFSET:
> > + *val = 25000;
> > + return IIO_VAL_INT;
>
> If the data is returned processed there is no need to specify scale and
> offset.
>
> But since the transformation to turn the data into standard units is a
> simple linear transform the preferred way to handle this is to return
> RAW data and specify scale and offset.
Hi Jonathan,
no problem, I can rework temperature to have the same raw output for both cases (with just decreased resolution for the FIFO case).
v4 series is coming soon with the fix.
Thanks,
JB
From: Jonathan Cameron <[email protected]>
Sent: Saturday, June 20, 2020 17:57
To: Jean-Baptiste Maneyrol <[email protected]>
Cc: Lars-Peter Clausen <[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] <[email protected]>
Subject: Re: [PATCH v3 06/13] iio: imu: inv_icm42600: add temperature sensor support
?
?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 Sun, 14 Jun 2020 20:35:13 +0000
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Hello Lars,
>
> for the temperature data, the problem is that temperature in the FIFO (used in buffer) is not in the same format than when reading the register.
>
> Reading the temperature register return a full precision value on 16 bits. I am using a PROCESSED attribute for it.
> Temperature data in buffer (coming from the FIFO) is on 8 bits in lower precision. It is reported as raw data, thus the need of the offset and scale values.
>
> So offset and scale values are only for transforming the temperature data from the buffer, and direct read is a full precision already processed in m?C.
That is a problem.? We have no means of describing it in IIO.
If the channel is processed via sysfs the assumption would normally
be that it is processed in the buffer as well. We don't really
have any means of describing the two separately.
In cases where we've seen this before the way around it was to
change the data in the fifo so that the scaling was the same as the
sysfs channel (usually it's just a bit of padding).
Can we do that here?? Looks like the ratio is 1:64 so should
be possible.
>
> Thanks for the review,
> JB
>
> From: Lars-Peter Clausen <[email protected]>
> Sent: Sunday, June 14, 2020 17:10
> To: Jean-Baptiste Maneyrol <[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]>
> Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
> Subject: Re: [PATCH v3 06/13] iio: imu: inv_icm42600: add temperature sensor support
> ?
> ?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 6/8/20 10:42 PM, Jean-Baptiste Maneyrol wrote:
> > +???? case IIO_CHAN_INFO_PROCESSED:
> > +???????????? ret = iio_device_claim_direct_mode(indio_dev);
> > +???????????? if (ret)
> > +???????????????????? return ret;
> > +???????????? ret = inv_icm42600_temp_read(st, &temp);
> > +???????????? iio_device_release_direct_mode(indio_dev);
> > +???????????? if (ret)
> > +???????????????????? return ret;
> > +???????????? *val = temp;
> > +???????????? return IIO_VAL_INT;
> > +???? case IIO_CHAN_INFO_SCALE:
> > +???????????? *val = 483;
> > +???????????? *val2 = 91787;
> > +???????????? return IIO_VAL_INT_PLUS_MICRO;
> > +???? case IIO_CHAN_INFO_OFFSET:
> > +???????????? *val = 25000;
> > +???????????? return IIO_VAL_INT;?
>
> If the data is returned processed there is no need to specify scale and
> offset.
>
> But since the transformation to turn the data into standard units is a
> simple linear transform the preferred way to handle this is to return
> RAW data and specify scale and offset.