iio: imu: inv_mpu6050: add WoM suspend wakeup with low-power mode
Add wakeup from suspend for WoM when enabled and put accel in low-power mode when suspended. Requires rewriting pwr_mgmt_1 register handling and factorize out accel LPF settings. Use a low-power rate similar to the chip sampling rate but always lower for a best match of the sampling rate while saving power and adjust threshold to follow the required roc value. Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com> Link: https://lore.kernel.org/r/20240311160557.437337-5-inv.git-commit@tdk.com Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Jean-Baptiste Maneyrol
Jonathan Cameron
parent
5537f653d9
commit
305914d018
drivers/iio/imu/inv_mpu6050
@ -289,7 +289,7 @@ static const struct inv_mpu6050_hw hw_info[] = {
|
||||
};
|
||||
|
||||
static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,
|
||||
int clock, int temp_dis)
|
||||
bool cycle, int clock, int temp_dis)
|
||||
{
|
||||
u8 val;
|
||||
|
||||
@ -303,6 +303,8 @@ static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep
|
||||
val |= INV_MPU6050_BIT_TEMP_DIS;
|
||||
if (sleep)
|
||||
val |= INV_MPU6050_BIT_SLEEP;
|
||||
if (cycle)
|
||||
val |= INV_MPU6050_BIT_CYCLE;
|
||||
|
||||
dev_dbg(regmap_get_device(st->map), "pwr_mgmt_1: 0x%x\n", val);
|
||||
return regmap_write(st->map, st->reg->pwr_mgmt_1, val);
|
||||
@ -318,7 +320,7 @@ static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st,
|
||||
case INV_MPU6000:
|
||||
case INV_MPU9150:
|
||||
/* old chips: switch clock manually */
|
||||
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, clock, -1);
|
||||
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, clock, -1);
|
||||
if (ret)
|
||||
return ret;
|
||||
st->chip_config.clk = clock;
|
||||
@ -360,7 +362,7 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
|
||||
|
||||
/* turn on/off temperature sensor */
|
||||
if (mask & INV_MPU6050_SENSOR_TEMP) {
|
||||
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, !en);
|
||||
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, !en);
|
||||
if (ret)
|
||||
return ret;
|
||||
st->chip_config.temp_en = en;
|
||||
@ -467,7 +469,7 @@ static int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st,
|
||||
{
|
||||
int result;
|
||||
|
||||
result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, -1, -1);
|
||||
result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, false, -1, -1);
|
||||
if (result)
|
||||
return result;
|
||||
|
||||
@ -497,22 +499,9 @@ static int inv_mpu6050_set_gyro_fsr(struct inv_mpu6050_state *st,
|
||||
return regmap_write(st->map, st->reg->gyro_config, data);
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
|
||||
*
|
||||
* MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
|
||||
* MPU6500 and above have a dedicated register for accelerometer
|
||||
*/
|
||||
static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
|
||||
enum inv_mpu6050_filter_e val)
|
||||
static int inv_mpu6050_set_accel_lpf_regs(struct inv_mpu6050_state *st,
|
||||
enum inv_mpu6050_filter_e val)
|
||||
{
|
||||
int result;
|
||||
|
||||
result = regmap_write(st->map, st->reg->lpf, val);
|
||||
if (result)
|
||||
return result;
|
||||
|
||||
/* set accel lpf */
|
||||
switch (st->chip_type) {
|
||||
case INV_MPU6050:
|
||||
case INV_MPU6000:
|
||||
@ -531,6 +520,25 @@ static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
|
||||
return regmap_write(st->map, st->reg->accel_lpf, val);
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
|
||||
*
|
||||
* MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
|
||||
* MPU6500 and above have a dedicated register for accelerometer
|
||||
*/
|
||||
static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
|
||||
enum inv_mpu6050_filter_e val)
|
||||
{
|
||||
int result;
|
||||
|
||||
result = regmap_write(st->map, st->reg->lpf, val);
|
||||
if (result)
|
||||
return result;
|
||||
|
||||
/* set accel lpf */
|
||||
return inv_mpu6050_set_accel_lpf_regs(st, val);
|
||||
}
|
||||
|
||||
/*
|
||||
* inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
|
||||
*
|
||||
@ -1002,6 +1010,84 @@ static int inv_mpu6050_set_wom_threshold(struct inv_mpu6050_state *st, u64 value
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int inv_mpu6050_set_lp_odr(struct inv_mpu6050_state *st, unsigned int freq_div,
|
||||
unsigned int *lp_div)
|
||||
{
|
||||
static const unsigned int freq_dividers[] = {2, 4, 8, 16, 32, 64, 128, 256};
|
||||
static const unsigned int reg_values[] = {
|
||||
INV_MPU6050_LPOSC_500HZ, INV_MPU6050_LPOSC_250HZ,
|
||||
INV_MPU6050_LPOSC_125HZ, INV_MPU6050_LPOSC_62HZ,
|
||||
INV_MPU6050_LPOSC_31HZ, INV_MPU6050_LPOSC_16HZ,
|
||||
INV_MPU6050_LPOSC_8HZ, INV_MPU6050_LPOSC_4HZ,
|
||||
};
|
||||
unsigned int val, i;
|
||||
|
||||
switch (st->chip_type) {
|
||||
case INV_ICM20609:
|
||||
case INV_ICM20689:
|
||||
case INV_ICM20600:
|
||||
case INV_ICM20602:
|
||||
case INV_ICM20690:
|
||||
/* nothing to do */
|
||||
*lp_div = INV_MPU6050_FREQ_DIVIDER(st);
|
||||
return 0;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/* found the nearest superior frequency divider */
|
||||
i = ARRAY_SIZE(reg_values) - 1;
|
||||
val = reg_values[i];
|
||||
*lp_div = freq_dividers[i];
|
||||
for (i = 0; i < ARRAY_SIZE(freq_dividers); ++i) {
|
||||
if (freq_div <= freq_dividers[i]) {
|
||||
val = reg_values[i];
|
||||
*lp_div = freq_dividers[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
dev_dbg(regmap_get_device(st->map), "lp_odr: 0x%x\n", val);
|
||||
return regmap_write(st->map, INV_MPU6500_REG_LP_ODR, val);
|
||||
}
|
||||
|
||||
static int inv_mpu6050_set_wom_lp(struct inv_mpu6050_state *st, bool on)
|
||||
{
|
||||
unsigned int lp_div;
|
||||
int result;
|
||||
|
||||
if (on) {
|
||||
/* set low power ODR */
|
||||
result = inv_mpu6050_set_lp_odr(st, INV_MPU6050_FREQ_DIVIDER(st), &lp_div);
|
||||
if (result)
|
||||
return result;
|
||||
/* disable accel low pass filter */
|
||||
result = inv_mpu6050_set_accel_lpf_regs(st, INV_MPU6050_FILTER_NOLPF);
|
||||
if (result)
|
||||
return result;
|
||||
/* update wom threshold with new low-power frequency divider */
|
||||
result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold, lp_div);
|
||||
if (result)
|
||||
return result;
|
||||
/* set cycle mode */
|
||||
result = inv_mpu6050_pwr_mgmt_1_write(st, false, true, -1, -1);
|
||||
} else {
|
||||
/* disable cycle mode */
|
||||
result = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, -1);
|
||||
if (result)
|
||||
return result;
|
||||
/* restore wom threshold */
|
||||
result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold,
|
||||
INV_MPU6050_FREQ_DIVIDER(st));
|
||||
if (result)
|
||||
return result;
|
||||
/* restore accel low pass filter */
|
||||
result = inv_mpu6050_set_accel_lpf_regs(st, st->chip_config.lpf);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static int inv_mpu6050_enable_wom(struct inv_mpu6050_state *st, bool en)
|
||||
{
|
||||
struct device *pdev = regmap_get_device(st->map);
|
||||
@ -1836,6 +1922,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
|
||||
irq_type);
|
||||
return -EINVAL;
|
||||
}
|
||||
device_set_wakeup_capable(dev, true);
|
||||
|
||||
st->vdd_supply = devm_regulator_get(dev, "vdd");
|
||||
if (IS_ERR(st->vdd_supply))
|
||||
@ -2001,16 +2088,27 @@ static int inv_mpu_resume(struct device *dev)
|
||||
{
|
||||
struct iio_dev *indio_dev = dev_get_drvdata(dev);
|
||||
struct inv_mpu6050_state *st = iio_priv(indio_dev);
|
||||
bool wakeup;
|
||||
int result;
|
||||
|
||||
mutex_lock(&st->lock);
|
||||
result = inv_mpu_core_enable_regulator_vddio(st);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
guard(mutex)(&st->lock);
|
||||
|
||||
result = inv_mpu6050_set_power_itg(st, true);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
|
||||
|
||||
if (wakeup) {
|
||||
enable_irq(st->irq);
|
||||
disable_irq_wake(st->irq);
|
||||
result = inv_mpu6050_set_wom_lp(st, false);
|
||||
if (result)
|
||||
return result;
|
||||
} else {
|
||||
result = inv_mpu_core_enable_regulator_vddio(st);
|
||||
if (result)
|
||||
return result;
|
||||
result = inv_mpu6050_set_power_itg(st, true);
|
||||
if (result)
|
||||
return result;
|
||||
}
|
||||
|
||||
pm_runtime_disable(dev);
|
||||
pm_runtime_set_active(dev);
|
||||
@ -2018,20 +2116,17 @@ static int inv_mpu_resume(struct device *dev)
|
||||
|
||||
result = inv_mpu6050_switch_engine(st, true, st->suspended_sensors);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
return result;
|
||||
|
||||
if (st->chip_config.wom_en) {
|
||||
if (st->chip_config.wom_en && !wakeup) {
|
||||
result = inv_mpu6050_set_wom_int(st, true);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
return result;
|
||||
}
|
||||
|
||||
if (iio_buffer_enabled(indio_dev))
|
||||
result = inv_mpu6050_prepare_fifo(st, true);
|
||||
|
||||
out_unlock:
|
||||
mutex_unlock(&st->lock);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@ -2039,29 +2134,30 @@ static int inv_mpu_suspend(struct device *dev)
|
||||
{
|
||||
struct iio_dev *indio_dev = dev_get_drvdata(dev);
|
||||
struct inv_mpu6050_state *st = iio_priv(indio_dev);
|
||||
bool wakeup;
|
||||
int result;
|
||||
|
||||
mutex_lock(&st->lock);
|
||||
guard(mutex)(&st->lock);
|
||||
|
||||
st->suspended_sensors = 0;
|
||||
if (pm_runtime_suspended(dev)) {
|
||||
result = 0;
|
||||
goto out_unlock;
|
||||
}
|
||||
if (pm_runtime_suspended(dev))
|
||||
return 0;
|
||||
|
||||
if (iio_buffer_enabled(indio_dev)) {
|
||||
result = inv_mpu6050_prepare_fifo(st, false);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
return result;
|
||||
}
|
||||
|
||||
if (st->chip_config.wom_en) {
|
||||
wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
|
||||
|
||||
if (st->chip_config.wom_en && !wakeup) {
|
||||
result = inv_mpu6050_set_wom_int(st, false);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
return result;
|
||||
}
|
||||
|
||||
if (st->chip_config.accl_en)
|
||||
if (st->chip_config.accl_en && !wakeup)
|
||||
st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL;
|
||||
if (st->chip_config.gyro_en)
|
||||
st->suspended_sensors |= INV_MPU6050_SENSOR_GYRO;
|
||||
@ -2069,21 +2165,26 @@ static int inv_mpu_suspend(struct device *dev)
|
||||
st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP;
|
||||
if (st->chip_config.magn_en)
|
||||
st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN;
|
||||
if (st->chip_config.wom_en)
|
||||
if (st->chip_config.wom_en && !wakeup)
|
||||
st->suspended_sensors |= INV_MPU6050_SENSOR_WOM;
|
||||
result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
return result;
|
||||
|
||||
result = inv_mpu6050_set_power_itg(st, false);
|
||||
if (result)
|
||||
goto out_unlock;
|
||||
if (wakeup) {
|
||||
result = inv_mpu6050_set_wom_lp(st, true);
|
||||
if (result)
|
||||
return result;
|
||||
enable_irq_wake(st->irq);
|
||||
disable_irq(st->irq);
|
||||
} else {
|
||||
result = inv_mpu6050_set_power_itg(st, false);
|
||||
if (result)
|
||||
return result;
|
||||
inv_mpu_core_disable_regulator_vddio(st);
|
||||
}
|
||||
|
||||
inv_mpu_core_disable_regulator_vddio(st);
|
||||
out_unlock:
|
||||
mutex_unlock(&st->lock);
|
||||
|
||||
return result;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int inv_mpu_runtime_suspend(struct device *dev)
|
||||
|
||||
@ -305,6 +305,7 @@ struct inv_mpu6050_state {
|
||||
#define INV_MPU6050_REG_PWR_MGMT_1 0x6B
|
||||
#define INV_MPU6050_BIT_H_RESET 0x80
|
||||
#define INV_MPU6050_BIT_SLEEP 0x40
|
||||
#define INV_MPU6050_BIT_CYCLE 0x20
|
||||
#define INV_MPU6050_BIT_TEMP_DIS 0x08
|
||||
#define INV_MPU6050_BIT_CLK_MASK 0x7
|
||||
|
||||
@ -336,6 +337,7 @@ struct inv_mpu6050_state {
|
||||
/* mpu6500 registers */
|
||||
#define INV_MPU6500_REG_ACCEL_CONFIG_2 0x1D
|
||||
#define INV_ICM20689_BITS_FIFO_SIZE_MAX 0xC0
|
||||
#define INV_MPU6500_REG_LP_ODR 0x1E
|
||||
#define INV_MPU6500_REG_WOM_THRESHOLD 0x1F
|
||||
#define INV_MPU6500_REG_ACCEL_INTEL_CTRL 0x69
|
||||
#define INV_MPU6500_BIT_ACCEL_INTEL_EN BIT(7)
|
||||
@ -452,6 +454,18 @@ enum inv_mpu6050_filter_e {
|
||||
NUM_MPU6050_FILTER
|
||||
};
|
||||
|
||||
enum inv_mpu6050_lposc_e {
|
||||
INV_MPU6050_LPOSC_4HZ = 4,
|
||||
INV_MPU6050_LPOSC_8HZ,
|
||||
INV_MPU6050_LPOSC_16HZ,
|
||||
INV_MPU6050_LPOSC_31HZ,
|
||||
INV_MPU6050_LPOSC_62HZ,
|
||||
INV_MPU6050_LPOSC_125HZ,
|
||||
INV_MPU6050_LPOSC_250HZ,
|
||||
INV_MPU6050_LPOSC_500HZ,
|
||||
NUM_MPU6050_LPOSC,
|
||||
};
|
||||
|
||||
/* IIO attribute address */
|
||||
enum INV_MPU6050_IIO_ATTR_ADDR {
|
||||
ATTR_GYRO_MATRIX,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user