[PATCH v3 7/8] iio: imu: inv_icm42607: Add accelerometer calibbias support

[Kanak Shilledar]

Expose IIO_CHAN_INFO_CALIBBIAS on the accelerometer 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_USER4
to OFFSET_USER8 registers in MREG1.

Note: The accelerometer functionality is tested with Invensense,
ICM42370-P development board.

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_accel.c | 238 +++++++++++++++++++++-
 1 file changed, 236 insertions(+), 2 deletions(-)

diff --git a/drivers/iio/imu/inv_icm42607/inv_icm42607_accel.c b/drivers/iio/imu/inv_icm42607/inv_icm42607_accel.c
index 318373efeb9e..af2ac9d6a47c 100644
--- a/drivers/iio/imu/inv_icm42607/inv_icm42607_accel.c
+++ b/drivers/iio/imu/inv_icm42607/inv_icm42607_accel.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_ACCEL,							\
 	.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,							\
@@ -56,6 +59,16 @@ static const struct iio_chan_spec inv_icm42607_accel_channels[] = {
 	INV_ICM42607_TEMP_CHAN(INV_ICM42607_ACCEL_SCAN_TEMP),
 };
 
+/*
+ * 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_icm42607_accel_calibbias[] = {
+	-10, 42010, /* Min : -2^11 * 0.0005 * 9.80665	   = -10.042010 m/s²	*/
+	  0,  4903, /* Step: 0.5 * 0.00980655		   = 0.004903 m/s²	*/
+	 10, 37106, /* Max : (2^11 - 1) * 0.0005 * 9.80665 = 10.037106 m/s²	*/
+};
+
 static const int inv_icm42607_accel_scale_nano[][2] = {
 	[INV_ICM42607_ACCEL_FS_16G] = { 0, 4788403 },
 	[INV_ICM42607_ACCEL_FS_8G] = { 0, 2394202 },
@@ -176,6 +189,221 @@ static int inv_icm42607_accel_write_odr(struct iio_dev *indio_dev,
 	return inv_icm42607_set_sensor_conf(st, &conf, IIO_ACCEL);
 }
 
+static int inv_icm42607_accel_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);
+	u8 buffer_data[2];
+	unsigned int reg;
+	s16 offset;
+	s64 val64;
+	s32 bias;
+	int ret;
+
+	if (chan->type != IIO_ACCEL)
+		return -EINVAL;
+
+	switch (chan->channel2) {
+	case IIO_MOD_X:
+		reg = INV_ICM42607_REG_OFFSET_USER4;
+		break;
+	case IIO_MOD_Y:
+		reg = INV_ICM42607_REG_OFFSET_USER6;
+		break;
+	case IIO_MOD_Z:
+		reg = INV_ICM42607_REG_OFFSET_USER7;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm);
+	ret = PM_RUNTIME_ACQUIRE_ERR(&pm);
+	if (ret)
+		return ret;
+
+	guard(mutex)(&st->lock);
+
+	ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1, reg, &buffer_data[0]);
+	if (ret)
+		return ret;
+
+	ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1, reg + 1,
+				     &buffer_data[1]);
+	if (ret)
+		return ret;
+
+	/* 12 bits signed value */
+	switch (chan->channel2) {
+	case IIO_MOD_X:
+		offset = sign_extend32(((buffer_data[0] & 0xF0) << 4) | buffer_data[1], 11);
+		break;
+	case IIO_MOD_Y:
+		offset = sign_extend32(((buffer_data[1] & 0x0F) << 8) | buffer_data[0], 11);
+		break;
+	case IIO_MOD_Z:
+		offset = sign_extend32(((buffer_data[0] & 0xF0) << 4) | buffer_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 = (s64)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 / (long)MEGA;
+	*val2 = bias % (long)MEGA;
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int inv_icm42607_accel_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;
+	s32 min, max;
+	s16 offset;
+	s64 val64;
+	int ret;
+
+	if (chan->type != IIO_ACCEL)
+		return -EINVAL;
+
+	/* inv_icm42607_accel_calibbias: min - step - max in micro */
+	min = inv_icm42607_accel_calibbias[0] * (long)MEGA -
+	      inv_icm42607_accel_calibbias[1];
+	max = inv_icm42607_accel_calibbias[4] * (long)MEGA +
+	      inv_icm42607_accel_calibbias[5];
+
+	val64 = (s64)val * (s64)MEGA;
+	if (val >= 0)
+		val64 += (s64)val2;
+	else
+		val64 -= (s64)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_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_USER4 register is shared */
+		ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+				INV_ICM42607_REG_OFFSET_USER4, &regval);
+		if (ret)
+			return ret;
+
+		hi = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+		lo = offset & 0xFF;
+
+		ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+				INV_ICM42607_REG_OFFSET_USER4, hi);
+		if (ret)
+			return ret;
+
+		ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+					      INV_ICM42607_REG_OFFSET_USER5, lo);
+
+		if (ret)
+			return ret;
+		break;
+
+	case IIO_MOD_Y:
+		/* OFFSET_USER7 register is shared */
+		ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+					     INV_ICM42607_REG_OFFSET_USER7,
+					     &regval);
+		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_USER7,
+					      hi);
+		if (ret)
+			return ret;
+
+		ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+					      INV_ICM42607_REG_OFFSET_USER6,
+					      lo);
+		if (ret)
+			return ret;
+		break;
+
+	case IIO_MOD_Z:
+		/* OFFSET_USER7 register is shared */
+		ret = inv_icm42607_mreg_read(st, INV_ICM42607_MREG1,
+					     INV_ICM42607_REG_OFFSET_USER7,
+					     &regval);
+		if (ret)
+			return ret;
+
+		hi = ((offset & 0xF00) >> 4) | (regval & 0x0F);
+		lo = offset & 0xFF;
+
+		ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+					      INV_ICM42607_REG_OFFSET_USER7, hi);
+		if (ret)
+			return ret;
+
+		ret = inv_icm42607_mreg_write(st, INV_ICM42607_MREG1,
+					      INV_ICM42607_REG_OFFSET_USER8, lo);
+		if (ret)
+			return ret;
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int inv_icm42607_accel_read_raw(struct iio_dev *indio_dev,
 				       struct iio_chan_spec const *chan,
 				       int *val, int *val2, long mask)
@@ -207,6 +435,8 @@ static int inv_icm42607_accel_read_raw(struct iio_dev *indio_dev,
 		return inv_icm42607_accel_read_scale(indio_dev, val, val2);
 	case IIO_CHAN_INFO_SAMP_FREQ:
 		return inv_icm42607_accel_read_odr(st, val, val2);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		return inv_icm42607_accel_read_offset(st, chan, val, val2);
 	default:
 		return -EINVAL;
 	}
@@ -250,6 +480,8 @@ static int inv_icm42607_accel_write_raw(struct iio_dev *indio_dev,
 		return ret;
 	case IIO_CHAN_INFO_SAMP_FREQ:
 		return inv_icm42607_accel_write_odr(indio_dev, val, val2);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		return inv_icm42607_accel_write_offset(indio_dev, chan, val, val2);
 	default:
 		return -EINVAL;
 	}
@@ -266,6 +498,8 @@ static int inv_icm42607_accel_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