diff options
Diffstat (limited to 'drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c')
| -rw-r--r-- | drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c | 65 |
1 files changed, 23 insertions, 42 deletions
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c index 7c4ed981db04..ada968be954d 100644 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c @@ -5,6 +5,7 @@ #include <linux/kernel.h> #include <linux/device.h> +#include <linux/minmax.h> #include <linux/mutex.h> #include <linux/pm_runtime.h> #include <linux/regmap.h> @@ -100,7 +101,7 @@ ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel, void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st) { - u32 period_gyro, period_accel, period; + u32 period_gyro, period_accel; if (st->fifo.en & INV_ICM42600_SENSOR_GYRO) period_gyro = inv_icm42600_odr_to_period(st->conf.gyro.odr); @@ -112,12 +113,7 @@ void inv_icm42600_buffer_update_fifo_period(struct inv_icm42600_state *st) else period_accel = U32_MAX; - if (period_gyro <= period_accel) - period = period_gyro; - else - period = period_accel; - - st->fifo.period = period; + st->fifo.period = min(period_gyro, period_accel); } int inv_icm42600_buffer_set_fifo_en(struct inv_icm42600_state *st, @@ -204,7 +200,7 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) { size_t packet_size, wm_size; unsigned int wm_gyro, wm_accel, watermark; - u32 period_gyro, period_accel, period; + u32 period_gyro, period_accel; u32 latency_gyro, latency_accel, latency; bool restore; __le16 raw_wm; @@ -237,13 +233,8 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) latency = latency_gyro - (latency_accel % latency_gyro); else latency = latency_accel - (latency_gyro % latency_accel); - /* use the shortest period */ - if (period_gyro <= period_accel) - period = period_gyro; - else - period = period_accel; /* all this works because periods are multiple of each others */ - watermark = latency / period; + watermark = latency / min(period_gyro, period_accel); if (watermark < 1) watermark = 1; /* update effective watermark */ @@ -292,9 +283,8 @@ static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev) pm_runtime_get_sync(dev); - mutex_lock(&st->lock); + guard(mutex)(&st->lock); inv_sensors_timestamp_reset(ts); - mutex_unlock(&st->lock); return 0; } @@ -308,43 +298,39 @@ static int inv_icm42600_buffer_postenable(struct iio_dev *indio_dev) struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); int ret; - mutex_lock(&st->lock); + guard(mutex)(&st->lock); - /* exit if FIFO is already on */ if (st->fifo.on) { - ret = 0; - goto out_on; + st->fifo.on++; + return 0; } /* set FIFO threshold interrupt */ ret = regmap_set_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); if (ret) - goto out_unlock; + return ret; /* flush FIFO data */ ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); if (ret) - goto out_unlock; + return ret; /* set FIFO in streaming mode */ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, INV_ICM42600_FIFO_CONFIG_STREAM); if (ret) - goto out_unlock; + return ret; /* workaround: first read of FIFO count after reset is always 0 */ ret = regmap_bulk_read(st->map, INV_ICM42600_REG_FIFO_COUNT, st->buffer, 2); if (ret) - goto out_unlock; + return ret; -out_on: - /* increase FIFO on counter */ st->fifo.on++; -out_unlock: - mutex_unlock(&st->lock); - return ret; + + return 0; } static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev) @@ -352,38 +338,34 @@ static int inv_icm42600_buffer_predisable(struct iio_dev *indio_dev) struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev); int ret; - mutex_lock(&st->lock); + guard(mutex)(&st->lock); - /* exit if there are several sensors using the FIFO */ if (st->fifo.on > 1) { - ret = 0; - goto out_off; + st->fifo.on--; + return 0; } /* set FIFO in bypass mode */ ret = regmap_write(st->map, INV_ICM42600_REG_FIFO_CONFIG, INV_ICM42600_FIFO_CONFIG_BYPASS); if (ret) - goto out_unlock; + return ret; /* flush FIFO data */ ret = regmap_write(st->map, INV_ICM42600_REG_SIGNAL_PATH_RESET, INV_ICM42600_SIGNAL_PATH_RESET_FIFO_FLUSH); if (ret) - goto out_unlock; + return ret; /* disable FIFO threshold interrupt */ ret = regmap_clear_bits(st->map, INV_ICM42600_REG_INT_SOURCE0, INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN); if (ret) - goto out_unlock; + return ret; -out_off: - /* decrease FIFO on counter */ st->fifo.on--; -out_unlock: - mutex_unlock(&st->lock); - return ret; + + return 0; } static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev) @@ -439,7 +421,6 @@ out_unlock: if (sleep) msleep(sleep); - pm_runtime_mark_last_busy(dev); pm_runtime_put_autosuspend(dev); return ret; |