From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
Add ODRs accessible only in low-power mode. Switch automatically to
low-power or low-noise depending on the mode supported by the ODR.
Add channel attributes "power_mode" and "power_mode_available" for
setting the power mode to use (low-noise or low-power) for ODRs
supporting both modes (500Hz to 12.5Hz).
By default, low-power mode is used for all frequencies < 1kHz and
low-noise is used for the others.
Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@xxxxxxx>
---
drivers/iio/imu/inv_icm42600/inv_icm42600.h | 4 +
.../iio/imu/inv_icm42600/inv_icm42600_accel.c | 95 ++++++++++++++++++-
.../iio/imu/inv_icm42600/inv_icm42600_core.c | 22 +++++
3 files changed, 117 insertions(+), 4 deletions(-)
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600.h b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
index c4ac91f6bafe..3a07e43e4cf1 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600.h
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600.h
@@ -177,11 +177,15 @@ 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.
+ * @power_mode: sensor requested power mode (for common frequencies)
+ * @filter: sensor filter.
* @ts: timestamp module states.
*/
struct inv_icm42600_sensor_state {
const int *scales;
size_t scales_len;
+ enum inv_icm42600_sensor_mode power_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..6ade0a5d4d1d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
@@ -55,8 +55,79 @@ enum inv_icm42600_accel_scan {
INV_ICM42600_ACCEL_SCAN_TIMESTAMP,
};
+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 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->power_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->power_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 +191,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->power_mode;
+ conf.filter = accel_st->filter;
ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_accel);
if (ret)
goto out_unlock;
@@ -144,10 +216,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 +249,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->power_mode;
+ conf.filter = accel_st->filter;
ret = inv_icm42600_set_accel_conf(st, &conf, NULL);
if (ret)
goto exit;
@@ -277,6 +352,12 @@ static int inv_icm42600_accel_write_scale(struct iio_dev *indio_dev,
/* IIO format int + micro */
static const int inv_icm42600_accel_odr[] = {
+ /* 1.5625Hz */
+ 1, 562500,
+ /* 3.125Hz */
+ 3, 125000,
+ /* 6.25Hz */
+ 6, 250000,
/* 12.5Hz */
12, 500000,
/* 25Hz */
@@ -296,6 +377,9 @@ static const int inv_icm42600_accel_odr[] = {
};
static const int inv_icm42600_accel_odr_conv[] = {
+ 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,
@@ -581,7 +665,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;
@@ -754,6 +838,9 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
accel_st->scales_len = ARRAY_SIZE(inv_icm42600_accel_scale);
break;
}
+ /* low-power by default at init */
+ accel_st->power_mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
+ accel_st->filter = INV_ICM42600_FILTER_AVG_16X;
/*
* 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..b14ff58d8a9d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
@@ -248,6 +248,18 @@ int inv_icm42600_set_accel_conf(struct inv_icm42600_state *st,
if (conf->filter < 0)
conf->filter = oldconf->filter;
+ /* force power mode against ODR when sensor is on */
+ if (conf->mode != INV_ICM42600_SENSOR_MODE_OFF) {
+ if (conf->odr <= INV_ICM42600_ODR_1KHZ_LN) {
+ conf->mode = INV_ICM42600_SENSOR_MODE_LOW_NOISE;
+ conf->filter = INV_ICM42600_FILTER_BW_ODR_DIV_2;
+ } else if (conf->odr >= INV_ICM42600_ODR_6_25HZ_LP &&
+ conf->odr <= INV_ICM42600_ODR_1_5625HZ_LP) {
+ conf->mode = INV_ICM42600_SENSOR_MODE_LOW_POWER;
+ conf->filter = INV_ICM42600_FILTER_AVG_16X;
+ }
+ }
+
/* 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 +453,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
