diff options
Diffstat (limited to 'drivers/iio/imu/inv_mpu6050/inv_mpu_core.c')
-rw-r--r-- | drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 651 |
1 files changed, 482 insertions, 169 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index 5096fc49012d..7cb9ff3d3e94 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -16,6 +16,8 @@ #include <linux/acpi.h> #include <linux/platform_device.h> #include <linux/regulator/consumer.h> +#include <linux/pm.h> +#include <linux/pm_runtime.h> #include "inv_mpu_iio.h" #include "inv_mpu_magn.h" @@ -99,9 +101,31 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = { }; static const struct inv_mpu6050_chip_config chip_config_6050 = { + .clk = INV_CLK_INTERNAL, .fsr = INV_MPU6050_FSR_2000DPS, .lpf = INV_MPU6050_FILTER_20HZ, - .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE), + .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50), + .gyro_en = true, + .accl_en = true, + .temp_en = true, + .magn_en = false, + .gyro_fifo_enable = false, + .accl_fifo_enable = false, + .temp_fifo_enable = false, + .magn_fifo_enable = false, + .accl_fs = INV_MPU6050_FS_02G, + .user_ctrl = 0, +}; + +static const struct inv_mpu6050_chip_config chip_config_6500 = { + .clk = INV_CLK_PLL, + .fsr = INV_MPU6050_FSR_2000DPS, + .lpf = INV_MPU6050_FILTER_20HZ, + .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50), + .gyro_en = true, + .accl_en = true, + .temp_en = true, + .magn_en = false, .gyro_fifo_enable = false, .accl_fifo_enable = false, .temp_fifo_enable = false, @@ -124,7 +148,7 @@ static const struct inv_mpu6050_hw hw_info[] = { .whoami = INV_MPU6500_WHOAMI_VALUE, .name = "MPU6500", .reg = ®_set_6500, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 512, .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, }, @@ -132,7 +156,7 @@ static const struct inv_mpu6050_hw hw_info[] = { .whoami = INV_MPU6515_WHOAMI_VALUE, .name = "MPU6515", .reg = ®_set_6500, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 512, .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, }, @@ -156,7 +180,7 @@ static const struct inv_mpu6050_hw hw_info[] = { .whoami = INV_MPU9250_WHOAMI_VALUE, .name = "MPU9250", .reg = ®_set_6500, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 512, .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, }, @@ -164,7 +188,7 @@ static const struct inv_mpu6050_hw hw_info[] = { .whoami = INV_MPU9255_WHOAMI_VALUE, .name = "MPU9255", .reg = ®_set_6500, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 512, .temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE}, }, @@ -172,104 +196,242 @@ static const struct inv_mpu6050_hw hw_info[] = { .whoami = INV_ICM20608_WHOAMI_VALUE, .name = "ICM20608", .reg = ®_set_6500, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 512, .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, }, { + .whoami = INV_ICM20609_WHOAMI_VALUE, + .name = "ICM20609", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 4 * 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + }, + { + .whoami = INV_ICM20689_WHOAMI_VALUE, + .name = "ICM20689", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 4 * 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + }, + { .whoami = INV_ICM20602_WHOAMI_VALUE, .name = "ICM20602", .reg = ®_set_icm20602, - .config = &chip_config_6050, + .config = &chip_config_6500, .fifo_size = 1008, .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, }, + { + .whoami = INV_ICM20690_WHOAMI_VALUE, + .name = "ICM20690", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 1024, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + }, + { + .whoami = INV_IAM20680_WHOAMI_VALUE, + .name = "IAM20680", + .reg = ®_set_6500, + .config = &chip_config_6500, + .fifo_size = 512, + .temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE}, + }, }; -int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask) +static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep, + int clock, int temp_dis) { - unsigned int d, mgmt_1; - int result; - /* - * switch clock needs to be careful. Only when gyro is on, can - * clock source be switched to gyro. Otherwise, it must be set to - * internal clock - */ - if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) { - result = regmap_read(st->map, st->reg->pwr_mgmt_1, &mgmt_1); - if (result) - return result; + u8 val; + + if (clock < 0) + clock = st->chip_config.clk; + if (temp_dis < 0) + temp_dis = !st->chip_config.temp_en; - mgmt_1 &= ~INV_MPU6050_BIT_CLK_MASK; + val = clock & INV_MPU6050_BIT_CLK_MASK; + if (temp_dis) + val |= INV_MPU6050_BIT_TEMP_DIS; + if (sleep) + val |= INV_MPU6050_BIT_SLEEP; + + 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); +} + +static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st, + unsigned int clock) +{ + int ret; + + switch (st->chip_type) { + case INV_MPU6050: + case INV_MPU6000: + case INV_MPU9150: + /* old chips: switch clock manually */ + ret = inv_mpu6050_pwr_mgmt_1_write(st, false, clock, -1); + if (ret) + return ret; + st->chip_config.clk = clock; + break; + default: + /* automatic clock switching, nothing to do */ + break; } - if ((mask == INV_MPU6050_BIT_PWR_GYRO_STBY) && (!en)) { - /* - * turning off gyro requires switch to internal clock first. - * Then turn off gyro engine - */ - mgmt_1 |= INV_CLK_INTERNAL; - result = regmap_write(st->map, st->reg->pwr_mgmt_1, mgmt_1); - if (result) - return result; + return 0; +} + +int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, + unsigned int mask) +{ + unsigned int sleep; + u8 pwr_mgmt2, user_ctrl; + int ret; + + /* delete useless requests */ + if (mask & INV_MPU6050_SENSOR_ACCL && en == st->chip_config.accl_en) + mask &= ~INV_MPU6050_SENSOR_ACCL; + if (mask & INV_MPU6050_SENSOR_GYRO && en == st->chip_config.gyro_en) + mask &= ~INV_MPU6050_SENSOR_GYRO; + if (mask & INV_MPU6050_SENSOR_TEMP && en == st->chip_config.temp_en) + mask &= ~INV_MPU6050_SENSOR_TEMP; + if (mask & INV_MPU6050_SENSOR_MAGN && en == st->chip_config.magn_en) + mask &= ~INV_MPU6050_SENSOR_MAGN; + if (mask == 0) + return 0; + + /* turn on/off temperature sensor */ + if (mask & INV_MPU6050_SENSOR_TEMP) { + ret = inv_mpu6050_pwr_mgmt_1_write(st, false, -1, !en); + if (ret) + return ret; + st->chip_config.temp_en = en; } - result = regmap_read(st->map, st->reg->pwr_mgmt_2, &d); - if (result) - return result; - if (en) - d &= ~mask; - else - d |= mask; - result = regmap_write(st->map, st->reg->pwr_mgmt_2, d); - if (result) - return result; + /* update user_crtl for driving magnetometer */ + if (mask & INV_MPU6050_SENSOR_MAGN) { + user_ctrl = st->chip_config.user_ctrl; + if (en) + user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN; + else + user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN; + ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl); + if (ret) + return ret; + st->chip_config.user_ctrl = user_ctrl; + st->chip_config.magn_en = en; + } - if (en) { - /* Wait for output to stabilize */ - msleep(INV_MPU6050_TEMP_UP_TIME); - if (mask == INV_MPU6050_BIT_PWR_GYRO_STBY) { - /* switch internal clock to PLL */ - mgmt_1 |= INV_CLK_PLL; - result = regmap_write(st->map, - st->reg->pwr_mgmt_1, mgmt_1); - if (result) - return result; + /* manage accel & gyro engines */ + if (mask & (INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO)) { + /* compute power management 2 current value */ + pwr_mgmt2 = 0; + if (!st->chip_config.accl_en) + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY; + if (!st->chip_config.gyro_en) + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY; + + /* update to new requested value */ + if (mask & INV_MPU6050_SENSOR_ACCL) { + if (en) + pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_ACCL_STBY; + else + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY; + } + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (en) + pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_GYRO_STBY; + else + pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY; + } + + /* switch clock to internal when turning gyro off */ + if (mask & INV_MPU6050_SENSOR_GYRO && !en) { + ret = inv_mpu6050_clock_switch(st, INV_CLK_INTERNAL); + if (ret) + return ret; + } + + /* update sensors engine */ + dev_dbg(regmap_get_device(st->map), "pwr_mgmt_2: 0x%x\n", + pwr_mgmt2); + ret = regmap_write(st->map, st->reg->pwr_mgmt_2, pwr_mgmt2); + if (ret) + return ret; + if (mask & INV_MPU6050_SENSOR_ACCL) + st->chip_config.accl_en = en; + if (mask & INV_MPU6050_SENSOR_GYRO) + st->chip_config.gyro_en = en; + + /* compute required time to have sensors stabilized */ + sleep = 0; + if (en) { + if (mask & INV_MPU6050_SENSOR_ACCL) { + if (sleep < INV_MPU6050_ACCEL_UP_TIME) + sleep = INV_MPU6050_ACCEL_UP_TIME; + } + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (sleep < INV_MPU6050_GYRO_UP_TIME) + sleep = INV_MPU6050_GYRO_UP_TIME; + } + } else { + if (mask & INV_MPU6050_SENSOR_GYRO) { + if (sleep < INV_MPU6050_GYRO_DOWN_TIME) + sleep = INV_MPU6050_GYRO_DOWN_TIME; + } + } + if (sleep) + msleep(sleep); + + /* switch clock to PLL when turning gyro on */ + if (mask & INV_MPU6050_SENSOR_GYRO && en) { + ret = inv_mpu6050_clock_switch(st, INV_CLK_PLL); + if (ret) + return ret; } } return 0; } -int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on) +static int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, + bool power_on) { int result; - if (power_on) { - if (!st->powerup_count) { - result = regmap_write(st->map, st->reg->pwr_mgmt_1, 0); - if (result) - return result; - usleep_range(INV_MPU6050_REG_UP_TIME_MIN, - INV_MPU6050_REG_UP_TIME_MAX); - } - st->powerup_count++; - } else { - if (st->powerup_count == 1) { - result = regmap_write(st->map, st->reg->pwr_mgmt_1, - INV_MPU6050_BIT_SLEEP); - if (result) - return result; - } - st->powerup_count--; - } + result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, -1, -1); + if (result) + return result; - dev_dbg(regmap_get_device(st->map), "set power %d, count=%u\n", - power_on, st->powerup_count); + if (power_on) + usleep_range(INV_MPU6050_REG_UP_TIME_MIN, + INV_MPU6050_REG_UP_TIME_MAX); return 0; } -EXPORT_SYMBOL_GPL(inv_mpu6050_set_power_itg); + +static int inv_mpu6050_set_gyro_fsr(struct inv_mpu6050_state *st, + enum inv_mpu6050_fsr_e val) +{ + unsigned int gyro_shift; + u8 data; + + switch (st->chip_type) { + case INV_ICM20690: + gyro_shift = INV_ICM20690_GYRO_CONFIG_FSR_SHIFT; + break; + default: + gyro_shift = INV_MPU6050_GYRO_CONFIG_FSR_SHIFT; + break; + } + + data = val << gyro_shift; + return regmap_write(st->map, st->reg->gyro_config, data); +} /** * inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent @@ -286,20 +448,23 @@ static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st, if (result) return result; + /* set accel lpf */ switch (st->chip_type) { case INV_MPU6050: case INV_MPU6000: case INV_MPU9150: /* old chips, nothing to do */ - result = 0; + return 0; + case INV_ICM20689: + case INV_ICM20690: + /* set FIFO size to maximum value */ + val |= INV_ICM20689_BITS_FIFO_SIZE_MAX; break; default: - /* set accel lpf */ - result = regmap_write(st->map, st->reg->accel_lpf, val); break; } - return result; + return regmap_write(st->map, st->reg->accel_lpf, val); } /** @@ -317,35 +482,28 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) u8 d; struct inv_mpu6050_state *st = iio_priv(indio_dev); - result = inv_mpu6050_set_power_itg(st, true); + result = inv_mpu6050_set_gyro_fsr(st, st->chip_config.fsr); if (result) return result; - d = (INV_MPU6050_FSR_2000DPS << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); - result = regmap_write(st->map, st->reg->gyro_config, d); - if (result) - goto error_power_off; - result = inv_mpu6050_set_lpf_regs(st, INV_MPU6050_FILTER_20HZ); + result = inv_mpu6050_set_lpf_regs(st, st->chip_config.lpf); if (result) - goto error_power_off; + return result; - d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE); + d = st->chip_config.divider; result = regmap_write(st->map, st->reg->sample_rate_div, d); if (result) - goto error_power_off; + return result; - d = (INV_MPU6050_FS_02G << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); + d = (st->chip_config.accl_fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT); result = regmap_write(st->map, st->reg->accl_config, d); if (result) - goto error_power_off; + return result; result = regmap_write(st->map, st->reg->int_pin_cfg, st->irq_mask); if (result) return result; - memcpy(&st->chip_config, hw_info[st->chip_type].config, - sizeof(struct inv_mpu6050_chip_config)); - /* * Internal chip period is 1ms (1kHz). * Let's use at the beginning the theorical value before measuring @@ -356,13 +514,9 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) /* magn chip init, noop if not present in the chip */ result = inv_mpu_magn_probe(st); if (result) - goto error_power_off; - - return inv_mpu6050_set_power_itg(st, false); + return result; -error_power_off: - inv_mpu6050_set_power_itg(st, false); - return result; + return 0; } static int inv_mpu6050_sensor_set(struct inv_mpu6050_state *st, int reg, @@ -399,45 +553,85 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev, int *val) { struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); + unsigned int freq_hz, period_us, min_sleep_us, max_sleep_us; int result; int ret; - result = inv_mpu6050_set_power_itg(st, true); - if (result) + /* compute sample period */ + freq_hz = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider); + period_us = 1000000 / freq_hz; + + result = pm_runtime_get_sync(pdev); + if (result < 0) { + pm_runtime_put_noidle(pdev); return result; + } switch (chan->type) { case IIO_ANGL_VEL: - result = inv_mpu6050_switch_engine(st, true, - INV_MPU6050_BIT_PWR_GYRO_STBY); - if (result) - goto error_power_off; + if (!st->chip_config.gyro_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_GYRO); + if (result) + goto error_power_off; + /* need to wait 2 periods to have first valid sample */ + min_sleep_us = 2 * period_us; + max_sleep_us = 2 * (period_us + period_us / 2); + usleep_range(min_sleep_us, max_sleep_us); + } ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro, chan->channel2, val); - result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_GYRO_STBY); - if (result) - goto error_power_off; break; case IIO_ACCEL: - result = inv_mpu6050_switch_engine(st, true, - INV_MPU6050_BIT_PWR_ACCL_STBY); - if (result) - goto error_power_off; + if (!st->chip_config.accl_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_ACCL); + if (result) + goto error_power_off; + /* wait 1 period for first sample availability */ + min_sleep_us = period_us; + max_sleep_us = period_us + period_us / 2; + usleep_range(min_sleep_us, max_sleep_us); + } ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl, chan->channel2, val); - result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_ACCL_STBY); - if (result) - goto error_power_off; break; case IIO_TEMP: - /* wait for stablization */ - msleep(INV_MPU6050_SENSOR_UP_TIME); + /* temperature sensor work only with accel and/or gyro */ + if (!st->chip_config.accl_en && !st->chip_config.gyro_en) { + result = -EBUSY; + goto error_power_off; + } + if (!st->chip_config.temp_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_TEMP); + if (result) + goto error_power_off; + /* wait 1 period for first sample availability */ + min_sleep_us = period_us; + max_sleep_us = period_us + period_us / 2; + usleep_range(min_sleep_us, max_sleep_us); + } ret = inv_mpu6050_sensor_show(st, st->reg->temperature, IIO_MOD_X, val); break; case IIO_MAGN: + if (!st->chip_config.magn_en) { + result = inv_mpu6050_switch_engine(st, true, + INV_MPU6050_SENSOR_MAGN); + if (result) + goto error_power_off; + /* frequency is limited for magnetometer */ + if (freq_hz > INV_MPU_MAGN_FREQ_HZ_MAX) { + freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX; + period_us = 1000000 / freq_hz; + } + /* need to wait 2 periods to have first valid sample */ + min_sleep_us = 2 * period_us; + max_sleep_us = 2 * (period_us + period_us / 2); + usleep_range(min_sleep_us, max_sleep_us); + } ret = inv_mpu_magn_read(st, chan->channel2, val); break; default: @@ -445,14 +639,13 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev, break; } - result = inv_mpu6050_set_power_itg(st, false); - if (result) - goto error_power_off; + pm_runtime_mark_last_busy(pdev); + pm_runtime_put_autosuspend(pdev); return ret; error_power_off: - inv_mpu6050_set_power_itg(st, false); + pm_runtime_put_autosuspend(pdev); return result; } @@ -533,12 +726,10 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev, static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val) { int result, i; - u8 d; for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) { if (gyro_scale_6050[i] == val) { - d = (i << INV_MPU6050_GYRO_CONFIG_FSR_SHIFT); - result = regmap_write(st->map, st->reg->gyro_config, d); + result = inv_mpu6050_set_gyro_fsr(st, i); if (result) return result; @@ -593,6 +784,7 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, int val, int val2, long mask) { struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); int result; /* @@ -604,9 +796,11 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, return result; mutex_lock(&st->lock); - result = inv_mpu6050_set_power_itg(st, true); - if (result) + result = pm_runtime_get_sync(pdev); + if (result < 0) { + pm_runtime_put_noidle(pdev); goto error_write_raw_unlock; + } switch (mask) { case IIO_CHAN_INFO_SCALE: @@ -644,7 +838,8 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, break; } - result |= inv_mpu6050_set_power_itg(st, false); + pm_runtime_mark_last_busy(pdev); + pm_runtime_put_autosuspend(pdev); error_write_raw_unlock: mutex_unlock(&st->lock); iio_device_release_direct_mode(indio_dev); @@ -655,30 +850,32 @@ error_write_raw_unlock: /** * inv_mpu6050_set_lpf() - set low pass filer based on fifo rate. * - * Based on the Nyquist principle, the sampling rate must - * exceed twice of the bandwidth of the signal, or there - * would be alising. This function basically search for the - * correct low pass parameters based on the fifo rate, e.g, - * sampling frequency. + * Based on the Nyquist principle, the bandwidth of the low + * pass filter must not exceed the signal sampling rate divided + * by 2, or there would be aliasing. + * This function basically search for the correct low pass + * parameters based on the fifo rate, e.g, sampling frequency. * * lpf is set automatically when setting sampling rate to avoid any aliases. */ static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate) { - static const int hz[] = {188, 98, 42, 20, 10, 5}; + static const int hz[] = {400, 200, 90, 40, 20, 10}; static const int d[] = { - INV_MPU6050_FILTER_188HZ, INV_MPU6050_FILTER_98HZ, - INV_MPU6050_FILTER_42HZ, INV_MPU6050_FILTER_20HZ, + INV_MPU6050_FILTER_200HZ, INV_MPU6050_FILTER_100HZ, + INV_MPU6050_FILTER_45HZ, INV_MPU6050_FILTER_20HZ, INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ }; - int i, h, result; + int i, result; u8 data; - h = (rate >> 1); - i = 0; - while ((h < hz[i]) && (i < ARRAY_SIZE(d) - 1)) - i++; - data = d[i]; + data = INV_MPU6050_FILTER_5HZ; + for (i = 0; i < ARRAY_SIZE(hz); ++i) { + if (rate >= hz[i]) { + data = d[i]; + break; + } + } result = inv_mpu6050_set_lpf_regs(st, data); if (result) return result; @@ -699,6 +896,7 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, int result; struct iio_dev *indio_dev = dev_to_iio_dev(dev); struct inv_mpu6050_state *st = iio_priv(indio_dev); + struct device *pdev = regmap_get_device(st->map); if (kstrtoint(buf, 10, &fifo_rate)) return -EINVAL; @@ -706,10 +904,6 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, fifo_rate > INV_MPU6050_MAX_FIFO_RATE) return -EINVAL; - result = iio_device_claim_direct_mode(indio_dev); - if (result) - return result; - /* compute the chip sample rate divider */ d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate); /* compute back the fifo rate to handle truncation cases */ @@ -720,9 +914,11 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, result = 0; goto fifo_rate_fail_unlock; } - result = inv_mpu6050_set_power_itg(st, true); - if (result) + result = pm_runtime_get_sync(pdev); + if (result < 0) { + pm_runtime_put_noidle(pdev); goto fifo_rate_fail_unlock; + } result = regmap_write(st->map, st->reg->sample_rate_div, d); if (result) @@ -738,11 +934,11 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, if (result) goto fifo_rate_fail_power_off; + pm_runtime_mark_last_busy(pdev); fifo_rate_fail_power_off: - result |= inv_mpu6050_set_power_itg(st, false); + pm_runtime_put_autosuspend(pdev); fifo_rate_fail_unlock: mutex_unlock(&st->lock); - iio_device_release_direct_mode(indio_dev); if (result) return result; @@ -1066,11 +1262,13 @@ static const struct iio_info mpu_info = { static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) { int result; - unsigned int regval; + unsigned int regval, mask; int i; st->hw = &hw_info[st->chip_type]; st->reg = hw_info[st->chip_type].reg; + memcpy(&st->chip_config, hw_info[st->chip_type].config, + sizeof(st->chip_config)); /* check chip self-identification */ result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, ®val); @@ -1102,6 +1300,24 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) if (result) return result; msleep(INV_MPU6050_POWER_UP_TIME); + switch (st->chip_type) { + case INV_MPU6000: + case INV_MPU6500: + case INV_MPU6515: + case INV_MPU9250: + case INV_MPU9255: + /* reset signal path (required for spi connection) */ + regval = INV_MPU6050_BIT_TEMP_RST | INV_MPU6050_BIT_ACCEL_RST | + INV_MPU6050_BIT_GYRO_RST; + result = regmap_write(st->map, INV_MPU6050_REG_SIGNAL_PATH_RESET, + regval); + if (result) + return result; + msleep(INV_MPU6050_POWER_UP_TIME); + break; + default: + break; + } /* * Turn power on. After reset, the sleep bit could be on @@ -1112,17 +1328,13 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st) result = inv_mpu6050_set_power_itg(st, true); if (result) return result; - - result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_ACCL_STBY); - if (result) - goto error_power_off; - result = inv_mpu6050_switch_engine(st, false, - INV_MPU6050_BIT_PWR_GYRO_STBY); + mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO | + INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN; + result = inv_mpu6050_switch_engine(st, false, mask); if (result) goto error_power_off; - return inv_mpu6050_set_power_itg(st, false); + return 0; error_power_off: inv_mpu6050_set_power_itg(st, false); @@ -1139,7 +1351,7 @@ static int inv_mpu_core_enable_regulator_vddio(struct inv_mpu6050_state *st) "Failed to enable vddio regulator: %d\n", result); } else { /* Give the device a little bit of time to start up. */ - usleep_range(35000, 70000); + usleep_range(3000, 5000); } return result; @@ -1170,6 +1382,14 @@ static void inv_mpu_core_disable_regulator_action(void *_data) inv_mpu_core_disable_regulator_vddio(st); } +static void inv_mpu_pm_disable(void *data) +{ + struct device *dev = data; + + pm_runtime_put_sync_suspend(dev); + pm_runtime_disable(dev); +} + int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type) { @@ -1194,7 +1414,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, st = iio_priv(indio_dev); mutex_init(&st->lock); st->chip_type = chip_type; - st->powerup_count = 0; st->irq = irq; st->map = regmap; @@ -1259,6 +1478,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, dev_err(dev, "Failed to enable vdd regulator: %d\n", result); return result; } + msleep(INV_MPU6050_POWER_UP_TIME); result = inv_mpu_core_enable_regulator_vddio(st); if (result) { @@ -1287,7 +1507,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, result = inv_mpu6050_init_config(indio_dev); if (result) { dev_err(dev, "Could not initialize device.\n"); - return result; + goto error_power_off; } dev_set_drvdata(dev, indio_dev); @@ -1299,8 +1519,24 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, indio_dev->name = dev_name(dev); /* requires parent device set in indio_dev */ - if (inv_mpu_bus_setup) - inv_mpu_bus_setup(indio_dev); + if (inv_mpu_bus_setup) { + result = inv_mpu_bus_setup(indio_dev); + if (result) + goto error_power_off; + } + + /* chip init is done, turning on runtime power management */ + result = pm_runtime_set_active(dev); + if (result) + goto error_power_off; + pm_runtime_get_noresume(dev); + pm_runtime_enable(dev); + pm_runtime_set_autosuspend_delay(dev, INV_MPU6050_SUSPEND_DELAY_MS); + pm_runtime_use_autosuspend(dev); + pm_runtime_put(dev); + result = devm_add_action_or_reset(dev, inv_mpu_pm_disable, dev); + if (result) + return result; switch (chip_type) { case INV_MPU9150: @@ -1359,14 +1595,17 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, } return 0; + +error_power_off: + inv_mpu6050_set_power_itg(st, false); + return result; } EXPORT_SYMBOL_GPL(inv_mpu_core_probe); -#ifdef CONFIG_PM_SLEEP - -static int inv_mpu_resume(struct device *dev) +static int __maybe_unused inv_mpu_resume(struct device *dev) { - struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); int result; mutex_lock(&st->lock); @@ -1375,27 +1614,101 @@ static int inv_mpu_resume(struct device *dev) goto out_unlock; result = inv_mpu6050_set_power_itg(st, true); + if (result) + goto out_unlock; + + result = inv_mpu6050_switch_engine(st, true, st->suspended_sensors); + if (result) + goto out_unlock; + + if (iio_buffer_enabled(indio_dev)) + result = inv_mpu6050_prepare_fifo(st, true); + out_unlock: mutex_unlock(&st->lock); return result; } -static int inv_mpu_suspend(struct device *dev) +static int __maybe_unused inv_mpu_suspend(struct device *dev) { - struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + struct iio_dev *indio_dev = dev_get_drvdata(dev); + struct inv_mpu6050_state *st = iio_priv(indio_dev); int result; mutex_lock(&st->lock); + + if (iio_buffer_enabled(indio_dev)) { + result = inv_mpu6050_prepare_fifo(st, false); + if (result) + goto out_unlock; + } + + st->suspended_sensors = 0; + if (st->chip_config.accl_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL; + if (st->chip_config.gyro_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_GYRO; + if (st->chip_config.temp_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP; + if (st->chip_config.magn_en) + st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN; + result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors); + if (result) + goto out_unlock; + result = inv_mpu6050_set_power_itg(st, false); + if (result) + goto out_unlock; + inv_mpu_core_disable_regulator_vddio(st); +out_unlock: mutex_unlock(&st->lock); return result; } -#endif /* CONFIG_PM_SLEEP */ -SIMPLE_DEV_PM_OPS(inv_mpu_pmops, inv_mpu_suspend, inv_mpu_resume); +static int __maybe_unused inv_mpu_runtime_suspend(struct device *dev) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + unsigned int sensors; + int ret; + + mutex_lock(&st->lock); + + sensors = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO | + INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN; + ret = inv_mpu6050_switch_engine(st, false, sensors); + if (ret) + goto out_unlock; + + ret = inv_mpu6050_set_power_itg(st, false); + if (ret) + goto out_unlock; + + inv_mpu_core_disable_regulator_vddio(st); + +out_unlock: + mutex_unlock(&st->lock); + return ret; +} + +static int __maybe_unused inv_mpu_runtime_resume(struct device *dev) +{ + struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev)); + int ret; + + ret = inv_mpu_core_enable_regulator_vddio(st); + if (ret) + return ret; + + return inv_mpu6050_set_power_itg(st, true); +} + +const struct dev_pm_ops inv_mpu_pmops = { + SET_SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume) + SET_RUNTIME_PM_OPS(inv_mpu_runtime_suspend, inv_mpu_runtime_resume, NULL) +}; EXPORT_SYMBOL_GPL(inv_mpu_pmops); MODULE_AUTHOR("Invensense Corporation"); |