Changelog
v1
-initial patch submission
v2
- formatting reworks, missing headers, code cleanup ...
- delete all debug traces
- add commentaries for better explanation of suspend/resume, timestamp, ...
- delete i2c/spi table ids keeping only of, and use I2C probe_new function
- switch calibbias to SI units and add calibias_available attribute
- use DMA-safe buffer for all regmap_bulk_* calls
- delete iio trigger usage and setup/handle interrupt in core module
- add open-drain interrupt support
- add FIFO on reference counter and buffer postenable/predisable to replace
iio trigger usage
- check that temperature data is present before copying in buffer
- add temperature sensor off when fifo is turned off
- delete timestamp channel reading
- move timestamp state in IIO device private data
- allow only 1 ODR change in a batch of data
- add driver-open-drain in devicetree YAML and delete spi options
v3
- delete const pointer cast for iio_device_get_drvdata
- change gyro and accel init to return the allocated iio_dev structure
- delete manual parent device assignment
- correct style and improve readability
- add commentaries about IIO buffer and watermark complex computation
- add timestamp alignment in IIO buffer structure
- wrap lines 80 columns for dt bindings
- add ABI documentation for calibbias values in SI units
v4
- return high resolution 16 bits temperature as raw data when polled with the
corresponding scale and offset.
- for data buffer return temperature in the same 16 bits using the same
scale and offset. Convert low resolution temperature FIFO data to high
resolution format.
- explicitely zero out data buffer before copying to iio buffer.
This series add a new driver for managing InvenSense ICM-426xx 6-axis IMUs.
This next generation of chips includes new generations of 3-axis gyroscope
and 3-axis accelerometer, support of I3C in addition to I2C and SPI, and
intelligent MotionTracking features like pedometer, tilt detection, and
tap detection.
This series is delivering a driver supporting gyroscope, accelerometer and
temperature data, with polling and buffering using hwfifo and watermark,
on I2C and SPI busses.
Gyroscope and accelerometer sensors are completely independent and can have
different ODRs. Since there is only a single FIFO a specific value is used to
mark invalid data. For keeping the device standard we are de-multiplexing data
from the FIFO to 2 IIO devices with 2 buffers, 1 for the accelerometer and 1
for the gyroscope. This architecture also enables to easily turn each sensor
on/off without impacting the other. The device interrupt is used to read the
FIFO and launch parsing of accelerometer and gyroscope data. A complex
timestamping mechanism is added to handle correctly FIFO watermark and dynamic
changes of settings.
Jean-Baptiste Maneyrol (13):
iio: imu: inv_icm42600: add core of new inv_icm42600 driver
iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
iio: imu: inv_icm42600: add SPI driver for inv_icm42600 driver
iio: imu: inv_icm42600: add gyroscope IIO device
iio: imu: inv_icm42600: add accelerometer IIO device
iio: imu: inv_icm42600: add temperature sensor support
iio: imu: add Kconfig and Makefile for inv_icm42600 driver
Documentation: ABI: add specific icm42600 documentation
iio: imu: inv_icm42600: add device interrupt
iio: imu: inv_icm42600: add buffer support in iio devices
iio: imu: inv_icm42600: add accurate timestamping
dt-bindings: iio: imu: Add inv_icm42600 documentation
MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor
.../ABI/testing/sysfs-bus-iio-icm42600 | 20 +
.../bindings/iio/imu/invensense,icm42600.yaml | 90 ++
MAINTAINERS | 8 +
drivers/iio/imu/Kconfig | 1 +
drivers/iio/imu/Makefile | 1 +
drivers/iio/imu/inv_icm42600/Kconfig | 29 +
drivers/iio/imu/inv_icm42600/Makefile | 15 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 395 +++++++++
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 787 +++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.c | 601 +++++++++++++
.../imu/inv_icm42600/inv_icm42600_buffer.h | 98 +++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 786 +++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 798 ++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 101 +++
.../iio/imu/inv_icm42600/inv_icm42600_spi.c | 100 +++
.../iio/imu/inv_icm42600/inv_icm42600_temp.c | 84 ++
.../iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 +
.../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 +++++
.../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++
19 files changed, 4224 insertions(+)
create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
create mode 100644 Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
create mode 100644 drivers/iio/imu/inv_icm42600/Kconfig
create mode 100644 drivers/iio/imu/inv_icm42600/Makefile
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
--
2.17.1
Add IIO device for gyroscope sensor with data polling interface.
Attributes: raw, scale, sampling_frequency, calibbias.
Gyroscope in low noise mode.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 6 +
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 603 ++++++++++++++++++
3 files changed, 613 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 14c8ef152418..d155470d770a 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -120,6 +120,8 @@ struct inv_icm42600_suspended {
* @orientation: sensor chip orientation relative to main hardware.
* @conf: chip sensors configurations.
* @suspended: suspended sensors configuration.
+ * @indio_gyro: gyroscope IIO device.
+ * @buffer: data transfer buffer aligned for DMA.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -131,6 +133,8 @@ struct inv_icm42600_state {
struct iio_mount_matrix orientation;
struct inv_icm42600_conf conf;
struct inv_icm42600_suspended suspended;
+ struct iio_dev *indio_gyro;
+ uint8_t buffer[2] ____cacheline_aligned;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
@@ -369,4 +373,6 @@ int inv_icm42600_debugfs_reg(struct iio_dev *indio_dev, unsigned int reg,
int inv_icm42600_core_probe(struct regmap *regmap, int chip,
inv_icm42600_bus_setup bus_setup);
+struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 2eb25c5f77f8..84e9ff320b3b 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -509,6 +509,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (ret)
return ret;
+ st->indio_gyro = inv_icm42600_gyro_init(st);
+ if (IS_ERR(st->indio_gyro))
+ return PTR_ERR(st->indio_gyro);
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
new file mode 100644
index 000000000000..3875ecbee67e
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -0,0 +1,603 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42600.h"
+
+#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
+ { \
+ .type = IIO_ANGL_VEL, \
+ .modified = 1, \
+ .channel2 = _modifier, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_RAW) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_type = \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .info_mask_shared_by_type_available = \
+ BIT(IIO_CHAN_INFO_SCALE) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_all = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .info_mask_shared_by_all_available = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_BE, \
+ }, \
+ .ext_info = _ext_info, \
+ }
+
+enum inv_icm42600_gyro_scan {
+ INV_ICM42600_GYRO_SCAN_X,
+ INV_ICM42600_GYRO_SCAN_Y,
+ INV_ICM42600_GYRO_SCAN_Z,
+};
+
+static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
+ IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
+ {},
+};
+
+static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_X, INV_ICM42600_GYRO_SCAN_X,
+ inv_icm42600_gyro_ext_infos),
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_Y, INV_ICM42600_GYRO_SCAN_Y,
+ inv_icm42600_gyro_ext_infos),
+ INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
+ inv_icm42600_gyro_ext_infos),
+};
+
+static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int reg;
+ __be16 *data;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_GYRO_DATA_X;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_GYRO_DATA_Y;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_GYRO_DATA_Z;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ /* enable gyro sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+ if (ret)
+ goto exit;
+
+ /* read gyro register data */
+ data = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpup(data);
+ if (*val == INV_ICM42600_DATA_INVALID)
+ ret = -EINVAL;
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+/* IIO format int + nano */
+static const int inv_icm42600_gyro_scale[] = {
+ /* +/- 2000dps => 0.001065264 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_2000DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_2000DPS + 1] = 1065264,
+ /* +/- 1000dps => 0.000532632 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_1000DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_1000DPS + 1] = 532632,
+ /* +/- 500dps => 0.000266316 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_500DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_500DPS + 1] = 266316,
+ /* +/- 250dps => 0.000133158 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_250DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_250DPS + 1] = 133158,
+ /* +/- 125dps => 0.000066579 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_125DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_125DPS + 1] = 66579,
+ /* +/- 62.5dps => 0.000033290 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_62_5DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_62_5DPS + 1] = 33290,
+ /* +/- 31.25dps => 0.000016645 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_31_25DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_31_25DPS + 1] = 16645,
+ /* +/- 15.625dps => 0.000008322 rad/s */
+ [2 * INV_ICM42600_GYRO_FS_15_625DPS] = 0,
+ [2 * INV_ICM42600_GYRO_FS_15_625DPS + 1] = 8322,
+};
+
+static int inv_icm42600_gyro_read_scale(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int idx;
+
+ idx = st->conf.gyro.fs;
+
+ *val = inv_icm42600_gyro_scale[2 * idx];
+ *val2 = inv_icm42600_gyro_scale[2 * idx + 1];
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_gyro_write_scale(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_scale); idx += 2) {
+ if (val == inv_icm42600_gyro_scale[idx] &&
+ val2 == inv_icm42600_gyro_scale[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_gyro_scale))
+ return -EINVAL;
+
+ conf.fs = idx / 2;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/* IIO format int + micro */
+static const int inv_icm42600_gyro_odr[] = {
+ /* 12.5Hz */
+ 12, 500000,
+ /* 25Hz */
+ 25, 0,
+ /* 50Hz */
+ 50, 0,
+ /* 100Hz */
+ 100, 0,
+ /* 200Hz */
+ 200, 0,
+ /* 1kHz */
+ 1000, 0,
+ /* 2kHz */
+ 2000, 0,
+ /* 4kHz */
+ 4000, 0,
+};
+
+static const int inv_icm42600_gyro_odr_conv[] = {
+ INV_ICM42600_ODR_12_5HZ,
+ INV_ICM42600_ODR_25HZ,
+ INV_ICM42600_ODR_50HZ,
+ INV_ICM42600_ODR_100HZ,
+ INV_ICM42600_ODR_200HZ,
+ INV_ICM42600_ODR_1KHZ_LN,
+ INV_ICM42600_ODR_2KHZ_LN,
+ INV_ICM42600_ODR_4KHZ_LN,
+};
+
+static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int odr;
+ unsigned int i;
+
+ odr = st->conf.gyro.odr;
+
+ for (i = 0; i < ARRAY_SIZE(inv_icm42600_gyro_odr_conv); ++i) {
+ if (inv_icm42600_gyro_odr_conv[i] == odr)
+ break;
+ }
+ if (i >= ARRAY_SIZE(inv_icm42600_gyro_odr_conv))
+ return -EINVAL;
+
+ *val = inv_icm42600_gyro_odr[2 * i];
+ *val2 = inv_icm42600_gyro_odr[2 * i + 1];
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_gyro_odr); idx += 2) {
+ if (val == inv_icm42600_gyro_odr[idx] &&
+ val2 == inv_icm42600_gyro_odr[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_gyro_odr))
+ return -EINVAL;
+
+ conf.odr = inv_icm42600_gyro_odr_conv[idx / 2];
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/*
+ * Calibration bias values, IIO range format int + nano.
+ * Value is limited to +/-64dps coded on 12 bits signed. Step is 1/32 dps.
+ */
+static int inv_icm42600_gyro_calibbias[] = {
+ -1, 117010721, /* min: -1.117010721 rad/s */
+ 0, 545415, /* step: 0.000545415 rad/s */
+ 1, 116465306, /* max: 1.116465306 rad/s */
+};
+
+static int inv_icm42600_gyro_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t bias;
+ unsigned int reg;
+ int16_t offset;
+ uint8_t data[2];
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER0;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER1;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER3;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
+ memcpy(data, st->buffer, sizeof(data));
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ if (ret)
+ return ret;
+
+ /* 12 bits signed value */
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ case IIO_MOD_Y:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ case IIO_MOD_Z:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /*
+ * convert raw offset to dps then to rad/s
+ * 12 bits signed raw max 64 to dps: 64 / 2048
+ * dps to rad: Pi / 180
+ * result in nano (1000000000)
+ * (offset * 64 * Pi * 1000000000) / (2048 * 180)
+ */
+ val64 = (int64_t)offset * 64LL * 3141592653LL;
+ /* for rounding, add + or - divisor (2048 * 180) divided by 2 */
+ if (val64 >= 0)
+ val64 += 2048 * 180 / 2;
+ else
+ val64 -= 2048 * 180 / 2;
+ bias = div_s64(val64, 2048 * 180);
+ *val = bias / 1000000000L;
+ *val2 = bias % 1000000000L;
+
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_gyro_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64, min, max;
+ unsigned int reg, regval;
+ int16_t offset;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER0;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER1;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER3;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* inv_icm42600_gyro_calibbias: min - step - max in nano */
+ min = (int64_t)inv_icm42600_gyro_calibbias[0] * 1000000000LL +
+ (int64_t)inv_icm42600_gyro_calibbias[1];
+ max = (int64_t)inv_icm42600_gyro_calibbias[4] * 1000000000LL +
+ (int64_t)inv_icm42600_gyro_calibbias[5];
+ val64 = (int64_t)val * 1000000000LL + (int64_t)val2;
+ if (val64 < min || val64 > max)
+ return -EINVAL;
+
+ /*
+ * convert rad/s to dps then to raw value
+ * rad to dps: 180 / Pi
+ * dps to raw 12 bits signed, max 64: 2048 / 64
+ * val in nano (1000000000)
+ * val * 180 * 2048 / (Pi * 1000000000 * 64)
+ */
+ val64 = val64 * 180LL * 2048LL;
+ /* for rounding, add + or - divisor (3141592653 * 64) divided by 2 */
+ if (val64 >= 0)
+ val64 += 3141592653LL * 64LL / 2LL;
+ else
+ val64 -= 3141592653LL * 64LL / 2LL;
+ offset = div64_s64(val64, 3141592653LL * 64LL);
+
+ /* clamp value limited to 12 bits signed */
+ if (offset < -2048)
+ offset = -2048;
+ else if (offset > 2047)
+ offset = 2047;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ /* OFFSET_USER1 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
+ break;
+ case IIO_MOD_Y:
+ /* OFFSET_USER1 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER1,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ case IIO_MOD_Z:
+ /* OFFSET_USER4 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = (regval & 0xF0) | ((offset & 0xF00) >> 8);
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int16_t data;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_read_sensor(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ case IIO_CHAN_INFO_SCALE:
+ return inv_icm42600_gyro_read_scale(st, val, val2);
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_gyro_read_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42600_gyro_read_offset(st, chan, val, val2);
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_read_avail(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ const int **vals,
+ int *type, int *length, long mask)
+{
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ *vals = inv_icm42600_gyro_scale;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ *length = ARRAY_SIZE(inv_icm42600_gyro_scale);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ *vals = inv_icm42600_gyro_odr;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ *length = ARRAY_SIZE(inv_icm42600_gyro_odr);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ *vals = inv_icm42600_gyro_calibbias;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ return IIO_AVAIL_RANGE;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_write_scale(st, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_gyro_write_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_gyro_write_offset(st, chan, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ long mask)
+{
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ return IIO_VAL_INT_PLUS_NANO;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return IIO_VAL_INT_PLUS_MICRO;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return IIO_VAL_INT_PLUS_NANO;
+ default:
+ return -EINVAL;
+ }
+}
+
+static const struct iio_info inv_icm42600_gyro_info = {
+ .read_raw = inv_icm42600_gyro_read_raw,
+ .read_avail = inv_icm42600_gyro_read_avail,
+ .write_raw = inv_icm42600_gyro_write_raw,
+ .write_raw_get_fmt = inv_icm42600_gyro_write_raw_get_fmt,
+ .debugfs_reg_access = inv_icm42600_debugfs_reg,
+};
+
+struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
+{
+ struct device *dev = regmap_get_device(st->map);
+ const char *name;
+ struct iio_dev *indio_dev;
+ int ret;
+
+ name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->name);
+ if (!name)
+ return ERR_PTR(-ENOMEM);
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return ERR_PTR(-ENOMEM);
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_gyro_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_gyro_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_gyro_channels);
+
+ ret = devm_iio_device_register(dev, indio_dev);
+ if (ret)
+ return ERR_PTR(ret);
+
+ return indio_dev;
+}
--
2.17.1
Add temperature channel in gyroscope and accelerometer devices.
Temperature is available in full 16 bits resolution when reading
register and in low 8 bits resolution in the FIFO. Return full
precision raw temperature with corresponding scale and offset.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 11 ++-
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 11 ++-
.../iio/imu/inv_icm42600/inv_icm42600_temp.c | 84 +++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 +++++++
4 files changed, 134 insertions(+), 2 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 717c6b0869fc..3f214df44093 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -13,6 +13,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -45,6 +46,7 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_X,
INV_ICM42600_ACCEL_SCAN_Y,
INV_ICM42600_ACCEL_SCAN_Z,
+ INV_ICM42600_ACCEL_SCAN_TEMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
@@ -59,6 +61,7 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
inv_icm42600_accel_ext_infos),
INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
inv_icm42600_accel_ext_infos),
+ INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
};
static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
@@ -450,8 +453,14 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
int16_t data;
int ret;
- if (chan->type != IIO_ACCEL)
+ switch (chan->type) {
+ case IIO_ACCEL:
+ break;
+ case IIO_TEMP:
+ return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
+ default:
return -EINVAL;
+ }
switch (mask) {
case IIO_CHAN_INFO_RAW:
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index 3875ecbee67e..6a0e7661fa48 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -13,6 +13,7 @@
#include <linux/iio/iio.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -45,6 +46,7 @@ enum inv_icm42600_gyro_scan {
INV_ICM42600_GYRO_SCAN_X,
INV_ICM42600_GYRO_SCAN_Y,
INV_ICM42600_GYRO_SCAN_Z,
+ INV_ICM42600_GYRO_SCAN_TEMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
@@ -59,6 +61,7 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
inv_icm42600_gyro_ext_infos),
INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
inv_icm42600_gyro_ext_infos),
+ INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
};
static int inv_icm42600_gyro_read_sensor(struct inv_icm42600_state *st,
@@ -461,8 +464,14 @@ static int inv_icm42600_gyro_read_raw(struct iio_dev *indio_dev,
int16_t data;
int ret;
- if (chan->type != IIO_ANGL_VEL)
+ switch (chan->type) {
+ case IIO_ANGL_VEL:
+ break;
+ case IIO_TEMP:
+ return inv_icm42600_temp_read_raw(indio_dev, chan, val, val2, mask);
+ default:
return -EINVAL;
+ }
switch (mask) {
case IIO_CHAN_INFO_RAW:
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
new file mode 100644
index 000000000000..213cce1c3111
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
@@ -0,0 +1,84 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_temp.h"
+
+static int inv_icm42600_temp_read(struct inv_icm42600_state *st, int16_t *temp)
+{
+ struct device *dev = regmap_get_device(st->map);
+ __be16 *raw;
+ int ret;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_temp_conf(st, true, NULL);
+ if (ret)
+ goto exit;
+
+ raw = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_TEMP_DATA, raw, sizeof(*raw));
+ if (ret)
+ goto exit;
+
+ *temp = (int16_t)be16_to_cpup(raw);
+ if (*temp == INV_ICM42600_DATA_INVALID)
+ ret = -EINVAL;
+
+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);
+ int16_t temp;
+ int ret;
+
+ if (chan->type != IIO_TEMP)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ 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;
+ /*
+ * T°C = (temp / 132.48) + 25
+ * Tm°C = 1000 * ((temp * 100 / 13248) + 25)
+ * scale: 100000 / 13248 ~= 7.548309
+ * offset: 25000
+ */
+ case IIO_CHAN_INFO_SCALE:
+ *val = 7;
+ *val2 = 548309;
+ 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..3941186512fb
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
@@ -0,0 +1,30 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_TEMP_H_
+#define INV_ICM42600_TEMP_H_
+
+#include <linux/iio/iio.h>
+
+#define INV_ICM42600_TEMP_CHAN(_index) \
+ { \
+ .type = IIO_TEMP, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_RAW) | \
+ BIT(IIO_CHAN_INFO_OFFSET) | \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ }, \
+ }
+
+int inv_icm42600_temp_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask);
+
+#endif
--
2.17.1
Add a timestamping mechanism for buffer that provides accurate
event timestamps when using watermark. This mechanism estimates
device internal clock by comparing FIFO interrupts delta time and
device elapsed time computed by parsing FIFO data.
Take interrupt timestamp in hard irq handler and add IIO device
specific timestamp structures in device private allocation.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/Makefile | 1 +
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 5 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 40 +++-
.../imu/inv_icm42600/inv_icm42600_buffer.c | 28 +++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 17 +-
.../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 40 +++-
.../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 ++++++++++++++++++
.../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++++++++
8 files changed, 396 insertions(+), 15 deletions(-)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
diff --git a/drivers/iio/imu/inv_icm42600/Makefile b/drivers/iio/imu/inv_icm42600/Makefile
index 0f49f6df3647..291714d9aa54 100644
--- a/drivers/iio/imu/inv_icm42600/Makefile
+++ b/drivers/iio/imu/inv_icm42600/Makefile
@@ -6,6 +6,7 @@ inv-icm42600-y += inv_icm42600_gyro.o
inv-icm42600-y += inv_icm42600_accel.o
inv-icm42600-y += inv_icm42600_temp.o
inv-icm42600-y += inv_icm42600_buffer.o
+inv-icm42600-y += inv_icm42600_timestamp.o
obj-$(CONFIG_INV_ICM42600_I2C) += inv-icm42600-i2c.o
inv-icm42600-i2c-y += inv_icm42600_i2c.o
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index 7b52d92739c3..c0f5059b13b3 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -126,6 +126,7 @@ struct inv_icm42600_suspended {
* @indio_accel: accelerometer IIO device.
* @buffer: data transfer buffer aligned for DMA.
* @fifo: FIFO management structure.
+ * @timestamp: interrupt timestamps.
*/
struct inv_icm42600_state {
struct mutex lock;
@@ -141,6 +142,10 @@ struct inv_icm42600_state {
struct iio_dev *indio_accel;
uint8_t buffer[2] ____cacheline_aligned;
struct inv_icm42600_fifo fifo;
+ struct {
+ int64_t gyro;
+ int64_t accel;
+ } timestamp;
};
/* Virtual register addresses: @bank on MSB (4 upper bits), @address on LSB */
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index fbc029bd6e72..3441b0d61c5d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -17,6 +17,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -50,6 +51,7 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_Y,
INV_ICM42600_ACCEL_SCAN_Z,
INV_ICM42600_ACCEL_SCAN_TEMP,
+ INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
@@ -65,15 +67,17 @@ static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
inv_icm42600_accel_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_ACCEL_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_ACCEL_SCAN_TIMESTAMP),
};
/*
- * IIO buffer data: size must be a power of 2
- * 8 bytes: 6 bytes acceleration and 2 bytes temperature
+ * IIO buffer data: size must be a power of 2 and timestamp aligned
+ * 16 bytes: 6 bytes acceleration, 2 bytes temperature, 8 bytes timestamp
*/
struct inv_icm42600_accel_buffer {
struct inv_icm42600_fifo_sensor_data accel;
int16_t temp;
+ int64_t timestamp __aligned(8);
};
#define INV_ICM42600_SCAN_MASK_ACCEL_3AXIS \
@@ -94,6 +98,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
const unsigned long *scan_mask)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0;
unsigned int sleep_temp = 0;
@@ -121,6 +126,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write */
+ inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -301,9 +307,11 @@ static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
return IIO_VAL_INT_PLUS_MICRO;
}
-static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
+static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
int val, int val2)
{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct device *dev = regmap_get_device(st->map);
unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
@@ -322,6 +330,11 @@ static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
pm_runtime_get_sync(dev);
mutex_lock(&st->lock);
+ ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
+ iio_buffer_enabled(indio_dev));
+ if (ret)
+ goto out_unlock;
+
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
if (ret)
goto out_unlock;
@@ -611,7 +624,7 @@ static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
iio_device_release_direct_mode(indio_dev);
return ret;
case IIO_CHAN_INFO_SAMP_FREQ:
- return inv_icm42600_accel_write_odr(st, val, val2);
+ return inv_icm42600_accel_write_odr(indio_dev, val, val2);
case IIO_CHAN_INFO_CALIBBIAS:
ret = iio_device_claim_direct_mode(indio_dev);
if (ret)
@@ -694,6 +707,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
{
struct device *dev = regmap_get_device(st->map);
const char *name;
+ struct inv_icm42600_timestamp *ts;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
int ret;
@@ -702,7 +716,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!name)
return ERR_PTR(-ENOMEM);
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return ERR_PTR(-ENOMEM);
@@ -710,6 +724,9 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!buffer)
return ERR_PTR(-ENOMEM);
+ ts = iio_priv(indio_dev);
+ inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name;
indio_dev->info = &inv_icm42600_accel_info;
@@ -731,14 +748,17 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *timestamp;
const int8_t *temp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_accel_buffer buffer;
/* parse all fifo packets */
- for (i = 0; i < st->fifo.count; i += size) {
+ for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
&accel, &gyro, &temp, ×tamp, &odr);
/* quit if error or FIFO is empty */
@@ -749,12 +769,18 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
if (accel == NULL || !inv_icm42600_fifo_is_data_valid(accel))
continue;
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_ACCEL)
+ inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
+ st->fifo.nb.total, no);
+
/* buffer is copied to userspace, zeroing it to avoid any data leak */
memset(&buffer, 0, sizeof(buffer));
memcpy(&buffer.accel, accel, sizeof(buffer.accel));
/* convert 8 bits FIFO temperature in high resolution format */
buffer.temp = temp ? (*temp * 64) : 0;
- iio_push_to_buffers(indio_dev, &buffer);
+ ts_val = inv_icm42600_timestamp_pop(ts);
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
}
return 0;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
index e58e6f0c5698..99576b2c171f 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -13,6 +13,7 @@
#include <linux/iio/buffer.h>
#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
#include "inv_icm42600_buffer.h"
/* FIFO header: 1 byte */
@@ -374,6 +375,7 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
struct device *dev = regmap_get_device(st->map);
unsigned int sensor;
unsigned int *watermark;
+ struct inv_icm42600_timestamp *ts;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int sleep_temp = 0;
unsigned int sleep_sensor = 0;
@@ -383,9 +385,11 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
if (indio_dev == st->indio_gyro) {
sensor = INV_ICM42600_SENSOR_GYRO;
watermark = &st->fifo.watermark.gyro;
+ ts = iio_priv(st->indio_gyro);
} else if (indio_dev == st->indio_accel) {
sensor = INV_ICM42600_SENSOR_ACCEL;
watermark = &st->fifo.watermark.accel;
+ ts = iio_priv(st->indio_accel);
} else {
return -EINVAL;
}
@@ -413,6 +417,8 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
if (!st->fifo.on)
ret = inv_icm42600_set_temp_conf(st, false, &sleep_temp);
+ inv_icm42600_timestamp_reset(ts);
+
out_unlock:
mutex_unlock(&st->lock);
@@ -498,17 +504,26 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
{
+ struct inv_icm42600_timestamp *ts;
int ret;
if (st->fifo.nb.total == 0)
return 0;
+ /* handle gyroscope timestamp and FIFO data parsing */
+ ts = iio_priv(st->indio_gyro);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.gyro, st->timestamp.gyro);
if (st->fifo.nb.gyro > 0) {
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
+ /* handle accelerometer timestamp and FIFO data parsing */
+ ts = iio_priv(st->indio_accel);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
+ st->fifo.nb.accel, st->timestamp.accel);
if (st->fifo.nb.accel > 0) {
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
@@ -521,8 +536,13 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
unsigned int count)
{
+ struct inv_icm42600_timestamp *ts;
+ int64_t gyro_ts, accel_ts;
int ret;
+ gyro_ts = iio_get_time_ns(st->indio_gyro);
+ accel_ts = iio_get_time_ns(st->indio_accel);
+
ret = inv_icm42600_buffer_fifo_read(st, count);
if (ret)
return ret;
@@ -531,12 +551,20 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
return 0;
if (st->fifo.nb.gyro > 0) {
+ ts = iio_priv(st->indio_gyro);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
+ st->fifo.nb.total, st->fifo.nb.gyro,
+ gyro_ts);
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
if (st->fifo.nb.accel > 0) {
+ ts = iio_priv(st->indio_accel);
+ inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
+ st->fifo.nb.total, st->fifo.nb.accel,
+ accel_ts);
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
return ret;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 9e1f6e65fd45..8bd77185ccb7 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -19,6 +19,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
static const struct regmap_range_cfg inv_icm42600_regmap_ranges[] = {
{
@@ -412,6 +413,16 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st,
return inv_icm42600_set_conf(st, hw->conf);
}
+static irqreturn_t inv_icm42600_irq_timestamp(int irq, void *_data)
+{
+ struct inv_icm42600_state *st = _data;
+
+ st->timestamp.gyro = iio_get_time_ns(st->indio_gyro);
+ st->timestamp.accel = iio_get_time_ns(st->indio_accel);
+
+ return IRQ_WAKE_THREAD;
+}
+
static irqreturn_t inv_icm42600_irq_handler(int irq, void *_data)
{
struct inv_icm42600_state *st = _data;
@@ -495,7 +506,7 @@ static int inv_icm42600_irq_init(struct inv_icm42600_state *st, int irq,
if (ret)
return ret;
- return devm_request_threaded_irq(dev, irq, NULL,
+ return devm_request_threaded_irq(dev, irq, inv_icm42600_irq_timestamp,
inv_icm42600_irq_handler, irq_type,
"inv_icm42600", st);
}
@@ -617,6 +628,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip, int irq,
if (ret)
return ret;
+ ret = inv_icm42600_timestamp_setup(st);
+ if (ret)
+ return ret;
+
ret = inv_icm42600_buffer_init(st);
if (ret)
return ret;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
index 3b1f7594b60b..aee7b9ff4bf4 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
@@ -17,6 +17,7 @@
#include "inv_icm42600.h"
#include "inv_icm42600_temp.h"
#include "inv_icm42600_buffer.h"
+#include "inv_icm42600_timestamp.h"
#define INV_ICM42600_GYRO_CHAN(_modifier, _index, _ext_info) \
{ \
@@ -50,6 +51,7 @@ enum inv_icm42600_gyro_scan {
INV_ICM42600_GYRO_SCAN_Y,
INV_ICM42600_GYRO_SCAN_Z,
INV_ICM42600_GYRO_SCAN_TEMP,
+ INV_ICM42600_GYRO_SCAN_TIMESTAMP,
};
static const struct iio_chan_spec_ext_info inv_icm42600_gyro_ext_infos[] = {
@@ -65,15 +67,17 @@ static const struct iio_chan_spec inv_icm42600_gyro_channels[] = {
INV_ICM42600_GYRO_CHAN(IIO_MOD_Z, INV_ICM42600_GYRO_SCAN_Z,
inv_icm42600_gyro_ext_infos),
INV_ICM42600_TEMP_CHAN(INV_ICM42600_GYRO_SCAN_TEMP),
+ IIO_CHAN_SOFT_TIMESTAMP(INV_ICM42600_GYRO_SCAN_TIMESTAMP),
};
/*
- * IIO buffer data: size must be a power of 2
- * 8 bytes: 6 bytes angular velocity and 2 bytes temperature
+ * IIO buffer data: size must be a power of 2 and timestamp aligned
+ * 16 bytes: 6 bytes angular velocity, 2 bytes temperature, 8 bytes timestamp
*/
struct inv_icm42600_gyro_buffer {
struct inv_icm42600_fifo_sensor_data gyro;
int16_t temp;
+ int64_t timestamp __aligned(8);
};
#define INV_ICM42600_SCAN_MASK_GYRO_3AXIS \
@@ -94,6 +98,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
const unsigned long *scan_mask)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0;
unsigned int sleep_gyro = 0;
@@ -121,6 +126,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
}
/* update data FIFO write */
+ inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret)
goto out_unlock;
@@ -313,9 +319,11 @@ static int inv_icm42600_gyro_read_odr(struct inv_icm42600_state *st,
return IIO_VAL_INT_PLUS_MICRO;
}
-static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
+static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
int val, int val2)
{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
struct device *dev = regmap_get_device(st->map);
unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
@@ -334,6 +342,11 @@ static int inv_icm42600_gyro_write_odr(struct inv_icm42600_state *st,
pm_runtime_get_sync(dev);
mutex_lock(&st->lock);
+ ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
+ iio_buffer_enabled(indio_dev));
+ if (ret)
+ goto out_unlock;
+
ret = inv_icm42600_set_gyro_conf(st, &conf, NULL);
if (ret)
goto out_unlock;
@@ -622,7 +635,7 @@ static int inv_icm42600_gyro_write_raw(struct iio_dev *indio_dev,
iio_device_release_direct_mode(indio_dev);
return ret;
case IIO_CHAN_INFO_SAMP_FREQ:
- return inv_icm42600_gyro_write_odr(st, val, val2);
+ return inv_icm42600_gyro_write_odr(indio_dev, val, val2);
case IIO_CHAN_INFO_CALIBBIAS:
ret = iio_device_claim_direct_mode(indio_dev);
if (ret)
@@ -705,6 +718,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
{
struct device *dev = regmap_get_device(st->map);
const char *name;
+ struct inv_icm42600_timestamp *ts;
struct iio_dev *indio_dev;
struct iio_buffer *buffer;
int ret;
@@ -713,7 +727,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!name)
return ERR_PTR(-ENOMEM);
- indio_dev = devm_iio_device_alloc(dev, 0);
+ indio_dev = devm_iio_device_alloc(dev, sizeof(*ts));
if (!indio_dev)
return ERR_PTR(-ENOMEM);
@@ -721,6 +735,9 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!buffer)
return ERR_PTR(-ENOMEM);
+ ts = iio_priv(indio_dev);
+ inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
+
iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name;
indio_dev->info = &inv_icm42600_gyro_info;
@@ -742,14 +759,17 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
ssize_t i, size;
+ unsigned int no;
const void *accel, *gyro, *timestamp;
const int8_t *temp;
unsigned int odr;
+ int64_t ts_val;
struct inv_icm42600_gyro_buffer buffer;
/* parse all fifo packets */
- for (i = 0; i < st->fifo.count; i += size) {
+ for (i = 0, no = 0; i < st->fifo.count; i += size, ++no) {
size = inv_icm42600_fifo_decode_packet(&st->fifo.data[i],
&accel, &gyro, &temp, ×tamp, &odr);
/* quit if error or FIFO is empty */
@@ -760,12 +780,18 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
if (gyro == NULL || !inv_icm42600_fifo_is_data_valid(gyro))
continue;
+ /* update odr */
+ if (odr & INV_ICM42600_SENSOR_GYRO)
+ inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
+ st->fifo.nb.total, no);
+
/* buffer is copied to userspace, zeroing it to avoid any data leak */
memset(&buffer, 0, sizeof(buffer));
memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
/* convert 8 bits FIFO temperature in high resolution format */
buffer.temp = temp ? (*temp * 64) : 0;
- iio_push_to_buffers(indio_dev, &buffer);
+ ts_val = inv_icm42600_timestamp_pop(ts);
+ iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
}
return 0;
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
new file mode 100644
index 000000000000..7f2dc41f807b
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
@@ -0,0 +1,195 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/regmap.h>
+#include <linux/math64.h>
+
+#include "inv_icm42600.h"
+#include "inv_icm42600_timestamp.h"
+
+/* internal chip period is 32kHz, 31250ns */
+#define INV_ICM42600_TIMESTAMP_PERIOD 31250
+/* allow a jitter of +/- 2% */
+#define INV_ICM42600_TIMESTAMP_JITTER 2
+/* compute min and max periods accepted */
+#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \
+ (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
+#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
+ (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
+
+/* Add a new value inside an accumulator and update the estimate value */
+static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
+{
+ uint64_t sum = 0;
+ size_t i;
+
+ acc->values[acc->idx++] = val;
+ if (acc->idx >= ARRAY_SIZE(acc->values))
+ acc->idx = 0;
+
+ /* compute the mean of all stored values, use 0 as empty slot */
+ for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
+ if (acc->values[i] == 0)
+ break;
+ sum += acc->values[i];
+ }
+
+ acc->val = div_u64(sum, i);
+}
+
+void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
+ uint32_t period)
+{
+ /* initial odr for sensor after reset is 1kHz */
+ const uint32_t default_period = 1000000;
+
+ /* current multiplier and period values after reset */
+ ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ ts->period = default_period;
+ /* new set multiplier is the one from chip initialization */
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+
+ /* use theoretical value for chip period */
+ inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
+}
+
+int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
+{
+ unsigned int val;
+
+ /* enable timestamp register */
+ val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
+ INV_ICM42600_TMST_CONFIG_TMST_EN;
+ return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
+ INV_ICM42600_TMST_CONFIG_MASK, val);
+}
+
+int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t period, bool fifo)
+{
+ /* when FIFO is on, prevent odr change if one is already pending */
+ if (fifo && ts->new_mult != 0)
+ return -EAGAIN;
+
+ ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
+
+ return 0;
+}
+
+static bool inv_validate_period(uint32_t period, uint32_t mult)
+{
+ const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
+ uint32_t period_min, period_max;
+
+ /* check that period is acceptable */
+ period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
+ period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
+ if (period > period_min && period < period_max)
+ return true;
+ else
+ return false;
+}
+
+static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
+ uint32_t mult, uint32_t period)
+{
+ uint32_t new_chip_period;
+
+ if (!inv_validate_period(period, mult))
+ return false;
+
+ /* update chip internal period estimation */
+ new_chip_period = period / mult;
+ inv_update_acc(&ts->chip_period, new_chip_period);
+
+ return true;
+}
+
+void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ size_t sensor_nb, int64_t timestamp)
+{
+ struct inv_icm42600_timestamp_interval *it;
+ int64_t delta, interval;
+ const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ uint32_t period = ts->period;
+ int32_t m;
+ bool valid = false;
+
+ if (fifo_nb == 0)
+ return;
+
+ /* update interrupt timestamp and compute chip and sensor periods */
+ it = &ts->it;
+ it->lo = it->up;
+ it->up = timestamp;
+ delta = it->up - it->lo;
+ if (it->lo != 0) {
+ /* compute period: delta time divided by number of samples */
+ period = div_s64(delta, fifo_nb);
+ valid = inv_compute_chip_period(ts, fifo_mult, period);
+ /* update sensor period if chip internal period is updated */
+ if (valid)
+ ts->period = ts->mult * ts->chip_period.val;
+ }
+
+ /* no previous data, compute theoritical value from interrupt */
+ if (ts->timestamp == 0) {
+ /* elapsed time: sensor period * sensor samples number */
+ interval = (int64_t)ts->period * (int64_t)sensor_nb;
+ ts->timestamp = it->up - interval;
+ return;
+ }
+
+ /* if interrupt interval is valid, sync with interrupt timestamp */
+ if (valid) {
+ /* compute measured fifo_period */
+ fifo_period = fifo_mult * ts->chip_period.val;
+ /* delta time between last sample and last interrupt */
+ delta = it->lo - ts->timestamp;
+ /* if there are multiple samples, go back to first one */
+ while (delta >= (fifo_period * 3 / 2))
+ delta -= fifo_period;
+ /* compute maximal adjustment value */
+ m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
+ if (delta > m)
+ delta = m;
+ else if (delta < -m)
+ delta = -m;
+ ts->timestamp += delta;
+ }
+}
+
+void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ unsigned int fifo_no)
+{
+ int64_t interval;
+ uint32_t fifo_mult;
+
+ if (ts->new_mult == 0)
+ return;
+
+ /* update to new multiplier and update period */
+ ts->mult = ts->new_mult;
+ ts->new_mult = 0;
+ ts->period = ts->mult * ts->chip_period.val;
+
+ /*
+ * After ODR change the time interval with the previous sample is
+ * undertermined (depends when the change occures). So we compute the
+ * timestamp from the current interrupt using the new FIFO period, the
+ * total number of samples and the current sample numero.
+ */
+ if (ts->timestamp != 0) {
+ /* compute measured fifo period */
+ fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
+ fifo_period = fifo_mult * ts->chip_period.val;
+ /* computes time interval between interrupt and this sample */
+ interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
+ ts->timestamp = ts->it.up - interval;
+ }
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
new file mode 100644
index 000000000000..4e4f331d4fe4
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#ifndef INV_ICM42600_TIMESTAMP_H_
+#define INV_ICM42600_TIMESTAMP_H_
+
+#include <linux/kernel.h>
+
+struct inv_icm42600_state;
+
+/**
+ * struct inv_icm42600_timestamp_interval - timestamps interval
+ * @lo: interval lower bound
+ * @up: interval upper bound
+ */
+struct inv_icm42600_timestamp_interval {
+ int64_t lo;
+ int64_t up;
+};
+
+/**
+ * struct inv_icm42600_timestamp_acc - accumulator for computing an estimation
+ * @val: current estimation of the value, the mean of all values
+ * @idx: current index of the next free place in values table
+ * @values: table of all measured values, use for computing the mean
+ */
+struct inv_icm42600_timestamp_acc {
+ uint32_t val;
+ size_t idx;
+ uint32_t values[32];
+};
+
+/**
+ * struct inv_icm42600_timestamp - timestamp management states
+ * @it: interrupts interval timestamps
+ * @timestamp: store last timestamp for computing next data timestamp
+ * @mult: current internal period multiplier
+ * @new_mult: new set internal period multiplier (not yet effective)
+ * @period: measured current period of the sensor
+ * @chip_period: accumulator for computing internal chip period
+ */
+struct inv_icm42600_timestamp {
+ struct inv_icm42600_timestamp_interval it;
+ int64_t timestamp;
+ uint32_t mult;
+ uint32_t new_mult;
+ uint32_t period;
+ struct inv_icm42600_timestamp_acc chip_period;
+};
+
+void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
+ uint32_t period);
+
+int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st);
+
+int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t period, bool fifo);
+
+void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ size_t sensor_nb, int64_t timestamp);
+
+static inline int64_t
+inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts)
+{
+ ts->timestamp += ts->period;
+ return ts->timestamp;
+}
+
+void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
+ uint32_t fifo_period, size_t fifo_nb,
+ unsigned int fifo_no);
+
+static inline void
+inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts)
+{
+ const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL};
+
+ ts->it = interval_init;
+ ts->timestamp = 0;
+}
+
+#endif
--
2.17.1
Hardware offset available as calibscale sysfs attributes are real
physical values exprimed in SI units.
calibscale_available sysfs attributes represents the range of
acceptable values.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
.../ABI/testing/sysfs-bus-iio-icm42600 | 20 +++++++++++++++++++
1 file changed, 20 insertions(+)
create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
diff --git a/Documentation/ABI/testing/sysfs-bus-iio-icm42600 b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
new file mode 100644
index 000000000000..0bf1fd4f5bf1
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
@@ -0,0 +1,20 @@
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_x_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_y_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_z_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibbias
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibbias
+KernelVersion: 5.8
+Contact: [email protected]
+Description:
+ Hardware applied calibration offset (assumed to fix production
+ inaccuracies). Values represent a real physical offset expressed
+ in SI units (m/s^2 for accelerometer and rad/s for gyroscope).
+
+What: /sys/bus/iio/devices/iio:deviceX/in_accel_calibbias_available
+What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_calibbias_available
+KernelVersion: 5.8
+Contact: [email protected]
+Description:
+ Range of available values for hardware offset. Values in SI
+ units (m/s^2 for accelerometer and rad/s for gyroscope).
--
2.17.1
Add IIO device for accelerometer sensor with data polling
interface.
Attributes: raw, scale, sampling_frequency, calibbias.
Accelerometer in low noise mode.
Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 592 ++++++++++++++++++
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 4 +
3 files changed, 600 insertions(+)
create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index d155470d770a..3b190461a2b6 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -121,6 +121,7 @@ struct inv_icm42600_suspended {
* @conf: chip sensors configurations.
* @suspended: suspended sensors configuration.
* @indio_gyro: gyroscope IIO device.
+ * @indio_accel: accelerometer IIO device.
* @buffer: data transfer buffer aligned for DMA.
*/
struct inv_icm42600_state {
@@ -134,6 +135,7 @@ struct inv_icm42600_state {
struct inv_icm42600_conf conf;
struct inv_icm42600_suspended suspended;
struct iio_dev *indio_gyro;
+ struct iio_dev *indio_accel;
uint8_t buffer[2] ____cacheline_aligned;
};
@@ -375,4 +377,6 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st);
+struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st);
+
#endif
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
new file mode 100644
index 000000000000..717c6b0869fc
--- /dev/null
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -0,0 +1,592 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Copyright (C) 2020 Invensense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/iio/iio.h>
+
+#include "inv_icm42600.h"
+
+#define INV_ICM42600_ACCEL_CHAN(_modifier, _index, _ext_info) \
+ { \
+ .type = IIO_ACCEL, \
+ .modified = 1, \
+ .channel2 = _modifier, \
+ .info_mask_separate = \
+ BIT(IIO_CHAN_INFO_RAW) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_type = \
+ BIT(IIO_CHAN_INFO_SCALE), \
+ .info_mask_shared_by_type_available = \
+ BIT(IIO_CHAN_INFO_SCALE) | \
+ BIT(IIO_CHAN_INFO_CALIBBIAS), \
+ .info_mask_shared_by_all = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .info_mask_shared_by_all_available = \
+ BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+ .scan_index = _index, \
+ .scan_type = { \
+ .sign = 's', \
+ .realbits = 16, \
+ .storagebits = 16, \
+ .endianness = IIO_BE, \
+ }, \
+ .ext_info = _ext_info, \
+ }
+
+enum inv_icm42600_accel_scan {
+ INV_ICM42600_ACCEL_SCAN_X,
+ INV_ICM42600_ACCEL_SCAN_Y,
+ INV_ICM42600_ACCEL_SCAN_Z,
+};
+
+static const struct iio_chan_spec_ext_info inv_icm42600_accel_ext_infos[] = {
+ IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42600_get_mount_matrix),
+ {},
+};
+
+static const struct iio_chan_spec inv_icm42600_accel_channels[] = {
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_X, INV_ICM42600_ACCEL_SCAN_X,
+ inv_icm42600_accel_ext_infos),
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_Y, INV_ICM42600_ACCEL_SCAN_Y,
+ inv_icm42600_accel_ext_infos),
+ INV_ICM42600_ACCEL_CHAN(IIO_MOD_Z, INV_ICM42600_ACCEL_SCAN_Z,
+ inv_icm42600_accel_ext_infos),
+};
+
+static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int16_t *val)
+{
+ struct device *dev = regmap_get_device(st->map);
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ unsigned int reg;
+ __be16 *data;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_ACCEL_DATA_X;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_ACCEL_DATA_Y;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_ACCEL_DATA_Z;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ /* enable accel sensor */
+ conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+ if (ret)
+ goto exit;
+
+ /* read accel register data */
+ data = (__be16 *)&st->buffer[0];
+ ret = regmap_bulk_read(st->map, reg, data, sizeof(*data));
+ if (ret)
+ goto exit;
+
+ *val = (int16_t)be16_to_cpup(data);
+ if (*val == INV_ICM42600_DATA_INVALID)
+ ret = -EINVAL;
+exit:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+/* IIO format int + nano */
+static const int inv_icm42600_accel_scale[] = {
+ /* +/- 16G => 0.004788403 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_16G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_16G + 1] = 4788403,
+ /* +/- 8G => 0.002394202 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_8G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_8G + 1] = 2394202,
+ /* +/- 4G => 0.001197101 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_4G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_4G + 1] = 1197101,
+ /* +/- 2G => 0.000598550 m/s-2 */
+ [2 * INV_ICM42600_ACCEL_FS_2G] = 0,
+ [2 * INV_ICM42600_ACCEL_FS_2G + 1] = 598550,
+};
+
+static int inv_icm42600_accel_read_scale(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int idx;
+
+ idx = st->conf.accel.fs;
+
+ *val = inv_icm42600_accel_scale[2 * idx];
+ *val2 = inv_icm42600_accel_scale[2 * idx + 1];
+ return IIO_VAL_INT_PLUS_NANO;
+}
+
+static int inv_icm42600_accel_write_scale(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_scale); idx += 2) {
+ if (val == inv_icm42600_accel_scale[idx] &&
+ val2 == inv_icm42600_accel_scale[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_accel_scale))
+ return -EINVAL;
+
+ conf.fs = idx / 2;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/* IIO format int + micro */
+static const int inv_icm42600_accel_odr[] = {
+ /* 12.5Hz */
+ 12, 500000,
+ /* 25Hz */
+ 25, 0,
+ /* 50Hz */
+ 50, 0,
+ /* 100Hz */
+ 100, 0,
+ /* 200Hz */
+ 200, 0,
+ /* 1kHz */
+ 1000, 0,
+ /* 2kHz */
+ 2000, 0,
+ /* 4kHz */
+ 4000, 0,
+};
+
+static const int inv_icm42600_accel_odr_conv[] = {
+ INV_ICM42600_ODR_12_5HZ,
+ INV_ICM42600_ODR_25HZ,
+ INV_ICM42600_ODR_50HZ,
+ INV_ICM42600_ODR_100HZ,
+ INV_ICM42600_ODR_200HZ,
+ INV_ICM42600_ODR_1KHZ_LN,
+ INV_ICM42600_ODR_2KHZ_LN,
+ INV_ICM42600_ODR_4KHZ_LN,
+};
+
+static int inv_icm42600_accel_read_odr(struct inv_icm42600_state *st,
+ int *val, int *val2)
+{
+ unsigned int odr;
+ unsigned int i;
+
+ odr = st->conf.accel.odr;
+
+ for (i = 0; i < ARRAY_SIZE(inv_icm42600_accel_odr_conv); ++i) {
+ if (inv_icm42600_accel_odr_conv[i] == odr)
+ break;
+ }
+ if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
+ return -EINVAL;
+
+ *val = inv_icm42600_accel_odr[2 * i];
+ *val2 = inv_icm42600_accel_odr[2 * i + 1];
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_accel_write_odr(struct inv_icm42600_state *st,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ unsigned int idx;
+ struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
+ int ret;
+
+ for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_odr); idx += 2) {
+ if (val == inv_icm42600_accel_odr[idx] &&
+ val2 == inv_icm42600_accel_odr[idx + 1])
+ break;
+ }
+ if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
+ return -EINVAL;
+
+ conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+
+ return ret;
+}
+
+/*
+ * Calibration bias values, IIO range format int + micro.
+ * Value is limited to +/-1g coded on 12 bits signed. Step is 0.5mg.
+ */
+static int inv_icm42600_accel_calibbias[] = {
+ -10, 42010, /* min: -10.042010 m/s² */
+ 0, 4903, /* step: 0.004903 m/s² */
+ 10, 37106, /* max: 10.037106 m/s² */
+};
+
+static int inv_icm42600_accel_read_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t bias;
+ unsigned int reg;
+ int16_t offset;
+ uint8_t data[2];
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER4;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER6;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER7;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ ret = regmap_bulk_read(st->map, reg, st->buffer, sizeof(data));
+ memcpy(data, st->buffer, sizeof(data));
+
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ if (ret)
+ return ret;
+
+ /* 12 bits signed value */
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ case IIO_MOD_Y:
+ offset = sign_extend32(((data[1] & 0x0F) << 8) | data[0], 11);
+ break;
+ case IIO_MOD_Z:
+ offset = sign_extend32(((data[0] & 0xF0) << 4) | data[1], 11);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /*
+ * convert raw offset to g then to m/s²
+ * 12 bits signed raw step 0.5mg to g: 5 / 10000
+ * g to m/s²: 9.806650
+ * result in micro (1000000)
+ * (offset * 5 * 9.806650 * 1000000) / 10000
+ */
+ val64 = (int64_t)offset * 5LL * 9806650LL;
+ /* for rounding, add + or - divisor (10000) divided by 2 */
+ if (val64 >= 0)
+ val64 += 10000LL / 2LL;
+ else
+ val64 -= 10000LL / 2LL;
+ bias = div_s64(val64, 10000L);
+ *val = bias / 1000000L;
+ *val2 = bias % 1000000L;
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42600_accel_write_offset(struct inv_icm42600_state *st,
+ struct iio_chan_spec const *chan,
+ int val, int val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ int64_t val64;
+ int32_t min, max;
+ unsigned int reg, regval;
+ int16_t offset;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ reg = INV_ICM42600_REG_OFFSET_USER4;
+ break;
+ case IIO_MOD_Y:
+ reg = INV_ICM42600_REG_OFFSET_USER6;
+ break;
+ case IIO_MOD_Z:
+ reg = INV_ICM42600_REG_OFFSET_USER7;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ /* inv_icm42600_accel_calibbias: min - step - max in micro */
+ min = inv_icm42600_accel_calibbias[0] * 1000000L +
+ inv_icm42600_accel_calibbias[1];
+ max = inv_icm42600_accel_calibbias[4] * 1000000L +
+ inv_icm42600_accel_calibbias[5];
+ val64 = (int64_t)val * 1000000LL + (int64_t)val2;
+ if (val64 < min || val64 > max)
+ return -EINVAL;
+
+ /*
+ * convert m/s² to g then to raw value
+ * m/s² to g: 1 / 9.806650
+ * g to raw 12 bits signed, step 0.5mg: 10000 / 5
+ * val in micro (1000000)
+ * val * 10000 / (9.806650 * 1000000 * 5)
+ */
+ val64 = val64 * 10000LL;
+ /* for rounding, add + or - divisor (9806650 * 5) divided by 2 */
+ if (val64 >= 0)
+ val64 += 9806650 * 5 / 2;
+ else
+ val64 -= 9806650 * 5 / 2;
+ offset = div_s64(val64, 9806650 * 5);
+
+ /* clamp value limited to 12 bits signed */
+ if (offset < -2048)
+ offset = -2048;
+ else if (offset > 2047)
+ offset = 2047;
+
+ pm_runtime_get_sync(dev);
+ mutex_lock(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ /* OFFSET_USER4 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER4,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ case IIO_MOD_Y:
+ /* OFFSET_USER7 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = offset & 0xFF;
+ st->buffer[1] = ((offset & 0xF00) >> 8) | (regval & 0xF0);
+ break;
+ case IIO_MOD_Z:
+ /* OFFSET_USER7 register is shared */
+ ret = regmap_read(st->map, INV_ICM42600_REG_OFFSET_USER7,
+ ®val);
+ if (ret)
+ goto out_unlock;
+ st->buffer[0] = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+ st->buffer[1] = offset & 0xFF;
+ break;
+ default:
+ ret = -EINVAL;
+ goto out_unlock;
+ }
+
+ ret = regmap_bulk_write(st->map, reg, st->buffer, 2);
+
+out_unlock:
+ mutex_unlock(&st->lock);
+ pm_runtime_mark_last_busy(dev);
+ pm_runtime_put_autosuspend(dev);
+ return ret;
+}
+
+static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int16_t data;
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_RAW:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_read_sensor(st, chan, &data);
+ iio_device_release_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ *val = data;
+ return IIO_VAL_INT;
+ case IIO_CHAN_INFO_SCALE:
+ return inv_icm42600_accel_read_scale(st, val, val2);
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_accel_read_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42600_accel_read_offset(st, chan, val, val2);
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ const int **vals,
+ int *type, int *length, long mask)
+{
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ *vals = inv_icm42600_accel_scale;
+ *type = IIO_VAL_INT_PLUS_NANO;
+ *length = ARRAY_SIZE(inv_icm42600_accel_scale);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ *vals = inv_icm42600_accel_odr;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ *length = ARRAY_SIZE(inv_icm42600_accel_odr);
+ return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ *vals = inv_icm42600_accel_calibbias;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ return IIO_AVAIL_RANGE;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_write_raw(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2, long mask)
+{
+ struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+ int ret;
+
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_write_scale(st, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return inv_icm42600_accel_write_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ ret = iio_device_claim_direct_mode(indio_dev);
+ if (ret)
+ return ret;
+ ret = inv_icm42600_accel_write_offset(st, chan, val, val2);
+ iio_device_release_direct_mode(indio_dev);
+ return ret;
+ default:
+ return -EINVAL;
+ }
+}
+
+static int inv_icm42600_accel_write_raw_get_fmt(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ long mask)
+{
+ if (chan->type != IIO_ACCEL)
+ return -EINVAL;
+
+ switch (mask) {
+ case IIO_CHAN_INFO_SCALE:
+ return IIO_VAL_INT_PLUS_NANO;
+ case IIO_CHAN_INFO_SAMP_FREQ:
+ return IIO_VAL_INT_PLUS_MICRO;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return IIO_VAL_INT_PLUS_MICRO;
+ default:
+ return -EINVAL;
+ }
+}
+
+static const struct iio_info inv_icm42600_accel_info = {
+ .read_raw = inv_icm42600_accel_read_raw,
+ .read_avail = inv_icm42600_accel_read_avail,
+ .write_raw = inv_icm42600_accel_write_raw,
+ .write_raw_get_fmt = inv_icm42600_accel_write_raw_get_fmt,
+ .debugfs_reg_access = inv_icm42600_debugfs_reg,
+};
+
+struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
+{
+ struct device *dev = regmap_get_device(st->map);
+ const char *name;
+ struct iio_dev *indio_dev;
+ int ret;
+
+ name = devm_kasprintf(dev, GFP_KERNEL, "%s-accel", st->name);
+ if (!name)
+ return ERR_PTR(-ENOMEM);
+
+ indio_dev = devm_iio_device_alloc(dev, 0);
+ if (!indio_dev)
+ return ERR_PTR(-ENOMEM);
+
+ iio_device_set_drvdata(indio_dev, st);
+ indio_dev->name = name;
+ indio_dev->info = &inv_icm42600_accel_info;
+ indio_dev->modes = INDIO_DIRECT_MODE;
+ indio_dev->channels = inv_icm42600_accel_channels;
+ indio_dev->num_channels = ARRAY_SIZE(inv_icm42600_accel_channels);
+
+ ret = devm_iio_device_register(dev, indio_dev);
+ if (ret)
+ return ERR_PTR(ret);
+
+ return indio_dev;
+}
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 84e9ff320b3b..a35ff21f50bb 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -513,6 +513,10 @@ int inv_icm42600_core_probe(struct regmap *regmap, int chip,
if (IS_ERR(st->indio_gyro))
return PTR_ERR(st->indio_gyro);
+ st->indio_accel = inv_icm42600_accel_init(st);
+ if (IS_ERR(st->indio_accel))
+ return PTR_ERR(st->indio_accel);
+
/* setup runtime power management */
ret = pm_runtime_set_active(dev);
if (ret)
--
2.17.1
On Mon, 22 Jun 2020 17:37:16 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
Looks good to me. Whole series applied (picking up Rob's ack
for the DT binding docs patch) to the togreg branch of iio.git and pushed
out as testing for the various autobuilders to poke at it.
Thanks,
Jonathan
> Changelog
> v1
> -initial patch submission
> v2
> - formatting reworks, missing headers, code cleanup ...
> - delete all debug traces
> - add commentaries for better explanation of suspend/resume, timestamp, ...
> - delete i2c/spi table ids keeping only of, and use I2C probe_new function
> - switch calibbias to SI units and add calibias_available attribute
> - use DMA-safe buffer for all regmap_bulk_* calls
> - delete iio trigger usage and setup/handle interrupt in core module
> - add open-drain interrupt support
> - add FIFO on reference counter and buffer postenable/predisable to replace
> iio trigger usage
> - check that temperature data is present before copying in buffer
> - add temperature sensor off when fifo is turned off
> - delete timestamp channel reading
> - move timestamp state in IIO device private data
> - allow only 1 ODR change in a batch of data
> - add driver-open-drain in devicetree YAML and delete spi options
> v3
> - delete const pointer cast for iio_device_get_drvdata
> - change gyro and accel init to return the allocated iio_dev structure
> - delete manual parent device assignment
> - correct style and improve readability
> - add commentaries about IIO buffer and watermark complex computation
> - add timestamp alignment in IIO buffer structure
> - wrap lines 80 columns for dt bindings
> - add ABI documentation for calibbias values in SI units
> v4
> - return high resolution 16 bits temperature as raw data when polled with the
> corresponding scale and offset.
> - for data buffer return temperature in the same 16 bits using the same
> scale and offset. Convert low resolution temperature FIFO data to high
> resolution format.
> - explicitely zero out data buffer before copying to iio buffer.
>
> This series add a new driver for managing InvenSense ICM-426xx 6-axis IMUs.
> This next generation of chips includes new generations of 3-axis gyroscope
> and 3-axis accelerometer, support of I3C in addition to I2C and SPI, and
> intelligent MotionTracking features like pedometer, tilt detection, and
> tap detection.
>
> This series is delivering a driver supporting gyroscope, accelerometer and
> temperature data, with polling and buffering using hwfifo and watermark,
> on I2C and SPI busses.
>
> Gyroscope and accelerometer sensors are completely independent and can have
> different ODRs. Since there is only a single FIFO a specific value is used to
> mark invalid data. For keeping the device standard we are de-multiplexing data
> from the FIFO to 2 IIO devices with 2 buffers, 1 for the accelerometer and 1
> for the gyroscope. This architecture also enables to easily turn each sensor
> on/off without impacting the other. The device interrupt is used to read the
> FIFO and launch parsing of accelerometer and gyroscope data. A complex
> timestamping mechanism is added to handle correctly FIFO watermark and dynamic
> changes of settings.
>
>
>
> Jean-Baptiste Maneyrol (13):
> iio: imu: inv_icm42600: add core of new inv_icm42600 driver
> iio: imu: inv_icm42600: add I2C driver for inv_icm42600 driver
> iio: imu: inv_icm42600: add SPI driver for inv_icm42600 driver
> iio: imu: inv_icm42600: add gyroscope IIO device
> iio: imu: inv_icm42600: add accelerometer IIO device
> iio: imu: inv_icm42600: add temperature sensor support
> iio: imu: add Kconfig and Makefile for inv_icm42600 driver
> Documentation: ABI: add specific icm42600 documentation
> iio: imu: inv_icm42600: add device interrupt
> iio: imu: inv_icm42600: add buffer support in iio devices
> iio: imu: inv_icm42600: add accurate timestamping
> dt-bindings: iio: imu: Add inv_icm42600 documentation
> MAINTAINERS: add entry for inv_icm42600 6-axis imu sensor
>
> .../ABI/testing/sysfs-bus-iio-icm42600 | 20 +
> .../bindings/iio/imu/invensense,icm42600.yaml | 90 ++
> MAINTAINERS | 8 +
> drivers/iio/imu/Kconfig | 1 +
> drivers/iio/imu/Makefile | 1 +
> drivers/iio/imu/inv_icm42600/Kconfig | 29 +
> drivers/iio/imu/inv_icm42600/Makefile | 15 +
> drivers/iio/imu/inv_icm42600/inv_icm42600.h | 395 +++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 787 +++++++++++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.c | 601 +++++++++++++
> .../imu/inv_icm42600/inv_icm42600_buffer.h | 98 +++
> .../iio/imu/inv_icm42600/inv_icm42600_core.c | 786 +++++++++++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_gyro.c | 798 ++++++++++++++++++
> .../iio/imu/inv_icm42600/inv_icm42600_i2c.c | 101 +++
> .../iio/imu/inv_icm42600/inv_icm42600_spi.c | 100 +++
> .../iio/imu/inv_icm42600/inv_icm42600_temp.c | 84 ++
> .../iio/imu/inv_icm42600/inv_icm42600_temp.h | 30 +
> .../imu/inv_icm42600/inv_icm42600_timestamp.c | 195 +++++
> .../imu/inv_icm42600/inv_icm42600_timestamp.h | 85 ++
> 19 files changed, 4224 insertions(+)
> create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
> create mode 100644 Documentation/devicetree/bindings/iio/imu/invensense,icm42600.yaml
> create mode 100644 drivers/iio/imu/inv_icm42600/Kconfig
> create mode 100644 drivers/iio/imu/inv_icm42600/Makefile
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600.h
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.h
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_i2c.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_spi.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_temp.h
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c
> create mode 100644 drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.h
>
On Mon, 22 Jun 2020 17:37:24 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Hardware offset available as calibscale sysfs attributes are real
> physical values exprimed in SI units.
>
> calibscale_available sysfs attributes represents the range of
> acceptable values.
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
Hi Jean-Baptiste.
This is causing us some issues as the ABI docs are now added to the
generated html docs for the kernel. It's been a while, so I've kind
of forgotten why we have this file. Was the issue that these are
in SI units as opposed to most calibbias controls which are offsets
applied to the raw analog reading hitting the ADC?
Would you mind if we moved this into the main doc as a note for this
particular device?
i.e. something in sysfs-bus-iio saying
icm42600: Hardware applied calibration offset is in SI units (rad/s or m/s^2 as appropriate)
?
Thanks,
Jonathan
> ---
> .../ABI/testing/sysfs-bus-iio-icm42600 | 20 +++++++++++++++++++
> 1 file changed, 20 insertions(+)
> create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
>
> diff --git a/Documentation/ABI/testing/sysfs-bus-iio-icm42600 b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
> new file mode 100644
> index 000000000000..0bf1fd4f5bf1
> --- /dev/null
> +++ b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
> @@ -0,0 +1,20 @@
> +What: /sys/bus/iio/devices/iio:deviceX/in_accel_x_calibbias
> +What: /sys/bus/iio/devices/iio:deviceX/in_accel_y_calibbias
> +What: /sys/bus/iio/devices/iio:deviceX/in_accel_z_calibbias
> +What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibbias
> +What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibbias
> +What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibbias
> +KernelVersion: 5.8
> +Contact: [email protected]
> +Description:
> + Hardware applied calibration offset (assumed to fix production
> + inaccuracies). Values represent a real physical offset expressed
> + in SI units (m/s^2 for accelerometer and rad/s for gyroscope).
> +
> +What: /sys/bus/iio/devices/iio:deviceX/in_accel_calibbias_available
> +What: /sys/bus/iio/devices/iio:deviceX/in_anglvel_calibbias_available
> +KernelVersion: 5.8
> +Contact: [email protected]
> +Description:
> + Range of available values for hardware offset. Values in SI
> + units (m/s^2 for accelerometer and rad/s for gyroscope).
Hello Jonathan,
no problem for me, you can move it wherever you see fit.
Thank you.
JB
From: Jonathan Cameron <[email protected]>
Sent: Saturday, November 14, 2020 16:23
To: Jean-Baptiste Maneyrol <[email protected]>
Cc: [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>; [email protected] <[email protected]>
Subject: Re: [PATCH v4 08/13] Documentation: ABI: add specific icm42600 documentation
?
?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 Mon, 22 Jun 2020 17:37:24 +0200
Jean-Baptiste Maneyrol <[email protected]> wrote:
> Hardware offset available as calibscale sysfs attributes are real
> physical values exprimed in SI units.
>
> calibscale_available sysfs attributes represents the range of
> acceptable values.
>
> Signed-off-by: Jean-Baptiste Maneyrol <[email protected]>
Hi Jean-Baptiste.
This is causing us some issues as the ABI docs are now added to the
generated html docs for the kernel.? It's been a while, so I've kind
of forgotten why we have this file.? Was the issue that these are
in SI units as opposed to most calibbias controls which are offsets
applied to the raw analog reading hitting the ADC?
Would you mind if we moved this into the main doc as a note for this
particular device?
i.e. something in sysfs-bus-iio saying
icm42600: Hardware applied calibration offset is in SI units (rad/s or m/s^2 as appropriate)
?
Thanks,
Jonathan
?
> ---
>? .../ABI/testing/sysfs-bus-iio-icm42600??????? | 20 +++++++++++++++++++
>? 1 file changed, 20 insertions(+)
>? create mode 100644 Documentation/ABI/testing/sysfs-bus-iio-icm42600
>
> diff --git a/Documentation/ABI/testing/sysfs-bus-iio-icm42600 b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
> new file mode 100644
> index 000000000000..0bf1fd4f5bf1
> --- /dev/null
> +++ b/Documentation/ABI/testing/sysfs-bus-iio-icm42600
> @@ -0,0 +1,20 @@
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_accel_x_calibbias
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_accel_y_calibbias
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_accel_z_calibbias
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_anglvel_x_calibbias
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_anglvel_y_calibbias
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_anglvel_z_calibbias
> +KernelVersion:? 5.8
> +Contact:??????? [email protected]
> +Description:
> +???????????? Hardware applied calibration offset (assumed to fix production
> +???????????? inaccuracies). Values represent a real physical offset expressed
> +???????????? in SI units (m/s^2 for accelerometer and rad/s for gyroscope).
> +
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_accel_calibbias_available
> +What:??????????????? /sys/bus/iio/devices/iio:deviceX/in_anglvel_calibbias_available
> +KernelVersion:? 5.8
> +Contact:??????? [email protected]
> +Description:
> +???????????? Range of available values for hardware offset. Values in SI
> +???????????? units (m/s^2 for accelerometer and rad/s for gyroscope).