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 | 19 |
1 files changed, 17 insertions, 2 deletions
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c index 63b85ec88c13..a8cf74c84c3c 100644 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c +++ b/drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c @@ -222,10 +222,15 @@ 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) @@ -241,6 +246,13 @@ int inv_icm42600_buffer_update_watermark(struct inv_icm42600_state *st) watermark = latency / period; 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 */ @@ -514,7 +526,7 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) /* handle gyroscope timestamp and FIFO data parsing */ if (st->fifo.nb.gyro > 0) { ts = &gyro_st->ts; - inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, + inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro, st->timestamp.gyro); ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); if (ret) @@ -524,7 +536,7 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st) /* handle accelerometer timestamp and FIFO data parsing */ if (st->fifo.nb.accel > 0) { ts = &accel_st->ts; - inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, + inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel, st->timestamp.accel); ret = inv_icm42600_accel_parse_fifo(st->indio_accel); if (ret) @@ -577,6 +589,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 |