summaryrefslogtreecommitdiff
path: root/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c')
-rw-r--r--drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c161
1 files changed, 77 insertions, 84 deletions
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
index 32d7f8364230..ada968be954d 100644
--- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
+++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
@@ -5,15 +5,17 @@
#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>
#include <linux/delay.h>
-#include <linux/iio/iio.h>
+
#include <linux/iio/buffer.h>
+#include <linux/iio/common/inv_sensors_timestamp.h>
+#include <linux/iio/iio.h>
#include "inv_icm42600.h"
-#include "inv_icm42600_timestamp.h"
#include "inv_icm42600_buffer.h"
/* FIFO header: 1 byte */
@@ -25,28 +27,28 @@
#define INV_ICM42600_FIFO_HEADER_ODR_GYRO BIT(0)
struct inv_icm42600_fifo_1sensor_packet {
- uint8_t header;
+ u8 header;
struct inv_icm42600_fifo_sensor_data data;
- int8_t temp;
+ s8 temp;
} __packed;
#define INV_ICM42600_FIFO_1SENSOR_PACKET_SIZE 8
struct inv_icm42600_fifo_2sensors_packet {
- uint8_t header;
+ u8 header;
struct inv_icm42600_fifo_sensor_data accel;
struct inv_icm42600_fifo_sensor_data gyro;
- int8_t temp;
+ s8 temp;
__be16 timestamp;
} __packed;
#define INV_ICM42600_FIFO_2SENSORS_PACKET_SIZE 16
ssize_t inv_icm42600_fifo_decode_packet(const void *packet, const void **accel,
- const void **gyro, const int8_t **temp,
+ const void **gyro, const s8 **temp,
const void **timestamp, unsigned int *odr)
{
const struct inv_icm42600_fifo_1sensor_packet *pack1 = packet;
const struct inv_icm42600_fifo_2sensors_packet *pack2 = packet;
- uint8_t header = *((const uint8_t *)packet);
+ u8 header = *((const u8 *)packet);
/* FIFO empty */
if (header & INV_ICM42600_FIFO_HEADER_MSG) {
@@ -99,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)
{
- uint32_t 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);
@@ -111,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,
@@ -203,8 +200,8 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
{
size_t packet_size, wm_size;
unsigned int wm_gyro, wm_accel, watermark;
- uint32_t period_gyro, period_accel, period;
- uint32_t latency_gyro, latency_accel, latency;
+ u32 period_gyro, period_accel;
+ u32 latency_gyro, latency_accel, latency;
bool restore;
__le16 raw_wm;
int ret;
@@ -221,25 +218,32 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
latency_accel = period_accel * wm_accel;
/* 0 value for watermark means that the sensor is turned off */
+ if (wm_gyro == 0 && wm_accel == 0)
+ return 0;
+
if (latency_gyro == 0) {
watermark = wm_accel;
+ st->fifo.watermark.eff_accel = wm_accel;
} else if (latency_accel == 0) {
watermark = wm_gyro;
+ st->fifo.watermark.eff_gyro = wm_gyro;
} else {
/* compute the smallest latency that is a multiple of both */
if (latency_gyro <= latency_accel)
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 */
+ st->fifo.watermark.eff_gyro = latency / period_gyro;
+ if (st->fifo.watermark.eff_gyro < 1)
+ st->fifo.watermark.eff_gyro = 1;
+ st->fifo.watermark.eff_accel = latency / period_accel;
+ if (st->fifo.watermark.eff_accel < 1)
+ st->fifo.watermark.eff_accel = 1;
}
/* compute watermark value in bytes */
@@ -261,9 +265,8 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st)
/* restore watermark interrupt */
if (restore) {
- ret = regmap_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
- INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
- INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
+ ret = regmap_set_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
+ INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
if (ret)
return ret;
}
@@ -275,13 +278,13 @@ static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
{
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
struct device *dev = regmap_get_device(st->map);
- struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
+ struct inv_icm42600_sensor_state *sensor_st = iio_priv(indio_dev);
+ struct inv_sensors_timestamp *ts = &sensor_st->ts;
pm_runtime_get_sync(dev);
- mutex_lock(&st->lock);
- inv_icm42600_timestamp_reset(ts);
- mutex_unlock(&st->lock);
+ guard(mutex)(&st->lock);
+ inv_sensors_timestamp_reset(ts);
return 0;
}
@@ -295,44 +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_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
- INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN,
- INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN);
+ 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)
@@ -340,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_update_bits(st->map, INV_ICM42600_REG_INT_SOURCE0,
- INV_ICM42600_INT_SOURCE0_FIFO_THS_INT1_EN, 0);
+ 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)
@@ -410,7 +404,7 @@ static int inv_icm42600_buffer_postdisable(struct iio_dev *indio_dev)
conf.mode = INV_ICM42600_SENSOR_MODE_OFF;
if (sensor == INV_ICM42600_SENSOR_GYRO)
ret = inv_icm42600_set_gyro_conf(st, &conf, &sleep_sensor);
- else
+ else if (!st->apex.on)
ret = inv_icm42600_set_accel_conf(st, &conf, &sleep_sensor);
if (ret)
goto out_unlock;
@@ -423,14 +417,10 @@ out_unlock:
mutex_unlock(&st->lock);
/* sleep maximum required time */
- if (sleep_sensor > sleep_temp)
- sleep = sleep_sensor;
- else
- sleep = sleep_temp;
+ sleep = max(sleep_sensor, sleep_temp);
if (sleep)
msleep(sleep);
- pm_runtime_mark_last_busy(dev);
pm_runtime_put_autosuspend(dev);
return ret;
@@ -450,7 +440,7 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
__be16 *raw_fifo_count;
ssize_t i, size;
const void *accel, *gyro, *timestamp;
- const int8_t *temp;
+ const s8 *temp;
unsigned int odr;
int ret;
@@ -504,27 +494,29 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
{
- struct inv_icm42600_timestamp *ts;
+ struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
+ struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel);
+ struct inv_sensors_timestamp *ts;
int ret;
if (st->fifo.nb.total == 0)
return 0;
/* handle gyroscope timestamp and FIFO data parsing */
- ts = iio_priv(st->indio_gyro);
- inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
- st->fifo.nb.gyro, st->timestamp.gyro);
if (st->fifo.nb.gyro > 0) {
+ ts = &gyro_st->ts;
+ inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro,
+ st->timestamp.gyro);
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
/* handle accelerometer timestamp and FIFO data parsing */
- ts = iio_priv(st->indio_accel);
- inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
- st->fifo.nb.accel, st->timestamp.accel);
if (st->fifo.nb.accel > 0) {
+ ts = &accel_st->ts;
+ inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel,
+ st->timestamp.accel);
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
return ret;
@@ -536,8 +528,10 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
unsigned int count)
{
- struct inv_icm42600_timestamp *ts;
- int64_t gyro_ts, accel_ts;
+ struct inv_icm42600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
+ struct inv_icm42600_sensor_state *accel_st = iio_priv(st->indio_accel);
+ struct inv_sensors_timestamp *ts;
+ s64 gyro_ts, accel_ts;
int ret;
gyro_ts = iio_get_time_ns(st->indio_gyro);
@@ -551,20 +545,16 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
return 0;
if (st->fifo.nb.gyro > 0) {
- ts = iio_priv(st->indio_gyro);
- inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
- st->fifo.nb.total, st->fifo.nb.gyro,
- gyro_ts);
+ ts = &gyro_st->ts;
+ inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts);
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret)
return ret;
}
if (st->fifo.nb.accel > 0) {
- ts = iio_priv(st->indio_accel);
- inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
- st->fifo.nb.total, st->fifo.nb.accel,
- accel_ts);
+ ts = &accel_st->ts;
+ inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts);
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret)
return ret;
@@ -578,6 +568,9 @@ int inv_icm42600_buffer_init(struct inv_icm42600_state *st)
unsigned int val;
int ret;
+ st->fifo.watermark.eff_gyro = 1;
+ st->fifo.watermark.eff_accel = 1;
+
/*
* Default FIFO configuration (bits 7 to 5)
* - use invalid value
generated by cgit (git 2.34.1) at 2025-12-26 07:01:34 +0000