[PATCH v3 8/8] iio: imu: inv_icm42607: Add gyroscope calibbias support
From: Kanak Shilledar
Date: Tue Sep 01 2026 - 12:17:17 EST
Expose IIO_CHAN_INFO_CALIBBIAS on the gyroscope channels. The
registers are stored in MREG1, which can be accessed by the mreg_read
and mreg_write routines. The calibration bias is written to OFFSET_USER0
to OFFSET_USER3 registers in MREG1.
Note: The gyroscope calibbias functionality is not tested on actual
hardware.
Datasheet: https://www.invensense.tdk.com/en-us/products/3-axis/icm-42370-p
Datasheet: https://www.lcsc.com/product-detail/C5129967.html
Signed-off-by: Kanak Shilledar <kanak.shilledar@xxxxxxxx>
---
drivers/iio/imu/inv_icm42607/inv_icm42607_gyro.c | 233 ++++++++++++++++++++++-
1 file changed, 231 insertions(+), 2 deletions(-)
diff --git a/drivers/iio/imu/inv_icm42607/inv_icm42607_gyro.c b/drivers/iio/imu/inv_icm42607/inv_icm42607_gyro.c
index fb9bacee4e8d..58647cbd4ea4 100644
--- a/drivers/iio/imu/inv_icm42607/inv_icm42607_gyro.c
+++ b/drivers/iio/imu/inv_icm42607/inv_icm42607_gyro.c
@@ -13,6 +13,7 @@
#include <linux/pm_runtime.h>
#include <linux/regmap.h>
#include <linux/types.h>
+#include <linux/units.h>
#include "inv_icm42607.h"
#include "inv_icm42607_temp.h"
@@ -22,9 +23,11 @@
.type = IIO_ANGL_VEL, \
.modified = 1, \
.channel2 = _modifier, \
- .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+ .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_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, \
@@ -63,6 +66,16 @@ static const int inv_icm42607_gyro_scale_nano[][2] = {
[INV_ICM42607_GYRO_FS_250DPS] = { 0, 133158 },
};
+/*
+ * Calibration bias values, IIO range format int + micro.
+ * Value is limited to +/-64 dps coded on 12 bits signed. Step is 1/32 dps.
+ */
+static int inv_icm42607_gyro_calibbias[] = {
+ -1, 117011, /* Min : -2^11 * (1/32) * (pi/180) = -1.117011 rad/s */
+ 0, 545, /* Step: (1/32) * (pi/180) = 0.000545 rad/s */
+ 1, 116465, /* Max : (2^11 - 1) * (1/32) * (pi/180) = 1.116465 rad/s */
+};
+
static int inv_icm42607_gyro_read_scale(struct iio_dev *indio_dev,
int *val, int *val2)
{
@@ -173,6 +186,209 @@ static int inv_icm42607_gyro_write_odr(struct iio_dev *indio_dev,
return inv_icm42607_set_sensor_conf(st, &conf, IIO_ANGL_VEL);
}
+static int inv_icm42607_gyro_read_offset(struct inv_icm42607_state *st,
+ struct iio_chan_spec const *chan,
+ int *val, int *val2)
+{
+ struct device *dev = regmap_get_device(st->map);
+ s16 offset;
+ s64 val64;
+ u8 lo, hi;
+ s32 bias;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm);
+ ret = PM_RUNTIME_ACQUIRE_ERR(&pm);
+ if (ret)
+ return ret;
+
+ guard(mutex)(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER0, &lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, &hi);
+ if (ret)
+ return ret;
+ offset = sign_extend32(((hi & 0x0F) << 8) | lo, 11);
+ break;
+
+ case IIO_MOD_Y:
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER2, &lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, &hi);
+ if (ret)
+ return ret;
+ offset = sign_extend32((((hi & 0xF0) >> 4) << 8) | lo, 11);
+ break;
+
+ case IIO_MOD_Z:
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER3, &lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER4, &hi);
+ if (ret)
+ return ret;
+ offset = sign_extend32(((hi & 0x0F) << 8) | lo, 11);
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ /*
+ * Convert raw offset to dps then to rad/s
+ * 12 bits signed raw, step 1/32 dps
+ * dps to rad/s: pi / 180
+ * Result in micro (1000000): offset * 5760 / (pi * 1e6) [inverse below]
+ * offset * pi * 1e6 / 5760, using pi*1e6 ~= 3141593
+ */
+ val64 = (s64)offset * 3141593LL;
+ if (val64 >= 0)
+ val64 += 5760LL / 2LL;
+ else
+ val64 -= 5760LL / 2LL;
+
+ bias = div_s64(val64, 5760L);
+ *val = bias / (long)MEGA;
+ *val2 = bias % (long)MEGA;
+
+ return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42607_gyro_write_offset(struct iio_dev *indio_dev,
+ struct iio_chan_spec const *chan,
+ int val, int val2)
+{
+ struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev);
+ struct device *dev = regmap_get_device(st->map);
+ u8 hi, lo, regval;
+ s64 min64, max64;
+ s16 offset;
+ s64 val64;
+ int ret;
+
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+
+ /* inv_icm42607_gyro_calibbias: min - step - max in micro */
+ min64 = (s64)inv_icm42607_gyro_calibbias[0] * MEGA -
+ inv_icm42607_gyro_calibbias[1];
+ max64 = (s64)inv_icm42607_gyro_calibbias[4] * MEGA +
+ inv_icm42607_gyro_calibbias[5];
+
+ val64 = (s64)val * (s64)MEGA;
+ if (val >= 0)
+ val64 += (s64)val2;
+ else
+ val64 -= (s64)val2;
+
+ if (val64 < min64 || val64 > max64)
+ return -EINVAL;
+
+ /*
+ * Convert rad/s to dps then to raw 12-bit signed value, step 1/32 dps
+ * offset = val(rad/s, micro) * 5760 / (pi * 1e6), pi*1e6 ~= 3141593
+ */
+ val64 = val64 * 5760LL;
+ if (val64 >= 0)
+ val64 += 3141593LL / 2LL;
+ else
+ val64 -= 3141593LL / 2LL;
+ offset = div_s64(val64, 3141593LL);
+
+ if (offset < -2048)
+ offset = -2048;
+ else if (offset > 2047)
+ offset = 2047;
+
+ PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm);
+ ret = PM_RUNTIME_ACQUIRE_ERR(&pm);
+ if (ret)
+ return ret;
+
+ guard(mutex)(&st->lock);
+
+ switch (chan->channel2) {
+ case IIO_MOD_X:
+ /* OFFSET_USER1 register is shared with Y */
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, ®val);
+ if (ret)
+ return ret;
+
+ lo = offset & 0xFF;
+ hi = ((offset & 0xF00) >> 8) | (regval & 0xF0);
+
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER0, lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, hi);
+ if (ret)
+ return ret;
+ break;
+
+ case IIO_MOD_Y:
+ /* OFFSET_USER1 register is shared with X */
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, ®val);
+ if (ret)
+ return ret;
+
+ lo = offset & 0xFF;
+ hi = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER2, lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER1, hi);
+ if (ret)
+ return ret;
+ break;
+
+ case IIO_MOD_Z:
+ /* OFFSET_USER4 register is shared with accel X */
+ ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER4, ®val);
+ if (ret)
+ return ret;
+
+ lo = offset & 0xFF;
+ hi = ((offset & 0xF00) >> 8) | (regval & 0xF0);
+
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER3, lo);
+ if (ret)
+ return ret;
+ ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+ INV_ICM42607_REG_OFFSET_USER4, hi);
+ if (ret)
+ return ret;
+ break;
+
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
static int inv_icm42607_gyro_read_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val, int *val2, long mask)
@@ -204,6 +420,8 @@ static int inv_icm42607_gyro_read_raw(struct iio_dev *indio_dev,
return inv_icm42607_gyro_read_scale(indio_dev, val, val2);
case IIO_CHAN_INFO_SAMP_FREQ:
return inv_icm42607_gyro_read_odr(st, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42607_gyro_read_offset(st, chan, val, val2);
default:
return -EINVAL;
}
@@ -228,6 +446,13 @@ static int inv_icm42607_gyro_read_avail(struct iio_dev *indio_dev,
*length = (ARRAY_SIZE(inv_icm42607_gyro_odr) -
INV_ICM42607_ODR_1600HZ) * 2;
return IIO_AVAIL_LIST;
+ case IIO_CHAN_INFO_CALIBBIAS:
+ if (chan->type != IIO_ANGL_VEL)
+ return -EINVAL;
+ *vals = inv_icm42607_gyro_calibbias;
+ *type = IIO_VAL_INT_PLUS_MICRO;
+ *length = ARRAY_SIZE(inv_icm42607_gyro_calibbias);
+ return IIO_AVAIL_RANGE;
default:
return -EINVAL;
}
@@ -247,6 +472,8 @@ static int inv_icm42607_gyro_write_raw(struct iio_dev *indio_dev,
return ret;
case IIO_CHAN_INFO_SAMP_FREQ:
return inv_icm42607_gyro_write_odr(indio_dev, val, val2);
+ case IIO_CHAN_INFO_CALIBBIAS:
+ return inv_icm42607_gyro_write_offset(indio_dev, chan, val, val2);
default:
return -EINVAL;
}
@@ -263,6 +490,8 @@ static int inv_icm42607_gyro_write_raw_get_fmt(struct iio_dev *indio_dev,
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;
}
--
2.43.0