* [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 13:28 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for " Jean-Baptiste Maneyrol
` (10 subsequent siblings)
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
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 <jmaneyrol@invensense.com>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 372 +++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 618 ++++++++++++++++++
2 files changed, 990 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..8da4c8249aed
--- /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: chip access lock.
+ * @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);
+
+int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep);
+
+int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
+ unsigned int *sleep);
+
+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..35bdf4f9d31e
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -0,0 +1,618 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/interrupt.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_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((struct iio_dev *)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)
+{
+ 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;
+ dev_dbg(regmap_get_device(st->map), "pwr_mgmt0: %#02x\n", val);
+ 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)
+ *sleep = 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)
+{
+ 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);
+ dev_dbg(regmap_get_device(st->map), "accel_config0: %#02x\n", val);
+ 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);
+ dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
+ 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);
+}
+
+int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
+ struct inv_icm42600_sensor_conf *conf,
+ unsigned int *sleep)
+{
+ 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);
+ dev_dbg(regmap_get_device(st->map), "gyro_config0: %#02x\n", val);
+ 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);
+ dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
+ 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);
+
+ return 0;
+}
+
+int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
+ unsigned int *sleep)
+{
+ return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
+ st->conf.accel.mode, enable,
+ sleep);
+}
+
+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.
+ */
+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 mask, 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;
+ }
+ dev_info(dev, "found %s (%#02x)\n", hw->name, hw->whoami);
+ 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) */
+ mask = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
+ val = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ mask, val);
+ 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_regulators(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);
+
+ ret = regulator_disable(st->vdd_supply);
+ if (ret)
+ dev_err(dev, "failed to disable vdd 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;
+
+ BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
+ 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 = inv_icm42600_enable_regulator_vddio(st);
+ if (ret) {
+ regulator_disable(st->vdd_supply);
+ return ret;
+ }
+
+ ret = devm_add_action_or_reset(dev, inv_icm42600_disable_regulators,
+ 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);
+
+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;
+}
+
+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;
+}
+
+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;
+}
+
+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
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
2020-05-07 14:42 ` [PATCH 01/12] iio: imu: inv_icm42600: add core of " Jean-Baptiste Maneyrol
@ 2020-05-08 13:28 ` Jonathan Cameron
2020-05-18 14:14 ` Jean-Baptiste Maneyrol
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 13:28 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:11 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> 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 <jmaneyrol@invensense.com>
Hi Jean-Baptiste,
A few minor things inline.
Thanks,
Jonathan
> ---
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 372 +++++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 618 ++++++++++++++++++
> 2 files changed, 990 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..8da4c8249aed
> --- /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;
> +};
> +
> +/*
/**
It's valid kernel doc so lets mark it as such.
> + * struct inv_icm42600_state - driver state variables
> + * @lock: chip access lock.
Nice to be a bit more specific on that. What about the chip needs
a lock at this level as opposed to bus locks etc?
> + * @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);
> +
> +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
> + struct inv_icm42600_sensor_conf *conf,
> + unsigned int *sleep);
> +
> +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
> + unsigned int *sleep);
> +
> +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..35bdf4f9d31e
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -0,0 +1,618 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/module.h>
> +#include <linux/slab.h>
> +#include <linux/delay.h>
> +#include <linux/interrupt.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_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((struct iio_dev *)indio_dev);
Interesting... iio_device_get_drvdata is never going to modify
the struct iio_dev. Should we just change that to take a
const struct iio_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)
msleep or similar that indicates the units of the sleep time.
> +{
> + 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;
> + dev_dbg(regmap_get_device(st->map), "pwr_mgmt0: %#02x\n", val);
I wonder if you have a little too much in the way of debug prints.
These are internal to the code and so could only be wrong due to a local
bug. Once you've finished writing the driver I'd hope we won't need these!
> + 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)
> + *sleep = 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)
> +{
> + 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);
> + dev_dbg(regmap_get_device(st->map), "accel_config0: %#02x\n", val);
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
> + 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);
> +}
> +
> +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
> + struct inv_icm42600_sensor_conf *conf,
> + unsigned int *sleep)
> +{
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_config0: %#02x\n", val);
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
> + 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);
> +
> + return 0;
> +}
> +
> +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
> + unsigned int *sleep)
> +{
> + return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
> + st->conf.accel.mode, enable,
> + sleep);
> +}
> +
> +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.
If doing kernel-doc (which is good) you should do it all.
So document the parameters as well.
It's worth running the kernel-doc script over any file where
you put some and fixing up any warnings / errors.
> + */
> +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 mask, 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;
> + }
> + dev_info(dev, "found %s (%#02x)\n", hw->name, hw->whoami);
Hmm. I'm never that keen on this sort of log noise. Why do you need it
except for initial debug?
> + 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);
blank line here to separate two logical blocks of code.
Slightly helps readability.
> + 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) */
> + mask = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
> + val = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
Long line, but I'd rather you just didn't bother will local variables
in cases like this where you just set them to a constant.
Take the 80 chars thing as guidance not a rule :)
> + 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_regulators(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);
> +
> + ret = regulator_disable(st->vdd_supply);
> + if (ret)
> + dev_err(dev, "failed to disable vdd 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;
> +
> + BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
Why not just give the array an explicit size when you define it above?
I guess it would in theory be possible to not instantiate all of the array
but relying on different size of a variable length array seems less than
ideal.
> + 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;
nitpick: blank line here.
> + 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 = inv_icm42600_enable_regulator_vddio(st);
> + if (ret) {
> + regulator_disable(st->vdd_supply);
> + return ret;
> + }
> +
> + ret = devm_add_action_or_reset(dev, inv_icm42600_disable_regulators,
> + st);
I'd prefer to see two devm_add_action_or_reset calls. One for each regulator.
That means you don't have to do the extra disable logic above which is
a bit fragile in amongst a whole load of device managed calls.
> + 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);
> +
> +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;
> +}
> +
> +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;
You may need this later, but for now it's a bit comic so ideally introduce
it only when needed.
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +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;
> +}
> +
> +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);
> +
Why don't we need to reenable all the sensors we disabled in runtime suspend?
I can guess why we might not, but a comment here to explain would save on
possible confusion..
> + 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");
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
2020-05-08 13:28 ` Jonathan Cameron
@ 2020-05-18 14:14 ` Jean-Baptiste Maneyrol
2020-05-21 17:47 ` Jonathan Cameron
0 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-18 14:14 UTC (permalink / raw)
To: Jonathan Cameron
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
Hi Jonathan,
thanks for the feedbacks, I'm sorry but I will not be able to have a correct email formatting to respond you inline.
No problem with all the comments. For iio_device_get_drvdata, it would make more sense to use a const struct iio_dev * as argument. I am obliged to do the pointer conversion since iio_get_mount_matrix requires the use of a const struct iio_dev *.
For resume/suspend, I will add commentaries to explain what it is really doing and for which purpose. Sensor states save and restore will remain in this patch, since it makes more sense to have it as a core functionnality, as much as gyro/accel turn on/off.
Thanks.
JB
From: linux-iio-owner@vger.kernel.org <linux-iio-owner@vger.kernel.org> on behalf of Jonathan Cameron <Jonathan.Cameron@Huawei.com>
Sent: Friday, May 8, 2020 15:28
To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
Subject: Re: [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
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 Thu, 7 May 2020 16:42:11 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> 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 <jmaneyrol@invensense.com>
Hi Jean-Baptiste,
A few minor things inline.
Thanks,
Jonathan
> ---
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 372 +++++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 618 ++++++++++++++++++
> 2 files changed, 990 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..8da4c8249aed
> --- /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;
> +};
> +
> +/*
/**
It's valid kernel doc so lets mark it as such.
> + * struct inv_icm42600_state - driver state variables
> + * @lock: chip access lock.
Nice to be a bit more specific on that. What about the chip needs
a lock at this level as opposed to bus locks etc?
> + * @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);
> +
> +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
> + struct inv_icm42600_sensor_conf *conf,
> + unsigned int *sleep);
> +
> +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
> + unsigned int *sleep);
> +
> +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..35bdf4f9d31e
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -0,0 +1,618 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/module.h>
> +#include <linux/slab.h>
> +#include <linux/delay.h>
> +#include <linux/interrupt.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_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((struct iio_dev *)indio_dev);
Interesting... iio_device_get_drvdata is never going to modify
the struct iio_dev. Should we just change that to take a
const struct iio_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)
msleep or similar that indicates the units of the sleep time.
> +{
> + 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;
> + dev_dbg(regmap_get_device(st->map), "pwr_mgmt0: %#02x\n", val);
I wonder if you have a little too much in the way of debug prints.
These are internal to the code and so could only be wrong due to a local
bug. Once you've finished writing the driver I'd hope we won't need these!
> + 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)
> + *sleep = 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)
> +{
> + 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);
> + dev_dbg(regmap_get_device(st->map), "accel_config0: %#02x\n", val);
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
> + 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);
> +}
> +
> +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
> + struct inv_icm42600_sensor_conf *conf,
> + unsigned int *sleep)
> +{
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_config0: %#02x\n", val);
> + 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);
> + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
> + 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);
> +
> + return 0;
> +}
> +
> +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
> + unsigned int *sleep)
> +{
> + return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
> + st->conf.accel.mode, enable,
> + sleep);
> +}
> +
> +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.
If doing kernel-doc (which is good) you should do it all.
So document the parameters as well.
It's worth running the kernel-doc script over any file where
you put some and fixing up any warnings / errors.
> + */
> +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 mask, 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;
> + }
> + dev_info(dev, "found %s (%#02x)\n", hw->name, hw->whoami);
Hmm. I'm never that keen on this sort of log noise. Why do you need it
except for initial debug?
> + 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);
blank line here to separate two logical blocks of code.
Slightly helps readability.
> + 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) */
> + mask = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
> + val = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
Long line, but I'd rather you just didn't bother will local variables
in cases like this where you just set them to a constant.
Take the 80 chars thing as guidance not a rule :)
> + 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_regulators(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);
> +
> + ret = regulator_disable(st->vdd_supply);
> + if (ret)
> + dev_err(dev, "failed to disable vdd 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;
> +
> + BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
Why not just give the array an explicit size when you define it above?
I guess it would in theory be possible to not instantiate all of the array
but relying on different size of a variable length array seems less than
ideal.
> + 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;
nitpick: blank line here.
> + 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 = inv_icm42600_enable_regulator_vddio(st);
> + if (ret) {
> + regulator_disable(st->vdd_supply);
> + return ret;
> + }
> +
> + ret = devm_add_action_or_reset(dev, inv_icm42600_disable_regulators,
> + st);
I'd prefer to see two devm_add_action_or_reset calls. One for each regulator.
That means you don't have to do the extra disable logic above which is
a bit fragile in amongst a whole load of device managed calls.
> + 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);
> +
> +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;
> +}
> +
> +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;
You may need this later, but for now it's a bit comic so ideally introduce
it only when needed.
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +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;
> +}
> +
> +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);
> +
Why don't we need to reenable all the sensors we disabled in runtime suspend?
I can guess why we might not, but a comment here to explain would save on
possible confusion..
> + 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");
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
2020-05-18 14:14 ` Jean-Baptiste Maneyrol
@ 2020-05-21 17:47 ` Jonathan Cameron
0 siblings, 0 replies; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-21 17:47 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: Jonathan Cameron, robh+dt, robh, mchehab+huawei, davem, gregkh,
linux-iio, devicetree, linux-kernel
On Mon, 18 May 2020 14:14:26 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:
> Hi Jonathan,
>
> thanks for the feedbacks, I'm sorry but I will not be able to have a correct email formatting to respond you inline.
>
> No problem with all the comments. For iio_device_get_drvdata, it would make more sense to use a const struct iio_dev * as argument. I am obliged to do the pointer conversion since iio_get_mount_matrix requires the use of a const struct iio_dev *.
Absolutely. My argument was that we should change
iio_device_get_drvdata to take a const struct iio_dev * as well.
Seems logical. dev_get_drvdata takes a const device * and
that's all that is being called inside there.
That change can happen in parallel to this set so no problem
if you would rather leave it alone for now.
Jonathan
>
> For resume/suspend, I will add commentaries to explain what it is really doing and for which purpose. Sensor states save and restore will remain in this patch, since it makes more sense to have it as a core functionnality, as much as gyro/accel turn on/off.
>
> Thanks.
> JB
>
>
> From: linux-iio-owner@vger.kernel.org <linux-iio-owner@vger.kernel.org> on behalf of Jonathan Cameron <Jonathan.Cameron@Huawei.com>
>
> Sent: Friday, May 8, 2020 15:28
>
> To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
>
> Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
> linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
>
> Subject: Re: [PATCH 01/12] iio: imu: inv_icm42600: add core of new inv_icm42600 driver
>
>
>
>
> 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 Thu, 7 May 2020 16:42:11 +0200
>
> Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
>
>
>
> > 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 <jmaneyrol@invensense.com>
>
>
>
> Hi Jean-Baptiste,
>
>
>
> A few minor things inline.
>
>
>
> Thanks,
>
>
>
> Jonathan
>
>
>
> > ---
>
> > drivers/iio/imu/inv_icm42600/inv_icm42600.h | 372 +++++++++++
>
> > .../iio/imu/inv_icm42600/inv_icm42600_core.c | 618 ++++++++++++++++++
>
> > 2 files changed, 990 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..8da4c8249aed
>
> > --- /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;
>
> > +};
>
> > +
>
> > +/*
>
> /**
>
>
>
> It's valid kernel doc so lets mark it as such.
>
>
>
> > + * struct inv_icm42600_state - driver state variables
>
> > + * @lock: chip access lock.
>
>
>
> Nice to be a bit more specific on that. What about the chip needs
>
> a lock at this level as opposed to bus locks etc?
>
>
>
> > + * @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);
>
> > +
>
> > +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
>
> > + struct inv_icm42600_sensor_conf *conf,
>
> > + unsigned int *sleep);
>
> > +
>
> > +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
>
> > + unsigned int *sleep);
>
> > +
>
> > +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..35bdf4f9d31e
>
> > --- /dev/null
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
>
> > @@ -0,0 +1,618 @@
>
> > +// SPDX-License-Identifier: GPL-2.0-or-later
>
> > +/*
>
> > + * Copyright (C) 2020 Invensense, Inc.
>
> > + */
>
> > +
>
> > +#include <linux/device.h>
>
> > +#include <linux/module.h>
>
> > +#include <linux/slab.h>
>
> > +#include <linux/delay.h>
>
> > +#include <linux/interrupt.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_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((struct iio_dev *)indio_dev);
>
>
>
> Interesting... iio_device_get_drvdata is never going to modify
>
> the struct iio_dev. Should we just change that to take a
>
> const struct iio_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)
>
>
>
> msleep or similar that indicates the units of the sleep time.
>
>
>
> > +{
>
> > + 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;
>
> > + dev_dbg(regmap_get_device(st->map), "pwr_mgmt0: %#02x\n", val);
>
>
>
> I wonder if you have a little too much in the way of debug prints.
>
> These are internal to the code and so could only be wrong due to a local
>
> bug. Once you've finished writing the driver I'd hope we won't need these!
>
>
>
> > + 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)
>
> > + *sleep = 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)
>
> > +{
>
> > + 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);
>
> > + dev_dbg(regmap_get_device(st->map), "accel_config0: %#02x\n", val);
>
> > + 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);
>
> > + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
>
> > + 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);
>
> > +}
>
> > +
>
> > +int inv_icm42600_set_gyro_conf(struct inv_icm42600_state *st,
>
> > + struct inv_icm42600_sensor_conf *conf,
>
> > + unsigned int *sleep)
>
> > +{
>
> > + 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);
>
> > + dev_dbg(regmap_get_device(st->map), "gyro_config0: %#02x\n", val);
>
> > + 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);
>
> > + dev_dbg(regmap_get_device(st->map), "gyro_accel_config0: %#02x\n", val);
>
> > + 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);
>
> > +
>
> > + return 0;
>
> > +}
>
> > +
>
> > +int inv_icm42600_set_temp_conf(struct inv_icm42600_state *st, bool enable,
>
> > + unsigned int *sleep)
>
> > +{
>
> > + return inv_icm42600_set_pwr_mgmt0(st, st->conf.gyro.mode,
>
> > + st->conf.accel.mode, enable,
>
> > + sleep);
>
> > +}
>
> > +
>
> > +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.
>
>
>
> If doing kernel-doc (which is good) you should do it all.
>
> So document the parameters as well.
>
> It's worth running the kernel-doc script over any file where
>
> you put some and fixing up any warnings / errors.
>
>
>
> > + */
>
> > +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 mask, 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;
>
> > + }
>
> > + dev_info(dev, "found %s (%#02x)\n", hw->name, hw->whoami);
>
>
>
> Hmm. I'm never that keen on this sort of log noise. Why do you need it
>
> except for initial debug?
>
>
>
> > + 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);
>
>
>
> blank line here to separate two logical blocks of code.
>
> Slightly helps readability.
>
>
>
> > + 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) */
>
> > + mask = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
>
> > + val = INV_ICM42600_INTF_CONFIG0_SENSOR_DATA_ENDIAN;
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
>
> > + mask, val);
>
>
>
> Long line, but I'd rather you just didn't bother will local variables
>
> in cases like this where you just set them to a constant.
>
> Take the 80 chars thing as guidance not a rule :)
>
>
>
> > + 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_regulators(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);
>
> > +
>
> > + ret = regulator_disable(st->vdd_supply);
>
> > + if (ret)
>
> > + dev_err(dev, "failed to disable vdd 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;
>
> > +
>
> > + BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
>
>
>
> Why not just give the array an explicit size when you define it above?
>
> I guess it would in theory be possible to not instantiate all of the array
>
> but relying on different size of a variable length array seems less than
>
> ideal.
>
>
>
> > + 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;
>
> nitpick: blank line here.
>
>
>
> > + 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 = inv_icm42600_enable_regulator_vddio(st);
>
> > + if (ret) {
>
> > + regulator_disable(st->vdd_supply);
>
> > + return ret;
>
> > + }
>
> > +
>
> > + ret = devm_add_action_or_reset(dev, inv_icm42600_disable_regulators,
>
> > + st);
>
>
>
> I'd prefer to see two devm_add_action_or_reset calls. One for each regulator.
>
> That means you don't have to do the extra disable logic above which is
>
> a bit fragile in amongst a whole load of device managed calls.
>
>
>
> > + 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);
>
> > +
>
> > +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;
>
> > +}
>
> > +
>
> > +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;
>
>
>
> You may need this later, but for now it's a bit comic so ideally introduce
>
> it only when needed.
>
>
>
> > +
>
> > +out_unlock:
>
> > + mutex_unlock(&st->lock);
>
> > + return ret;
>
> > +}
>
> > +
>
> > +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;
>
> > +}
>
> > +
>
> > +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);
>
> > +
>
>
>
> Why don't we need to reenable all the sensors we disabled in runtime suspend?
>
> I can guess why we might not, but a comment here to explain would save on
>
> possible confusion..
>
>
>
> > + 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");
>
>
>
>
>
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 01/12] iio: imu: inv_icm42600: add core of " Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 13:44 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 03/12] iio: imu: inv_icm42600: add SPI " Jean-Baptiste Maneyrol
` (9 subsequent siblings)
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add I2C driver for InvenSense ICM-426xxx devices.
Configure bus signal slew rates as indicated in the datasheet.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 117 ++++++++++++++++++
1 file changed, 117 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..b61f993beacf
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
@@ -0,0 +1,117 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 InvenSense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/of_device.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 */
+ mask = INV_ICM42600_INTF_CONFIG6_MASK;
+ val = INV_ICM42600_INTF_CONFIG6_I3C_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
+ mask, val);
+ if (ret)
+ return ret;
+
+ mask = INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY;
+ val = 0;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
+ mask, val);
+ 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 */
+ mask = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK;
+ val = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ mask, val);
+}
+
+static int inv_icm42600_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ 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)
+ chip = (enum inv_icm42600_chip)match;
+ else if (id)
+ chip = (enum inv_icm42600_chip)id->driver_data;
+ else
+ return -EINVAL;
+
+ 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 const struct i2c_device_id inv_icm42600_ids[] = {
+ {"icm42600", INV_CHIP_ICM42600},
+ {"icm42602", INV_CHIP_ICM42602},
+ {"icm42605", INV_CHIP_ICM42605},
+ {"icm42622", INV_CHIP_ICM42622},
+ {}
+};
+MODULE_DEVICE_TABLE(i2c, inv_icm42600_ids);
+
+static struct i2c_driver inv_icm42600_driver = {
+ .probe = inv_icm42600_probe,
+ .id_table = inv_icm42600_ids,
+ .driver = {
+ .of_match_table = inv_icm42600_of_matches,
+ .name = "inv-icm42600-i2c",
+ .pm = &inv_icm42600_pm_ops,
+ },
+};
+module_i2c_driver(inv_icm42600_driver);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver");
+MODULE_LICENSE("GPL");
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
2020-05-07 14:42 ` [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for " Jean-Baptiste Maneyrol
@ 2020-05-08 13:44 ` Jonathan Cameron
2020-05-18 14:19 ` Jean-Baptiste Maneyrol
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 13:44 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:12 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add I2C driver for InvenSense ICM-426xxx devices.
>
> Configure bus signal slew rates as indicated in the datasheet.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Some incoherent rambling inline. + a few comments
Jonathan
> ---
> .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 117 ++++++++++++++++++
> 1 file changed, 117 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..b61f993beacf
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> @@ -0,0 +1,117 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 InvenSense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/module.h>
> +#include <linux/i2c.h>
> +#include <linux/regmap.h>
> +#include <linux/of_device.h>
Why? Looks like you need the table and the device property stuff neither
of which are in that file.
linux/mod_devicetable.h
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 */
> + mask = INV_ICM42600_INTF_CONFIG6_MASK;
> + val = INV_ICM42600_INTF_CONFIG6_I3C_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
> + mask, val);
I'd put the values inline where they are simple like these rather than
using local variables.
> + if (ret)
> + return ret;
> +
> + mask = INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY;
> + val = 0;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
> + mask, val);
> + 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 */
> + mask = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK;
> + val = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
> +}
> +
> +static int inv_icm42600_probe(struct i2c_client *client,
> + const struct i2c_device_id *id)
> +{
> + 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);
Hmm. Annoyingly if one were to call the of specific option
of i2c_of_match_device it would handle the old style i2c match just fine without
needing special handling. However, it would fail to handle PRP0001 ACPI.
Rather feels like there should be something similar for
device_get_match_data so we could use the probe_new version of i2c device
probing.
Oh well. I guess thats a separate question for another day ;)
Mind you can we actually probe this driver via the sysfs route?
If not why do we need to handle the non firmware case at all?
> + if (match)
> + chip = (enum inv_icm42600_chip)match;
> + else if (id)
> + chip = (enum inv_icm42600_chip)id->driver_data;
> + else
> + return -EINVAL;
> +
> + 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 const struct i2c_device_id inv_icm42600_ids[] = {
> + {"icm42600", INV_CHIP_ICM42600},
> + {"icm42602", INV_CHIP_ICM42602},
> + {"icm42605", INV_CHIP_ICM42605},
> + {"icm42622", INV_CHIP_ICM42622},
> + {}
> +};
> +MODULE_DEVICE_TABLE(i2c, inv_icm42600_ids);
> +
> +static struct i2c_driver inv_icm42600_driver = {
> + .probe = inv_icm42600_probe,
> + .id_table = inv_icm42600_ids,
> + .driver = {
> + .of_match_table = inv_icm42600_of_matches,
> + .name = "inv-icm42600-i2c",
> + .pm = &inv_icm42600_pm_ops,
> + },
> +};
> +module_i2c_driver(inv_icm42600_driver);
> +
> +MODULE_AUTHOR("InvenSense, Inc.");
> +MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver");
> +MODULE_LICENSE("GPL");
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
2020-05-08 13:44 ` Jonathan Cameron
@ 2020-05-18 14:19 ` Jean-Baptiste Maneyrol
2020-05-21 17:50 ` Jonathan Cameron
0 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-18 14:19 UTC (permalink / raw)
To: Jonathan Cameron
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
Hi Jonathan,
I am using generic device_get_match_data because I was thinking it was now the way to go. But since only of is supported with the driver, I can switch to using of_device_get_match_data instead.
Tell me what do you think is better.
I could definitely use the new probe interface indeed, good idea.
Thanks,
JB
From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
Sent: Friday, May 8, 2020 15:44
To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
Subject: Re: [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
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 Thu, 7 May 2020 16:42:12 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add I2C driver for InvenSense ICM-426xxx devices.
>
> Configure bus signal slew rates as indicated in the datasheet.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Some incoherent rambling inline. + a few comments
Jonathan
> ---
> .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 117 ++++++++++++++++++
> 1 file changed, 117 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..b61f993beacf
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> @@ -0,0 +1,117 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 InvenSense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/module.h>
> +#include <linux/i2c.h>
> +#include <linux/regmap.h>
> +#include <linux/of_device.h>
Why? Looks like you need the table and the device property stuff neither
of which are in that file.
linux/mod_devicetable.h
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 */
> + mask = INV_ICM42600_INTF_CONFIG6_MASK;
> + val = INV_ICM42600_INTF_CONFIG6_I3C_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
> + mask, val);
I'd put the values inline where they are simple like these rather than
using local variables.
> + if (ret)
> + return ret;
> +
> + mask = INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY;
> + val = 0;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
> + mask, val);
> + 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 */
> + mask = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK;
> + val = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
> +}
> +
> +static int inv_icm42600_probe(struct i2c_client *client,
> + const struct i2c_device_id *id)
> +{
> + 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);
Hmm. Annoyingly if one were to call the of specific option
of i2c_of_match_device it would handle the old style i2c match just fine without
needing special handling. However, it would fail to handle PRP0001 ACPI.
Rather feels like there should be something similar for
device_get_match_data so we could use the probe_new version of i2c device
probing.
Oh well. I guess thats a separate question for another day ;)
Mind you can we actually probe this driver via the sysfs route?
If not why do we need to handle the non firmware case at all?
> + if (match)
> + chip = (enum inv_icm42600_chip)match;
> + else if (id)
> + chip = (enum inv_icm42600_chip)id->driver_data;
> + else
> + return -EINVAL;
> +
> + 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 const struct i2c_device_id inv_icm42600_ids[] = {
> + {"icm42600", INV_CHIP_ICM42600},
> + {"icm42602", INV_CHIP_ICM42602},
> + {"icm42605", INV_CHIP_ICM42605},
> + {"icm42622", INV_CHIP_ICM42622},
> + {}
> +};
> +MODULE_DEVICE_TABLE(i2c, inv_icm42600_ids);
> +
> +static struct i2c_driver inv_icm42600_driver = {
> + .probe = inv_icm42600_probe,
> + .id_table = inv_icm42600_ids,
> + .driver = {
> + .of_match_table = inv_icm42600_of_matches,
> + .name = "inv-icm42600-i2c",
> + .pm = &inv_icm42600_pm_ops,
> + },
> +};
> +module_i2c_driver(inv_icm42600_driver);
> +
> +MODULE_AUTHOR("InvenSense, Inc.");
> +MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver");
> +MODULE_LICENSE("GPL");
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
2020-05-18 14:19 ` Jean-Baptiste Maneyrol
@ 2020-05-21 17:50 ` Jonathan Cameron
0 siblings, 0 replies; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-21 17:50 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: Jonathan Cameron, robh+dt, robh, mchehab+huawei, davem, gregkh,
linux-iio, devicetree, linux-kernel
On Mon, 18 May 2020 14:19:11 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:
> Hi Jonathan,
>
> I am using generic device_get_match_data because I was thinking it was now the way to go. But since only of is supported with the driver, I can switch to using of_device_get_match_data instead.
>
> Tell me what do you think is better.
Leave it as you have it. We can look at if we can tidy up
what I was rambling about as a separate exercise.
J
>
> I could definitely use the new probe interface indeed, good idea.
>
> Thanks,
> JB
>
>
>
> From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
>
> Sent: Friday, May 8, 2020 15:44
>
> To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
>
> Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
> linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
>
> Subject: Re: [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
>
>
>
>
> 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 Thu, 7 May 2020 16:42:12 +0200
>
> Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
>
>
>
> > Add I2C driver for InvenSense ICM-426xxx devices.
>
> >
>
> > Configure bus signal slew rates as indicated in the datasheet.
>
> >
>
> > Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
>
> Some incoherent rambling inline. + a few comments
>
>
>
> Jonathan
>
>
>
> > ---
>
> > .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 117 ++++++++++++++++++
>
> > 1 file changed, 117 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..b61f993beacf
>
> > --- /dev/null
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
>
> > @@ -0,0 +1,117 @@
>
> > +// SPDX-License-Identifier: GPL-2.0-or-later
>
> > +/*
>
> > + * Copyright (C) 2020 InvenSense, Inc.
>
> > + */
>
> > +
>
> > +#include <linux/device.h>
>
> > +#include <linux/module.h>
>
> > +#include <linux/i2c.h>
>
> > +#include <linux/regmap.h>
>
> > +#include <linux/of_device.h>
>
>
>
> Why? Looks like you need the table and the device property stuff neither
>
> of which are in that file.
>
>
>
> linux/mod_devicetable.h
>
> 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 */
>
> > + mask = INV_ICM42600_INTF_CONFIG6_MASK;
>
> > + val = INV_ICM42600_INTF_CONFIG6_I3C_EN;
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG6,
>
> > + mask, val);
>
>
>
> I'd put the values inline where they are simple like these rather than
>
> using local variables.
>
>
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + mask = INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY;
>
> > + val = 0;
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
>
> > + mask, val);
>
> > + 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 */
>
> > + mask = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK;
>
> > + val = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_SPI_DIS;
>
> > + return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
>
> > + mask, val);
>
> > +}
>
> > +
>
> > +static int inv_icm42600_probe(struct i2c_client *client,
>
> > + const struct i2c_device_id *id)
>
> > +{
>
> > + 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);
>
>
>
> Hmm. Annoyingly if one were to call the of specific option
>
> of i2c_of_match_device it would handle the old style i2c match just fine without
>
> needing special handling. However, it would fail to handle PRP0001 ACPI.
>
>
>
> Rather feels like there should be something similar for
>
> device_get_match_data so we could use the probe_new version of i2c device
>
> probing.
>
>
>
> Oh well. I guess thats a separate question for another day ;)
>
>
>
> Mind you can we actually probe this driver via the sysfs route?
>
> If not why do we need to handle the non firmware case at all?
>
>
>
> > + if (match)
>
> > + chip = (enum inv_icm42600_chip)match;
>
> > + else if (id)
>
> > + chip = (enum inv_icm42600_chip)id->driver_data;
>
> > + else
>
> > + return -EINVAL;
>
> > +
>
> > + 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 const struct i2c_device_id inv_icm42600_ids[] = {
>
> > + {"icm42600", INV_CHIP_ICM42600},
>
> > + {"icm42602", INV_CHIP_ICM42602},
>
> > + {"icm42605", INV_CHIP_ICM42605},
>
> > + {"icm42622", INV_CHIP_ICM42622},
>
> > + {}
>
> > +};
>
> > +MODULE_DEVICE_TABLE(i2c, inv_icm42600_ids);
>
> > +
>
> > +static struct i2c_driver inv_icm42600_driver = {
>
> > + .probe = inv_icm42600_probe,
>
> > + .id_table = inv_icm42600_ids,
>
> > + .driver = {
>
> > + .of_match_table = inv_icm42600_of_matches,
>
> > + .name = "inv-icm42600-i2c",
>
> > + .pm = &inv_icm42600_pm_ops,
>
> > + },
>
> > +};
>
> > +module_i2c_driver(inv_icm42600_driver);
>
> > +
>
> > +MODULE_AUTHOR("InvenSense, Inc.");
>
> > +MODULE_DESCRIPTION("InvenSense ICM-426xx I2C driver");
>
> > +MODULE_LICENSE("GPL");
>
>
>
>
>
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 03/12] iio: imu: inv_icm42600: add SPI driver for inv_icm42600 driver
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 01/12] iio: imu: inv_icm42600: add core of " Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 02/12] iio: imu: inv_icm42600: add I2C driver for " Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 04/12] iio: imu: inv_icm42600: add gyroscope IIO device Jean-Baptiste Maneyrol
` (8 subsequent siblings)
11 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add SPI driver for InvenSense ICM-426xxx devices.
Configure bus signal slew rates as indicated in the datasheet.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 117 ++++++++++++++++++
1 file changed, 117 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..835ced68a3a3
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
@@ -0,0 +1,117 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 InvenSense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/regmap.h>
+#include <linux/of_device.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 */
+ mask = INV_ICM42600_INTF_CONFIG6_MASK;
+ 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,
+ mask, val);
+ if (ret)
+ return ret;
+
+ mask = INV_ICM42600_INTF_CONFIG4_I3C_BUS_ONLY;
+ val = 0;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG4,
+ mask, val);
+ 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 */
+ mask = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_MASK;
+ val = INV_ICM42600_INTF_CONFIG0_UI_SIFS_CFG_I2C_DIS;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ mask, val);
+}
+
+static int inv_icm42600_probe(struct spi_device *spi)
+{
+ const void *match;
+ const struct spi_device_id *spi_id;
+ enum inv_icm42600_chip chip;
+ struct regmap *regmap;
+
+ match = device_get_match_data(&spi->dev);
+ spi_id = spi_get_device_id(spi);
+ if (match)
+ chip = (enum inv_icm42600_chip)match;
+ else if (spi_id)
+ chip = (enum inv_icm42600_chip)spi_id->driver_data;
+ else
+ return -EINVAL;
+
+ 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 const struct spi_device_id inv_icm42600_ids[] = {
+ {"icm42600", INV_CHIP_ICM42600},
+ {"icm42602", INV_CHIP_ICM42602},
+ {"icm42605", INV_CHIP_ICM42605},
+ {"icm42622", INV_CHIP_ICM42622},
+ {}
+};
+MODULE_DEVICE_TABLE(spi, inv_icm42600_ids);
+
+static struct spi_driver inv_icm42600_driver = {
+ .probe = inv_icm42600_probe,
+ .id_table = inv_icm42600_ids,
+ .driver = {
+ .of_match_table = inv_icm42600_of_matches,
+ .name = "inv-icm42600-spi",
+ .pm = &inv_icm42600_pm_ops,
+ },
+};
+module_spi_driver(inv_icm42600_driver);
+
+MODULE_AUTHOR("InvenSense, Inc.");
+MODULE_DESCRIPTION("InvenSense ICM-426xx SPI driver");
+MODULE_LICENSE("GPL");
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* [PATCH 04/12] iio: imu: inv_icm42600: add gyroscope IIO device
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (2 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 03/12] iio: imu: inv_icm42600: add SPI " Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 14:01 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 05/12] iio: imu: inv_icm42600: add accelerometer " Jean-Baptiste Maneyrol
` (7 subsequent siblings)
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
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 <jmaneyrol@invensense.com>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 5 +
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 549 ++++++++++++++++++
3 files changed, 558 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 8da4c8249aed..ca41a9d6404a 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -120,6 +120,7 @@ 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.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -131,6 +132,7 @@ struct inv_icm42600_state {
struct iio_mount_matrix orientation;
struct inv_icm42600_conf conf;
struct inv_icm42600_suspended suspended;
+ struct iio_dev *indio_gyro;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -369,4 +371,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);
+int 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 35bdf4f9d31e..151257652ce6 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -503,6 +503,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (ret)
return ret;
+ /* create and init gyroscope iio device */
+ ret = inv_icm42600_gyro_init(st);
+ 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_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
new file mode 100644
index 000000000000..74aa2b5fa611
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -0,0 +1,549 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.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), \
+ .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, \
+ .shift = 0, \
+ .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 */
+ ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpu(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;
+
+ mutex_lock(&st->lock);
+ idx = st->conf.gyro.fs;
+ mutex_unlock(&st->lock);
+
+ *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;
+
+ /* update gyro fs */
+ pm_runtime_get_sync(dev);
+
+ mutex_lock(&st->lock);
+ conf.fs = idx / 2;
+ 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;
+
+ mutex_lock(&st->lock);
+ odr = st->conf.gyro.odr;
+ mutex_unlock(&st->lock);
+
+ 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;
+
+ /* update gyro odr */
+ pm_runtime_get_sync(dev);
+
+ mutex_lock(&st->lock);
+ conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
+ 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;
+}
+
+static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int reg;
+ 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);
+
+ /* read gyro offset data */
+ mutex_lock(&st->lock);
+ ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
+ mutex_unlock(&st->lock);
+ if (ret)
+ goto exit;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ *val = (int16_t)(((data[1] & 0x0F) << 8) | data[0]);
+ break;
+ case IIO_MOD_Y:
+ *val = (int16_t)(((data[0] & 0xF0) << 4) | data[1]);
+ break;
+ case IIO_MOD_Z:
+ *val = (int16_t)(((data[1] & 0x0F) << 8) | data[0]);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+exit:
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int reg, regval;
+ 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;
+ }
+
+ /* value is limited to 12 bits signed */
+ if (val < -2048 || val > 2047)
+ return -EINVAL;
+
+ 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;
+ data[0] = val & 0xFF;
+ data[1] = (regval & 0xF0) | ((val & 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;
+ data[0] = ((val & 0xF00) >> 4) | (regval & 0x0F);
+ data[1] = val & 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;
+ data[0] = val & 0xFF;
+ data[1] = (regval & 0xF0) | ((val & 0xF00) >> 8);
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, data, sizeof(data));
+
+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:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_read_offset(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ 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;
+ 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);
+ 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;
+ 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,
+};
+
+int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
+{
+ struct device *dev = regmap_get_device(st->map);
+ const char *name;
+ struct iio_dev *indio_dev;
+
+ name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
+ if (!name)
+ return -ENOMEM;
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return -ENOMEM;
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->dev.parent = dev;
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_gyro_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_gyro_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+
+ st->indio_gyro = indio_dev;
+ return devm_iio_device_register(dev, st->indio_gyro);
+}
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 04/12] iio: imu: inv_icm42600: add gyroscope IIO device
2020-05-07 14:42 ` [PATCH 04/12] iio: imu: inv_icm42600: add gyroscope IIO device Jean-Baptiste Maneyrol
@ 2020-05-08 14:01 ` Jonathan Cameron
[not found] ` <MN2PR12MB4422B32CB3C4BFD0AF5FFF3CC4B80@MN2PR12MB4422.namprd12.prod.outlook.com>
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 14:01 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:14 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> 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 <jmaneyrol@invensense.com>
Few trivial things and questions inline.
J
> ---
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 5 +
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 549 ++++++++++++++++++
> 3 files changed, 558 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 8da4c8249aed..ca41a9d6404a 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -120,6 +120,7 @@ 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.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -131,6 +132,7 @@ struct inv_icm42600_state {
> struct iio_mount_matrix orientation;
> struct inv_icm42600_conf conf;
> struct inv_icm42600_suspended suspended;
> + struct iio_dev *indio_gyro;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -369,4 +371,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);
>
> +int 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 35bdf4f9d31e..151257652ce6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -503,6 +503,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
> if (ret)
> return ret;
>
> + /* create and init gyroscope iio device */
'Kind' of obvious from function name? Maybe drop the comment?
> + ret = inv_icm42600_gyro_init(st);
> + 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_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> new file mode 100644
> index 000000000000..74aa2b5fa611
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -0,0 +1,549 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/interrupt.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/delay.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), \
> + .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, \
> + .shift = 0, \
Shift has the 'obviously' default of 0, so normally we don't bother explicitly
setting it to 0 like this.
> + .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 */
> + ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
IIRC bulk reads need to be to dma safe buffers. So typically on the stack and
in appropriately aligned location in any containing structure.
> + if (ret)
> + goto exit;
> +
> + *val = (int16_t)be16_to_cpu(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;
> +
> + mutex_lock(&st->lock);
> + idx = st->conf.gyro.fs;
Seems like we shouldn't need the lock to retrieve a single value.
Is there some odd intermediate state somewhere I'm missing?
> + mutex_unlock(&st->lock);
> +
> + *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])
Alignment of code seems odd.
> + break;
> + }
> + if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
> + return -EINVAL;
> +
> + /* update gyro fs */
> + pm_runtime_get_sync(dev);
> +
> + mutex_lock(&st->lock);
> + conf.fs = idx / 2;
> + 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;
> +
> + mutex_lock(&st->lock);
> + odr = st->conf.gyro.odr;
> + mutex_unlock(&st->lock);
> +
> + 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;
> +
> + /* update gyro odr */
> + pm_runtime_get_sync(dev);
> +
> + mutex_lock(&st->lock);
> + conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> + 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;
> +}
> +
> +static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
> + struct iio_chan_spec const *chan,
> + int16_t *val)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int reg;
> + 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);
> +
> + /* read gyro offset data */
> + mutex_lock(&st->lock);
> + ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
> + mutex_unlock(&st->lock);
> + if (ret)
> + goto exit;
> +
> + switch (chan->channel2) {
> + case IIO_MOD_X:
> + *val = (int16_t)(((data[1] & 0x0F) << 8) | data[0]);
This doesn't look right for negative values. You would be better
off with a sign extend of the 12 bit value.
> + break;
> + case IIO_MOD_Y:
> + *val = (int16_t)(((data[0] & 0xF0) << 4) | data[1]);
> + break;
> + case IIO_MOD_Z:
> + *val = (int16_t)(((data[1] & 0x0F) << 8) | data[0]);
> + break;
> + default:
> + ret = -EINVAL;
> + break;
> + }
> +
> +exit:
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> + return ret;
> +}
> +
> +static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
> + struct iio_chan_spec const *chan,
> + int val)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int reg, regval;
> + 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;
> + }
> +
> + /* value is limited to 12 bits signed */
> + if (val < -2048 || val > 2047)
> + return -EINVAL;
Perhaps worth an available callback to give the range?
> +
> + 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;
> + data[0] = val & 0xFF;
> + data[1] = (regval & 0xF0) | ((val & 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;
> + data[0] = ((val & 0xF00) >> 4) | (regval & 0x0F);
> + data[1] = val & 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;
> + data[0] = val & 0xFF;
> + data[1] = (regval & 0xF0) | ((val & 0xF00) >> 8);
> + break;
> + default:
> + ret = -EINVAL;
> + goto out_unlock;
> + }
> +
> + ret = regmap_bulk_write(st->map, reg, data, sizeof(data));
> +
> +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:
> + ret = iio_device_claim_direct_mode(indio_dev);
> + if (ret)
> + return ret;
I'm curious. Why can't we read back a calibration offset whilst doing
buffered capture?
> + ret = inv_icm42600_gyro_read_offset(st, chan, &data);
> + iio_device_release_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + *val = data;
> + return IIO_VAL_INT;
> + 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;
> + 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);
> + 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;
> + 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,
> +};
> +
> +int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + const char *name;
> + struct iio_dev *indio_dev;
> +
> + name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> + if (!name)
> + return -ENOMEM;
> +
> + indio_dev = devm_iio_device_alloc(dev, 0);
> + if (!indio_dev)
> + return -ENOMEM;
> +
> + iio_device_set_drvdata(indio_dev, st);
> + indio_dev->dev.parent = dev;
> + indio_dev->name = name;
> + indio_dev->info = &inv_icm42600_gyro_info;
> + indio_dev->modes = INDIO_DIRECT_MODE;
> + indio_dev->channels = inv_icm42600_gyro_channels;
> + indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
> +
> + st->indio_gyro = indio_dev;
> + return devm_iio_device_register(dev, st->indio_gyro);
> +}
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 05/12] iio: imu: inv_icm42600: add accelerometer IIO device
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (3 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 04/12] iio: imu: inv_icm42600: add gyroscope IIO device Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 06/12] iio: imu: inv_icm42600: add temperature sensor support Jean-Baptiste Maneyrol
` (6 subsequent siblings)
11 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
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 <jmaneyrol@invensense.com>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 537 ++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 5 +
3 files changed, 546 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 ca41a9d6404a..bc963b3d1800 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.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -133,6 +134,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;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -373,4 +375,6 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+int 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..397e3d0fd42b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -0,0 +1,537 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.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), \
+ .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, \
+ .shift = 0, \
+ .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 */
+ ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpu(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;
+
+ mutex_lock(&st->lock);
+ idx = st->conf.accel.fs;
+ mutex_unlock(&st->lock);
+
+ *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;
+
+ /* update accel fs */
+ pm_runtime_get_sync(dev);
+
+ mutex_lock(&st->lock);
+ conf.fs = idx / 2;
+ 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;
+
+ mutex_lock(&st->lock);
+ odr = st->conf.accel.odr;
+ mutex_unlock(&st->lock);
+
+ 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;
+
+ /* update accel odr */
+ pm_runtime_get_sync(dev);
+
+ mutex_lock(&st->lock);
+ conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
+ 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;
+}
+
+static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int reg;
+ 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);
+
+ /* read accel offset data */
+ mutex_lock(&st->lock);
+ ret = regmap_bulk_read(st->map, reg, &data, sizeof(data));
+ mutex_unlock(&st->lock);
+ if (ret)
+ goto exit;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ *val = (int16_t)(((data[0] & 0xF0) << 4) | data[1]);
+ break;
+ case IIO_MOD_Y:
+ *val = (int16_t)(((data[1] & 0x0F) << 8) | data[0]);
+ break;
+ case IIO_MOD_Z:
+ *val = (int16_t)(((data[0] & 0xF0) << 4) | data[1]);
+ break;
+ default:
+ ret = -EINVAL;
+ break;
+ }
+
+exit:
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int reg, regval;
+ 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;
+ }
+
+ /* value is limited to 12 bits signed */
+ if (val < -2048 || val > 2047)
+ return -EINVAL;
+
+ 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;
+ data[0] = ((val & 0xF00) >> 4) | (regval & 0x0F);
+ data[1] = val & 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;
+ data[0] = val & 0xFF;
+ data[1] = ((val & 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;
+ data[0] = ((val & 0xF00) >> 4) | (regval & 0x0F);
+ data[1] = val & 0xFF;
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, data, sizeof(data));
+
+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:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_read_offset(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ 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;
+ 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);
+ 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;
+ 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,
+};
+
+int inv_icm42600_accel_init(struct inv_icm42600_state *st)
+{
+ struct device *dev = regmap_get_device(st->map);
+ const char *name;
+ struct iio_dev *indio_dev;
+
+ name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
+ if (!name)
+ return -ENOMEM;
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return -ENOMEM;
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->dev.parent = dev;
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_accel_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_accel_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+
+ st->indio_accel = indio_dev;
+ return devm_iio_device_register(dev, st->indio_accel);
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 151257652ce6..4e33f263d3ea 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -508,6 +508,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (ret)
return ret;
+ /* create and init accelerometer iio device */
+ ret = inv_icm42600_accel_init(st);
+ if (ret)
+ return ret;
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* [PATCH 06/12] iio: imu: inv_icm42600: add temperature sensor support
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (4 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 05/12] iio: imu: inv_icm42600: add accelerometer " Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 07/12] iio: imu: add Kconfig and Makefile for inv_icm42600 driver Jean-Baptiste Maneyrol
` (5 subsequent siblings)
11 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
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 <jmaneyrol@invensense.com>
---
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 12 ++-
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 12 ++-
.../iio/imu/inv_icm42600/inv_icm42600_temp.c | 86 +++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_temp.h | 32 +++++++
4 files changed, 140 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 397e3d0fd42b..74dac5f283d4 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -12,6 +12,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -44,6 +45,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[] = {
@@ -58,6 +60,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,
@@ -394,8 +397,15 @@ 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 74aa2b5fa611..c0164ab2830e 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -12,6 +12,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -44,6 +45,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[] = {
@@ -58,6 +60,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,
@@ -406,8 +409,15 @@ 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..e5407b17c407
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
@@ -0,0 +1,86 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/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);
+ __be16 raw;
+ int16_t val;
+ int64_t data;
+ int ret;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_temp_conf(st, true, NULL);
+ if (ret)
+ goto exit;
+
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TEMP_DATA,
+ &raw, sizeof(raw));
+ if (ret)
+ goto exit;
+
+ val = (int16_t)be16_to_cpu(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..7c0854d38e1e
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
@@ -0,0 +1,32 @@
+/* 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/device.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, \
+ .shift = 0, \
+ }, \
+ }
+
+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
^ permalink raw reply related [flat|nested] 30+ messages in thread* [PATCH 07/12] iio: imu: add Kconfig and Makefile for inv_icm42600 driver
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (5 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 06/12] iio: imu: inv_icm42600: add temperature sensor support Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger Jean-Baptiste Maneyrol
` (4 subsequent siblings)
11 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add 3 modules: inv-icm42600, inv-icm42600-i2c, inv-icm42600-spi.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
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
^ permalink raw reply related [flat|nested] 30+ messages in thread* [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (6 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 07/12] iio: imu: add Kconfig and Makefile for inv_icm42600 driver Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 14:22 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices Jean-Baptiste Maneyrol
` (3 subsequent siblings)
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add INT1 interrupt support and use it as an iio trigger.
Support interrupt edge and level, active high or low.
Push-pull configuration only.
Trigger enables FIFO and will be useful with buffer support.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
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_core.c | 19 +-
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 2 +-
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 2 +-
.../imu/inv_icm42600/inv_icm42600_trigger.c | 177 ++++++++++++++++++
7 files changed, 206 insertions(+), 4 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
index 22390a72f0a3..7b3eaeb2aa4a 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_TRIGGER
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..e1f2aacbe888 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_trigger.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 bc963b3d1800..175c1f67faee 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -13,6 +13,7 @@
#include <linux/regulator/consumer.h>
#include <linux/pm.h>
#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
enum inv_icm42600_chip {
INV_CHIP_ICM42600,
@@ -122,6 +123,7 @@ struct inv_icm42600_suspended {
* @suspended: suspended sensors configuration.
* @indio_gyro: gyroscope IIO device.
* @indio_accel: accelerometer IIO device.
+ * @trigger: device internal interrupt trigger
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -135,6 +137,7 @@ struct inv_icm42600_state {
struct inv_icm42600_suspended suspended;
struct iio_dev *indio_gyro;
struct iio_dev *indio_accel;
+ struct iio_trigger *trigger;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -370,11 +373,14 @@ 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);
int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
int inv_icm42600_accel_init(struct inv_icm42600_state *st);
+int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
+ int irq_type);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 4e33f263d3ea..1102c54396e3 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -447,11 +447,13 @@ 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;
int ret;
BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
@@ -460,6 +462,16 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
return -ENODEV;
}
+ /* get irq data, 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;
+
st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
if (!st)
return -ENOMEM;
@@ -503,6 +515,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (ret)
return ret;
+ /* setup interrupt trigger */
+ ret = inv_icm42600_trigger_init(st, irq, irq_type);
+ if (ret)
+ return ret;
+
/* create and init gyroscope iio device */
ret = inv_icm42600_gyro_init(st);
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 b61f993beacf..b1478ece43f6 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
@@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct i2c_client *client,
if (IS_ERR(regmap))
return PTR_ERR(regmap);
- return inv_icm42600_core_probe(regmap, chip,
+ return inv_icm42600_core_probe(regmap, chip, client->irq,
inv_icm42600_i2c_bus_setup);
}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
index 835ced68a3a3..ec784f9e3c2c 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
@@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct spi_device *spi)
if (IS_ERR(regmap))
return PTR_ERR(regmap);
- return inv_icm42600_core_probe(regmap, chip,
+ return inv_icm42600_core_probe(regmap, chip, spi->irq,
inv_icm42600_spi_bus_setup);
}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
new file mode 100644
index 000000000000..7a5e76305f0b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
@@ -0,0 +1,177 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include "inv_icm42600.h"
+
+static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+
+ if (st->indio_gyro)
+ iio_pollfunc_store_time(irq, st->indio_gyro->pollfunc);
+ if (st->indio_accel)
+ iio_pollfunc_store_time(irq, st->indio_accel->pollfunc);
+
+ return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t inv_icm42600_trigger_int_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;
+ dev_dbg(dev, "int_status = %#02x\n", status);
+
+ /* FIFO full */
+ 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)
+ iio_trigger_poll_chained(st->trigger);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ return IRQ_HANDLED;
+}
+
+static int inv_icm42600_trigger_set_state(struct iio_trigger *trig, bool state)
+{
+ struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
+ unsigned int val;
+ uint16_t dummy;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ /*
+ * IIO buffers preenable and postdisable are managing power on/off.
+ * update_scan_mode is setting data FIFO enabled.
+ */
+ if (state) {
+ /* set FIFO threshold interrupt */
+ val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ val, val);
+ 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: dummy read of FIFO count */
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
+ &dummy, sizeof(dummy));
+ } else {
+ /* 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 */
+ val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ val, 0);
+ }
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ return ret;
+}
+
+static int inv_icm42600_trigger_validate(struct iio_trigger *trig,
+ struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
+
+ if (iio_device_get_drvdata(indio_dev) != st)
+ return -ENODEV;
+
+ return 0;
+}
+
+static const struct iio_trigger_ops inv_icm42600_trigger_ops = {
+ .set_trigger_state = &inv_icm42600_trigger_set_state,
+ .validate_device = &inv_icm42600_trigger_validate,
+};
+
+int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
+ int irq_type)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int val;
+ int ret;
+
+ st->trigger = devm_iio_trigger_alloc(dev, "%s-dev", st->name);
+ if (!st->trigger)
+ return -ENOMEM;
+
+ st->trigger->dev.parent = dev;
+ st->trigger->ops = &inv_icm42600_trigger_ops;
+ iio_trigger_set_drvdata(st->trigger, st);
+
+ /* configure INT1 with correct mode */
+ /* falling or both-edge */
+ if (irq_type & IRQF_TRIGGER_FALLING) {
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW;
+ } else if (irq_type == IRQF_TRIGGER_RISING) {
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH;
+ } else if (irq_type == IRQF_TRIGGER_LOW) {
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
+ INV_ICM42600_INT_CONFIG_INT1_LATCHED;
+ } else if (irq_type == IRQF_TRIGGER_HIGH) {
+ val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
+ INV_ICM42600_INT_CONFIG_INT1_LATCHED;
+ } else {
+ dev_err(dev, "invalid interrupt type %#x\n", irq_type);
+ return -EINVAL;
+ }
+ 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;
+
+ ret = devm_request_threaded_irq(dev, irq,
+ inv_icm42600_trigger_timestamp,
+ inv_icm42600_trigger_int_handler,
+ irq_type, "inv_icm42600", st);
+ if (ret)
+ return ret;
+
+ return devm_iio_trigger_register(dev, st->trigger);
+}
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger
2020-05-07 14:42 ` [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger Jean-Baptiste Maneyrol
@ 2020-05-08 14:22 ` Jonathan Cameron
2020-05-18 15:41 ` Jean-Baptiste Maneyrol
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 14:22 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:18 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add INT1 interrupt support and use it as an iio trigger.
> Support interrupt edge and level, active high or low.
> Push-pull configuration only.
>
> Trigger enables FIFO and will be useful with buffer support.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
So it's an odd trigger... But you check its only used for the
device itself which stops that being visible to anyone.
The open question in my mind is why we need a trigger at all?
If there is no advantage we don't need to expose that. Can just
directly call the buffer functions from the interrupt handler.
It's perfectly acceptable to not have a trigger exposed if:
* It would only be useful for the device providing it.
* You don't need to use it to select between various options.
For some of the other fifo devices IIRC we do support other
triggers but if you don't provide one the local fifo is used.
Here I don't think we actually support other triggers though we
don't prevent them being used? So even simpler.
With the complex interleaved fifo I suspect it would be hard
to support other triggers anyway except in a trivial poll like
mode so probably not of interest to anyone...
J
> ---
> 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_core.c | 19 +-
> .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 2 +-
> .../iio/imu/inv_icm42600/inv_icm42600_spi.c | 2 +-
> .../imu/inv_icm42600/inv_icm42600_trigger.c | 177 ++++++++++++++++++
> 7 files changed, 206 insertions(+), 4 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
>
> diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
> index 22390a72f0a3..7b3eaeb2aa4a 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_TRIGGER
>
> 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..e1f2aacbe888 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_trigger.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 bc963b3d1800..175c1f67faee 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -13,6 +13,7 @@
> #include <linux/regulator/consumer.h>
> #include <linux/pm.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/trigger.h>
>
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> @@ -122,6 +123,7 @@ struct inv_icm42600_suspended {
> * @suspended: suspended sensors configuration.
> * @indio_gyro: gyroscope IIO device.
> * @indio_accel: accelerometer IIO device.
> + * @trigger: device internal interrupt trigger
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -135,6 +137,7 @@ struct inv_icm42600_state {
> struct inv_icm42600_suspended suspended;
> struct iio_dev *indio_gyro;
> struct iio_dev *indio_accel;
> + struct iio_trigger *trigger;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -370,11 +373,14 @@ 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);
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> + int irq_type);
> +
> #endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 4e33f263d3ea..1102c54396e3 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -447,11 +447,13 @@ 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;
> int ret;
>
> BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
> @@ -460,6 +462,16 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
> return -ENODEV;
> }
>
> + /* get irq data, 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;
> +
> st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
> if (!st)
> return -ENOMEM;
> @@ -503,6 +515,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
> if (ret)
> return ret;
>
> + /* setup interrupt trigger */
> + ret = inv_icm42600_trigger_init(st, irq, irq_type);
> + if (ret)
> + return ret;
> +
> /* create and init gyroscope iio device */
> ret = inv_icm42600_gyro_init(st);
> 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 b61f993beacf..b1478ece43f6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> @@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct i2c_client *client,
> if (IS_ERR(regmap))
> return PTR_ERR(regmap);
>
> - return inv_icm42600_core_probe(regmap, chip,
> + return inv_icm42600_core_probe(regmap, chip, client->irq,
> inv_icm42600_i2c_bus_setup);
> }
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> index 835ced68a3a3..ec784f9e3c2c 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> @@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct spi_device *spi)
> if (IS_ERR(regmap))
> return PTR_ERR(regmap);
>
> - return inv_icm42600_core_probe(regmap, chip,
> + return inv_icm42600_core_probe(regmap, chip, spi->irq,
> inv_icm42600_spi_bus_setup);
> }
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> new file mode 100644
> index 000000000000..7a5e76305f0b
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> @@ -0,0 +1,177 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/interrupt.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/trigger.h>
> +#include <linux/iio/trigger_consumer.h>
> +
> +#include "inv_icm42600.h"
> +
> +static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
> +{
> + struct inv_icm42600_state *st = _data;
> +
> + if (st->indio_gyro)
> + iio_pollfunc_store_time(irq, st->indio_gyro->pollfunc);
> + if (st->indio_accel)
> + iio_pollfunc_store_time(irq, st->indio_accel->pollfunc);
> +
> + return IRQ_WAKE_THREAD;
> +}
> +
> +static irqreturn_t inv_icm42600_trigger_int_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;
> + dev_dbg(dev, "int_status = %#02x\n", status);
> +
> + /* FIFO full */
> + 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)
> + iio_trigger_poll_chained(st->trigger);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return IRQ_HANDLED;
> +}
> +
> +static int inv_icm42600_trigger_set_state(struct iio_trigger *trig, bool state)
> +{
> + struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
> + unsigned int val;
> + uint16_t dummy;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + /*
> + * IIO buffers preenable and postdisable are managing power on/off.
> + * update_scan_mode is setting data FIFO enabled.
> + */
> + if (state) {
> + /* set FIFO threshold interrupt */
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + val, val);
> + if (ret)
> + goto out_unlock;
blank line after each block. Makes it easier for my tired eyes to parse :)
> + /* 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: dummy read of FIFO count */
Work around for what?
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
> + &dummy, sizeof(dummy));
> + } else {
> + /* 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 */
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + val, 0);
> + }
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +static int inv_icm42600_trigger_validate(struct iio_trigger *trig,
> + struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
> +
> + if (iio_device_get_drvdata(indio_dev) != st)
> + return -ENODEV;
> +
> + return 0;
> +}
> +
> +static const struct iio_trigger_ops inv_icm42600_trigger_ops = {
> + .set_trigger_state = &inv_icm42600_trigger_set_state,
> + .validate_device = &inv_icm42600_trigger_validate,
> +};
> +
> +int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> + int irq_type)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int val;
> + int ret;
> +
> + st->trigger = devm_iio_trigger_alloc(dev, "%s-dev", st->name);
> + if (!st->trigger)
> + return -ENOMEM;
> +
> + st->trigger->dev.parent = dev;
> + st->trigger->ops = &inv_icm42600_trigger_ops;
> + iio_trigger_set_drvdata(st->trigger, st);
> +
> + /* configure INT1 with correct mode */
> + /* falling or both-edge */
> + if (irq_type & IRQF_TRIGGER_FALLING) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW;
> + } else if (irq_type == IRQF_TRIGGER_RISING) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH;
> + } else if (irq_type == IRQF_TRIGGER_LOW) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
> + INV_ICM42600_INT_CONFIG_INT1_LATCHED;
> + } else if (irq_type == IRQF_TRIGGER_HIGH) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
> + INV_ICM42600_INT_CONFIG_INT1_LATCHED;
> + } else {
> + dev_err(dev, "invalid interrupt type %#x\n", irq_type);
> + return -EINVAL;
> + }
> + 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;
> +
> + ret = devm_request_threaded_irq(dev, irq,
> + inv_icm42600_trigger_timestamp,
> + inv_icm42600_trigger_int_handler,
> + irq_type, "inv_icm42600", st);
> + if (ret)
> + return ret;
> +
> + return devm_iio_trigger_register(dev, st->trigger);
> +}
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger
2020-05-08 14:22 ` Jonathan Cameron
@ 2020-05-18 15:41 ` Jean-Baptiste Maneyrol
0 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-18 15:41 UTC (permalink / raw)
To: Jonathan Cameron
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
Hi Jonathan,
you're right, making a trigger is not very useful.
I will move this code to the buffer file or rename the file, and replace the triggered buffer setup in iio devices.
Thanks,
JB
From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
Sent: Friday, May 8, 2020 16:22
To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
Subject: Re: [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger
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 Thu, 7 May 2020 16:42:18 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add INT1 interrupt support and use it as an iio trigger.
> Support interrupt edge and level, active high or low.
> Push-pull configuration only.
>
> Trigger enables FIFO and will be useful with buffer support.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
So it's an odd trigger... But you check its only used for the
device itself which stops that being visible to anyone.
The open question in my mind is why we need a trigger at all?
If there is no advantage we don't need to expose that. Can just
directly call the buffer functions from the interrupt handler.
It's perfectly acceptable to not have a trigger exposed if:
* It would only be useful for the device providing it.
* You don't need to use it to select between various options.
For some of the other fifo devices IIRC we do support other
triggers but if you don't provide one the local fifo is used.
Here I don't think we actually support other triggers though we
don't prevent them being used? So even simpler.
With the complex interleaved fifo I suspect it would be hard
to support other triggers anyway except in a trivial poll like
mode so probably not of interest to anyone...
J
> ---
> 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_core.c | 19 +-
> .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 2 +-
> .../iio/imu/inv_icm42600/inv_icm42600_spi.c | 2 +-
> .../imu/inv_icm42600/inv_icm42600_trigger.c | 177 ++++++++++++++++++
> 7 files changed, 206 insertions(+), 4 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
>
> diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
> index 22390a72f0a3..7b3eaeb2aa4a 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_TRIGGER
>
> 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..e1f2aacbe888 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_trigger.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 bc963b3d1800..175c1f67faee 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -13,6 +13,7 @@
> #include <linux/regulator/consumer.h>
> #include <linux/pm.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/trigger.h>
>
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> @@ -122,6 +123,7 @@ struct inv_icm42600_suspended {
> * @suspended: suspended sensors configuration.
> * @indio_gyro: gyroscope IIO device.
> * @indio_accel: accelerometer IIO device.
> + * @trigger: device internal interrupt trigger
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -135,6 +137,7 @@ struct inv_icm42600_state {
> struct inv_icm42600_suspended suspended;
> struct iio_dev *indio_gyro;
> struct iio_dev *indio_accel;
> + struct iio_trigger *trigger;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -370,11 +373,14 @@ 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);
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> + int irq_type);
> +
> #endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 4e33f263d3ea..1102c54396e3 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -447,11 +447,13 @@ 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;
> int ret;
>
> BUILD_BUG_ON(ARRAY_SIZE(inv_icm42600_hw) != INV_CHIP_NB);
> @@ -460,6 +462,16 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
> return -ENODEV;
> }
>
> + /* get irq data, 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;
> +
> st = devm_kzalloc(dev, sizeof(*st), GFP_KERNEL);
> if (!st)
> return -ENOMEM;
> @@ -503,6 +515,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
> if (ret)
> return ret;
>
> + /* setup interrupt trigger */
> + ret = inv_icm42600_trigger_init(st, irq, irq_type);
> + if (ret)
> + return ret;
> +
> /* create and init gyroscope iio device */
> ret = inv_icm42600_gyro_init(st);
> 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 b61f993beacf..b1478ece43f6 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> @@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct i2c_client *client,
> if (IS_ERR(regmap))
> return PTR_ERR(regmap);
>
> - return inv_icm42600_core_probe(regmap, chip,
> + return inv_icm42600_core_probe(regmap, chip, client->irq,
> inv_icm42600_i2c_bus_setup);
> }
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> index 835ced68a3a3..ec784f9e3c2c 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> @@ -70,7 +70,7 @@ static int inv_icm42600_probe(struct spi_device *spi)
> if (IS_ERR(regmap))
> return PTR_ERR(regmap);
>
> - return inv_icm42600_core_probe(regmap, chip,
> + return inv_icm42600_core_probe(regmap, chip, spi->irq,
> inv_icm42600_spi_bus_setup);
> }
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> new file mode 100644
> index 000000000000..7a5e76305f0b
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> @@ -0,0 +1,177 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/interrupt.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/trigger.h>
> +#include <linux/iio/trigger_consumer.h>
> +
> +#include "inv_icm42600.h"
> +
> +static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
> +{
> + struct inv_icm42600_state *st = _data;
> +
> + if (st->indio_gyro)
> + iio_pollfunc_store_time(irq, st->indio_gyro->pollfunc);
> + if (st->indio_accel)
> + iio_pollfunc_store_time(irq, st->indio_accel->pollfunc);
> +
> + return IRQ_WAKE_THREAD;
> +}
> +
> +static irqreturn_t inv_icm42600_trigger_int_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;
> + dev_dbg(dev, "int_status = %#02x\n", status);
> +
> + /* FIFO full */
> + 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)
> + iio_trigger_poll_chained(st->trigger);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return IRQ_HANDLED;
> +}
> +
> +static int inv_icm42600_trigger_set_state(struct iio_trigger *trig, bool state)
> +{
> + struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
> + unsigned int val;
> + uint16_t dummy;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + /*
> + * IIO buffers preenable and postdisable are managing power on/off.
> + * update_scan_mode is setting data FIFO enabled.
> + */
> + if (state) {
> + /* set FIFO threshold interrupt */
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + val, val);
> + if (ret)
> + goto out_unlock;
blank line after each block. Makes it easier for my tired eyes to parse :)
> + /* 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: dummy read of FIFO count */
Work around for what?
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
> + &dummy, sizeof(dummy));
> + } else {
> + /* 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 */
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + val, 0);
> + }
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + return ret;
> +}
> +
> +static int inv_icm42600_trigger_validate(struct iio_trigger *trig,
> + struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_trigger_get_drvdata(trig);
> +
> + if (iio_device_get_drvdata(indio_dev) != st)
> + return -ENODEV;
> +
> + return 0;
> +}
> +
> +static const struct iio_trigger_ops inv_icm42600_trigger_ops = {
> + .set_trigger_state = &inv_icm42600_trigger_set_state,
> + .validate_device = &inv_icm42600_trigger_validate,
> +};
> +
> +int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> + int irq_type)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int val;
> + int ret;
> +
> + st->trigger = devm_iio_trigger_alloc(dev, "%s-dev", st->name);
> + if (!st->trigger)
> + return -ENOMEM;
> +
> + st->trigger->dev.parent = dev;
> + st->trigger->ops = &inv_icm42600_trigger_ops;
> + iio_trigger_set_drvdata(st->trigger, st);
> +
> + /* configure INT1 with correct mode */
> + /* falling or both-edge */
> + if (irq_type & IRQF_TRIGGER_FALLING) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW;
> + } else if (irq_type == IRQF_TRIGGER_RISING) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_HIGH;
> + } else if (irq_type == IRQF_TRIGGER_LOW) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
> + INV_ICM42600_INT_CONFIG_INT1_LATCHED;
> + } else if (irq_type == IRQF_TRIGGER_HIGH) {
> + val = INV_ICM42600_INT_CONFIG_INT1_ACTIVE_LOW |
> + INV_ICM42600_INT_CONFIG_INT1_LATCHED;
> + } else {
> + dev_err(dev, "invalid interrupt type %#x\n", irq_type);
> + return -EINVAL;
> + }
> + 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;
> +
> + ret = devm_request_threaded_irq(dev, irq,
> + inv_icm42600_trigger_timestamp,
> + inv_icm42600_trigger_int_handler,
> + irq_type, "inv_icm42600", st);
> + if (ret)
> + return ret;
> +
> + return devm_iio_trigger_register(dev, st->trigger);
> +}
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (7 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 08/12] iio: imu: inv_icm42600: add device interrupt trigger Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 14:19 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping Jean-Baptiste Maneyrol
` (2 subsequent siblings)
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Use triggered buffer by parsing FIFO data read in device trigger.
Support hwfifo watermark by multiplexing gyro and accel settings.
Support hwfifo flush.
Simply use interrupt timestamp first.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
drivers/iio/imu/inv_icm42600/Kconfig | 3 +-
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.c | 353 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 162 ++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 23 ++
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 183 +++++++++
.../imu/inv_icm42600/inv_icm42600_trigger.c | 15 +-
9 files changed, 928 insertions(+), 3 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
index 7b3eaeb2aa4a..8c0969319c49 100644
--- a/drivers/iio/imu/inv_icm42600/Kconfig
+++ b/drivers/iio/imu/inv_icm42600/Kconfig
@@ -2,7 +2,8 @@
config INV_ICM42600
tristate
- select IIO_TRIGGER
+ select IIO_BUFFER
+ select IIO_TRIGGERED_BUFFER
config INV_ICM42600_I2C
tristate "InvenSense ICM-426xx I2C driver"
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index e1f2aacbe888..d6732118010c 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
inv-icm42600-y += inv_icm42600_accel.o
inv-icm42600-y += inv_icm42600_temp.o
inv-icm42600-y += inv_icm42600_trigger.o
+inv-icm42600-y += inv_icm42600_buffer.o
obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
inv-icm42600-i2c-y += inv_icm42600_i2c.o
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 175c1f67faee..947ca4dd245b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -15,6 +15,8 @@
#include <linux/iio/iio.h>
#include <linux/iio/trigger.h>
+#include "inv_icm42600_buffer.h"
+
enum inv_icm42600_chip {
INV_CHIP_ICM42600,
INV_CHIP_ICM42602,
@@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
* @indio_gyro: gyroscope IIO device.
* @indio_accel: accelerometer IIO device.
* @trigger: device internal interrupt trigger
+ * @fifo: FIFO management structure.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -138,6 +141,7 @@ struct inv_icm42600_state {
struct iio_dev *indio_gyro;
struct iio_dev *indio_accel;
struct iio_trigger *trigger;
+ struct inv_icm42600_fifo fifo;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -378,8 +382,12 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+
int inv_icm42600_accel_init(struct inv_icm42600_state *st);
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+
int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
int irq_type);
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 74dac5f283d4..4206be54d057 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -10,9 +10,13 @@
#include <linux/regmap.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -46,6 +50,7 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_Y,
INV_ICM42600_ACCEL_SCAN_Z,
INV_ICM42600_ACCEL_SCAN_TEMP,
+ INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
@@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
inv_icm42600_accel_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
};
+/* IIO buffer data */
+struct inv_icm42600_accel_buffer {
+ struct inv_icm42600_fifo_sensor_data accel;
+ int8_t temp;
+ int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
+ (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
+ BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
+ BIT(INV_ICM42600_ACCEL_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_accel_scan_masks[] = {
+ /* 3-axis accel + temperature */
+ INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+ 0,
+};
+
+static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
+{
+ struct iio_poll_func *pf = _data;
+ struct iio_dev *indio_dev = pf->indio_dev;
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t fifo_nb = st->fifo.nb.total;
+ int ret;
+
+ /* exit if no sample */
+ if (fifo_nb == 0)
+ goto out;
+
+ ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
+ if (ret)
+ dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
+ ret);
+
+out:
+ iio_trigger_notify_done(indio_dev->trig);
+ return IRQ_HANDLED;
+}
+
+/* enable accelerometer sensor and FIFO write */
+static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
+ const unsigned long *scan_mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int fifo_en = 0;
+ unsigned int sleep_temp = 0;
+ unsigned int sleep_accel = 0;
+ unsigned int sleep;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+ /* enable temp sensor */
+ ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_TEMP;
+ }
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
+ /* enable accel sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_ACCEL;
+ }
+
+ /* update data FIFO write and FIFO watermark */
+ ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+ if (ret)
+ goto out_unlock;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ /* sleep maximum required time */
+ if (sleep_accel > sleep_temp)
+ sleep = sleep_accel;
+ else
+ sleep = sleep_temp;
+ if (sleep)
+ msleep(sleep);
+ return ret;
+}
+
static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
struct iio_chan_spec const *chan,
int16_t *val)
@@ -250,6 +347,8 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
@@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
}
}
+static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
+ unsigned int val)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ st->fifo.watermark.accel = val;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
+static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
+ unsigned int count)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (count == 0)
+ return 0;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+ if (!ret)
+ ret = st->fifo.nb.accel;
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
static const struct iio_info inv_icm42600_accel_info = {
.read_raw = inv_icm42600_accel_read_raw,
.read_avail = inv_icm42600_accel_read_avail,
.write_raw = inv_icm42600_accel_write_raw,
.write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
.debugfs_reg_access = inv_icm42600_debugfs_reg,
+ .update_scan_mode = inv_icm42600_accel_update_scan_mode,
+ .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
+ .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
};
int inv_icm42600_accel_init(struct inv_icm42600_state *st)
@@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
struct device *dev = regmap_get_device(st->map);
const char *name;
struct iio_dev *indio_dev;
+ int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
if (!name)
@@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = inv_icm42600_accel_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+ indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
+
+ ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+ inv_icm42600_accel_handler,
+ &inv_icm42600_buffer_ops);
+ if (ret)
+ return ret;
+
+ indio_dev->trig = iio_trigger_get(st->trigger);
st->indio_accel = indio_dev;
return devm_iio_device_register(dev, st->indio_accel);
}
+
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t accel_nb = st->fifo.nb.accel;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ struct inv_icm42600_accel_buffer buffer;
+
+ /* exit if no accel sample */
+ if (accel_nb == 0)
+ return 0;
+
+ /* parse all fifo packets */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
+ size);
+ /* quit if error or FIFO is empty */
+ if (size <= 0)
+ return size;
+ /* skip packet if no accel data or data is invalid */
+ if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
+ dev_dbg(regmap_get_device(st->map), "skip accel data\n");
+ continue;
+ }
+ memcpy(&buffer.accel, accel, sizeof(buffer.accel));
+ memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ }
+
+ return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
new file mode 100644
index 000000000000..b428abdc92ee
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -0,0 +1,353 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
+{
+ uint32_t period_gyro, period_accel, period;
+
+ if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
+ period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+ else
+ period_gyro = U32_MAX;
+
+ if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
+ period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+ else
+ period_accel = U32_MAX;
+
+ if (period_gyro <= period_accel)
+ period = period_gyro;
+ else
+ period = period_accel;
+
+ st->fifo.period = period;
+}
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+ unsigned int fifo_en)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /* update only FIFO EN bits */
+ mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
+ INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
+ INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
+ INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+
+ val = 0;
+ if (fifo_en & INV_ICM42600_SENSOR_GYRO)
+ val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
+ if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
+ val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
+ if (fifo_en & INV_ICM42600_SENSOR_TEMP)
+ val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
+
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+ mask, val);
+ if (ret)
+ return ret;
+
+ st->fifo.en = fifo_en;
+ inv_icm42600_buffer_update_fifo_period(st);
+
+ return 0;
+}
+
+static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
+{
+ size_t packet_size;
+
+ if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
+ (fifo_en & INV_ICM42600_SENSOR_ACCEL))
+ packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+ else
+ packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+
+ return packet_size;
+}
+
+static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
+ size_t packet_size)
+{
+ size_t wm_size;
+ unsigned int wm;
+
+ wm_size = watermark * packet_size;
+ if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
+ wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
+
+ wm = wm_size / packet_size;
+
+ return wm;
+}
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
+{
+ size_t packet_size, wm_size;
+ unsigned int wm_gyro, wm_accel, watermark;
+ uint32_t period_gyro, period_accel, period;
+ int64_t latency_gyro, latency_accel, latency;
+ unsigned int mask, val;
+ bool restore;
+ __le16 raw_wm;
+ int ret;
+
+ packet_size = inv_icm42600_get_packet_size(st->fifo.en);
+
+ /* get minimal latency, depending on sensor watermark and odr */
+ wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
+ packet_size);
+ wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
+ packet_size);
+ period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
+ period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
+ latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
+ latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
+ if (latency_gyro == 0) {
+ latency = latency_accel;
+ watermark = wm_accel;
+ } else if (latency_accel == 0) {
+ latency = latency_gyro;
+ watermark = wm_gyro;
+ } else {
+ /* compute the smallest latency that is a multiple of both */
+ if (latency_gyro <= latency_accel) {
+ latency = latency_gyro;
+ latency -= latency_accel % latency_gyro;
+ } else {
+ latency = latency_accel;
+ latency -= latency_gyro % latency_accel;
+ }
+ /* use the shortest period */
+ if (period_gyro <= period_accel)
+ period = period_gyro;
+ else
+ period = period_accel;
+ /* all this works because periods are multiple of each others */
+ watermark = div_s64(latency, period);
+ if (watermark < 1)
+ watermark = 1;
+ }
+ wm_size = watermark * packet_size;
+ dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
+ watermark, wm_size);
+
+ /* changing FIFO watermark requires to turn off watermark interrupt */
+ mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ val = 0;
+ ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ mask, val, &restore);
+ if (ret)
+ return ret;
+
+ raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
+ ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
+ &raw_wm, sizeof(raw_wm));
+ if (ret)
+ return ret;
+
+ /* restore watermark interrupt */
+ if (restore) {
+ mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ mask, val);
+ if (ret)
+ return ret;
+ }
+
+ return 0;
+}
+
+static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct device *dev = regmap_get_device(st->map);
+
+ pm_runtime_get_sync(dev);
+
+ return 0;
+}
+
+static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int sensor;
+ unsigned int *watermark;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int sleep = 0;
+ int ret;
+
+ if (indio_dev == st->indio_gyro) {
+ sensor = INV_ICM42600_SENSOR_GYRO;
+ watermark = &st->fifo.watermark.gyro;
+ } else if (indio_dev == st->indio_accel) {
+ sensor = INV_ICM42600_SENSOR_ACCEL;
+ watermark = &st->fifo.watermark.accel;
+ } else {
+ return -EINVAL;
+ }
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
+ if (ret)
+ goto out_unlock;
+
+ *watermark = 0;
+ ret = inv_icm42600_buffer_update_watermark(st);
+ if (ret)
+ goto out_unlock;
+
+ conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
+ if (sensor == INV_ICM42600_SENSOR_GYRO)
+ ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
+ else
+ ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ if (sleep)
+ msleep(sleep);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
+ .preenable = inv_icm42600_buffer_preenable,
+ .postenable = iio_triggered_buffer_postenable,
+ .predisable = iio_triggered_buffer_predisable,
+ .postdisable = inv_icm42600_buffer_postdisable,
+};
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+ unsigned int max)
+{
+ struct device *dev = regmap_get_device(st->map);
+ __be16 raw_fifo_count;
+ size_t max_count;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ int ret;
+
+ /* reset all samples counters */
+ st->fifo.count = 0;
+ st->fifo.nb.gyro = 0;
+ st->fifo.nb.accel = 0;
+ st->fifo.nb.total = 0;
+
+ /* compute maximum FIFO read size */
+ if (max == 0)
+ max_count = sizeof(st->fifo.data);
+ else
+ max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
+
+ /* read FIFO count value */
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
+ &raw_fifo_count, sizeof(raw_fifo_count));
+ if (ret)
+ return ret;
+ st->fifo.count = be16_to_cpu(raw_fifo_count);
+ dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
+
+ /* check and sanitize FIFO count value */
+ if (st->fifo.count == 0)
+ return 0;
+ if (st->fifo.count > max_count)
+ st->fifo.count = max_count;
+
+ /* read all FIFO data in internal buffer */
+ ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
+ st->fifo.data, st->fifo.count);
+ if (ret)
+ return ret;
+
+ /* compute number of samples for each sensor */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ if (size <= 0)
+ break;
+ if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
+ st->fifo.nb.gyro++;
+ if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
+ st->fifo.nb.accel++;
+ st->fifo.nb.total++;
+ }
+
+ return 0;
+}
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+ unsigned int count)
+{
+ int64_t ts_gyro, ts_accel;
+ int ret;
+
+ dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
+
+ ts_gyro = iio_get_time_ns(st->indio_gyro);
+ ts_accel = iio_get_time_ns(st->indio_accel);
+ ret = inv_icm42600_buffer_fifo_read(st, count);
+ if (ret)
+ return ret;
+
+ if (st->fifo.nb.total == 0)
+ return 0;
+
+ ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
+ if (ret)
+ return ret;
+
+ return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
+}
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
+{
+ unsigned int mask, val;
+ int ret;
+
+ /*
+ * Default FIFO configuration (bits 7 to 5)
+ * - use invalid value
+ * - FIFO count in bytes
+ * - FIFO count in big endian
+ */
+ mask = GENMASK(7, 5);
+ val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
+ ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
+ mask, val);
+ if (ret)
+ return ret;
+
+ /*
+ * Enable FIFO partial read and continuous watermark interrupt.
+ * Disable all FIFO EN bits.
+ */
+ mask = GENMASK(6, 5) | GENMASK(3, 0);
+ val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
+ INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
+ mask, val);
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
new file mode 100644
index 000000000000..74b91c0e664b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
@@ -0,0 +1,162 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_BUFFER_H_
+#define INV_ICM42600_BUFFER_H_
+
+#include <linux/kernel.h>
+#include <linux/bits.h>
+
+struct inv_icm42600_state;
+
+#define INV_ICM42600_SENSOR_GYRO BIT(0)
+#define INV_ICM42600_SENSOR_ACCEL BIT(1)
+#define INV_ICM42600_SENSOR_TEMP BIT(2)
+
+struct inv_icm42600_fifo {
+ unsigned int en;
+ uint32_t period;
+ struct {
+ unsigned int gyro;
+ unsigned int accel;
+ } watermark;
+ size_t count;
+ struct {
+ size_t gyro;
+ size_t accel;
+ size_t total;
+ } nb;
+ uint8_t data[2080];
+};
+
+/* FIFO header: 1 byte */
+#define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
+#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
+#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
+#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
+#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
+#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
+
+/* FIFO data packet */
+struct inv_icm42600_fifo_sensor_data {
+ __be16 x;
+ __be16 y;
+ __be16 z;
+} __packed;
+#define INV_ICM42600_FIFO_DATA_INVALID -32768
+
+struct inv_icm42600_fifo_1sensor_packet {
+ uint8_t header;
+ struct inv_icm42600_fifo_sensor_data data;
+ int8_t temp;
+} __packed;
+#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
+
+struct inv_icm42600_fifo_2sensors_packet {
+ uint8_t header;
+ struct inv_icm42600_fifo_sensor_data accel;
+ struct inv_icm42600_fifo_sensor_data gyro;
+ int8_t temp;
+ __be16 timestamp;
+} __packed;
+#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
+
+static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
+{
+ return be16_to_cpu(d);
+}
+
+static inline bool
+inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
+{
+ int16_t x, y, z;
+
+ x = inv_icm42600_fifo_get_sensor_data(s->x);
+ y = inv_icm42600_fifo_get_sensor_data(s->y);
+ z = inv_icm42600_fifo_get_sensor_data(s->z);
+
+ if (x == INV_ICM42600_FIFO_DATA_INVALID &&
+ y == INV_ICM42600_FIFO_DATA_INVALID &&
+ z == INV_ICM42600_FIFO_DATA_INVALID)
+ return false;
+
+ return true;
+}
+
+static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
+ const void **accel, const void **gyro, const void **temp,
+ const void **timestamp, unsigned int *odr)
+{
+ const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
+ const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
+ uint8_t header = *((const uint8_t *)packet);
+
+ /* FIFO empty */
+ if (header & INV_ICM42600_FIFO_HEADER_MSG) {
+ *accel = NULL;
+ *gyro = NULL;
+ *temp = NULL;
+ *timestamp = NULL;
+ *odr = 0;
+ return 0;
+ }
+
+ /* handle odr flags */
+ *odr = 0;
+ if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
+ *odr |= INV_ICM42600_SENSOR_GYRO;
+ if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
+ *odr |= INV_ICM42600_SENSOR_ACCEL;
+
+ /* accel + gyro */
+ if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
+ (header & INV_ICM42600_FIFO_HEADER_GYRO)) {
+ *accel = &pack2->accel;
+ *gyro = &pack2->gyro;
+ *temp = &pack2->temp;
+ *timestamp = &pack2->timestamp;
+ return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
+ }
+
+ /* accel only */
+ if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
+ *accel = &pack1->data;
+ *gyro = NULL;
+ *temp = &pack1->temp;
+ *timestamp = NULL;
+ return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+ }
+
+ /* gyro only */
+ if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
+ *accel = NULL;
+ *gyro = &pack1->data;
+ *temp = &pack1->temp;
+ *timestamp = NULL;
+ return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
+ }
+
+ /* invalid packet if here */
+ return -EINVAL;
+}
+
+extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
+
+int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
+
+void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
+ unsigned int fifo_en);
+
+int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
+
+int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
+ unsigned int max);
+
+int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
+ unsigned int count);
+
+#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 1102c54396e3..689089065ff9 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -14,6 +14,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
{
@@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ /* setup FIFO buffer */
+ ret = inv_icm42600_buffer_init(st);
+ if (ret)
+ return ret;
+
/* setup interrupt trigger */
ret = inv_icm42600_trigger_init(st, irq, irq_type);
if (ret)
@@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
goto out_unlock;
}
+ /* disable FIFO data streaming */
+ if (iio_buffer_enabled(st->indio_gyro) ||
+ iio_buffer_enabled(st->indio_accel)) {
+ /* set FIFO in bypass mode */
+ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+ INV_ICM42600_FIFO_CONFIG_BYPASS);
+ if (ret)
+ goto out_unlock;
+ }
+
ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
INV_ICM42600_SENSOR_MODE_OFF, false,
NULL);
@@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
if (ret)
goto out_unlock;
+ /* restore FIFO data streaming */
+ if (iio_buffer_enabled(st->indio_gyro) ||
+ iio_buffer_enabled(st->indio_accel)) {
+ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
+ INV_ICM42600_FIFO_CONFIG_STREAM);
+ }
+
out_unlock:
mutex_unlock(&st->lock);
return ret;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index c0164ab2830e..dafb104abc77 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -10,9 +10,13 @@
#include <linux/regmap.h>
#include <linux/delay.h>
#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
+#include "inv_icm42600_buffer.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -46,6 +50,7 @@ enum inv_icm42600_gyro_scan {
INV_ICM42600_GYRO_SCAN_Y,
INV_ICM42600_GYRO_SCAN_Z,
INV_ICM42600_GYRO_SCAN_TEMP,
+ INV_ICM42600_GYRO_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
@@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
inv_icm42600_gyro_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
};
+/* IIO buffer data */
+struct inv_icm42600_gyro_buffer {
+ struct inv_icm42600_fifo_sensor_data gyro;
+ int8_t temp;
+ int64_t timestamp;
+};
+
+#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
+ (BIT(INV_ICM42600_GYRO_SCAN_X) | \
+ BIT(INV_ICM42600_GYRO_SCAN_Y) | \
+ BIT(INV_ICM42600_GYRO_SCAN_Z))
+
+#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
+
+static const unsigned long inv_icm42600_gyro_scan_masks[] = {
+ /* 3-axis gyro + temperature */
+ INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
+ 0,
+};
+
+static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
+{
+ struct iio_poll_func *pf = _data;
+ struct iio_dev *indio_dev = pf->indio_dev;
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t fifo_nb = st->fifo.nb.total;
+ int ret;
+
+ /* exit if no sample */
+ if (fifo_nb == 0)
+ goto out;
+
+ ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
+ if (ret)
+ dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
+ ret);
+
+out:
+ iio_trigger_notify_done(indio_dev->trig);
+ return IRQ_HANDLED;
+}
+
+/* enable gyroscope sensor and FIFO write */
+static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
+ const unsigned long *scan_mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int fifo_en = 0;
+ unsigned int sleep_gyro = 0;
+ unsigned int sleep_temp = 0;
+ unsigned int sleep;
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
+ /* enable temp sensor */
+ ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_TEMP;
+ }
+
+ if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
+ /* enable gyro sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
+ if (ret)
+ goto out_unlock;
+ fifo_en |= INV_ICM42600_SENSOR_GYRO;
+ }
+
+ /* update data FIFO write and FIFO watermark */
+ ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
+ if (ret)
+ goto out_unlock;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ /* sleep maximum required time */
+ if (sleep_gyro > sleep_temp)
+ sleep = sleep_gyro;
+ else
+ sleep = sleep_temp;
+ if (sleep)
+ msleep(sleep);
+ return ret;
+}
+
static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
struct iio_chan_spec const *chan,
int16_t *val)
@@ -262,6 +359,8 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ inv_icm42600_buffer_update_fifo_period(st);
+ inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
pm_runtime_mark_last_busy(dev);
@@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
}
}
+static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
+ unsigned int val)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ mutex_lock(&st->lock);
+
+ st->fifo.watermark.gyro = val;
+ ret = inv_icm42600_buffer_update_watermark(st);
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
+static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
+ unsigned int count)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (count == 0)
+ return 0;
+
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_buffer_hwfifo_flush(st, count);
+ if (!ret)
+ ret = st->fifo.nb.gyro;
+
+ mutex_unlock(&st->lock);
+
+ return ret;
+}
+
static const struct iio_info inv_icm42600_gyro_info = {
.read_raw = inv_icm42600_gyro_read_raw,
.read_avail = inv_icm42600_gyro_read_avail,
.write_raw = inv_icm42600_gyro_write_raw,
.write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
.debugfs_reg_access = inv_icm42600_debugfs_reg,
+ .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
+ .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
+ .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
};
int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
@@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
struct device *dev = regmap_get_device(st->map);
const char *name;
struct iio_dev *indio_dev;
+ int ret;
name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
if (!name)
@@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
indio_dev->modes = INDIO_DIRECT_MODE;
indio_dev->channels = inv_icm42600_gyro_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+ indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
+
+ ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
+ inv_icm42600_gyro_handler,
+ &inv_icm42600_buffer_ops);
+ if (ret)
+ return ret;
+
+ indio_dev->trig = iio_trigger_get(st->trigger);
st->indio_gyro = indio_dev;
return devm_iio_device_register(dev, st->indio_gyro);
}
+
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ const size_t gyro_nb = st->fifo.nb.gyro;
+ ssize_t i, size;
+ const void *accel, *gyro, *temp, *timestamp;
+ unsigned int odr;
+ struct inv_icm42600_gyro_buffer buffer;
+
+ /* exit if no gyro sample */
+ if (gyro_nb == 0)
+ return 0;
+
+ /* parse all fifo packets */
+ for (i = 0; i < st->fifo.count; i += size) {
+ size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
+ &accel, &gyro, &temp, ×tamp, &odr);
+ dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
+ size);
+ /* quit if error or FIFO is empty */
+ if (size <= 0)
+ return size;
+ /* skip packet if no gyro data or data is invalid */
+ if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
+ dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
+ continue;
+ }
+ memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
+ memcpy(&buffer.temp, temp, sizeof(buffer.temp));
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ }
+
+ return 0;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
index 7a5e76305f0b..5667e0204722 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
@@ -13,6 +13,7 @@
#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_buffer.h"
static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
{
@@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
dev_warn(dev, "FIFO full data lost!\n");
/* FIFO threshold reached */
- if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
- iio_trigger_poll_chained(st->trigger);
+ if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
+ ret = inv_icm42600_buffer_fifo_read(st, 0);
+ if (ret)
+ dev_err(dev, "FIFO read error %d\n", ret);
+ } else {
+ st->fifo.count = 0;
+ st->fifo.nb.gyro = 0;
+ st->fifo.nb.accel = 0;
+ st->fifo.nb.total = 0;
+ }
+
+ iio_trigger_poll_chained(st->trigger);
out_unlock:
mutex_unlock(&st->lock);
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
2020-05-07 14:42 ` [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices Jean-Baptiste Maneyrol
@ 2020-05-08 14:19 ` Jonathan Cameron
2020-05-18 15:32 ` Jean-Baptiste Maneyrol
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 14:19 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:19 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Use triggered buffer by parsing FIFO data read in device trigger.
> Support hwfifo watermark by multiplexing gyro and accel settings.
> Support hwfifo flush.
>
> Simply use interrupt timestamp first.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
> drivers/iio/imu/inv_icm42600/Kconfig | 3 +-
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 353 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.h | 162 ++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 23 ++
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 183 +++++++++
> .../imu/inv_icm42600/inv_icm42600_trigger.c | 15 +-
> 9 files changed, 928 insertions(+), 3 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
>
> diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
> index 7b3eaeb2aa4a..8c0969319c49 100644
> --- a/drivers/iio/imu/inv_icm42600/Kconfig
> +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> @@ -2,7 +2,8 @@
>
> config INV_ICM42600
> tristate
> - select IIO_TRIGGER
> + select IIO_BUFFER
> + select IIO_TRIGGERED_BUFFER
>
> config INV_ICM42600_I2C
> tristate "InvenSense ICM-426xx I2C driver"
> diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
> index e1f2aacbe888..d6732118010c 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
> inv-icm42600-y += inv_icm42600_accel.o
> inv-icm42600-y += inv_icm42600_temp.o
> inv-icm42600-y += inv_icm42600_trigger.o
> +inv-icm42600-y += inv_icm42600_buffer.o
>
> obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
> inv-icm42600-i2c-y += inv_icm42600_i2c.o
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> index 175c1f67faee..947ca4dd245b 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -15,6 +15,8 @@
> #include <linux/iio/iio.h>
> #include <linux/iio/trigger.h>
>
> +#include "inv_icm42600_buffer.h"
> +
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> INV_CHIP_ICM42602,
> @@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
> * @indio_gyro: gyroscope IIO device.
> * @indio_accel: accelerometer IIO device.
> * @trigger: device internal interrupt trigger
> + * @fifo: FIFO management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -138,6 +141,7 @@ struct inv_icm42600_state {
> struct iio_dev *indio_gyro;
> struct iio_dev *indio_accel;
> struct iio_trigger *trigger;
> + struct inv_icm42600_fifo fifo;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -378,8 +382,12 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +
> int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> int irq_type);
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 74dac5f283d4..4206be54d057 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -10,9 +10,13 @@
> #include <linux/regmap.h>
> #include <linux/delay.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -46,6 +50,7 @@ enum inv_icm42600_accel_scan {
> INV_ICM42600_ACCEL_SCAN_Y,
> INV_ICM42600_ACCEL_SCAN_Z,
> INV_ICM42600_ACCEL_SCAN_TEMP,
> + INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
> };
>
> static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
> @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
> INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
> inv_icm42600_accel_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> };
>
> +/* IIO buffer data */
> +struct inv_icm42600_accel_buffer {
> + struct inv_icm42600_fifo_sensor_data accel;
> + int8_t temp;
> + int64_t timestamp;
> +};
> +
> +#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
> + (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
> + BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
> + BIT(INV_ICM42600_ACCEL_SCAN_Z))
> +
> +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
> +
> +static const unsigned long inv_icm42600_accel_scan_masks[] = {
> + /* 3-axis accel + temperature */
> + INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
> + 0,
> +};
> +
> +static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
> +{
> + struct iio_poll_func *pf = _data;
> + struct iio_dev *indio_dev = pf->indio_dev;
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t fifo_nb = st->fifo.nb.total;
> + int ret;
> +
> + /* exit if no sample */
> + if (fifo_nb == 0)
> + goto out;
> +
> + ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
> + if (ret)
> + dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
> + ret);
> +
> +out:
> + iio_trigger_notify_done(indio_dev->trig);
> + return IRQ_HANDLED;
> +}
> +
> +/* enable accelerometer sensor and FIFO write */
> +static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
> + const unsigned long *scan_mask)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int fifo_en = 0;
> + unsigned int sleep_temp = 0;
> + unsigned int sleep_accel = 0;
> + unsigned int sleep;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
> + /* enable temp sensor */
> + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_TEMP;
> + }
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
> + /* enable accel sensor */
> + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
> + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_ACCEL;
> + }
> +
> + /* update data FIFO write and FIFO watermark */
> + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> + if (ret)
> + goto out_unlock;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + /* sleep maximum required time */
> + if (sleep_accel > sleep_temp)
> + sleep = sleep_accel;
> + else
> + sleep = sleep_temp;
> + if (sleep)
> + msleep(sleep);
> + return ret;
> +}
> +
> static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
> struct iio_chan_spec const *chan,
> int16_t *val)
> @@ -250,6 +347,8 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
>
> pm_runtime_mark_last_busy(dev);
> @@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
> }
> }
>
> +static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
> + unsigned int val)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + st->fifo.watermark.accel = val;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> +static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
> + unsigned int count)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + if (count == 0)
> + return 0;
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
> + if (!ret)
> + ret = st->fifo.nb.accel;
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> static const struct iio_info inv_icm42600_accel_info = {
> .read_raw = inv_icm42600_accel_read_raw,
> .read_avail = inv_icm42600_accel_read_avail,
> .write_raw = inv_icm42600_accel_write_raw,
> .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
> .debugfs_reg_access = inv_icm42600_debugfs_reg,
> + .update_scan_mode = inv_icm42600_accel_update_scan_mode,
> + .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
> + .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
> };
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> @@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> struct iio_dev *indio_dev;
> + int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
> if (!name)
> @@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> indio_dev->modes = INDIO_DIRECT_MODE;
> indio_dev->channels = inv_icm42600_accel_channels;
> indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
> + indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
> +
> + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
> + inv_icm42600_accel_handler,
> + &inv_icm42600_buffer_ops);
> + if (ret)
> + return ret;
> +
> + indio_dev->trig = iio_trigger_get(st->trigger);
>
> st->indio_accel = indio_dev;
> return devm_iio_device_register(dev, st->indio_accel);
> }
> +
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t accel_nb = st->fifo.nb.accel;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + struct inv_icm42600_accel_buffer buffer;
> +
> + /* exit if no accel sample */
> + if (accel_nb == 0)
> + return 0;
> +
> + /* parse all fifo packets */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
> + size);
> + /* quit if error or FIFO is empty */
> + if (size <= 0)
> + return size;
> + /* skip packet if no accel data or data is invalid */
> + if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
> + dev_dbg(regmap_get_device(st->map), "skip accel data\n");
> + continue;
> + }
> + memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + }
> +
> + return 0;
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> new file mode 100644
> index 000000000000..b428abdc92ee
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,353 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/delay.h>
> +#include <linux/math64.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
> +
> +#include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
> +
> +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
> +{
> + uint32_t period_gyro, period_accel, period;
> +
> + if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
> + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
> + else
> + period_gyro = U32_MAX;
> +
> + if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
> + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
> + else
> + period_accel = U32_MAX;
> +
> + if (period_gyro <= period_accel)
> + period = period_gyro;
> + else
> + period = period_accel;
> +
> + st->fifo.period = period;
> +}
> +
> +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
> + unsigned int fifo_en)
> +{
> + unsigned int mask, val;
> + int ret;
> +
> + /* update only FIFO EN bits */
> + mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
> + INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
> + INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
> + INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
> +
> + val = 0;
> + if (fifo_en & INV_ICM42600_SENSOR_GYRO)
> + val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
> + if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
> + val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
> + if (fifo_en & INV_ICM42600_SENSOR_TEMP)
> + val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
> +
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
> + mask, val);
> + if (ret)
> + return ret;
> +
> + st->fifo.en = fifo_en;
> + inv_icm42600_buffer_update_fifo_period(st);
> +
> + return 0;
> +}
> +
> +static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
> +{
> + size_t packet_size;
> +
> + if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
> + (fifo_en & INV_ICM42600_SENSOR_ACCEL))
> + packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
> + else
> + packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> +
> + return packet_size;
> +}
> +
> +static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
> + size_t packet_size)
> +{
> + size_t wm_size;
> + unsigned int wm;
> +
> + wm_size = watermark * packet_size;
> + if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
> + wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
> +
> + wm = wm_size / packet_size;
> +
> + return wm;
> +}
> +
> +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
> +{
> + size_t packet_size, wm_size;
> + unsigned int wm_gyro, wm_accel, watermark;
> + uint32_t period_gyro, period_accel, period;
> + int64_t latency_gyro, latency_accel, latency;
> + unsigned int mask, val;
> + bool restore;
> + __le16 raw_wm;
> + int ret;
> +
> + packet_size = inv_icm42600_get_packet_size(st->fifo.en);
> +
> + /* get minimal latency, depending on sensor watermark and odr */
> + wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
> + packet_size);
> + wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
> + packet_size);
> + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
> + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
> + latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
> + latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
> + if (latency_gyro == 0) {
> + latency = latency_accel;
> + watermark = wm_accel;
> + } else if (latency_accel == 0) {
> + latency = latency_gyro;
> + watermark = wm_gyro;
> + } else {
> + /* compute the smallest latency that is a multiple of both */
> + if (latency_gyro <= latency_accel) {
> + latency = latency_gyro;
> + latency -= latency_accel % latency_gyro;
> + } else {
> + latency = latency_accel;
> + latency -= latency_gyro % latency_accel;
> + }
> + /* use the shortest period */
> + if (period_gyro <= period_accel)
> + period = period_gyro;
> + else
> + period = period_accel;
> + /* all this works because periods are multiple of each others */
> + watermark = div_s64(latency, period);
> + if (watermark < 1)
> + watermark = 1;
> + }
> + wm_size = watermark * packet_size;
> + dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
> + watermark, wm_size);
> +
> + /* changing FIFO watermark requires to turn off watermark interrupt */
> + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + val = 0;
> + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + mask, val, &restore);
> + if (ret)
> + return ret;
> +
> + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> + &raw_wm, sizeof(raw_wm));
> + if (ret)
> + return ret;
> +
> + /* restore watermark interrupt */
> + if (restore) {
> + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + mask, val);
> + if (ret)
> + return ret;
> + }
> +
> + return 0;
> +}
> +
> +static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> +
> + pm_runtime_get_sync(dev);
> +
> + return 0;
> +}
> +
> +static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int sensor;
> + unsigned int *watermark;
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int sleep = 0;
> + int ret;
> +
> + if (indio_dev == st->indio_gyro) {
> + sensor = INV_ICM42600_SENSOR_GYRO;
> + watermark = &st->fifo.watermark.gyro;
> + } else if (indio_dev == st->indio_accel) {
> + sensor = INV_ICM42600_SENSOR_ACCEL;
> + watermark = &st->fifo.watermark.accel;
> + } else {
> + return -EINVAL;
> + }
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
> + if (ret)
> + goto out_unlock;
> +
> + *watermark = 0;
> + ret = inv_icm42600_buffer_update_watermark(st);
> + if (ret)
> + goto out_unlock;
> +
> + conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
> + if (sensor == INV_ICM42600_SENSOR_GYRO)
> + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
> + else
> + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + if (sleep)
> + msleep(sleep);
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> +
> + return ret;
> +}
> +
> +const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
> + .preenable = inv_icm42600_buffer_preenable,
> + .postenable = iio_triggered_buffer_postenable,
> + .predisable = iio_triggered_buffer_predisable,
> + .postdisable = inv_icm42600_buffer_postdisable,
> +};
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> + unsigned int max)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + __be16 raw_fifo_count;
> + size_t max_count;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + int ret;
> +
> + /* reset all samples counters */
> + st->fifo.count = 0;
> + st->fifo.nb.gyro = 0;
> + st->fifo.nb.accel = 0;
> + st->fifo.nb.total = 0;
> +
> + /* compute maximum FIFO read size */
> + if (max == 0)
> + max_count = sizeof(st->fifo.data);
> + else
> + max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
> +
> + /* read FIFO count value */
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
> + &raw_fifo_count, sizeof(raw_fifo_count));
> + if (ret)
> + return ret;
> + st->fifo.count = be16_to_cpu(raw_fifo_count);
> + dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
> +
> + /* check and sanitize FIFO count value */
> + if (st->fifo.count == 0)
> + return 0;
> + if (st->fifo.count > max_count)
> + st->fifo.count = max_count;
> +
> + /* read all FIFO data in internal buffer */
> + ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
> + st->fifo.data, st->fifo.count);
> + if (ret)
> + return ret;
> +
> + /* compute number of samples for each sensor */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + if (size <= 0)
> + break;
> + if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
> + st->fifo.nb.gyro++;
> + if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
> + st->fifo.nb.accel++;
> + st->fifo.nb.total++;
> + }
> +
> + return 0;
> +}
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> + unsigned int count)
> +{
> + int64_t ts_gyro, ts_accel;
> + int ret;
> +
> + dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
> +
> + ts_gyro = iio_get_time_ns(st->indio_gyro);
> + ts_accel = iio_get_time_ns(st->indio_accel);
> + ret = inv_icm42600_buffer_fifo_read(st, count);
> + if (ret)
> + return ret;
> +
> + if (st->fifo.nb.total == 0)
> + return 0;
> +
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
> + if (ret)
> + return ret;
> +
> + return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
> +}
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> +{
> + unsigned int mask, val;
> + int ret;
> +
> + /*
> + * Default FIFO configuration (bits 7 to 5)
> + * - use invalid value
> + * - FIFO count in bytes
> + * - FIFO count in big endian
> + */
> + mask = GENMASK(7, 5);
> + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
> + if (ret)
> + return ret;
> +
> + /*
> + * Enable FIFO partial read and continuous watermark interrupt.
> + * Disable all FIFO EN bits.
> + */
> + mask = GENMASK(6, 5) | GENMASK(3, 0);
> + val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
> + INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
> + mask, val);
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> new file mode 100644
> index 000000000000..74b91c0e664b
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> @@ -0,0 +1,162 @@
> +/* SPDX-License-Identifier: GPL-2.0-or-later */
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#ifndef INV_ICM42600_BUFFER_H_
> +#define INV_ICM42600_BUFFER_H_
> +
> +#include <linux/kernel.h>
> +#include <linux/bits.h>
> +
> +struct inv_icm42600_state;
> +
> +#define INV_ICM42600_SENSOR_GYRO BIT(0)
> +#define INV_ICM42600_SENSOR_ACCEL BIT(1)
> +#define INV_ICM42600_SENSOR_TEMP BIT(2)
> +
> +struct inv_icm42600_fifo {
> + unsigned int en;
> + uint32_t period;
> + struct {
> + unsigned int gyro;
> + unsigned int accel;
> + } watermark;
> + size_t count;
> + struct {
> + size_t gyro;
> + size_t accel;
> + size_t total;
> + } nb;
> + uint8_t data[2080];
> +};
> +
> +/* FIFO header: 1 byte */
> +#define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
> +#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
> +#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
> +#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
> +#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
> +#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
> +
> +/* FIFO data packet */
> +struct inv_icm42600_fifo_sensor_data {
> + __be16 x;
> + __be16 y;
> + __be16 z;
> +} __packed;
> +#define INV_ICM42600_FIFO_DATA_INVALID -32768
> +
> +struct inv_icm42600_fifo_1sensor_packet {
> + uint8_t header;
> + struct inv_icm42600_fifo_sensor_data data;
> + int8_t temp;
> +} __packed;
> +#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
> +
> +struct inv_icm42600_fifo_2sensors_packet {
> + uint8_t header;
> + struct inv_icm42600_fifo_sensor_data accel;
> + struct inv_icm42600_fifo_sensor_data gyro;
> + int8_t temp;
> + __be16 timestamp;
> +} __packed;
> +#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
> +
> +static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
> +{
> + return be16_to_cpu(d);
> +}
> +
> +static inline bool
> +inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
> +{
> + int16_t x, y, z;
> +
> + x = inv_icm42600_fifo_get_sensor_data(s->x);
> + y = inv_icm42600_fifo_get_sensor_data(s->y);
> + z = inv_icm42600_fifo_get_sensor_data(s->z);
> +
> + if (x == INV_ICM42600_FIFO_DATA_INVALID &&
> + y == INV_ICM42600_FIFO_DATA_INVALID &&
> + z == INV_ICM42600_FIFO_DATA_INVALID)
> + return false;
> +
> + return true;
> +}
> +
> +static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
Bit big to be in the header..
> + const void **accel, const void **gyro, const void **temp,
> + const void **timestamp, unsigned int *odr)
> +{
> + const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
> + const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
> + uint8_t header = *((const uint8_t *)packet);
> +
> + /* FIFO empty */
> + if (header & INV_ICM42600_FIFO_HEADER_MSG) {
> + *accel = NULL;
> + *gyro = NULL;
> + *temp = NULL;
> + *timestamp = NULL;
> + *odr = 0;
> + return 0;
> + }
> +
> + /* handle odr flags */
> + *odr = 0;
> + if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
> + *odr |= INV_ICM42600_SENSOR_GYRO;
> + if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
> + *odr |= INV_ICM42600_SENSOR_ACCEL;
> +
> + /* accel + gyro */
> + if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
> + (header & INV_ICM42600_FIFO_HEADER_GYRO)) {
> + *accel = &pack2->accel;
> + *gyro = &pack2->gyro;
> + *temp = &pack2->temp;
> + *timestamp = &pack2->timestamp;
> + return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
> + }
> +
> + /* accel only */
> + if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
> + *accel = &pack1->data;
> + *gyro = NULL;
> + *temp = &pack1->temp;
> + *timestamp = NULL;
> + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> + }
> +
> + /* gyro only */
> + if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
> + *accel = NULL;
> + *gyro = &pack1->data;
> + *temp = &pack1->temp;
> + *timestamp = NULL;
> + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> + }
> +
> + /* invalid packet if here */
> + return -EINVAL;
> +}
> +
> +extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
> +
> +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
> +
> +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
> + unsigned int fifo_en);
> +
> +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> + unsigned int max);
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> + unsigned int count);
> +
> +#endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 1102c54396e3..689089065ff9 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -14,6 +14,7 @@
> #include <linux/iio/iio.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
>
> static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
> {
> @@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + /* setup FIFO buffer */
> + ret = inv_icm42600_buffer_init(st);
> + if (ret)
> + return ret;
> +
> /* setup interrupt trigger */
> ret = inv_icm42600_trigger_init(st, irq, irq_type);
> if (ret)
> @@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
> goto out_unlock;
> }
>
> + /* disable FIFO data streaming */
> + if (iio_buffer_enabled(st->indio_gyro) ||
> + iio_buffer_enabled(st->indio_accel)) {
> + /* set FIFO in bypass mode */
> + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> + INV_ICM42600_FIFO_CONFIG_BYPASS);
> + if (ret)
> + goto out_unlock;
> + }
> +
> ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
> INV_ICM42600_SENSOR_MODE_OFF, false,
> NULL);
> @@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
> if (ret)
> goto out_unlock;
>
> + /* restore FIFO data streaming */
> + if (iio_buffer_enabled(st->indio_gyro) ||
> + iio_buffer_enabled(st->indio_accel)) {
> + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> + INV_ICM42600_FIFO_CONFIG_STREAM);
> + }
> +
> out_unlock:
> mutex_unlock(&st->lock);
> return ret;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> index c0164ab2830e..dafb104abc77 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -10,9 +10,13 @@
> #include <linux/regmap.h>
> #include <linux/delay.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -46,6 +50,7 @@ enum inv_icm42600_gyro_scan {
> INV_ICM42600_GYRO_SCAN_Y,
> INV_ICM42600_GYRO_SCAN_Z,
> INV_ICM42600_GYRO_SCAN_TEMP,
> + INV_ICM42600_GYRO_SCAN_TIMESTAMP,
> };
>
> static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
> @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
> INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
> inv_icm42600_gyro_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
> };
>
> +/* IIO buffer data */
> +struct inv_icm42600_gyro_buffer {
> + struct inv_icm42600_fifo_sensor_data gyro;
> + int8_t temp;
> + int64_t timestamp;
> +};
> +
> +#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
> + (BIT(INV_ICM42600_GYRO_SCAN_X) | \
> + BIT(INV_ICM42600_GYRO_SCAN_Y) | \
> + BIT(INV_ICM42600_GYRO_SCAN_Z))
> +
> +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
> +
> +static const unsigned long inv_icm42600_gyro_scan_masks[] = {
> + /* 3-axis gyro + temperature */
> + INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
> + 0,
> +};
> +
> +static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
> +{
> + struct iio_poll_func *pf = _data;
> + struct iio_dev *indio_dev = pf->indio_dev;
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t fifo_nb = st->fifo.nb.total;
> + int ret;
> +
> + /* exit if no sample */
> + if (fifo_nb == 0)
> + goto out;
> +
> + ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
> + if (ret)
> + dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
> + ret);
> +
> +out:
> + iio_trigger_notify_done(indio_dev->trig);
> + return IRQ_HANDLED;
> +}
> +
> +/* enable gyroscope sensor and FIFO write */
> +static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
> + const unsigned long *scan_mask)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int fifo_en = 0;
> + unsigned int sleep_gyro = 0;
> + unsigned int sleep_temp = 0;
> + unsigned int sleep;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
> + /* enable temp sensor */
> + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_TEMP;
> + }
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
> + /* enable gyro sensor */
> + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
> + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_GYRO;
> + }
> +
> + /* update data FIFO write and FIFO watermark */
> + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> + if (ret)
> + goto out_unlock;
blank line
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + /* sleep maximum required time */
> + if (sleep_gyro > sleep_temp)
> + sleep = sleep_gyro;
> + else
> + sleep = sleep_temp;
> + if (sleep)
> + msleep(sleep);
if (sleep_gyro > sleep_temp)
msleep(sleep_gyro);
else
msleep(sleep_temp);
> + return ret;
> +}
> +
> static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
> struct iio_chan_spec const *chan,
> int16_t *val)
> @@ -262,6 +359,8 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
>
> pm_runtime_mark_last_busy(dev);
> @@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
> }
> }
>
> +static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
> + unsigned int val)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + st->fifo.watermark.gyro = val;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> +static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
> + unsigned int count)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + if (count == 0)
> + return 0;
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
> + if (!ret)
> + ret = st->fifo.nb.gyro;
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> static const struct iio_info inv_icm42600_gyro_info = {
> .read_raw = inv_icm42600_gyro_read_raw,
> .read_avail = inv_icm42600_gyro_read_avail,
> .write_raw = inv_icm42600_gyro_write_raw,
> .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
> .debugfs_reg_access = inv_icm42600_debugfs_reg,
> + .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
> + .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
> + .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
> };
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> @@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> struct iio_dev *indio_dev;
> + int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> if (!name)
> @@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> indio_dev->modes = INDIO_DIRECT_MODE;
> indio_dev->channels = inv_icm42600_gyro_channels;
> indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
> + indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
> +
> + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
> + inv_icm42600_gyro_handler,
> + &inv_icm42600_buffer_ops);
> + if (ret)
> + return ret;
> +
> + indio_dev->trig = iio_trigger_get(st->trigger);
>
> st->indio_gyro = indio_dev;
> return devm_iio_device_register(dev, st->indio_gyro);
> }
> +
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t gyro_nb = st->fifo.nb.gyro;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + struct inv_icm42600_gyro_buffer buffer;
> +
> + /* exit if no gyro sample */
> + if (gyro_nb == 0)
> + return 0;
> +
> + /* parse all fifo packets */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
> + size);
> + /* quit if error or FIFO is empty */
> + if (size <= 0)
> + return size;
blank line here.
> + /* skip packet if no gyro data or data is invalid */
> + if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
> + dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
Very noisy logging. I'd drop it for the final version of the driver.
> + continue;
> + }
> + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
buffer.temp = temp;
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + }
> +
> + return 0;
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> index 7a5e76305f0b..5667e0204722 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> @@ -13,6 +13,7 @@
> #include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
>
> static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
> {
> @@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
> dev_warn(dev, "FIFO full data lost!\n");
>
> /* FIFO threshold reached */
> - if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
> - iio_trigger_poll_chained(st->trigger);
> + if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
> + ret = inv_icm42600_buffer_fifo_read(st, 0);
> + if (ret)
> + dev_err(dev, "FIFO read error %d\n", ret);
> + } else {
> + st->fifo.count = 0;
> + st->fifo.nb.gyro = 0;
> + st->fifo.nb.accel = 0;
> + st->fifo.nb.total = 0;
> + }
> +
> + iio_trigger_poll_chained(st->trigger);
>
> out_unlock:
> mutex_unlock(&st->lock);
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
2020-05-08 14:19 ` Jonathan Cameron
@ 2020-05-18 15:32 ` Jean-Baptiste Maneyrol
2020-05-21 17:56 ` Jonathan Cameron
0 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-18 15:32 UTC (permalink / raw)
To: Jonathan Cameron
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
Hi Jonathan,
no problem with the comments.
fifo_decode_packet will move to source code,
For the following sleep comment:
> + if (sleep_gyro > sleep_temp)
> + sleep = sleep_gyro;
> + else
> + sleep = sleep_temp;
> + if (sleep)
> + msleep(sleep);
if (sleep_gyro > sleep_temp)
msleep(sleep_gyro);
else
msleep(sleep_temp);
I am using an intermediate local variable to avoid a call to msleep(0) which is in fact sleeping for real for 1 jiffies as far as I understand. So is it OK to keep it as is?
Thanks,
JB
From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
Sent: Friday, May 8, 2020 16:19
To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
Subject: Re: [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
On Thu, 7 May 2020 16:42:19 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Use triggered buffer by parsing FIFO data read in device trigger.
> Support hwfifo watermark by multiplexing gyro and accel settings.
> Support hwfifo flush.
>
> Simply use interrupt timestamp first.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
> drivers/iio/imu/inv_icm42600/Kconfig | 3 +-
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 353 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.h | 162 ++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 23 ++
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 183 +++++++++
> .../imu/inv_icm42600/inv_icm42600_trigger.c | 15 +-
> 9 files changed, 928 insertions(+), 3 deletions(-)
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
>
> diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
> index 7b3eaeb2aa4a..8c0969319c49 100644
> --- a/drivers/iio/imu/inv_icm42600/Kconfig
> +++ b/drivers/iio/imu/inv_icm42600/Kconfig
> @@ -2,7 +2,8 @@
>
> config INV_ICM42600
> tristate
> - select IIO_TRIGGER
> + select IIO_BUFFER
> + select IIO_TRIGGERED_BUFFER
>
> config INV_ICM42600_I2C
> tristate "InvenSense ICM-426xx I2C driver"
> diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
> index e1f2aacbe888..d6732118010c 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
> inv-icm42600-y += inv_icm42600_accel.o
> inv-icm42600-y += inv_icm42600_temp.o
> inv-icm42600-y += inv_icm42600_trigger.o
> +inv-icm42600-y += inv_icm42600_buffer.o
>
> obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
> inv-icm42600-i2c-y += inv_icm42600_i2c.o
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> index 175c1f67faee..947ca4dd245b 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -15,6 +15,8 @@
> #include <linux/iio/iio.h>
> #include <linux/iio/trigger.h>
>
> +#include "inv_icm42600_buffer.h"
> +
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> INV_CHIP_ICM42602,
> @@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
> * @indio_gyro: gyroscope IIO device.
> * @indio_accel: accelerometer IIO device.
> * @trigger: device internal interrupt trigger
> + * @fifo: FIFO management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -138,6 +141,7 @@ struct inv_icm42600_state {
> struct iio_dev *indio_gyro;
> struct iio_dev *indio_accel;
> struct iio_trigger *trigger;
> + struct inv_icm42600_fifo fifo;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -378,8 +382,12 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +
> int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> int irq_type);
>
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 74dac5f283d4..4206be54d057 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> @@ -10,9 +10,13 @@
> #include <linux/regmap.h>
> #include <linux/delay.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -46,6 +50,7 @@ enum inv_icm42600_accel_scan {
> INV_ICM42600_ACCEL_SCAN_Y,
> INV_ICM42600_ACCEL_SCAN_Z,
> INV_ICM42600_ACCEL_SCAN_TEMP,
> + INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
> };
>
> static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
> @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
> INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
> inv_icm42600_accel_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
> + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> };
>
> +/* IIO buffer data */
> +struct inv_icm42600_accel_buffer {
> + struct inv_icm42600_fifo_sensor_data accel;
> + int8_t temp;
> + int64_t timestamp;
> +};
> +
> +#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
> + (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
> + BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
> + BIT(INV_ICM42600_ACCEL_SCAN_Z))
> +
> +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
> +
> +static const unsigned long inv_icm42600_accel_scan_masks[] = {
> + /* 3-axis accel + temperature */
> + INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
> + 0,
> +};
> +
> +static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
> +{
> + struct iio_poll_func *pf = _data;
> + struct iio_dev *indio_dev = pf->indio_dev;
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t fifo_nb = st->fifo.nb.total;
> + int ret;
> +
> + /* exit if no sample */
> + if (fifo_nb == 0)
> + goto out;
> +
> + ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
> + if (ret)
> + dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
> + ret);
> +
> +out:
> + iio_trigger_notify_done(indio_dev->trig);
> + return IRQ_HANDLED;
> +}
> +
> +/* enable accelerometer sensor and FIFO write */
> +static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
> + const unsigned long *scan_mask)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int fifo_en = 0;
> + unsigned int sleep_temp = 0;
> + unsigned int sleep_accel = 0;
> + unsigned int sleep;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
> + /* enable temp sensor */
> + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_TEMP;
> + }
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
> + /* enable accel sensor */
> + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
> + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_ACCEL;
> + }
> +
> + /* update data FIFO write and FIFO watermark */
> + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> + if (ret)
> + goto out_unlock;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + /* sleep maximum required time */
> + if (sleep_accel > sleep_temp)
> + sleep = sleep_accel;
> + else
> + sleep = sleep_temp;
> + if (sleep)
> + msleep(sleep);
> + return ret;
> +}
> +
> static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
> struct iio_chan_spec const *chan,
> int16_t *val)
> @@ -250,6 +347,8 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
>
> pm_runtime_mark_last_busy(dev);
> @@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
> }
> }
>
> +static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
> + unsigned int val)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + st->fifo.watermark.accel = val;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> +static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
> + unsigned int count)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + if (count == 0)
> + return 0;
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
> + if (!ret)
> + ret = st->fifo.nb.accel;
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> static const struct iio_info inv_icm42600_accel_info = {
> .read_raw = inv_icm42600_accel_read_raw,
> .read_avail = inv_icm42600_accel_read_avail,
> .write_raw = inv_icm42600_accel_write_raw,
> .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
> .debugfs_reg_access = inv_icm42600_debugfs_reg,
> + .update_scan_mode = inv_icm42600_accel_update_scan_mode,
> + .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
> + .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
> };
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> @@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> struct iio_dev *indio_dev;
> + int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
> if (!name)
> @@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> indio_dev->modes = INDIO_DIRECT_MODE;
> indio_dev->channels = inv_icm42600_accel_channels;
> indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
> + indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
> +
> + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
> + inv_icm42600_accel_handler,
> + &inv_icm42600_buffer_ops);
> + if (ret)
> + return ret;
> +
> + indio_dev->trig = iio_trigger_get(st->trigger);
>
> st->indio_accel = indio_dev;
> return devm_iio_device_register(dev, st->indio_accel);
> }
> +
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t accel_nb = st->fifo.nb.accel;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + struct inv_icm42600_accel_buffer buffer;
> +
> + /* exit if no accel sample */
> + if (accel_nb == 0)
> + return 0;
> +
> + /* parse all fifo packets */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
> + size);
> + /* quit if error or FIFO is empty */
> + if (size <= 0)
> + return size;
> + /* skip packet if no accel data or data is invalid */
> + if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
> + dev_dbg(regmap_get_device(st->map), "skip accel data\n");
> + continue;
> + }
> + memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + }
> +
> + return 0;
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> new file mode 100644
> index 000000000000..b428abdc92ee
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -0,0 +1,353 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/delay.h>
> +#include <linux/math64.h>
> +#include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
> +
> +#include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
> +
> +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
> +{
> + uint32_t period_gyro, period_accel, period;
> +
> + if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
> + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
> + else
> + period_gyro = U32_MAX;
> +
> + if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
> + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
> + else
> + period_accel = U32_MAX;
> +
> + if (period_gyro <= period_accel)
> + period = period_gyro;
> + else
> + period = period_accel;
> +
> + st->fifo.period = period;
> +}
> +
> +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
> + unsigned int fifo_en)
> +{
> + unsigned int mask, val;
> + int ret;
> +
> + /* update only FIFO EN bits */
> + mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
> + INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
> + INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
> + INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
> +
> + val = 0;
> + if (fifo_en & INV_ICM42600_SENSOR_GYRO)
> + val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
> + if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
> + val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
> + if (fifo_en & INV_ICM42600_SENSOR_TEMP)
> + val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
> +
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
> + mask, val);
> + if (ret)
> + return ret;
> +
> + st->fifo.en = fifo_en;
> + inv_icm42600_buffer_update_fifo_period(st);
> +
> + return 0;
> +}
> +
> +static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
> +{
> + size_t packet_size;
> +
> + if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
> + (fifo_en & INV_ICM42600_SENSOR_ACCEL))
> + packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
> + else
> + packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> +
> + return packet_size;
> +}
> +
> +static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
> + size_t packet_size)
> +{
> + size_t wm_size;
> + unsigned int wm;
> +
> + wm_size = watermark * packet_size;
> + if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
> + wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
> +
> + wm = wm_size / packet_size;
> +
> + return wm;
> +}
> +
> +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
> +{
> + size_t packet_size, wm_size;
> + unsigned int wm_gyro, wm_accel, watermark;
> + uint32_t period_gyro, period_accel, period;
> + int64_t latency_gyro, latency_accel, latency;
> + unsigned int mask, val;
> + bool restore;
> + __le16 raw_wm;
> + int ret;
> +
> + packet_size = inv_icm42600_get_packet_size(st->fifo.en);
> +
> + /* get minimal latency, depending on sensor watermark and odr */
> + wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
> + packet_size);
> + wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
> + packet_size);
> + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
> + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
> + latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
> + latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
> + if (latency_gyro == 0) {
> + latency = latency_accel;
> + watermark = wm_accel;
> + } else if (latency_accel == 0) {
> + latency = latency_gyro;
> + watermark = wm_gyro;
> + } else {
> + /* compute the smallest latency that is a multiple of both */
> + if (latency_gyro <= latency_accel) {
> + latency = latency_gyro;
> + latency -= latency_accel % latency_gyro;
> + } else {
> + latency = latency_accel;
> + latency -= latency_gyro % latency_accel;
> + }
> + /* use the shortest period */
> + if (period_gyro <= period_accel)
> + period = period_gyro;
> + else
> + period = period_accel;
> + /* all this works because periods are multiple of each others */
> + watermark = div_s64(latency, period);
> + if (watermark < 1)
> + watermark = 1;
> + }
> + wm_size = watermark * packet_size;
> + dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
> + watermark, wm_size);
> +
> + /* changing FIFO watermark requires to turn off watermark interrupt */
> + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + val = 0;
> + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + mask, val, &restore);
> + if (ret)
> + return ret;
> +
> + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
> + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
> + &raw_wm, sizeof(raw_wm));
> + if (ret)
> + return ret;
> +
> + /* restore watermark interrupt */
> + if (restore) {
> + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
> + mask, val);
> + if (ret)
> + return ret;
> + }
> +
> + return 0;
> +}
> +
> +static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> +
> + pm_runtime_get_sync(dev);
> +
> + return 0;
> +}
> +
> +static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct device *dev = regmap_get_device(st->map);
> + unsigned int sensor;
> + unsigned int *watermark;
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int sleep = 0;
> + int ret;
> +
> + if (indio_dev == st->indio_gyro) {
> + sensor = INV_ICM42600_SENSOR_GYRO;
> + watermark = &st->fifo.watermark.gyro;
> + } else if (indio_dev == st->indio_accel) {
> + sensor = INV_ICM42600_SENSOR_ACCEL;
> + watermark = &st->fifo.watermark.accel;
> + } else {
> + return -EINVAL;
> + }
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
> + if (ret)
> + goto out_unlock;
> +
> + *watermark = 0;
> + ret = inv_icm42600_buffer_update_watermark(st);
> + if (ret)
> + goto out_unlock;
> +
> + conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
> + if (sensor == INV_ICM42600_SENSOR_GYRO)
> + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
> + else
> + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + if (sleep)
> + msleep(sleep);
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> +
> + return ret;
> +}
> +
> +const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
> + .preenable = inv_icm42600_buffer_preenable,
> + .postenable = iio_triggered_buffer_postenable,
> + .predisable = iio_triggered_buffer_predisable,
> + .postdisable = inv_icm42600_buffer_postdisable,
> +};
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> + unsigned int max)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + __be16 raw_fifo_count;
> + size_t max_count;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + int ret;
> +
> + /* reset all samples counters */
> + st->fifo.count = 0;
> + st->fifo.nb.gyro = 0;
> + st->fifo.nb.accel = 0;
> + st->fifo.nb.total = 0;
> +
> + /* compute maximum FIFO read size */
> + if (max == 0)
> + max_count = sizeof(st->fifo.data);
> + else
> + max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
> +
> + /* read FIFO count value */
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
> + &raw_fifo_count, sizeof(raw_fifo_count));
> + if (ret)
> + return ret;
> + st->fifo.count = be16_to_cpu(raw_fifo_count);
> + dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
> +
> + /* check and sanitize FIFO count value */
> + if (st->fifo.count == 0)
> + return 0;
> + if (st->fifo.count > max_count)
> + st->fifo.count = max_count;
> +
> + /* read all FIFO data in internal buffer */
> + ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
> + st->fifo.data, st->fifo.count);
> + if (ret)
> + return ret;
> +
> + /* compute number of samples for each sensor */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + if (size <= 0)
> + break;
> + if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
> + st->fifo.nb.gyro++;
> + if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
> + st->fifo.nb.accel++;
> + st->fifo.nb.total++;
> + }
> +
> + return 0;
> +}
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> + unsigned int count)
> +{
> + int64_t ts_gyro, ts_accel;
> + int ret;
> +
> + dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
> +
> + ts_gyro = iio_get_time_ns(st->indio_gyro);
> + ts_accel = iio_get_time_ns(st->indio_accel);
> + ret = inv_icm42600_buffer_fifo_read(st, count);
> + if (ret)
> + return ret;
> +
> + if (st->fifo.nb.total == 0)
> + return 0;
> +
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
> + if (ret)
> + return ret;
> +
> + return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
> +}
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
> +{
> + unsigned int mask, val;
> + int ret;
> +
> + /*
> + * Default FIFO configuration (bits 7 to 5)
> + * - use invalid value
> + * - FIFO count in bytes
> + * - FIFO count in big endian
> + */
> + mask = GENMASK(7, 5);
> + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
> + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
> + mask, val);
> + if (ret)
> + return ret;
> +
> + /*
> + * Enable FIFO partial read and continuous watermark interrupt.
> + * Disable all FIFO EN bits.
> + */
> + mask = GENMASK(6, 5) | GENMASK(3, 0);
> + val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
> + INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
> + return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
> + mask, val);
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> new file mode 100644
> index 000000000000..74b91c0e664b
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> @@ -0,0 +1,162 @@
> +/* SPDX-License-Identifier: GPL-2.0-or-later */
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#ifndef INV_ICM42600_BUFFER_H_
> +#define INV_ICM42600_BUFFER_H_
> +
> +#include <linux/kernel.h>
> +#include <linux/bits.h>
> +
> +struct inv_icm42600_state;
> +
> +#define INV_ICM42600_SENSOR_GYRO BIT(0)
> +#define INV_ICM42600_SENSOR_ACCEL BIT(1)
> +#define INV_ICM42600_SENSOR_TEMP BIT(2)
> +
> +struct inv_icm42600_fifo {
> + unsigned int en;
> + uint32_t period;
> + struct {
> + unsigned int gyro;
> + unsigned int accel;
> + } watermark;
> + size_t count;
> + struct {
> + size_t gyro;
> + size_t accel;
> + size_t total;
> + } nb;
> + uint8_t data[2080];
> +};
> +
> +/* FIFO header: 1 byte */
> +#define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
> +#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
> +#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
> +#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
> +#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
> +#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
> +
> +/* FIFO data packet */
> +struct inv_icm42600_fifo_sensor_data {
> + __be16 x;
> + __be16 y;
> + __be16 z;
> +} __packed;
> +#define INV_ICM42600_FIFO_DATA_INVALID -32768
> +
> +struct inv_icm42600_fifo_1sensor_packet {
> + uint8_t header;
> + struct inv_icm42600_fifo_sensor_data data;
> + int8_t temp;
> +} __packed;
> +#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
> +
> +struct inv_icm42600_fifo_2sensors_packet {
> + uint8_t header;
> + struct inv_icm42600_fifo_sensor_data accel;
> + struct inv_icm42600_fifo_sensor_data gyro;
> + int8_t temp;
> + __be16 timestamp;
> +} __packed;
> +#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
> +
> +static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
> +{
> + return be16_to_cpu(d);
> +}
> +
> +static inline bool
> +inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
> +{
> + int16_t x, y, z;
> +
> + x = inv_icm42600_fifo_get_sensor_data(s->x);
> + y = inv_icm42600_fifo_get_sensor_data(s->y);
> + z = inv_icm42600_fifo_get_sensor_data(s->z);
> +
> + if (x == INV_ICM42600_FIFO_DATA_INVALID &&
> + y == INV_ICM42600_FIFO_DATA_INVALID &&
> + z == INV_ICM42600_FIFO_DATA_INVALID)
> + return false;
> +
> + return true;
> +}
> +
> +static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
Bit big to be in the header..
> + const void **accel, const void **gyro, const void **temp,
> + const void **timestamp, unsigned int *odr)
> +{
> + const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
> + const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
> + uint8_t header = *((const uint8_t *)packet);
> +
> + /* FIFO empty */
> + if (header & INV_ICM42600_FIFO_HEADER_MSG) {
> + *accel = NULL;
> + *gyro = NULL;
> + *temp = NULL;
> + *timestamp = NULL;
> + *odr = 0;
> + return 0;
> + }
> +
> + /* handle odr flags */
> + *odr = 0;
> + if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
> + *odr |= INV_ICM42600_SENSOR_GYRO;
> + if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
> + *odr |= INV_ICM42600_SENSOR_ACCEL;
> +
> + /* accel + gyro */
> + if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
> + (header & INV_ICM42600_FIFO_HEADER_GYRO)) {
> + *accel = &pack2->accel;
> + *gyro = &pack2->gyro;
> + *temp = &pack2->temp;
> + *timestamp = &pack2->timestamp;
> + return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
> + }
> +
> + /* accel only */
> + if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
> + *accel = &pack1->data;
> + *gyro = NULL;
> + *temp = &pack1->temp;
> + *timestamp = NULL;
> + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> + }
> +
> + /* gyro only */
> + if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
> + *accel = NULL;
> + *gyro = &pack1->data;
> + *temp = &pack1->temp;
> + *timestamp = NULL;
> + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
> + }
> +
> + /* invalid packet if here */
> + return -EINVAL;
> +}
> +
> +extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
> +
> +int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
> +
> +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
> +
> +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
> + unsigned int fifo_en);
> +
> +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
> +
> +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
> + unsigned int max);
> +
> +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> + unsigned int count);
> +
> +#endif
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> index 1102c54396e3..689089065ff9 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -14,6 +14,7 @@
> #include <linux/iio/iio.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
>
> static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
> {
> @@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + /* setup FIFO buffer */
> + ret = inv_icm42600_buffer_init(st);
> + if (ret)
> + return ret;
> +
> /* setup interrupt trigger */
> ret = inv_icm42600_trigger_init(st, irq, irq_type);
> if (ret)
> @@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
> goto out_unlock;
> }
>
> + /* disable FIFO data streaming */
> + if (iio_buffer_enabled(st->indio_gyro) ||
> + iio_buffer_enabled(st->indio_accel)) {
> + /* set FIFO in bypass mode */
> + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> + INV_ICM42600_FIFO_CONFIG_BYPASS);
> + if (ret)
> + goto out_unlock;
> + }
> +
> ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
> INV_ICM42600_SENSOR_MODE_OFF, false,
> NULL);
> @@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
> if (ret)
> goto out_unlock;
>
> + /* restore FIFO data streaming */
> + if (iio_buffer_enabled(st->indio_gyro) ||
> + iio_buffer_enabled(st->indio_accel)) {
> + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
> + INV_ICM42600_FIFO_CONFIG_STREAM);
> + }
> +
> out_unlock:
> mutex_unlock(&st->lock);
> return ret;
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> index c0164ab2830e..dafb104abc77 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> @@ -10,9 +10,13 @@
> #include <linux/regmap.h>
> #include <linux/delay.h>
> #include <linux/iio/iio.h>
> +#include <linux/iio/buffer.h>
> +#include <linux/iio/triggered_buffer.h>
> +#include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> #include "inv_icm42600_temp.h"
> +#include "inv_icm42600_buffer.h"
>
> #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
> { \
> @@ -46,6 +50,7 @@ enum inv_icm42600_gyro_scan {
> INV_ICM42600_GYRO_SCAN_Y,
> INV_ICM42600_GYRO_SCAN_Z,
> INV_ICM42600_GYRO_SCAN_TEMP,
> + INV_ICM42600_GYRO_SCAN_TIMESTAMP,
> };
>
> static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
> @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
> INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
> inv_icm42600_gyro_ext_infos),
> INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
> + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
> };
>
> +/* IIO buffer data */
> +struct inv_icm42600_gyro_buffer {
> + struct inv_icm42600_fifo_sensor_data gyro;
> + int8_t temp;
> + int64_t timestamp;
> +};
> +
> +#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
> + (BIT(INV_ICM42600_GYRO_SCAN_X) | \
> + BIT(INV_ICM42600_GYRO_SCAN_Y) | \
> + BIT(INV_ICM42600_GYRO_SCAN_Z))
> +
> +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
> +
> +static const unsigned long inv_icm42600_gyro_scan_masks[] = {
> + /* 3-axis gyro + temperature */
> + INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
> + 0,
> +};
> +
> +static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
> +{
> + struct iio_poll_func *pf = _data;
> + struct iio_dev *indio_dev = pf->indio_dev;
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t fifo_nb = st->fifo.nb.total;
> + int ret;
> +
> + /* exit if no sample */
> + if (fifo_nb == 0)
> + goto out;
> +
> + ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
> + if (ret)
> + dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
> + ret);
> +
> +out:
> + iio_trigger_notify_done(indio_dev->trig);
> + return IRQ_HANDLED;
> +}
> +
> +/* enable gyroscope sensor and FIFO write */
> +static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
> + const unsigned long *scan_mask)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> + unsigned int fifo_en = 0;
> + unsigned int sleep_gyro = 0;
> + unsigned int sleep_temp = 0;
> + unsigned int sleep;
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
> + /* enable temp sensor */
> + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_TEMP;
> + }
> +
> + if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
> + /* enable gyro sensor */
> + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
> + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
> + if (ret)
> + goto out_unlock;
> + fifo_en |= INV_ICM42600_SENSOR_GYRO;
> + }
> +
> + /* update data FIFO write and FIFO watermark */
> + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> + if (ret)
> + goto out_unlock;
blank line
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> +out_unlock:
> + mutex_unlock(&st->lock);
> + /* sleep maximum required time */
> + if (sleep_gyro > sleep_temp)
> + sleep = sleep_gyro;
> + else
> + sleep = sleep_temp;
> + if (sleep)
> + msleep(sleep);
if (sleep_gyro > sleep_temp)
msleep(sleep_gyro);
else
msleep(sleep_temp);
> + return ret;
> +}
> +
> static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
> struct iio_chan_spec const *chan,
> int16_t *val)
> @@ -262,6 +359,8 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + inv_icm42600_buffer_update_fifo_period(st);
> + inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
>
> pm_runtime_mark_last_busy(dev);
> @@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
> }
> }
>
> +static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
> + unsigned int val)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + mutex_lock(&st->lock);
> +
> + st->fifo.watermark.gyro = val;
> + ret = inv_icm42600_buffer_update_watermark(st);
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> +static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
> + unsigned int count)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + int ret;
> +
> + if (count == 0)
> + return 0;
> +
> + mutex_lock(&st->lock);
> +
> + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
> + if (!ret)
> + ret = st->fifo.nb.gyro;
> +
> + mutex_unlock(&st->lock);
> +
> + return ret;
> +}
> +
> static const struct iio_info inv_icm42600_gyro_info = {
> .read_raw = inv_icm42600_gyro_read_raw,
> .read_avail = inv_icm42600_gyro_read_avail,
> .write_raw = inv_icm42600_gyro_write_raw,
> .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
> .debugfs_reg_access = inv_icm42600_debugfs_reg,
> + .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
> + .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
> + .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
> };
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> @@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> struct device *dev = regmap_get_device(st->map);
> const char *name;
> struct iio_dev *indio_dev;
> + int ret;
>
> name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
> if (!name)
> @@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> indio_dev->modes = INDIO_DIRECT_MODE;
> indio_dev->channels = inv_icm42600_gyro_channels;
> indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
> + indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
> +
> + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
> + inv_icm42600_gyro_handler,
> + &inv_icm42600_buffer_ops);
> + if (ret)
> + return ret;
> +
> + indio_dev->trig = iio_trigger_get(st->trigger);
>
> st->indio_gyro = indio_dev;
> return devm_iio_device_register(dev, st->indio_gyro);
> }
> +
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +{
> + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + const size_t gyro_nb = st->fifo.nb.gyro;
> + ssize_t i, size;
> + const void *accel, *gyro, *temp, *timestamp;
> + unsigned int odr;
> + struct inv_icm42600_gyro_buffer buffer;
> +
> + /* exit if no gyro sample */
> + if (gyro_nb == 0)
> + return 0;
> +
> + /* parse all fifo packets */
> + for (i = 0; i < st->fifo.count; i += size) {
> + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
> + &accel, &gyro, &temp, ×tamp, &odr);
> + dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
> + size);
> + /* quit if error or FIFO is empty */
> + if (size <= 0)
> + return size;
blank line here.
> + /* skip packet if no gyro data or data is invalid */
> + if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
> + dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
Very noisy logging. I'd drop it for the final version of the driver.
> + continue;
> + }
> + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
buffer.temp = temp;
> + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + }
> +
> + return 0;
> +}
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> index 7a5e76305f0b..5667e0204722 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
> @@ -13,6 +13,7 @@
> #include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_buffer.h"
>
> static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
> {
> @@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
> dev_warn(dev, "FIFO full data lost!\n");
>
> /* FIFO threshold reached */
> - if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
> - iio_trigger_poll_chained(st->trigger);
> + if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
> + ret = inv_icm42600_buffer_fifo_read(st, 0);
> + if (ret)
> + dev_err(dev, "FIFO read error %d\n", ret);
> + } else {
> + st->fifo.count = 0;
> + st->fifo.nb.gyro = 0;
> + st->fifo.nb.accel = 0;
> + st->fifo.nb.total = 0;
> + }
> +
> + iio_trigger_poll_chained(st->trigger);
>
> out_unlock:
> mutex_unlock(&st->lock);
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
2020-05-18 15:32 ` Jean-Baptiste Maneyrol
@ 2020-05-21 17:56 ` Jonathan Cameron
0 siblings, 0 replies; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-21 17:56 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: Jonathan Cameron, robh+dt, robh, mchehab+huawei, davem, gregkh,
linux-iio, devicetree, linux-kernel
On Mon, 18 May 2020 15:32:27 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:
> Hi Jonathan,
>
> no problem with the comments.
>
> fifo_decode_packet will move to source code,
>
> For the following sleep comment:
> > + if (sleep_gyro > sleep_temp)
> > + sleep = sleep_gyro;
> > + else
> > + sleep = sleep_temp;
> > + if (sleep)
> > + msleep(sleep);
>
> if (sleep_gyro > sleep_temp)
> msleep(sleep_gyro);
> else
> msleep(sleep_temp);
>
> I am using an intermediate local variable to avoid a call to msleep(0) which is in fact sleeping for real for 1 jiffies as far as I understand. So is it OK to keep it as is?
Doh. I missed that for some reason. Absolutely fine as
it is!
J
>
> Thanks,
> JB
>
>
>
> From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
>
> Sent: Friday, May 8, 2020 16:19
>
> To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
>
> Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
> linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
>
> Subject: Re: [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices
>
>
>
>
> CAUTION: This email originated from outside of the organization. Please make sure the sender is who they say they are and do not click links or open attachments unless you recognize the sender and know the content is safe.
>
>
>
> On Thu, 7 May 2020 16:42:19 +0200
>
> Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
>
>
>
> > Use triggered buffer by parsing FIFO data read in device trigger.
>
> > Support hwfifo watermark by multiplexing gyro and accel settings.
>
> > Support hwfifo flush.
>
> >
>
> > Simply use interrupt timestamp first.
>
> >
>
> > Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
>
> > ---
>
> > drivers/iio/imu/inv_icm42600/Kconfig | 3 +-
>
> > drivers/iio/imu/inv_icm42600/Makefile | 1 +
>
> > drivers/iio/imu/inv_icm42600/inv_icm42600.h | 8 +
>
> > .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 183 +++++++++
>
> > .../imu/inv_icm42600/inv_icm42600_buffer.c | 353 ++++++++++++++++++
>
> > .../imu/inv_icm42600/inv_icm42600_buffer.h | 162 ++++++++
>
> > .../iio/imu/inv_icm42600/inv_icm42600_core.c | 23 ++
>
> > .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 183 +++++++++
>
> > .../imu/inv_icm42600/inv_icm42600_trigger.c | 15 +-
>
> > 9 files changed, 928 insertions(+), 3 deletions(-)
>
> > create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
>
> > create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
>
> >
>
> > diff --git a/drivers/iio/imu/inv_icm42600/Kconfig b/drivers/iio/imu/inv_icm42600/Kconfig
>
> > index 7b3eaeb2aa4a..8c0969319c49 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/Kconfig
>
> > +++ b/drivers/iio/imu/inv_icm42600/Kconfig
>
> > @@ -2,7 +2,8 @@
>
> >
>
> > config INV_ICM42600
>
> > tristate
>
> > - select IIO_TRIGGER
>
> > + select IIO_BUFFER
>
> > + select IIO_TRIGGERED_BUFFER
>
> >
>
> > config INV_ICM42600_I2C
>
> > tristate "InvenSense ICM-426xx I2C driver"
>
> > diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
>
> > index e1f2aacbe888..d6732118010c 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/Makefile
>
> > +++ b/drivers/iio/imu/inv_icm42600/Makefile
>
> > @@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
>
> > inv-icm42600-y += inv_icm42600_accel.o
>
> > inv-icm42600-y += inv_icm42600_temp.o
>
> > inv-icm42600-y += inv_icm42600_trigger.o
>
> > +inv-icm42600-y += inv_icm42600_buffer.o
>
> >
>
> > obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
>
> > inv-icm42600-i2c-y += inv_icm42600_i2c.o
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
>
> > index 175c1f67faee..947ca4dd245b 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
>
> > @@ -15,6 +15,8 @@
>
> > #include <linux/iio/iio.h>
>
> > #include <linux/iio/trigger.h>
>
> >
>
> > +#include "inv_icm42600_buffer.h"
>
> > +
>
> > enum inv_icm42600_chip {
>
> > INV_CHIP_ICM42600,
>
> > INV_CHIP_ICM42602,
>
> > @@ -124,6 +126,7 @@ struct inv_icm42600_suspended {
>
> > * @indio_gyro: gyroscope IIO device.
>
> > * @indio_accel: accelerometer IIO device.
>
> > * @trigger: device internal interrupt trigger
>
> > + * @fifo: FIFO management structure.
>
> > */
>
> > struct inv_icm42600_state {
>
> > struct mutex lock;
>
> > @@ -138,6 +141,7 @@ struct inv_icm42600_state {
>
> > struct iio_dev *indio_gyro;
>
> > struct iio_dev *indio_accel;
>
> > struct iio_trigger *trigger;
>
> > + struct inv_icm42600_fifo fifo;
>
> > };
>
> >
>
> > /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
>
> > @@ -378,8 +382,12 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> >
>
> > int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> >
>
> > +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
>
> > +
>
> > int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> >
>
> > +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
>
> > +
>
> > int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
>
> > int irq_type);
>
> >
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
>
> > index 74dac5f283d4..4206be54d057 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
>
> > @@ -10,9 +10,13 @@
>
> > #include <linux/regmap.h>
>
> > #include <linux/delay.h>
>
> > #include <linux/iio/iio.h>
>
> > +#include <linux/iio/buffer.h>
>
> > +#include <linux/iio/triggered_buffer.h>
>
> > +#include <linux/iio/trigger_consumer.h>
>
> >
>
> > #include "inv_icm42600.h"
>
> > #include "inv_icm42600_temp.h"
>
> > +#include "inv_icm42600_buffer.h"
>
> >
>
> > #define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
>
> > { \
>
> > @@ -46,6 +50,7 @@ enum inv_icm42600_accel_scan {
>
> > INV_ICM42600_ACCEL_SCAN_Y,
>
> > INV_ICM42600_ACCEL_SCAN_Z,
>
> > INV_ICM42600_ACCEL_SCAN_TEMP,
>
> > + INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
>
> > };
>
> >
>
> > static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
>
> > @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
>
> > INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
>
> > inv_icm42600_accel_ext_infos),
>
> > INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
>
> > + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
>
> > };
>
> >
>
> > +/* IIO buffer data */
>
> > +struct inv_icm42600_accel_buffer {
>
> > + struct inv_icm42600_fifo_sensor_data accel;
>
> > + int8_t temp;
>
> > + int64_t timestamp;
>
> > +};
>
> > +
>
> > +#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
>
> > + (BIT(INV_ICM42600_ACCEL_SCAN_X) | \
>
> > + BIT(INV_ICM42600_ACCEL_SCAN_Y) | \
>
> > + BIT(INV_ICM42600_ACCEL_SCAN_Z))
>
> > +
>
> > +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_ACCEL_SCAN_TEMP)
>
> > +
>
> > +static const unsigned long inv_icm42600_accel_scan_masks[] = {
>
> > + /* 3-axis accel + temperature */
>
> > + INV_ICM42600_SCAN_MASK_ACCEL_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
>
> > + 0,
>
> > +};
>
> > +
>
> > +static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
>
> > +{
>
> > + struct iio_poll_func *pf = _data;
>
> > + struct iio_dev *indio_dev = pf->indio_dev;
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + const size_t fifo_nb = st->fifo.nb.total;
>
> > + int ret;
>
> > +
>
> > + /* exit if no sample */
>
> > + if (fifo_nb == 0)
>
> > + goto out;
>
> > +
>
> > + ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
>
> > + if (ret)
>
> > + dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
>
> > + ret);
>
> > +
>
> > +out:
>
> > + iio_trigger_notify_done(indio_dev->trig);
>
> > + return IRQ_HANDLED;
>
> > +}
>
> > +
>
> > +/* enable accelerometer sensor and FIFO write */
>
> > +static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
>
> > + const unsigned long *scan_mask)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
>
> > + unsigned int fifo_en = 0;
>
> > + unsigned int sleep_temp = 0;
>
> > + unsigned int sleep_accel = 0;
>
> > + unsigned int sleep;
>
> > + int ret;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
>
> > + /* enable temp sensor */
>
> > + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + fifo_en |= INV_ICM42600_SENSOR_TEMP;
>
> > + }
>
> > +
>
> > + if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
>
> > + /* enable accel sensor */
>
> > + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
>
> > + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + fifo_en |= INV_ICM42600_SENSOR_ACCEL;
>
> > + }
>
> > +
>
> > + /* update data FIFO write and FIFO watermark */
>
> > + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + ret = inv_icm42600_buffer_update_watermark(st);
>
> > +
>
> > +out_unlock:
>
> > + mutex_unlock(&st->lock);
>
> > + /* sleep maximum required time */
>
> > + if (sleep_accel > sleep_temp)
>
> > + sleep = sleep_accel;
>
> > + else
>
> > + sleep = sleep_temp;
>
> > + if (sleep)
>
> > + msleep(sleep);
>
> > + return ret;
>
> > +}
>
> > +
>
> > static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
>
> > struct iio_chan_spec const *chan,
>
> > int16_t *val)
>
> > @@ -250,6 +347,8 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
>
> > mutex_lock(&st->lock);
>
> > conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
>
> > ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
>
> > + inv_icm42600_buffer_update_fifo_period(st);
>
> > + inv_icm42600_buffer_update_watermark(st);
>
> > mutex_unlock(&st->lock);
>
> >
>
> > pm_runtime_mark_last_busy(dev);
>
> > @@ -512,12 +611,51 @@ static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
>
> > }
>
> > }
>
> >
>
> > +static int inv_icm42600_accel_hwfifo_set_watermark(struct iio_dev *indio_dev,
>
> > + unsigned int val)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + int ret;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + st->fifo.watermark.accel = val;
>
> > + ret = inv_icm42600_buffer_update_watermark(st);
>
> > +
>
> > + mutex_unlock(&st->lock);
>
> > +
>
> > + return ret;
>
> > +}
>
> > +
>
> > +static int inv_icm42600_accel_hwfifo_flush(struct iio_dev *indio_dev,
>
> > + unsigned int count)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + int ret;
>
> > +
>
> > + if (count == 0)
>
> > + return 0;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
>
> > + if (!ret)
>
> > + ret = st->fifo.nb.accel;
>
> > +
>
> > + mutex_unlock(&st->lock);
>
> > +
>
> > + return ret;
>
> > +}
>
> > +
>
> > static const struct iio_info inv_icm42600_accel_info = {
>
> > .read_raw = inv_icm42600_accel_read_raw,
>
> > .read_avail = inv_icm42600_accel_read_avail,
>
> > .write_raw = inv_icm42600_accel_write_raw,
>
> > .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
>
> > .debugfs_reg_access = inv_icm42600_debugfs_reg,
>
> > + .update_scan_mode = inv_icm42600_accel_update_scan_mode,
>
> > + .hwfifo_set_watermark = inv_icm42600_accel_hwfifo_set_watermark,
>
> > + .hwfifo_flush_to_buffer = inv_icm42600_accel_hwfifo_flush,
>
> > };
>
> >
>
> > int inv_icm42600_accel_init(struct inv_icm42600_state *st)
>
> > @@ -525,6 +663,7 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
>
> > struct device *dev = regmap_get_device(st->map);
>
> > const char *name;
>
> > struct iio_dev *indio_dev;
>
> > + int ret;
>
> >
>
> > name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
>
> > if (!name)
>
> > @@ -541,7 +680,51 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
>
> > indio_dev->modes = INDIO_DIRECT_MODE;
>
> > indio_dev->channels = inv_icm42600_accel_channels;
>
> > indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
>
> > + indio_dev->available_scan_masks = inv_icm42600_accel_scan_masks;
>
> > +
>
> > + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
>
> > + inv_icm42600_accel_handler,
>
> > + &inv_icm42600_buffer_ops);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + indio_dev->trig = iio_trigger_get(st->trigger);
>
> >
>
> > st->indio_accel = indio_dev;
>
> > return devm_iio_device_register(dev, st->indio_accel);
>
> > }
>
> > +
>
> > +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + const size_t accel_nb = st->fifo.nb.accel;
>
> > + ssize_t i, size;
>
> > + const void *accel, *gyro, *temp, *timestamp;
>
> > + unsigned int odr;
>
> > + struct inv_icm42600_accel_buffer buffer;
>
> > +
>
> > + /* exit if no accel sample */
>
> > + if (accel_nb == 0)
>
> > + return 0;
>
> > +
>
> > + /* parse all fifo packets */
>
> > + for (i = 0; i < st->fifo.count; i += size) {
>
> > + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
>
> > + &accel, &gyro, &temp, ×tamp, &odr);
>
> > + dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
>
> > + size);
>
> > + /* quit if error or FIFO is empty */
>
> > + if (size <= 0)
>
> > + return size;
>
> > + /* skip packet if no accel data or data is invalid */
>
> > + if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel)) {
>
> > + dev_dbg(regmap_get_device(st->map), "skip accel data\n");
>
> > + continue;
>
> > + }
>
> > + memcpy(&buffer.accel, accel, sizeof(buffer.accel));
>
> > + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
>
> > + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
>
> > + }
>
> > +
>
> > + return 0;
>
> > +}
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
>
> > new file mode 100644
>
> > index 000000000000..b428abdc92ee
>
> > --- /dev/null
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
>
> > @@ -0,0 +1,353 @@
>
> > +// SPDX-License-Identifier: GPL-2.0-or-later
>
> > +/*
>
> > + * Copyright (C) 2020 Invensense, Inc.
>
> > + */
>
> > +
>
> > +#include <linux/device.h>
>
> > +#include <linux/mutex.h>
>
> > +#include <linux/pm_runtime.h>
>
> > +#include <linux/regmap.h>
>
> > +#include <linux/delay.h>
>
> > +#include <linux/math64.h>
>
> > +#include <linux/iio/iio.h>
>
> > +#include <linux/iio/buffer.h>
>
> > +#include <linux/iio/triggered_buffer.h>
>
> > +#include <linux/iio/trigger_consumer.h>
>
> > +
>
> > +#include "inv_icm42600.h"
>
> > +#include "inv_icm42600_buffer.h"
>
> > +
>
> > +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
>
> > +{
>
> > + uint32_t period_gyro, period_accel, period;
>
> > +
>
> > + if (st->fifo.en & INV_ICM42600_SENSOR_GYRO)
>
> > + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
>
> > + else
>
> > + period_gyro = U32_MAX;
>
> > +
>
> > + if (st->fifo.en & INV_ICM42600_SENSOR_ACCEL)
>
> > + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
>
> > + else
>
> > + period_accel = U32_MAX;
>
> > +
>
> > + if (period_gyro <= period_accel)
>
> > + period = period_gyro;
>
> > + else
>
> > + period = period_accel;
>
> > +
>
> > + st->fifo.period = period;
>
> > +}
>
> > +
>
> > +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
>
> > + unsigned int fifo_en)
>
> > +{
>
> > + unsigned int mask, val;
>
> > + int ret;
>
> > +
>
> > + /* update only FIFO EN bits */
>
> > + mask = INV_ICM42600_FIFO_CONFIG1_TMST_FSYNC_EN |
>
> > + INV_ICM42600_FIFO_CONFIG1_TEMP_EN |
>
> > + INV_ICM42600_FIFO_CONFIG1_GYRO_EN |
>
> > + INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
>
> > +
>
> > + val = 0;
>
> > + if (fifo_en & INV_ICM42600_SENSOR_GYRO)
>
> > + val |= INV_ICM42600_FIFO_CONFIG1_GYRO_EN;
>
> > + if (fifo_en & INV_ICM42600_SENSOR_ACCEL)
>
> > + val |= INV_ICM42600_FIFO_CONFIG1_ACCEL_EN;
>
> > + if (fifo_en & INV_ICM42600_SENSOR_TEMP)
>
> > + val |= INV_ICM42600_FIFO_CONFIG1_TEMP_EN;
>
> > +
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
>
> > + mask, val);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + st->fifo.en = fifo_en;
>
> > + inv_icm42600_buffer_update_fifo_period(st);
>
> > +
>
> > + return 0;
>
> > +}
>
> > +
>
> > +static size_t inv_icm42600_get_packet_size(unsigned int fifo_en)
>
> > +{
>
> > + size_t packet_size;
>
> > +
>
> > + if ((fifo_en & INV_ICM42600_SENSOR_GYRO) &&
>
> > + (fifo_en & INV_ICM42600_SENSOR_ACCEL))
>
> > + packet_size = INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
>
> > + else
>
> > + packet_size = INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
>
> > +
>
> > + return packet_size;
>
> > +}
>
> > +
>
> > +static unsigned int inv_icm42600_wm_truncate(unsigned int watermark,
>
> > + size_t packet_size)
>
> > +{
>
> > + size_t wm_size;
>
> > + unsigned int wm;
>
> > +
>
> > + wm_size = watermark * packet_size;
>
> > + if (wm_size > INV_ICM42600_FIFO_WATERMARK_MAX)
>
> > + wm_size = INV_ICM42600_FIFO_WATERMARK_MAX;
>
> > +
>
> > + wm = wm_size / packet_size;
>
> > +
>
> > + return wm;
>
> > +}
>
> > +
>
> > +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
>
> > +{
>
> > + size_t packet_size, wm_size;
>
> > + unsigned int wm_gyro, wm_accel, watermark;
>
> > + uint32_t period_gyro, period_accel, period;
>
> > + int64_t latency_gyro, latency_accel, latency;
>
> > + unsigned int mask, val;
>
> > + bool restore;
>
> > + __le16 raw_wm;
>
> > + int ret;
>
> > +
>
> > + packet_size = inv_icm42600_get_packet_size(st->fifo.en);
>
> > +
>
> > + /* get minimal latency, depending on sensor watermark and odr */
>
> > + wm_gyro = inv_icm42600_wm_truncate(st->fifo.watermark.gyro,
>
> > + packet_size);
>
> > + wm_accel = inv_icm42600_wm_truncate(st->fifo.watermark.accel,
>
> > + packet_size);
>
> > + period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr);
>
> > + period_accel = inv_icm42600_odr_to_period(st->conf.accel.odr);
>
> > + latency_gyro = (int64_t)period_gyro * (int64_t)wm_gyro;
>
> > + latency_accel = (int64_t)period_accel * (int64_t)wm_accel;
>
> > + if (latency_gyro == 0) {
>
> > + latency = latency_accel;
>
> > + watermark = wm_accel;
>
> > + } else if (latency_accel == 0) {
>
> > + latency = latency_gyro;
>
> > + watermark = wm_gyro;
>
> > + } else {
>
> > + /* compute the smallest latency that is a multiple of both */
>
> > + if (latency_gyro <= latency_accel) {
>
> > + latency = latency_gyro;
>
> > + latency -= latency_accel % latency_gyro;
>
> > + } else {
>
> > + latency = latency_accel;
>
> > + latency -= latency_gyro % latency_accel;
>
> > + }
>
> > + /* use the shortest period */
>
> > + if (period_gyro <= period_accel)
>
> > + period = period_gyro;
>
> > + else
>
> > + period = period_accel;
>
> > + /* all this works because periods are multiple of each others */
>
> > + watermark = div_s64(latency, period);
>
> > + if (watermark < 1)
>
> > + watermark = 1;
>
> > + }
>
> > + wm_size = watermark * packet_size;
>
> > + dev_dbg(regmap_get_device(st->map), "watermark: %u (%zu)\n",
>
> > + watermark, wm_size);
>
> > +
>
> > + /* changing FIFO watermark requires to turn off watermark interrupt */
>
> > + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
>
> > + val = 0;
>
> > + ret = regmap_update_bits_check(st->map, INV_ICM42600_REG_INT_SOURCE0,
>
> > + mask, val, &restore);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + raw_wm = INV_ICM42600_FIFO_WATERMARK_VAL(wm_size);
>
> > + ret = regmap_bulk_write(st->map, INV_ICM42600_REG_FIFO_WATERMARK,
>
> > + &raw_wm, sizeof(raw_wm));
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + /* restore watermark interrupt */
>
> > + if (restore) {
>
> > + mask = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
>
> > + val = INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN;
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
>
> > + mask, val);
>
> > + if (ret)
>
> > + return ret;
>
> > + }
>
> > +
>
> > + return 0;
>
> > +}
>
> > +
>
> > +static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + struct device *dev = regmap_get_device(st->map);
>
> > +
>
> > + pm_runtime_get_sync(dev);
>
> > +
>
> > + return 0;
>
> > +}
>
> > +
>
> > +static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + struct device *dev = regmap_get_device(st->map);
>
> > + unsigned int sensor;
>
> > + unsigned int *watermark;
>
> > + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
>
> > + unsigned int sleep = 0;
>
> > + int ret;
>
> > +
>
> > + if (indio_dev == st->indio_gyro) {
>
> > + sensor = INV_ICM42600_SENSOR_GYRO;
>
> > + watermark = &st->fifo.watermark.gyro;
>
> > + } else if (indio_dev == st->indio_accel) {
>
> > + sensor = INV_ICM42600_SENSOR_ACCEL;
>
> > + watermark = &st->fifo.watermark.accel;
>
> > + } else {
>
> > + return -EINVAL;
>
> > + }
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + ret = inv_icm42600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > +
>
> > + *watermark = 0;
>
> > + ret = inv_icm42600_buffer_update_watermark(st);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > +
>
> > + conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
>
> > + if (sensor == INV_ICM42600_SENSOR_GYRO)
>
> > + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep);
>
> > + else
>
> > + ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
>
> > +
>
> > +out_unlock:
>
> > + mutex_unlock(&st->lock);
>
> > + if (sleep)
>
> > + msleep(sleep);
>
> > + pm_runtime_mark_last_busy(dev);
>
> > + pm_runtime_put_autosuspend(dev);
>
> > +
>
> > + return ret;
>
> > +}
>
> > +
>
> > +const struct iio_buffer_setup_ops inv_icm42600_buffer_ops = {
>
> > + .preenable = inv_icm42600_buffer_preenable,
>
> > + .postenable = iio_triggered_buffer_postenable,
>
> > + .predisable = iio_triggered_buffer_predisable,
>
> > + .postdisable = inv_icm42600_buffer_postdisable,
>
> > +};
>
> > +
>
> > +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
>
> > + unsigned int max)
>
> > +{
>
> > + struct device *dev = regmap_get_device(st->map);
>
> > + __be16 raw_fifo_count;
>
> > + size_t max_count;
>
> > + ssize_t i, size;
>
> > + const void *accel, *gyro, *temp, *timestamp;
>
> > + unsigned int odr;
>
> > + int ret;
>
> > +
>
> > + /* reset all samples counters */
>
> > + st->fifo.count = 0;
>
> > + st->fifo.nb.gyro = 0;
>
> > + st->fifo.nb.accel = 0;
>
> > + st->fifo.nb.total = 0;
>
> > +
>
> > + /* compute maximum FIFO read size */
>
> > + if (max == 0)
>
> > + max_count = sizeof(st->fifo.data);
>
> > + else
>
> > + max_count = max * inv_icm42600_get_packet_size(st->fifo.en);
>
> > +
>
> > + /* read FIFO count value */
>
> > + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT,
>
> > + &raw_fifo_count, sizeof(raw_fifo_count));
>
> > + if (ret)
>
> > + return ret;
>
> > + st->fifo.count = be16_to_cpu(raw_fifo_count);
>
> > + dev_dbg(dev, "FIFO count = %zu\n", st->fifo.count);
>
> > +
>
> > + /* check and sanitize FIFO count value */
>
> > + if (st->fifo.count == 0)
>
> > + return 0;
>
> > + if (st->fifo.count > max_count)
>
> > + st->fifo.count = max_count;
>
> > +
>
> > + /* read all FIFO data in internal buffer */
>
> > + ret = regmap_noinc_read(st->map, INV_ICM42600_REG_FIFO_DATA,
>
> > + st->fifo.data, st->fifo.count);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + /* compute number of samples for each sensor */
>
> > + for (i = 0; i < st->fifo.count; i += size) {
>
> > + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
>
> > + &accel, &gyro, &temp, ×tamp, &odr);
>
> > + if (size <= 0)
>
> > + break;
>
> > + if (gyro != NULL && inv_icm42600_fifo_is_data_valid(gyro))
>
> > + st->fifo.nb.gyro++;
>
> > + if (accel != NULL && inv_icm42600_fifo_is_data_valid(accel))
>
> > + st->fifo.nb.accel++;
>
> > + st->fifo.nb.total++;
>
> > + }
>
> > +
>
> > + return 0;
>
> > +}
>
> > +
>
> > +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
>
> > + unsigned int count)
>
> > +{
>
> > + int64_t ts_gyro, ts_accel;
>
> > + int ret;
>
> > +
>
> > + dev_dbg(regmap_get_device(st->map), "FIFO flush %u\n", count);
>
> > +
>
> > + ts_gyro = iio_get_time_ns(st->indio_gyro);
>
> > + ts_accel = iio_get_time_ns(st->indio_accel);
>
> > + ret = inv_icm42600_buffer_fifo_read(st, count);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + if (st->fifo.nb.total == 0)
>
> > + return 0;
>
> > +
>
> > + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
>
> > +}
>
> > +
>
> > +int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
>
> > +{
>
> > + unsigned int mask, val;
>
> > + int ret;
>
> > +
>
> > + /*
>
> > + * Default FIFO configuration (bits 7 to 5)
>
> > + * - use invalid value
>
> > + * - FIFO count in bytes
>
> > + * - FIFO count in big endian
>
> > + */
>
> > + mask = GENMASK(7, 5);
>
> > + val = INV_ICM42600_INTF_CONFIG0_FIFO_COUNT_ENDIAN;
>
> > + ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG0,
>
> > + mask, val);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + /*
>
> > + * Enable FIFO partial read and continuous watermark interrupt.
>
> > + * Disable all FIFO EN bits.
>
> > + */
>
> > + mask = GENMASK(6, 5) | GENMASK(3, 0);
>
> > + val = INV_ICM42600_FIFO_CONFIG1_RESUME_PARTIAL_RD |
>
> > + INV_ICM42600_FIFO_CONFIG1_WM_GT_TH;
>
> > + return regmap_update_bits(st->map, INV_ICM42600_REG_FIFO_CONFIG1,
>
> > + mask, val);
>
> > +}
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
>
> > new file mode 100644
>
> > index 000000000000..74b91c0e664b
>
> > --- /dev/null
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
>
> > @@ -0,0 +1,162 @@
>
> > +/* SPDX-License-Identifier: GPL-2.0-or-later */
>
> > +/*
>
> > + * Copyright (C) 2020 Invensense, Inc.
>
> > + */
>
> > +
>
> > +#ifndef INV_ICM42600_BUFFER_H_
>
> > +#define INV_ICM42600_BUFFER_H_
>
> > +
>
> > +#include <linux/kernel.h>
>
> > +#include <linux/bits.h>
>
> > +
>
> > +struct inv_icm42600_state;
>
> > +
>
> > +#define INV_ICM42600_SENSOR_GYRO BIT(0)
>
> > +#define INV_ICM42600_SENSOR_ACCEL BIT(1)
>
> > +#define INV_ICM42600_SENSOR_TEMP BIT(2)
>
> > +
>
> > +struct inv_icm42600_fifo {
>
> > + unsigned int en;
>
> > + uint32_t period;
>
> > + struct {
>
> > + unsigned int gyro;
>
> > + unsigned int accel;
>
> > + } watermark;
>
> > + size_t count;
>
> > + struct {
>
> > + size_t gyro;
>
> > + size_t accel;
>
> > + size_t total;
>
> > + } nb;
>
> > + uint8_t data[2080];
>
> > +};
>
> > +
>
> > +/* FIFO header: 1 byte */
>
> > +#define INV_ICM42600_FIFO_HEADER_MSG BIT(7)
>
> > +#define INV_ICM42600_FIFO_HEADER_ACCEL BIT(6)
>
> > +#define INV_ICM42600_FIFO_HEADER_GYRO BIT(5)
>
> > +#define INV_ICM42600_FIFO_HEADER_TMST_FSYNC GENMASK(3, 2)
>
> > +#define INV_ICM42600_FIFO_HEADER_ODR_ACCEL BIT(1)
>
> > +#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
>
> > +
>
> > +/* FIFO data packet */
>
> > +struct inv_icm42600_fifo_sensor_data {
>
> > + __be16 x;
>
> > + __be16 y;
>
> > + __be16 z;
>
> > +} __packed;
>
> > +#define INV_ICM42600_FIFO_DATA_INVALID -32768
>
> > +
>
> > +struct inv_icm42600_fifo_1sensor_packet {
>
> > + uint8_t header;
>
> > + struct inv_icm42600_fifo_sensor_data data;
>
> > + int8_t temp;
>
> > +} __packed;
>
> > +#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
>
> > +
>
> > +struct inv_icm42600_fifo_2sensors_packet {
>
> > + uint8_t header;
>
> > + struct inv_icm42600_fifo_sensor_data accel;
>
> > + struct inv_icm42600_fifo_sensor_data gyro;
>
> > + int8_t temp;
>
> > + __be16 timestamp;
>
> > +} __packed;
>
> > +#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
>
> > +
>
> > +static inline int16_t inv_icm42600_fifo_get_sensor_data(__be16 d)
>
> > +{
>
> > + return be16_to_cpu(d);
>
> > +}
>
> > +
>
> > +static inline bool
>
> > +inv_icm42600_fifo_is_data_valid(const struct inv_icm42600_fifo_sensor_data *s)
>
> > +{
>
> > + int16_t x, y, z;
>
> > +
>
> > + x = inv_icm42600_fifo_get_sensor_data(s->x);
>
> > + y = inv_icm42600_fifo_get_sensor_data(s->y);
>
> > + z = inv_icm42600_fifo_get_sensor_data(s->z);
>
> > +
>
> > + if (x == INV_ICM42600_FIFO_DATA_INVALID &&
>
> > + y == INV_ICM42600_FIFO_DATA_INVALID &&
>
> > + z == INV_ICM42600_FIFO_DATA_INVALID)
>
> > + return false;
>
> > +
>
> > + return true;
>
> > +}
>
> > +
>
> > +static inline ssize_t inv_icm42600_fifo_decode_packet(const void *packet,
>
>
>
> Bit big to be in the header..
>
>
>
> > + const void **accel, const void **gyro, const void **temp,
>
> > + const void **timestamp, unsigned int *odr)
>
> > +{
>
> > + const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
>
> > + const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
>
> > + uint8_t header = *((const uint8_t *)packet);
>
> > +
>
> > + /* FIFO empty */
>
> > + if (header & INV_ICM42600_FIFO_HEADER_MSG) {
>
> > + *accel = NULL;
>
> > + *gyro = NULL;
>
> > + *temp = NULL;
>
> > + *timestamp = NULL;
>
> > + *odr = 0;
>
> > + return 0;
>
> > + }
>
> > +
>
> > + /* handle odr flags */
>
> > + *odr = 0;
>
> > + if (header & INV_ICM42600_FIFO_HEADER_ODR_GYRO)
>
> > + *odr |= INV_ICM42600_SENSOR_GYRO;
>
> > + if (header & INV_ICM42600_FIFO_HEADER_ODR_ACCEL)
>
> > + *odr |= INV_ICM42600_SENSOR_ACCEL;
>
> > +
>
> > + /* accel + gyro */
>
> > + if ((header & INV_ICM42600_FIFO_HEADER_ACCEL) &&
>
> > + (header & INV_ICM42600_FIFO_HEADER_GYRO)) {
>
> > + *accel = &pack2->accel;
>
> > + *gyro = &pack2->gyro;
>
> > + *temp = &pack2->temp;
>
> > + *timestamp = &pack2->timestamp;
>
> > + return INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE;
>
> > + }
>
> > +
>
> > + /* accel only */
>
> > + if (header & INV_ICM42600_FIFO_HEADER_ACCEL) {
>
> > + *accel = &pack1->data;
>
> > + *gyro = NULL;
>
> > + *temp = &pack1->temp;
>
> > + *timestamp = NULL;
>
> > + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
>
> > + }
>
> > +
>
> > + /* gyro only */
>
> > + if (header & INV_ICM42600_FIFO_HEADER_GYRO) {
>
> > + *accel = NULL;
>
> > + *gyro = &pack1->data;
>
> > + *temp = &pack1->temp;
>
> > + *timestamp = NULL;
>
> > + return INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE;
>
> > + }
>
> > +
>
> > + /* invalid packet if here */
>
> > + return -EINVAL;
>
> > +}
>
> > +
>
> > +extern const struct iio_buffer_setup_ops inv_icm42600_buffer_ops;
>
> > +
>
> > +int inv_icm42600_buffer_init(struct inv_icm42600_state *st);
>
> > +
>
> > +void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st);
>
> > +
>
> > +int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st,
>
> > + unsigned int fifo_en);
>
> > +
>
> > +int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st);
>
> > +
>
> > +int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
>
> > + unsigned int max);
>
> > +
>
> > +int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
>
> > + unsigned int count);
>
> > +
>
> > +#endif
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
>
> > index 1102c54396e3..689089065ff9 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
>
> > @@ -14,6 +14,7 @@
>
> > #include <linux/iio/iio.h>
>
> >
>
> > #include "inv_icm42600.h"
>
> > +#include "inv_icm42600_buffer.h"
>
> >
>
> > static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
>
> > {
>
> > @@ -515,6 +516,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> > if (ret)
>
> > return ret;
>
> >
>
> > + /* setup FIFO buffer */
>
> > + ret = inv_icm42600_buffer_init(st);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > /* setup interrupt trigger */
>
> > ret = inv_icm42600_trigger_init(st, irq, irq_type);
>
> > if (ret)
>
> > @@ -559,6 +565,16 @@ static int __maybe_unused inv_icm42600_suspend(struct device *dev)
>
> > goto out_unlock;
>
> > }
>
> >
>
> > + /* disable FIFO data streaming */
>
> > + if (iio_buffer_enabled(st->indio_gyro) ||
>
> > + iio_buffer_enabled(st->indio_accel)) {
>
> > + /* set FIFO in bypass mode */
>
> > + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
>
> > + INV_ICM42600_FIFO_CONFIG_BYPASS);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + }
>
> > +
>
> > ret = inv_icm42600_set_pwr_mgmt0(st, INV_ICM42600_SENSOR_MODE_OFF,
>
> > INV_ICM42600_SENSOR_MODE_OFF, false,
>
> > NULL);
>
> > @@ -594,6 +610,13 @@ static int __maybe_unused inv_icm42600_resume(struct device *dev)
>
> > if (ret)
>
> > goto out_unlock;
>
> >
>
> > + /* restore FIFO data streaming */
>
> > + if (iio_buffer_enabled(st->indio_gyro) ||
>
> > + iio_buffer_enabled(st->indio_accel)) {
>
> > + ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG,
>
> > + INV_ICM42600_FIFO_CONFIG_STREAM);
>
> > + }
>
> > +
>
> > out_unlock:
>
> > mutex_unlock(&st->lock);
>
> > return ret;
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
>
> > index c0164ab2830e..dafb104abc77 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
>
> > @@ -10,9 +10,13 @@
>
> > #include <linux/regmap.h>
>
> > #include <linux/delay.h>
>
> > #include <linux/iio/iio.h>
>
> > +#include <linux/iio/buffer.h>
>
> > +#include <linux/iio/triggered_buffer.h>
>
> > +#include <linux/iio/trigger_consumer.h>
>
> >
>
> > #include "inv_icm42600.h"
>
> > #include "inv_icm42600_temp.h"
>
> > +#include "inv_icm42600_buffer.h"
>
> >
>
> > #define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
>
> > { \
>
> > @@ -46,6 +50,7 @@ enum inv_icm42600_gyro_scan {
>
> > INV_ICM42600_GYRO_SCAN_Y,
>
> > INV_ICM42600_GYRO_SCAN_Z,
>
> > INV_ICM42600_GYRO_SCAN_TEMP,
>
> > + INV_ICM42600_GYRO_SCAN_TIMESTAMP,
>
> > };
>
> >
>
> > static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
>
> > @@ -61,8 +66,100 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
>
> > INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
>
> > inv_icm42600_gyro_ext_infos),
>
> > INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
>
> > + IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
>
> > };
>
> >
>
> > +/* IIO buffer data */
>
> > +struct inv_icm42600_gyro_buffer {
>
> > + struct inv_icm42600_fifo_sensor_data gyro;
>
> > + int8_t temp;
>
> > + int64_t timestamp;
>
> > +};
>
> > +
>
> > +#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
>
> > + (BIT(INV_ICM42600_GYRO_SCAN_X) | \
>
> > + BIT(INV_ICM42600_GYRO_SCAN_Y) | \
>
> > + BIT(INV_ICM42600_GYRO_SCAN_Z))
>
> > +
>
> > +#define INV_ICM42600_SCAN_MASK_TEMP BIT(INV_ICM42600_GYRO_SCAN_TEMP)
>
> > +
>
> > +static const unsigned long inv_icm42600_gyro_scan_masks[] = {
>
> > + /* 3-axis gyro + temperature */
>
> > + INV_ICM42600_SCAN_MASK_GYRO_3AXIS | INV_ICM42600_SCAN_MASK_TEMP,
>
> > + 0,
>
> > +};
>
> > +
>
> > +static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
>
> > +{
>
> > + struct iio_poll_func *pf = _data;
>
> > + struct iio_dev *indio_dev = pf->indio_dev;
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + const size_t fifo_nb = st->fifo.nb.total;
>
> > + int ret;
>
> > +
>
> > + /* exit if no sample */
>
> > + if (fifo_nb == 0)
>
> > + goto out;
>
> > +
>
> > + ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
>
> > + if (ret)
>
> > + dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
>
> > + ret);
>
> > +
>
> > +out:
>
> > + iio_trigger_notify_done(indio_dev->trig);
>
> > + return IRQ_HANDLED;
>
> > +}
>
> > +
>
> > +/* enable gyroscope sensor and FIFO write */
>
> > +static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
>
> > + const unsigned long *scan_mask)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
>
> > + unsigned int fifo_en = 0;
>
> > + unsigned int sleep_gyro = 0;
>
> > + unsigned int sleep_temp = 0;
>
> > + unsigned int sleep;
>
> > + int ret;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + if (*scan_mask & INV_ICM42600_SCAN_MASK_TEMP) {
>
> > + /* enable temp sensor */
>
> > + ret = inv_icm42600_set_temp_conf(st, true, &sleep_temp);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + fifo_en |= INV_ICM42600_SENSOR_TEMP;
>
> > + }
>
> > +
>
> > + if (*scan_mask & INV_ICM42600_SCAN_MASK_GYRO_3AXIS) {
>
> > + /* enable gyro sensor */
>
> > + conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
>
> > + ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_gyro);
>
> > + if (ret)
>
> > + goto out_unlock;
>
> > + fifo_en |= INV_ICM42600_SENSOR_GYRO;
>
> > + }
>
> > +
>
> > + /* update data FIFO write and FIFO watermark */
>
> > + ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
>
> > + if (ret)
>
> > + goto out_unlock;
>
>
>
> blank line
>
>
>
> > + ret = inv_icm42600_buffer_update_watermark(st);
>
> > +
>
> > +out_unlock:
>
> > + mutex_unlock(&st->lock);
>
> > + /* sleep maximum required time */
>
> > + if (sleep_gyro > sleep_temp)
>
> > + sleep = sleep_gyro;
>
> > + else
>
> > + sleep = sleep_temp;
>
> > + if (sleep)
>
> > + msleep(sleep);
>
>
>
> if (sleep_gyro > sleep_temp)
>
> msleep(sleep_gyro);
>
> else
>
> msleep(sleep_temp);
>
>
>
> > + return ret;
>
> > +}
>
> > +
>
> > static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
>
> > struct iio_chan_spec const *chan,
>
> > int16_t *val)
>
> > @@ -262,6 +359,8 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
>
> > mutex_lock(&st->lock);
>
> > conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
>
> > ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
>
> > + inv_icm42600_buffer_update_fifo_period(st);
>
> > + inv_icm42600_buffer_update_watermark(st);
>
> > mutex_unlock(&st->lock);
>
> >
>
> > pm_runtime_mark_last_busy(dev);
>
> > @@ -524,12 +623,51 @@ static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
>
> > }
>
> > }
>
> >
>
> > +static int inv_icm42600_gyro_hwfifo_set_watermark(struct iio_dev *indio_dev,
>
> > + unsigned int val)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + int ret;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + st->fifo.watermark.gyro = val;
>
> > + ret = inv_icm42600_buffer_update_watermark(st);
>
> > +
>
> > + mutex_unlock(&st->lock);
>
> > +
>
> > + return ret;
>
> > +}
>
> > +
>
> > +static int inv_icm42600_gyro_hwfifo_flush(struct iio_dev *indio_dev,
>
> > + unsigned int count)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + int ret;
>
> > +
>
> > + if (count == 0)
>
> > + return 0;
>
> > +
>
> > + mutex_lock(&st->lock);
>
> > +
>
> > + ret = inv_icm42600_buffer_hwfifo_flush(st, count);
>
> > + if (!ret)
>
> > + ret = st->fifo.nb.gyro;
>
> > +
>
> > + mutex_unlock(&st->lock);
>
> > +
>
> > + return ret;
>
> > +}
>
> > +
>
> > static const struct iio_info inv_icm42600_gyro_info = {
>
> > .read_raw = inv_icm42600_gyro_read_raw,
>
> > .read_avail = inv_icm42600_gyro_read_avail,
>
> > .write_raw = inv_icm42600_gyro_write_raw,
>
> > .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
>
> > .debugfs_reg_access = inv_icm42600_debugfs_reg,
>
> > + .update_scan_mode = inv_icm42600_gyro_update_scan_mode,
>
> > + .hwfifo_set_watermark = inv_icm42600_gyro_hwfifo_set_watermark,
>
> > + .hwfifo_flush_to_buffer = inv_icm42600_gyro_hwfifo_flush,
>
> > };
>
> >
>
> > int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>
> > @@ -537,6 +675,7 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>
> > struct device *dev = regmap_get_device(st->map);
>
> > const char *name;
>
> > struct iio_dev *indio_dev;
>
> > + int ret;
>
> >
>
> > name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
>
> > if (!name)
>
> > @@ -553,7 +692,51 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
>
> > indio_dev->modes = INDIO_DIRECT_MODE;
>
> > indio_dev->channels = inv_icm42600_gyro_channels;
>
> > indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
>
> > + indio_dev->available_scan_masks = inv_icm42600_gyro_scan_masks;
>
> > +
>
> > + ret = devm_iio_triggered_buffer_setup(dev, indio_dev, NULL,
>
> > + inv_icm42600_gyro_handler,
>
> > + &inv_icm42600_buffer_ops);
>
> > + if (ret)
>
> > + return ret;
>
> > +
>
> > + indio_dev->trig = iio_trigger_get(st->trigger);
>
> >
>
> > st->indio_gyro = indio_dev;
>
> > return devm_iio_device_register(dev, st->indio_gyro);
>
> > }
>
> > +
>
> > +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
>
> > +{
>
> > + struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
>
> > + const size_t gyro_nb = st->fifo.nb.gyro;
>
> > + ssize_t i, size;
>
> > + const void *accel, *gyro, *temp, *timestamp;
>
> > + unsigned int odr;
>
> > + struct inv_icm42600_gyro_buffer buffer;
>
> > +
>
> > + /* exit if no gyro sample */
>
> > + if (gyro_nb == 0)
>
> > + return 0;
>
> > +
>
> > + /* parse all fifo packets */
>
> > + for (i = 0; i < st->fifo.count; i += size) {
>
> > + size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
>
> > + &accel, &gyro, &temp, ×tamp, &odr);
>
> > + dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
>
> > + size);
>
> > + /* quit if error or FIFO is empty */
>
> > + if (size <= 0)
>
> > + return size;
>
>
>
> blank line here.
>
>
>
> > + /* skip packet if no gyro data or data is invalid */
>
> > + if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro)) {
>
> > + dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
>
>
>
> Very noisy logging. I'd drop it for the final version of the driver.
>
>
>
> > + continue;
>
> > + }
>
> > + memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
>
> > + memcpy(&buffer.temp, temp, sizeof(buffer.temp));
>
>
>
> buffer.temp = temp;
>
>
>
> > + iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
>
> > + }
>
> > +
>
> > + return 0;
>
> > +}
>
> > diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
>
> > index 7a5e76305f0b..5667e0204722 100644
>
> > --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
>
> > +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_trigger.c
>
> > @@ -13,6 +13,7 @@
>
> > #include <linux/iio/trigger_consumer.h>
>
> >
>
> > #include "inv_icm42600.h"
>
> > +#include "inv_icm42600_buffer.h"
>
> >
>
> > static irqreturn_t inv_icm42600_trigger_timestamp(int irq, void *_data)
>
> > {
>
> > @@ -45,8 +46,18 @@ static irqreturn_t inv_icm42600_trigger_int_handler(int irq, void *_data)
>
> > dev_warn(dev, "FIFO full data lost!\n");
>
> >
>
> > /* FIFO threshold reached */
>
> > - if (status & INV_ICM42600_INT_STATUS_FIFO_THS)
>
> > - iio_trigger_poll_chained(st->trigger);
>
> > + if (status & INV_ICM42600_INT_STATUS_FIFO_THS) {
>
> > + ret = inv_icm42600_buffer_fifo_read(st, 0);
>
> > + if (ret)
>
> > + dev_err(dev, "FIFO read error %d\n", ret);
>
> > + } else {
>
> > + st->fifo.count = 0;
>
> > + st->fifo.nb.gyro = 0;
>
> > + st->fifo.nb.accel = 0;
>
> > + st->fifo.nb.total = 0;
>
> > + }
>
> > +
>
> > + iio_trigger_poll_chained(st->trigger);
>
> >
>
> > out_unlock:
>
> > mutex_unlock(&st->lock);
>
>
>
>
>
>
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (8 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 09/12] iio: imu: inv_icm42600: add buffer support in iio devices Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-08 14:42 ` Jonathan Cameron
2020-05-07 14:42 ` [PATCH 11/12] dt-bindings: iio: imu: Add inv_icm42600 documentation Jean-Baptiste Maneyrol
2020-05-07 14:42 ` [PATCH 12/12] MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor Jean-Baptiste Maneyrol
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add a timestamp channel with processed value that returns full
precision 20 bits timestamp.
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
corresponding device elapsed time computed by parsing FIFO data.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 10 +-
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 32 ++-
.../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +-
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 6 +
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 32 ++-
.../imu/inv_icm42600/inv_icm42600_timestamp.c | 246 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_timestamp.h | 82 ++++++
8 files changed, 421 insertions(+), 16 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 d6732118010c..1197b545a682 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -7,6 +7,7 @@ inv-icm42600-y += inv_icm42600_accel.o
inv-icm42600-y += inv_icm42600_temp.o
inv-icm42600-y += inv_icm42600_trigger.o
inv-icm42600-y += inv_icm42600_buffer.o
+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 947ca4dd245b..e15eddafe009 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -16,6 +16,7 @@
#include <linux/iio/trigger.h>
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
enum inv_icm42600_chip {
INV_CHIP_ICM42600,
@@ -127,6 +128,7 @@ struct inv_icm42600_suspended {
* @indio_accel: accelerometer IIO device.
* @trigger: device internal interrupt trigger
* @fifo: FIFO management structure.
+ * @timestamp: timestamp management structure.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -142,6 +144,10 @@ struct inv_icm42600_state {
struct iio_dev *indio_accel;
struct iio_trigger *trigger;
struct inv_icm42600_fifo fifo;
+ struct {
+ struct inv_icm42600_timestamp gyro;
+ struct inv_icm42600_timestamp accel;
+ } timestamp;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -382,11 +388,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
-int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
int inv_icm42600_accel_init(struct inv_icm42600_state *st);
-int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
+int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
int irq_type);
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 4206be54d057..ac140c824c03 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) \
{ \
@@ -66,7 +67,7 @@ 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),
+ INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
};
/* IIO buffer data */
@@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
struct iio_poll_func *pf = _data;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = &st->timestamp.accel;
const size_t fifo_nb = st->fifo.nb.total;
+ const size_t accel_nb = st->fifo.nb.accel;
+ const uint32_t fifo_period = st->fifo.period;
int ret;
/* exit if no sample */
if (fifo_nb == 0)
goto out;
- ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
+ inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, accel_nb,
+ pf->timestamp);
+
+ ret = inv_icm42600_accel_parse_fifo(indio_dev);
if (ret)
dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
ret);
@@ -143,6 +150,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write and FIFO watermark */
+ inv_icm42600_timestamp_apply_odr(&st->timestamp.accel, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -347,6 +355,7 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ inv_icm42600_timestamp_update_odr(&st->timestamp.accel, conf.odr);
inv_icm42600_buffer_update_fifo_period(st);
inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
@@ -502,6 +511,9 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
case IIO_TEMP:
return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
mask);
+ case IIO_TIMESTAMP:
+ return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
+ val2, mask);
default:
return -EINVAL;
}
@@ -694,13 +706,18 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
return devm_iio_device_register(dev, st->indio_accel);
}
-int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+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 = &st->timestamp.accel;
+ const size_t fifo_nb = st->fifo.nb.total;
const size_t accel_nb = st->fifo.nb.accel;
+ const uint32_t fifo_period = st->fifo.period;
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *temp, *timestamp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_accel_buffer buffer;
/* exit if no accel sample */
@@ -708,7 +725,7 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
return 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);
dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
@@ -721,9 +738,14 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
dev_dbg(regmap_get_device(st->map), "skip accel data\n");
continue;
}
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_ACCEL)
+ inv_icm42600_timestamp_apply_odr(ts, fifo_period,
+ fifo_nb, no);
memcpy(&buffer.accel, accel, sizeof(buffer.accel));
memcpy(&buffer.temp, temp, sizeof(buffer.temp));
- iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ ts_val = inv_icm42600_timestamp_get(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 b428abdc92ee..bea4c9810da7 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -15,6 +15,7 @@
#include <linux/iio/trigger_consumer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
#include "inv_icm42600_buffer.h"
void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
@@ -194,14 +195,17 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
unsigned int *watermark;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int sleep = 0;
+ struct inv_icm42600_timestamp *ts;
int ret;
if (indio_dev == st->indio_gyro) {
sensor = INV_ICM42600_SENSOR_GYRO;
watermark = &st->fifo.watermark.gyro;
+ ts = &st->timestamp.gyro;
} else if (indio_dev == st->indio_accel) {
sensor = INV_ICM42600_SENSOR_ACCEL;
watermark = &st->fifo.watermark.accel;
+ ts = &st->timestamp.accel;
} else {
return -EINVAL;
}
@@ -223,6 +227,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
else
ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
+ inv_icm42600_timestamp_reset(ts);
+
out_unlock:
mutex_unlock(&st->lock);
if (sleep)
@@ -316,11 +322,25 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
if (st->fifo.nb.total == 0)
return 0;
- ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
- if (ret)
- return ret;
+ if (st->fifo.nb.gyro > 0) {
+ inv_icm42600_timestamp_interrupt(&st->timestamp.gyro,
+ st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.gyro, ts_gyro);
+ ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
+ if (ret)
+ return ret;
+ }
- return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
+ if (st->fifo.nb.accel > 0) {
+ inv_icm42600_timestamp_interrupt(&st->timestamp.accel,
+ st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.accel, ts_accel);
+ ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
+ if (ret)
+ return ret;
+ }
+
+ return ret;
}
int inv_icm42600_buffer_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 689089065ff9..563c4348de01 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -15,6 +15,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[] = {
{
@@ -516,6 +517,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ /* initialize timestamping */
+ ret = inv_icm42600_timestamp_init(st);
+ if (ret)
+ return ret;
+
/* setup FIFO buffer */
ret = inv_icm42600_buffer_init(st);
if (ret)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index dafb104abc77..1245588170bd 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) \
{ \
@@ -66,7 +67,7 @@ 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),
+ INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
};
/* IIO buffer data */
@@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
struct iio_poll_func *pf = _data;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = &st->timestamp.gyro;
const size_t fifo_nb = st->fifo.nb.total;
+ const size_t gyro_nb = st->fifo.nb.gyro;
+ const uint32_t fifo_period = st->fifo.period;
int ret;
/* exit if no sample */
if (fifo_nb == 0)
goto out;
- ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
+ inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, gyro_nb,
+ pf->timestamp);
+
+ ret = inv_icm42600_gyro_parse_fifo(indio_dev);
if (ret)
dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
ret);
@@ -143,6 +150,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write and FIFO watermark */
+ inv_icm42600_timestamp_apply_odr(&st->timestamp.gyro, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -359,6 +367,7 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
mutex_lock(&st->lock);
conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ inv_icm42600_timestamp_update_odr(&st->timestamp.gyro, conf.odr);
inv_icm42600_buffer_update_fifo_period(st);
inv_icm42600_buffer_update_watermark(st);
mutex_unlock(&st->lock);
@@ -514,6 +523,9 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
case IIO_TEMP:
return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
mask);
+ case IIO_TIMESTAMP:
+ return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
+ val2, mask);
default:
return -EINVAL;
}
@@ -706,13 +718,18 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
return devm_iio_device_register(dev, st->indio_gyro);
}
-int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
+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 = &st->timestamp.gyro;
+ const size_t fifo_nb = st->fifo.nb.total;
const size_t gyro_nb = st->fifo.nb.gyro;
+ const uint32_t fifo_period = st->fifo.period;
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *temp, *timestamp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_gyro_buffer buffer;
/* exit if no gyro sample */
@@ -720,7 +737,7 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
return 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);
dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
@@ -733,9 +750,14 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
continue;
}
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_GYRO)
+ inv_icm42600_timestamp_apply_odr(ts, fifo_period,
+ fifo_nb, no);
memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
memcpy(&buffer.temp, temp, sizeof(buffer.temp));
- iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
+ ts_val = inv_icm42600_timestamp_get(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..79cf777e2e84
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
@@ -0,0 +1,246 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.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)
+
+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;
+
+ 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);
+}
+
+static int inv_icm42600_timestamp_read(struct inv_icm42600_state *st,
+ uint32_t *ts)
+{
+ struct device *dev = regmap_get_device(st->map);
+ __le32 raw;
+ int ret;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
+ INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE);
+ if (ret)
+ goto exit;
+
+ /* Read timestamp value 3 registers little-endian */
+ raw = 0;
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TMSTVAL, &raw, 3);
+ if (ret)
+ goto exit;
+
+ *ts = (uint32_t)le32_to_cpu(raw);
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+int inv_icm42600_timestamp_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);
+ uint32_t ts;
+ int ret;
+
+ if (chan->type != IIO_TIMESTAMP)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_PROCESSED:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_timestamp_read(st, &ts);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = ts * 1000;
+ return IIO_VAL_INT;
+ default:
+ return -EINVAL;
+ }
+}
+
+static void inv_icm42600_init_ts(struct inv_icm42600_timestamp *ts,
+ struct device *dev, uint32_t period)
+{
+ /* initial odr for sensor is 1kHz */
+ const uint32_t default_period = 1000000;
+
+ ts->dev = dev;
+ ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+ ts->period = default_period;
+
+ /* use theoretical value for chip period */
+ inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
+}
+
+int inv_icm42600_timestamp_init(struct inv_icm42600_state *st)
+{
+ unsigned int val;
+
+ inv_icm42600_init_ts(&st->timestamp.gyro, regmap_get_device(st->map),
+ inv_icm42600_odr_to_period(st->conf.gyro.odr));
+ inv_icm42600_init_ts(&st->timestamp.accel, regmap_get_device(st->map),
+ inv_icm42600_odr_to_period(st->conf.accel.odr));
+
+ /* 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);
+}
+
+void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ int odr)
+{
+ uint32_t period;
+
+ period = inv_icm42600_odr_to_period(odr);
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+ dev_dbg(ts->dev, "ts new mult: %u\n", ts->new_mult);
+}
+
+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 time interval between interrupts 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);
+
+ dev_dbg(ts->dev, "ts chip period: %u - val %u\n", new_chip_period,
+ ts->chip_period.val);
+
+ 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;
+ dev_dbg(ts->dev, "ts it: %lld - %lld - %lld\n", it->lo, it->up, delta);
+ if (it->lo != 0) {
+ period = div_s64(delta, fifo_nb);
+ valid = inv_compute_chip_period(ts, fifo_mult, period);
+ if (valid)
+ ts->period = ts->mult * ts->chip_period.val;
+ }
+
+ /* no previous data, compute theoritical value from interrupt */
+ if (ts->timestamp == 0) {
+ interval = (int64_t)ts->period * (int64_t)sensor_nb;
+ ts->timestamp = it->up - interval;
+ dev_dbg(ts->dev, "ts start: %lld\n", ts->timestamp);
+ return;
+ }
+
+ /* if interrupt interval is valid, sync with interrupt timestamp */
+ if (valid) {
+ /* compute real fifo_period */
+ fifo_period = fifo_mult * ts->chip_period.val;
+ delta = it->lo - ts->timestamp;
+ 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;
+ dev_dbg(ts->dev, "ts adj: %lld\n", delta);
+ 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;
+
+ ts->mult = ts->new_mult;
+ ts->period = ts->mult * ts->chip_period.val;
+ dev_dbg(ts->dev, "ts mult: %u\n", ts->mult);
+
+ if (ts->timestamp != 0) {
+ /* compute real fifo period */
+ fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ fifo_period = fifo_mult * ts->chip_period.val;
+ /* compute timestamp from current interrupt after ODR change */
+ interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
+ ts->timestamp = ts->it.up - interval;
+ dev_dbg(ts->dev, "ts new: %lld\n", ts->timestamp);
+ }
+}
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..c865e1a9a7c8
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
@@ -0,0 +1,82 @@
+/* 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/device.h>
+#include <linux/iio/iio.h>
+
+struct inv_icm42600_state;
+
+struct inv_icm42600_timestamp_interval {
+ int64_t lo;
+ int64_t up;
+};
+
+struct inv_icm42600_timestamp_acc {
+ uint32_t val;
+ size_t idx;
+ uint32_t values[32];
+};
+
+struct inv_icm42600_timestamp {
+ struct device *dev;
+ 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;
+};
+
+#define INV_ICM42600_TIMESTAMP_CHAN(_index) \
+ { \
+ .type = IIO_TIMESTAMP, \
+ .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 64, \
+ .storagebits = 64, \
+ }, \
+ }
+
+int inv_icm42600_timestamp_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask);
+
+int inv_icm42600_timestamp_init(struct inv_icm42600_state *st);
+
+void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ int odr);
+
+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_get(struct inv_icm42600_timestamp *ts)
+{
+ ts->timestamp += ts->period;
+ dev_dbg(ts->dev, "ts: %lld\n", ts->timestamp);
+
+ 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
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping
2020-05-07 14:42 ` [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping Jean-Baptiste Maneyrol
@ 2020-05-08 14:42 ` Jonathan Cameron
2020-05-18 15:48 ` Jean-Baptiste Maneyrol
0 siblings, 1 reply; 30+ messages in thread
From: Jonathan Cameron @ 2020-05-08 14:42 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
On Thu, 7 May 2020 16:42:20 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add a timestamp channel with processed value that returns full
> precision 20 bits timestamp.
>
> 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
> corresponding device elapsed time computed by parsing FIFO data.
Yikes. That is complex. Hmm. I always get lost in the maths in these
timestamp patches so my review may be a little superficial.
However a bit more in the way of docs would be good here.
The sysfs read of timestamp is unusual and I'm not really sure what it is for.
The delays in a sysfs read of that value are likely to be enough that it's
that useful for anything.
Also, do we make use of the timestamps within the fifo records?
J
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 10 +-
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +-
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 6 +
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 246 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_timestamp.h | 82 ++++++
> 8 files changed, 421 insertions(+), 16 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 d6732118010c..1197b545a682 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -7,6 +7,7 @@ inv-icm42600-y += inv_icm42600_accel.o
> inv-icm42600-y += inv_icm42600_temp.o
> inv-icm42600-y += inv_icm42600_trigger.o
> inv-icm42600-y += inv_icm42600_buffer.o
> +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 947ca4dd245b..e15eddafe009 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -16,6 +16,7 @@
> #include <linux/iio/trigger.h>
>
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> @@ -127,6 +128,7 @@ struct inv_icm42600_suspended {
> * @indio_accel: accelerometer IIO device.
> * @trigger: device internal interrupt trigger
> * @fifo: FIFO management structure.
> + * @timestamp: timestamp management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -142,6 +144,10 @@ struct inv_icm42600_state {
> struct iio_dev *indio_accel;
> struct iio_trigger *trigger;
> struct inv_icm42600_fifo fifo;
> + struct {
> + struct inv_icm42600_timestamp gyro;
> + struct inv_icm42600_timestamp accel;
> + } timestamp;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -382,11 +388,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> int irq_type);
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 4206be54d057..ac140c824c03 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) \
> { \
> @@ -66,7 +67,7 @@ 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),
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.accel;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, accel_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_accel_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.accel, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -347,6 +355,7 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.accel, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -502,6 +511,9 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -694,13 +706,18 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> return devm_iio_device_register(dev, st->indio_accel);
> }
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +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 = &st->timestamp.accel;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_accel_buffer buffer;
>
> /* exit if no accel sample */
> @@ -708,7 +725,7 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> return 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);
> dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
> @@ -721,9 +738,14 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip accel data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_ACCEL)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(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 b428abdc92ee..bea4c9810da7 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -15,6 +15,7 @@
> #include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> #include "inv_icm42600_buffer.h"
>
> void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
> @@ -194,14 +195,17 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> unsigned int *watermark;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int sleep = 0;
> + struct inv_icm42600_timestamp *ts;
> int ret;
>
> if (indio_dev == st->indio_gyro) {
> sensor = INV_ICM42600_SENSOR_GYRO;
> watermark = &st->fifo.watermark.gyro;
> + ts = &st->timestamp.gyro;
> } else if (indio_dev == st->indio_accel) {
> sensor = INV_ICM42600_SENSOR_ACCEL;
> watermark = &st->fifo.watermark.accel;
> + ts = &st->timestamp.accel;
> } else {
> return -EINVAL;
> }
> @@ -223,6 +227,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> else
> ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
>
> + inv_icm42600_timestamp_reset(ts);
> +
> out_unlock:
> mutex_unlock(&st->lock);
> if (sleep)
> @@ -316,11 +322,25 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> if (st->fifo.nb.total == 0)
> return 0;
>
> - ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
> - if (ret)
> - return ret;
> + if (st->fifo.nb.gyro > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.gyro,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.gyro, ts_gyro);
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> + if (ret)
> + return ret;
> + }
>
> - return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
> + if (st->fifo.nb.accel > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.accel,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.accel, ts_accel);
> + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> + if (ret)
> + return ret;
> + }
> +
> + return ret;
> }
>
> int inv_icm42600_buffer_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 689089065ff9..563c4348de01 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -15,6 +15,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[] = {
> {
> @@ -516,6 +517,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + /* initialize timestamping */
> + ret = inv_icm42600_timestamp_init(st);
> + if (ret)
> + return ret;
> +
> /* setup FIFO buffer */
> ret = inv_icm42600_buffer_init(st);
> if (ret)
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> index dafb104abc77..1245588170bd 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) \
> { \
> @@ -66,7 +67,7 @@ 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),
Interrupt timestamp was pretty much garbage, so I'd just not register that in the
first place. Introduce the timestamp for the first time in this patch.
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.gyro;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, gyro_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_gyro_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.gyro, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -359,6 +367,7 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.gyro, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -514,6 +523,9 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -706,13 +718,18 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> return devm_iio_device_register(dev, st->indio_gyro);
> }
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +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 = &st->timestamp.gyro;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_gyro_buffer buffer;
>
> /* exit if no gyro sample */
> @@ -720,7 +737,7 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> return 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);
> dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
> @@ -733,9 +750,14 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_GYRO)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(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..79cf777e2e84
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> @@ -0,0 +1,246 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/math64.h>
> +#include <linux/iio/iio.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)
> +
> +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;
> +
> + 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);
> +}
> +
> +static int inv_icm42600_timestamp_read(struct inv_icm42600_state *st,
> + uint32_t *ts)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + __le32 raw;
> + int ret;
> +
> + pm_runtime_get_sync(dev);
> + mutex_lock(&st->lock);
> +
> + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> + INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE);
> + if (ret)
> + goto exit;
> +
> + /* Read timestamp value 3 registers little-endian */
> + raw = 0;
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TMSTVAL, &raw, 3);
> + if (ret)
> + goto exit;
> +
> + *ts = (uint32_t)le32_to_cpu(raw);
> +exit:
> + mutex_unlock(&st->lock);
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> + return ret;
> +}
> +
> +int inv_icm42600_timestamp_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);
> + uint32_t ts;
> + int ret;
> +
> + if (chan->type != IIO_TIMESTAMP)
> + return -EINVAL;
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_PROCESSED:
> + ret = iio_device_claim_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + ret = inv_icm42600_timestamp_read(st, &ts);
> + iio_device_release_direct_mode(indio_dev);
Unusual to expose it as a readable channel. Why would you want to do this?
> + if (ret)
> + return ret;
> + *val = ts * 1000;
> + return IIO_VAL_INT;
> + default:
> + return -EINVAL;
> + }
> +}
> +
> +static void inv_icm42600_init_ts(struct inv_icm42600_timestamp *ts,
> + struct device *dev, uint32_t period)
> +{
> + /* initial odr for sensor is 1kHz */
> + const uint32_t default_period = 1000000;
> +
> + ts->dev = dev;
> + ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->period = default_period;
> +
> + /* use theoretical value for chip period */
> + inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
> +}
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st)
> +{
> + unsigned int val;
> +
> + inv_icm42600_init_ts(&st->timestamp.gyro, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.gyro.odr));
> + inv_icm42600_init_ts(&st->timestamp.accel, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.accel.odr));
> +
> + /* 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);
> +}
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr)
> +{
> + uint32_t period;
> +
> + period = inv_icm42600_odr_to_period(odr);
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + dev_dbg(ts->dev, "ts new mult: %u\n", ts->new_mult);
> +}
> +
> +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 time interval between interrupts 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);
> +
> + dev_dbg(ts->dev, "ts chip period: %u - val %u\n", new_chip_period,
> + ts->chip_period.val);
> +
> + 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;
> + dev_dbg(ts->dev, "ts it: %lld - %lld - %lld\n", it->lo, it->up, delta);
> + if (it->lo != 0) {
> + period = div_s64(delta, fifo_nb);
> + valid = inv_compute_chip_period(ts, fifo_mult, period);
> + if (valid)
> + ts->period = ts->mult * ts->chip_period.val;
> + }
> +
> + /* no previous data, compute theoritical value from interrupt */
> + if (ts->timestamp == 0) {
> + interval = (int64_t)ts->period * (int64_t)sensor_nb;
> + ts->timestamp = it->up - interval;
> + dev_dbg(ts->dev, "ts start: %lld\n", ts->timestamp);
> + return;
> + }
> +
> + /* if interrupt interval is valid, sync with interrupt timestamp */
> + if (valid) {
> + /* compute real fifo_period */
> + fifo_period = fifo_mult * ts->chip_period.val;
> + delta = it->lo - ts->timestamp;
> + 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;
> + dev_dbg(ts->dev, "ts adj: %lld\n", delta);
> + 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;
> +
> + ts->mult = ts->new_mult;
> + ts->period = ts->mult * ts->chip_period.val;
> + dev_dbg(ts->dev, "ts mult: %u\n", ts->mult);
> +
> + if (ts->timestamp != 0) {
> + /* compute real fifo period */
> + fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + fifo_period = fifo_mult * ts->chip_period.val;
> + /* compute timestamp from current interrupt after ODR change */
> + interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
> + ts->timestamp = ts->it.up - interval;
> + dev_dbg(ts->dev, "ts new: %lld\n", ts->timestamp);
> + }
> +}
> 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..c865e1a9a7c8
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> @@ -0,0 +1,82 @@
> +/* 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/device.h>
> +#include <linux/iio/iio.h>
> +
> +struct inv_icm42600_state;
> +
> +struct inv_icm42600_timestamp_interval {
> + int64_t lo;
> + int64_t up;
> +};
> +
> +struct inv_icm42600_timestamp_acc {
> + uint32_t val;
> + size_t idx;
> + uint32_t values[32];
> +};
> +
> +struct inv_icm42600_timestamp {
> + struct device *dev;
> + 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;
> +};
> +
> +#define INV_ICM42600_TIMESTAMP_CHAN(_index) \
> + { \
> + .type = IIO_TIMESTAMP, \
> + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
> + .scan_index = _index, \
> + .scan_type = { \
> + .sign = 's', \
> + .realbits = 64, \
> + .storagebits = 64, \
> + }, \
> + }
> +
> +int inv_icm42600_timestamp_read_raw(struct iio_dev *indio_dev,
> + struct iio_chan_spec const *chan,
> + int *val, int *val2, long mask);
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st);
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr);
> +
> +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_get(struct inv_icm42600_timestamp *ts)
Perhaps timestamp_pop? Kind of indicates that you are destructively
retrieving a timetamp.
> +{
> + ts->timestamp += ts->period;
> + dev_dbg(ts->dev, "ts: %lld\n", ts->timestamp);
> +
> + 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
^ permalink raw reply [flat|nested] 30+ messages in thread* Re: [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping
2020-05-08 14:42 ` Jonathan Cameron
@ 2020-05-18 15:48 ` Jean-Baptiste Maneyrol
0 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-18 15:48 UTC (permalink / raw)
To: Jonathan Cameron
Cc: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh, linux-iio,
devicetree, linux-kernel
Hi Jonathan,
reading timestamp register is here mainly because we can do it, and this is the only way to have the full 20 bits resolution.
But it is not very useful indeed. I will delete it.
I am not using timestamp stored inside the FIFO, because it is only present when having both accel and gyro on. There is no timestamp data for a single sensor, which is really too bad. That would have been helpful.
I will add more documentation about it. It is not that tricky, but need more deep explanations.
New series V2 is in preparation. And again my apologizes for ruining the mail text formatting.
Thanks,
JB
From: Jonathan Cameron <Jonathan.Cameron@Huawei.com>
Sent: Friday, May 8, 2020 16:42
To: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Cc: jic23@kernel.org <jic23@kernel.org>; robh+dt@kernel.org <robh+dt@kernel.org>; robh@kernel.org <robh@kernel.org>; mchehab+huawei@kernel.org <mchehab+huawei@kernel.org>; davem@davemloft.net <davem@davemloft.net>; gregkh@linuxfoundation.org <gregkh@linuxfoundation.org>;
linux-iio@vger.kernel.org <linux-iio@vger.kernel.org>; devicetree@vger.kernel.org <devicetree@vger.kernel.org>; linux-kernel@vger.kernel.org <linux-kernel@vger.kernel.org>
Subject: Re: [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping
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 Thu, 7 May 2020 16:42:20 +0200
Jean-Baptiste Maneyrol <jmaneyrol@invensense.com> wrote:
> Add a timestamp channel with processed value that returns full
> precision 20 bits timestamp.
>
> 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
> corresponding device elapsed time computed by parsing FIFO data.
Yikes. That is complex. Hmm. I always get lost in the maths in these
timestamp patches so my review may be a little superficial.
However a bit more in the way of docs would be good here.
The sysfs read of timestamp is unusual and I'm not really sure what it is for.
The delays in a sysfs read of that value are likely to be enough that it's
that useful for anything.
Also, do we make use of the timestamps within the fifo records?
J
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
> drivers/iio/imu/inv_icm42600/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 10 +-
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +-
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 6 +
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 32 ++-
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 246 ++++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_timestamp.h | 82 ++++++
> 8 files changed, 421 insertions(+), 16 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 d6732118010c..1197b545a682 100644
> --- a/drivers/iio/imu/inv_icm42600/Makefile
> +++ b/drivers/iio/imu/inv_icm42600/Makefile
> @@ -7,6 +7,7 @@ inv-icm42600-y += inv_icm42600_accel.o
> inv-icm42600-y += inv_icm42600_temp.o
> inv-icm42600-y += inv_icm42600_trigger.o
> inv-icm42600-y += inv_icm42600_buffer.o
> +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 947ca4dd245b..e15eddafe009 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
> @@ -16,6 +16,7 @@
> #include <linux/iio/trigger.h>
>
> #include "inv_icm42600_buffer.h"
> +#include "inv_icm42600_timestamp.h"
>
> enum inv_icm42600_chip {
> INV_CHIP_ICM42600,
> @@ -127,6 +128,7 @@ struct inv_icm42600_suspended {
> * @indio_accel: accelerometer IIO device.
> * @trigger: device internal interrupt trigger
> * @fifo: FIFO management structure.
> + * @timestamp: timestamp management structure.
> */
> struct inv_icm42600_state {
> struct mutex lock;
> @@ -142,6 +144,10 @@ struct inv_icm42600_state {
> struct iio_dev *indio_accel;
> struct iio_trigger *trigger;
> struct inv_icm42600_fifo fifo;
> + struct {
> + struct inv_icm42600_timestamp gyro;
> + struct inv_icm42600_timestamp accel;
> + } timestamp;
> };
>
> /* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
> @@ -382,11 +388,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
>
> int inv_icm42600_gyro_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_accel_init(struct inv_icm42600_state *st);
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts);
> +int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev);
>
> int inv_icm42600_trigger_init(struct inv_icm42600_state *st, int irq,
> int irq_type);
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> index 4206be54d057..ac140c824c03 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) \
> { \
> @@ -66,7 +67,7 @@ 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),
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_accel_handler(int irq, void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.accel;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_accel_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, accel_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_accel_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "accel fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.accel, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -347,6 +355,7 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
> ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.accel, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -502,6 +511,9 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -694,13 +706,18 @@ int inv_icm42600_accel_init(struct inv_icm42600_state *st)
> return devm_iio_device_register(dev, st->indio_accel);
> }
>
> -int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +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 = &st->timestamp.accel;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t accel_nb = st->fifo.nb.accel;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_accel_buffer buffer;
>
> /* exit if no accel sample */
> @@ -708,7 +725,7 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> return 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);
> dev_dbg(regmap_get_device(st->map), "accel packet size = %zd\n",
> @@ -721,9 +738,14 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip accel data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_ACCEL)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.accel, accel, sizeof(buffer.accel));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(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 b428abdc92ee..bea4c9810da7 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> @@ -15,6 +15,7 @@
> #include <linux/iio/trigger_consumer.h>
>
> #include "inv_icm42600.h"
> +#include "inv_icm42600_timestamp.h"
> #include "inv_icm42600_buffer.h"
>
> void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st)
> @@ -194,14 +195,17 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> unsigned int *watermark;
> struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
> unsigned int sleep = 0;
> + struct inv_icm42600_timestamp *ts;
> int ret;
>
> if (indio_dev == st->indio_gyro) {
> sensor = INV_ICM42600_SENSOR_GYRO;
> watermark = &st->fifo.watermark.gyro;
> + ts = &st->timestamp.gyro;
> } else if (indio_dev == st->indio_accel) {
> sensor = INV_ICM42600_SENSOR_ACCEL;
> watermark = &st->fifo.watermark.accel;
> + ts = &st->timestamp.accel;
> } else {
> return -EINVAL;
> }
> @@ -223,6 +227,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
> else
> ret = inv_icm42600_set_accel_conf(st, &conf, &sleep);
>
> + inv_icm42600_timestamp_reset(ts);
> +
> out_unlock:
> mutex_unlock(&st->lock);
> if (sleep)
> @@ -316,11 +322,25 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
> if (st->fifo.nb.total == 0)
> return 0;
>
> - ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro, ts_gyro);
> - if (ret)
> - return ret;
> + if (st->fifo.nb.gyro > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.gyro,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.gyro, ts_gyro);
> + ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
> + if (ret)
> + return ret;
> + }
>
> - return inv_icm42600_accel_parse_fifo(st->indio_accel, ts_accel);
> + if (st->fifo.nb.accel > 0) {
> + inv_icm42600_timestamp_interrupt(&st->timestamp.accel,
> + st->fifo.period, st->fifo.nb.total,
> + st->fifo.nb.accel, ts_accel);
> + ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
> + if (ret)
> + return ret;
> + }
> +
> + return ret;
> }
>
> int inv_icm42600_buffer_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 689089065ff9..563c4348de01 100644
> --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> @@ -15,6 +15,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[] = {
> {
> @@ -516,6 +517,11 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
> if (ret)
> return ret;
>
> + /* initialize timestamping */
> + ret = inv_icm42600_timestamp_init(st);
> + if (ret)
> + return ret;
> +
> /* setup FIFO buffer */
> ret = inv_icm42600_buffer_init(st);
> if (ret)
> diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> index dafb104abc77..1245588170bd 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) \
> { \
> @@ -66,7 +67,7 @@ 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),
Interrupt timestamp was pretty much garbage, so I'd just not register that in the
first place. Introduce the timestamp for the first time in this patch.
> + INV_ICM42600_TIMESTAMP_CHAN(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
> };
>
> /* IIO buffer data */
> @@ -94,14 +95,20 @@ static irqreturn_t inv_icm42600_gyro_handler(int irq, void *_data)
> struct iio_poll_func *pf = _data;
> struct iio_dev *indio_dev = pf->indio_dev;
> struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
> + struct inv_icm42600_timestamp *ts = &st->timestamp.gyro;
> const size_t fifo_nb = st->fifo.nb.total;
> + const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> int ret;
>
> /* exit if no sample */
> if (fifo_nb == 0)
> goto out;
>
> - ret = inv_icm42600_gyro_parse_fifo(indio_dev, pf->timestamp);
> + inv_icm42600_timestamp_interrupt(ts, fifo_period, fifo_nb, gyro_nb,
> + pf->timestamp);
> +
> + ret = inv_icm42600_gyro_parse_fifo(indio_dev);
> if (ret)
> dev_err(regmap_get_device(st->map), "gyro fifo error %d\n",
> ret);
> @@ -143,6 +150,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
> }
>
> /* update data FIFO write and FIFO watermark */
> + inv_icm42600_timestamp_apply_odr(&st->timestamp.gyro, 0, 0, 0);
> ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
> if (ret)
> goto out_unlock;
> @@ -359,6 +367,7 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
> mutex_lock(&st->lock);
> conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
> ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
> + inv_icm42600_timestamp_update_odr(&st->timestamp.gyro, conf.odr);
> inv_icm42600_buffer_update_fifo_period(st);
> inv_icm42600_buffer_update_watermark(st);
> mutex_unlock(&st->lock);
> @@ -514,6 +523,9 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
> case IIO_TEMP:
> return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2,
> mask);
> + case IIO_TIMESTAMP:
> + return inv_icm42600_timestamp_read_raw(indio_dev, chan, val,
> + val2, mask);
> default:
> return -EINVAL;
> }
> @@ -706,13 +718,18 @@ int inv_icm42600_gyro_init(struct inv_icm42600_state *st)
> return devm_iio_device_register(dev, st->indio_gyro);
> }
>
> -int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> +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 = &st->timestamp.gyro;
> + const size_t fifo_nb = st->fifo.nb.total;
> const size_t gyro_nb = st->fifo.nb.gyro;
> + const uint32_t fifo_period = st->fifo.period;
> ssize_t i, size;
> + unsigned int no;
> const void *accel, *gyro, *temp, *timestamp;
> unsigned int odr;
> + int64_t ts_val;
> struct inv_icm42600_gyro_buffer buffer;
>
> /* exit if no gyro sample */
> @@ -720,7 +737,7 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> return 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);
> dev_dbg(regmap_get_device(st->map), "gyro packet size = %zd\n",
> @@ -733,9 +750,14 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev, int64_t ts)
> dev_dbg(regmap_get_device(st->map), "skip gyro data\n");
> continue;
> }
> + /* update odr */
> + if (odr & INV_ICM42600_SENSOR_GYRO)
> + inv_icm42600_timestamp_apply_odr(ts, fifo_period,
> + fifo_nb, no);
> memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
> memcpy(&buffer.temp, temp, sizeof(buffer.temp));
> - iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts);
> + ts_val = inv_icm42600_timestamp_get(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..79cf777e2e84
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> @@ -0,0 +1,246 @@
> +// SPDX-License-Identifier: GPL-2.0-or-later
> +/*
> + * Copyright (C) 2020 Invensense, Inc.
> + */
> +
> +#include <linux/device.h>
> +#include <linux/mutex.h>
> +#include <linux/pm_runtime.h>
> +#include <linux/regmap.h>
> +#include <linux/math64.h>
> +#include <linux/iio/iio.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)
> +
> +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;
> +
> + 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);
> +}
> +
> +static int inv_icm42600_timestamp_read(struct inv_icm42600_state *st,
> + uint32_t *ts)
> +{
> + struct device *dev = regmap_get_device(st->map);
> + __le32 raw;
> + int ret;
> +
> + pm_runtime_get_sync(dev);
> + mutex_lock(&st->lock);
> +
> + ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET,
> + INV_ICM42600_SIGNAL_PATH_RESET_TMST_STROBE);
> + if (ret)
> + goto exit;
> +
> + /* Read timestamp value 3 registers little-endian */
> + raw = 0;
> + ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TMSTVAL, &raw, 3);
> + if (ret)
> + goto exit;
> +
> + *ts = (uint32_t)le32_to_cpu(raw);
> +exit:
> + mutex_unlock(&st->lock);
> + pm_runtime_mark_last_busy(dev);
> + pm_runtime_put_autosuspend(dev);
> + return ret;
> +}
> +
> +int inv_icm42600_timestamp_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);
> + uint32_t ts;
> + int ret;
> +
> + if (chan->type != IIO_TIMESTAMP)
> + return -EINVAL;
> +
> + switch (mask) {
> + case IIO_CHAN_INFO_PROCESSED:
> + ret = iio_device_claim_direct_mode(indio_dev);
> + if (ret)
> + return ret;
> + ret = inv_icm42600_timestamp_read(st, &ts);
> + iio_device_release_direct_mode(indio_dev);
Unusual to expose it as a readable channel. Why would you want to do this?
> + if (ret)
> + return ret;
> + *val = ts * 1000;
> + return IIO_VAL_INT;
> + default:
> + return -EINVAL;
> + }
> +}
> +
> +static void inv_icm42600_init_ts(struct inv_icm42600_timestamp *ts,
> + struct device *dev, uint32_t period)
> +{
> + /* initial odr for sensor is 1kHz */
> + const uint32_t default_period = 1000000;
> +
> + ts->dev = dev;
> + ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + ts->period = default_period;
> +
> + /* use theoretical value for chip period */
> + inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
> +}
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st)
> +{
> + unsigned int val;
> +
> + inv_icm42600_init_ts(&st->timestamp.gyro, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.gyro.odr));
> + inv_icm42600_init_ts(&st->timestamp.accel, regmap_get_device(st->map),
> + inv_icm42600_odr_to_period(st->conf.accel.odr));
> +
> + /* 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);
> +}
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr)
> +{
> + uint32_t period;
> +
> + period = inv_icm42600_odr_to_period(odr);
> + ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
> + dev_dbg(ts->dev, "ts new mult: %u\n", ts->new_mult);
> +}
> +
> +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 time interval between interrupts 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);
> +
> + dev_dbg(ts->dev, "ts chip period: %u - val %u\n", new_chip_period,
> + ts->chip_period.val);
> +
> + 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;
> + dev_dbg(ts->dev, "ts it: %lld - %lld - %lld\n", it->lo, it->up, delta);
> + if (it->lo != 0) {
> + period = div_s64(delta, fifo_nb);
> + valid = inv_compute_chip_period(ts, fifo_mult, period);
> + if (valid)
> + ts->period = ts->mult * ts->chip_period.val;
> + }
> +
> + /* no previous data, compute theoritical value from interrupt */
> + if (ts->timestamp == 0) {
> + interval = (int64_t)ts->period * (int64_t)sensor_nb;
> + ts->timestamp = it->up - interval;
> + dev_dbg(ts->dev, "ts start: %lld\n", ts->timestamp);
> + return;
> + }
> +
> + /* if interrupt interval is valid, sync with interrupt timestamp */
> + if (valid) {
> + /* compute real fifo_period */
> + fifo_period = fifo_mult * ts->chip_period.val;
> + delta = it->lo - ts->timestamp;
> + 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;
> + dev_dbg(ts->dev, "ts adj: %lld\n", delta);
> + 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;
> +
> + ts->mult = ts->new_mult;
> + ts->period = ts->mult * ts->chip_period.val;
> + dev_dbg(ts->dev, "ts mult: %u\n", ts->mult);
> +
> + if (ts->timestamp != 0) {
> + /* compute real fifo period */
> + fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
> + fifo_period = fifo_mult * ts->chip_period.val;
> + /* compute timestamp from current interrupt after ODR change */
> + interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
> + ts->timestamp = ts->it.up - interval;
> + dev_dbg(ts->dev, "ts new: %lld\n", ts->timestamp);
> + }
> +}
> 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..c865e1a9a7c8
> --- /dev/null
> +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
> @@ -0,0 +1,82 @@
> +/* 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/device.h>
> +#include <linux/iio/iio.h>
> +
> +struct inv_icm42600_state;
> +
> +struct inv_icm42600_timestamp_interval {
> + int64_t lo;
> + int64_t up;
> +};
> +
> +struct inv_icm42600_timestamp_acc {
> + uint32_t val;
> + size_t idx;
> + uint32_t values[32];
> +};
> +
> +struct inv_icm42600_timestamp {
> + struct device *dev;
> + 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;
> +};
> +
> +#define INV_ICM42600_TIMESTAMP_CHAN(_index) \
> + { \
> + .type = IIO_TIMESTAMP, \
> + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), \
> + .scan_index = _index, \
> + .scan_type = { \
> + .sign = 's', \
> + .realbits = 64, \
> + .storagebits = 64, \
> + }, \
> + }
> +
> +int inv_icm42600_timestamp_read_raw(struct iio_dev *indio_dev,
> + struct iio_chan_spec const *chan,
> + int *val, int *val2, long mask);
> +
> +int inv_icm42600_timestamp_init(struct inv_icm42600_state *st);
> +
> +void inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
> + int odr);
> +
> +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_get(struct inv_icm42600_timestamp *ts)
Perhaps timestamp_pop? Kind of indicates that you are destructively
retrieving a timetamp.
> +{
> + ts->timestamp += ts->period;
> + dev_dbg(ts->dev, "ts: %lld\n", ts->timestamp);
> +
> + 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
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 11/12] dt-bindings: iio: imu: Add inv_icm42600 documentation
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (9 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 10/12] iio: imu: inv_icm42600: add accurate timestamping Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
2020-05-15 3:00 ` Rob Herring
2020-05-07 14:42 ` [PATCH 12/12] MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor Jean-Baptiste Maneyrol
11 siblings, 1 reply; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Document the ICM-426xxx devices devicetree bindings.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
.../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..a7175f6543fa
--- /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 <jmaneyrol@invensense.com>
+
+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
+
+ spi-cpha: true
+
+ spi-cpol: true
+
+ spi-max-frequency:
+ maxItems: 1
+
+ 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
^ permalink raw reply related [flat|nested] 30+ messages in thread* Re: [PATCH 11/12] dt-bindings: iio: imu: Add inv_icm42600 documentation
2020-05-07 14:42 ` [PATCH 11/12] dt-bindings: iio: imu: Add inv_icm42600 documentation Jean-Baptiste Maneyrol
@ 2020-05-15 3:00 ` Rob Herring
0 siblings, 0 replies; 30+ messages in thread
From: Rob Herring @ 2020-05-15 3:00 UTC (permalink / raw)
To: Jean-Baptiste Maneyrol
Cc: jic23, mchehab+huawei, davem, gregkh, linux-iio, devicetree,
linux-kernel
On Thu, May 07, 2020 at 04:42:21PM +0200, Jean-Baptiste Maneyrol wrote:
> Document the ICM-426xxx devices devicetree bindings.
>
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
> ---
> .../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..a7175f6543fa
> --- /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 <jmaneyrol@invensense.com>
> +
> +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
> +
> + spi-cpha: true
> +
> + spi-cpol: true
It doesn't make much sense to specify these and not make them required.
I guess you could have a device with multiple modes, but generally
there's only one right combination of these properties. Also, this could
just be implied by the compatible.
> +
> + spi-max-frequency:
> + maxItems: 1
Not an array. Either give a frequency range (minimum/maximum) or just
'true'.
> +
> + 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
>
^ permalink raw reply [flat|nested] 30+ messages in thread
* [PATCH 12/12] MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor
2020-05-07 14:42 [PATCH 00/12] iio: imu: new inv_icm42600 driver Jean-Baptiste Maneyrol
` (10 preceding siblings ...)
2020-05-07 14:42 ` [PATCH 11/12] dt-bindings: iio: imu: Add inv_icm42600 documentation Jean-Baptiste Maneyrol
@ 2020-05-07 14:42 ` Jean-Baptiste Maneyrol
11 siblings, 0 replies; 30+ messages in thread
From: Jean-Baptiste Maneyrol @ 2020-05-07 14:42 UTC (permalink / raw)
To: jic23, robh+dt, robh, mchehab+huawei, davem, gregkh
Cc: linux-iio, devicetree, linux-kernel, Jean-Baptiste Maneyrol
Add MAINTAINERS entry for InvenSense ICM-426xx IMU device.
Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
MAINTAINERS | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/MAINTAINERS b/MAINTAINERS
index 10eb348c801c..1714390e2721 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -8864,6 +8864,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 <jmaneyrol@invensense.com>
+L: linux-iio@vger.kernel.org
+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 <linus.walleij@linaro.org>
L: linux-iio@vger.kernel.org
--
2.17.1
^ permalink raw reply related [flat|nested] 30+ messages in thread