[PATCH] iio: imu: inv_icm42600: add support of accel low-power mode

[inv.git-commit@tdk.com]

From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>

Add channel attributes "power_mode" and "power_mode_available" for
setting accel power mode (low-noise or low-power).

Differents ODRs and filter are possible depending on the power mode.
Thus make ODRs and filter dynamic and check values when applying.
---
 drivers/iio/imu/inv_icm42600/inv_icm42600.h   |  12 +
 .../iio/imu/inv_icm42600/inv_icm42600_accel.c | 230 ++++++++++++++----
 .../iio/imu/inv_icm42600/inv_icm42600_core.c  |  33 +++
 3 files changed, 226 insertions(+), 49 deletions(-)

diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index c4ac91f6bafe..83219088e697 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -177,11 +177,23 @@ struct inv_icm42600_state {
  * struct inv_icm42600_sensor_state - sensor state variables
  * @scales:		table of scales.
  * @scales_len:		length (nb of items) of the scales table.
+ * @odrs:		table of odrs.
+ * @odrs_len:		lenght (nb of items) of the odrs table.
+ * @odr_convs:		conversion table of odrs from Hz to register value.
+ * @odr_convs_len:	lenght (nb of items) of the odr_convs table.
+ * @running_mode:	sensor running mode (low power or low noise).
+ * @filter:		sensor filter.
  * @ts:			timestamp module states.
  */
 struct inv_icm42600_sensor_state {
 	const int *scales;
 	size_t scales_len;
+	const int *odrs;
+	size_t odrs_len;
+	const int *odr_convs;
+	size_t odr_convs_len;
+	enum inv_icm42600_sensor_mode running_mode;
+	enum inv_icm42600_filter filter;
 	struct inv_sensors_timestamp ts;
 };

diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
index 83d8504ebfff..d76288dee225 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -55,8 +55,158 @@ enum inv_icm42600_accel_scan {
 	INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
 };

+/* IIO format int + micro */
+static const int inv_icm42600_accel_ln_odrs[] = {
+	/* 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_lp_odrs[] = {
+	/* 1.5625Hz */
+	1, 562500,
+	/* 3.125Hz */
+	3, 125000,
+	/* 6.25Hz */
+	6, 250000,
+	/* 12.5Hz */
+	12, 500000,
+	/* 25Hz */
+	25, 0,
+	/* 50Hz */
+	50, 0,
+	/* 100Hz */
+	100, 0,
+	/* 200Hz */
+	200, 0,
+};
+
+static const int inv_icm42600_accel_ln_odr_convs[] = {
+	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 const int inv_icm42600_accel_lp_odr_convs[] = {
+	INV_ICM42600_ODR_1_5625HZ_LP,
+	INV_ICM42600_ODR_3_125HZ_LP,
+	INV_ICM42600_ODR_6_25HZ_LP,
+	INV_ICM42600_ODR_12_5HZ,
+	INV_ICM42600_ODR_25HZ,
+	INV_ICM42600_ODR_50HZ,
+	INV_ICM42600_ODR_100HZ,
+	INV_ICM42600_ODR_200HZ,
+};
+
+static const char * const inv_icm42600_accel_power_mode_items[] = {
+	"low-noise",
+	"low-power",
+};
+static const int inv_icm42600_accel_power_mode_values[] = {
+	INV_ICM42600_SENSOR_MODE_LOW_NOISE,
+	INV_ICM42600_SENSOR_MODE_LOW_POWER,
+};
+static const int inv_icm42600_accel_filter_values[] = {
+	INV_ICM42600_FILTER_BW_ODR_DIV_2,
+	INV_ICM42600_FILTER_AVG_16X,
+};
+static const int *inv_icm42600_accel_odrs[] = {
+	inv_icm42600_accel_ln_odrs,
+	inv_icm42600_accel_lp_odrs,
+};
+static size_t inv_icm42600_accel_odrs_len[] = {
+	ARRAY_SIZE(inv_icm42600_accel_ln_odrs),
+	ARRAY_SIZE(inv_icm42600_accel_lp_odrs),
+};
+static const int *inv_icm42600_accel_odr_convs[] = {
+	inv_icm42600_accel_ln_odr_convs,
+	inv_icm42600_accel_lp_odr_convs,
+};
+static size_t inv_icm42600_accel_odr_convs_len[] = {
+	ARRAY_SIZE(inv_icm42600_accel_ln_odr_convs),
+	ARRAY_SIZE(inv_icm42600_accel_lp_odr_convs),
+};
+
+static int inv_icm42600_accel_power_mode_set(struct iio_dev *indio_dev,
+					     const struct iio_chan_spec *chan,
+					     unsigned int idx)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
+
+	if (chan->type != IIO_ACCEL)
+		return -EINVAL;
+
+	if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
+		return -EINVAL;
+
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
+	guard(mutex)(&st->lock);
+
+	accel_st->odrs = inv_icm42600_accel_odrs[idx];
+	accel_st->odrs_len = inv_icm42600_accel_odrs_len[idx];
+	accel_st->odr_convs = inv_icm42600_accel_odr_convs[idx];
+	accel_st->odr_convs_len = inv_icm42600_accel_odr_convs_len[idx];
+	accel_st->running_mode = inv_icm42600_accel_power_mode_values[idx];
+	accel_st->filter = inv_icm42600_accel_filter_values[idx];
+
+	return 0;
+}
+
+static int inv_icm42600_accel_power_mode_get(struct iio_dev *indio_dev,
+					     const struct iio_chan_spec *chan)
+{
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
+	unsigned int idx;
+
+	if (chan->type != IIO_ACCEL)
+		return -EINVAL;
+
+	guard(mutex)(&st->lock);
+
+	for (idx = 0; idx < ARRAY_SIZE(inv_icm42600_accel_power_mode_values); ++idx) {
+		if (accel_st->running_mode ==
+				inv_icm42600_accel_power_mode_values[idx])
+			break;
+	}
+	if (idx >= ARRAY_SIZE(inv_icm42600_accel_power_mode_values))
+		return -EINVAL;
+
+	return idx;
+}
+
+static const struct iio_enum inv_icm42600_accel_power_mode_enum = {
+	.items = inv_icm42600_accel_power_mode_items,
+	.num_items = ARRAY_SIZE(inv_icm42600_accel_power_mode_items),
+	.set = inv_icm42600_accel_power_mode_set,
+	.get = inv_icm42600_accel_power_mode_get,
+};
+
 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),
+	IIO_ENUM_AVAILABLE("power_mode", IIO_SHARED_BY_TYPE,
+			   &inv_icm42600_accel_power_mode_enum),
+	IIO_ENUM("power_mode", IIO_SHARED_BY_TYPE,
+		 &inv_icm42600_accel_power_mode_enum),
 	{},
 };

@@ -120,7 +270,8 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,

 	if (*scan_mask & INV_ICM42600_SCAN_MASK_ACCEL_3AXIS) {
 		/* enable accel sensor */
-		conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+		conf.mode = accel_st->running_mode;
+		conf.filter = accel_st->filter;
 		ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
 		if (ret)
 			goto out_unlock;
@@ -144,10 +295,12 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
 	return ret;
 }

-static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
+static int inv_icm42600_accel_read_sensor(struct iio_dev *indio_dev,
 					  struct iio_chan_spec const *chan,
 					  int16_t *val)
 {
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
 	struct device *dev = regmap_get_device(st->map);
 	struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
 	unsigned int reg;
@@ -175,7 +328,8 @@ static int inv_icm42600_accel_read_sensor(struct inv_icm42600_state *st,
 	mutex_lock(&st->lock);

 	/* enable accel sensor */
-	conf.mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+	conf.mode = accel_st->running_mode;
+	conf.filter = accel_st->filter;
 	ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
 	if (ret)
 		goto exit;
@@ -275,54 +429,25 @@ static int inv_icm42600_accel_write_scale(struct iio_dev *indio_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,
+static int inv_icm42600_accel_read_odr(struct iio_dev *indio_dev,
 				       int *val, int *val2)
 {
+	struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
+	struct inv_icm42600_sensor_state *accel_st = iio_priv(indio_dev);
 	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)
+	for (i = 0; i < accel_st->odr_convs_len; ++i) {
+		if (accel_st->odr_convs[i] == odr)
 			break;
 	}
-	if (i >= ARRAY_SIZE(inv_icm42600_accel_odr_conv))
+	if (i >= accel_st->odr_convs_len)
 		return -EINVAL;

-	*val = inv_icm42600_accel_odr[2 * i];
-	*val2 = inv_icm42600_accel_odr[2 * i + 1];
+	*val = accel_st->odrs[2 * i];
+	*val2 = accel_st->odrs[2 * i + 1];

 	return IIO_VAL_INT_PLUS_MICRO;
 }
@@ -338,15 +463,15 @@ static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
 	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])
+	for (idx = 0; idx < accel_st->odrs_len; idx += 2) {
+		if (val == accel_st->odrs[idx] &&
+		    val2 == accel_st->odrs[idx + 1])
 			break;
 	}
-	if (idx >= ARRAY_SIZE(inv_icm42600_accel_odr))
+	if (idx >= accel_st->odrs_len)
 		return -EINVAL;

-	conf.odr = inv_icm42600_accel_odr_conv[idx / 2];
+	conf.odr = accel_st->odr_convs[idx / 2];

 	pm_runtime_get_sync(dev);
 	mutex_lock(&st->lock);
@@ -581,7 +706,7 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
 		ret = iio_device_claim_direct_mode(indio_dev);
 		if (ret)
 			return ret;
-		ret = inv_icm42600_accel_read_sensor(st, chan, &data);
+		ret = inv_icm42600_accel_read_sensor(indio_dev, chan, &data);
 		iio_device_release_direct_mode(indio_dev);
 		if (ret)
 			return ret;
@@ -590,7 +715,7 @@ static int inv_icm42600_accel_read_raw(struct iio_dev *indio_dev,
 	case IIO_CHAN_INFO_SCALE:
 		return inv_icm42600_accel_read_scale(indio_dev, val, val2);
 	case IIO_CHAN_INFO_SAMP_FREQ:
-		return inv_icm42600_accel_read_odr(st, val, val2);
+		return inv_icm42600_accel_read_odr(indio_dev, val, val2);
 	case IIO_CHAN_INFO_CALIBBIAS:
 		return inv_icm42600_accel_read_offset(st, chan, val, val2);
 	default:
@@ -615,9 +740,9 @@ static int inv_icm42600_accel_read_avail(struct iio_dev *indio_dev,
 		*length = accel_st->scales_len;
 		return IIO_AVAIL_LIST;
 	case IIO_CHAN_INFO_SAMP_FREQ:
-		*vals = inv_icm42600_accel_odr;
+		*vals = accel_st->odrs;
 		*type = IIO_VAL_INT_PLUS_MICRO;
-		*length = ARRAY_SIZE(inv_icm42600_accel_odr);
+		*length = accel_st->odrs_len;
 		return IIO_AVAIL_LIST;
 	case IIO_CHAN_INFO_CALIBBIAS:
 		*vals = inv_icm42600_accel_calibbias;
@@ -754,6 +879,13 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
 		accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
 		break;
 	}
+	/* low-noise mode and ODRs at init */
+	accel_st->odrs = inv_icm42600_accel_ln_odrs;
+	accel_st->odrs_len = ARRAY_SIZE(inv_icm42600_accel_ln_odrs);
+	accel_st->odr_convs = inv_icm42600_accel_ln_odr_convs;
+	accel_st->odr_convs_len = ARRAY_SIZE(inv_icm42600_accel_ln_odr_convs);
+	accel_st->running_mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+	accel_st->filter = INV_ICM42600_FILTER_BW_ODR_DIV_2;

 	/*
 	 * clock period is 32kHz (31250ns)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
index 96116a68ab29..50380f2a5ebb 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -235,6 +235,7 @@ int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
 				unsigned int *sleep_ms)
 {
 	struct inv_icm42600_sensor_conf *oldconf = &st->conf.accel;
+	int min_odr, max_odr;
 	unsigned int val;
 	int ret;

@@ -248,6 +249,28 @@ int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
 	if (conf->filter < 0)
 		conf->filter = oldconf->filter;

+	/* sanitize ODR setting against power mode */
+	switch (conf->mode) {
+	case INV_ICM42600_SENSOR_MODE_LOW_NOISE:
+		min_odr = INV_ICM42600_ODR_8KHZ_LN;
+		max_odr = INV_ICM42600_ODR_12_5HZ;
+		if (conf->odr < min_odr)
+			conf->odr = min_odr;
+		else if (conf->odr > max_odr)
+			conf->odr = max_odr;
+		break;
+	case INV_ICM42600_SENSOR_MODE_LOW_POWER:
+		min_odr = INV_ICM42600_ODR_200HZ;
+		max_odr = INV_ICM42600_ODR_1_5625HZ_LP;
+		if (conf->odr < min_odr)
+			conf->odr = min_odr;
+		else if (conf->odr > max_odr)
+			conf->odr = max_odr;
+		break;
+	default:
+		break;
+	}
+
 	/* set ACCEL_CONFIG0 register (accel fullscale & odr) */
 	if (conf->fs != oldconf->fs || conf->odr != oldconf->odr) {
 		val = INV_ICM42600_ACCEL_CONFIG0_FS(conf->fs) |
@@ -441,6 +464,16 @@ static int inv_icm42600_setup(struct inv_icm42600_state *st,
 	if (ret)
 		return ret;

+	/*
+	 * Use RC clock for accel low-power to fix glitches when switching
+	 * gyro on/off while accel low-power is on.
+	 */
+	ret = regmap_update_bits(st->map, INV_ICM42600_REG_INTF_CONFIG1,
+				 INV_ICM42600_INTF_CONFIG1_ACCEL_LP_CLK_RC,
+				 INV_ICM42600_INTF_CONFIG1_ACCEL_LP_CLK_RC);
+	if (ret)
+		return ret;
+
 	return inv_icm42600_set_conf(st, hw->conf);
 }

--
2.34.1