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 | 37 |
1 files changed, 25 insertions, 12 deletions
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c index 42618fe4f83e..d80ef468508a 100644 --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c @@ -20,8 +20,6 @@ #include <linux/jiffies.h> #include <linux/irq.h> #include <linux/interrupt.h> -#include <linux/kfifo.h> -#include <linux/spinlock.h> #include <linux/iio/iio.h> #include <linux/acpi.h> #include <linux/platform_device.h> @@ -84,7 +82,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = { static const struct inv_mpu6050_chip_config chip_config_6050 = { .fsr = INV_MPU6050_FSR_2000DPS, .lpf = INV_MPU6050_FILTER_20HZ, - .fifo_rate = INV_MPU6050_INIT_FIFO_RATE, + .divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE), .gyro_fifo_enable = false, .accl_fifo_enable = false, .accl_fs = INV_MPU6050_FS_02G, @@ -106,6 +104,12 @@ static const struct inv_mpu6050_hw hw_info[] = { .config = &chip_config_6050, }, { + .whoami = INV_MPU6515_WHOAMI_VALUE, + .name = "MPU6515", + .reg = ®_set_6500, + .config = &chip_config_6050, + }, + { .whoami = INV_MPU6000_WHOAMI_VALUE, .name = "MPU6000", .reg = ®_set_6050, @@ -280,7 +284,7 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) if (result) goto error_power_off; - d = INV_MPU6050_ONE_K_HZ / INV_MPU6050_INIT_FIFO_RATE - 1; + d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE); result = regmap_write(st->map, st->reg->sample_rate_div, d); if (result) goto error_power_off; @@ -297,6 +301,13 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev) 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 + * with interrupt timestamps. + */ + st->chip_period = NSEC_PER_MSEC; + return inv_mpu6050_set_power_itg(st, false); error_power_off: @@ -630,7 +641,7 @@ static ssize_t inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count) { - s32 fifo_rate; + int fifo_rate; u8 d; int result; struct iio_dev *indio_dev = dev_to_iio_dev(dev); @@ -646,8 +657,13 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, 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 */ + fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(d); + mutex_lock(&st->lock); - if (fifo_rate == st->chip_config.fifo_rate) { + if (d == st->chip_config.divider) { result = 0; goto fifo_rate_fail_unlock; } @@ -655,11 +671,10 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, if (result) goto fifo_rate_fail_unlock; - d = INV_MPU6050_ONE_K_HZ / fifo_rate - 1; result = regmap_write(st->map, st->reg->sample_rate_div, d); if (result) goto fifo_rate_fail_power_off; - st->chip_config.fifo_rate = fifo_rate; + st->chip_config.divider = d; result = inv_mpu6050_set_lpf(st, fifo_rate); if (result) @@ -687,7 +702,7 @@ inv_fifo_rate_show(struct device *dev, struct device_attribute *attr, unsigned fifo_rate; mutex_lock(&st->lock); - fifo_rate = st->chip_config.fifo_rate; + fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider); mutex_unlock(&st->lock); return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate); @@ -1005,7 +1020,7 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, indio_dev->modes = INDIO_BUFFER_TRIGGERED; result = devm_iio_triggered_buffer_setup(dev, indio_dev, - inv_mpu6050_irq_handler, + iio_pollfunc_store_time, inv_mpu6050_read_fifo, NULL); if (result) { @@ -1018,8 +1033,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name, return result; } - INIT_KFIFO(st->timestamps); - spin_lock_init(&st->time_stamp_lock); result = devm_iio_device_register(dev, indio_dev); if (result) { dev_err(dev, "IIO register fail %d\n", result); |