diff options
| -rw-r--r-- | drivers/iio/common/inv_sensors/inv_sensors_timestamp.c | 51 |
1 files changed, 19 insertions, 32 deletions
diff --git a/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c index ecfd54ffe7e9..2aaaa8df6d03 100644 --- a/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c +++ b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c @@ -10,9 +10,7 @@ #include <linux/iio/common/inv_sensors_timestamp.h> -/* compute jitter, min and max following jitter in per mille */ -#define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \ - (div_s64((_val) * (_jitter), 1000)) +/* compute min and max following jitter in per mille */ #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \ (((_val) * (1000 - (_jitter))) / 1000) #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \ @@ -102,34 +100,22 @@ static bool inv_update_chip_period(struct inv_sensors_timestamp *ts, /* update chip internal period estimation */ new_chip_period = period / ts->mult; inv_update_acc(&ts->chip_period, new_chip_period); - ts->period = ts->mult * ts->chip_period.val; return true; } -static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) +static u32 inv_align_timestamp_it(struct inv_sensors_timestamp *ts, + unsigned int sample_nb) { const s64 period_min = (s64)ts->min_period * ts->mult; const s64 period_max = (s64)ts->max_period * ts->mult; - s64 add_max, sub_max; - s64 delta, jitter; - s64 adjust; - - /* delta time between last sample and last interrupt */ - delta = ts->it.lo - ts->timestamp; - - /* adjust timestamp while respecting jitter */ - add_max = period_max - (s64)ts->period; - sub_max = period_min - (s64)ts->period; - jitter = INV_SENSORS_TIMESTAMP_JITTER((s64)ts->period, ts->chip.jitter); - if (delta > jitter) - adjust = add_max; - else if (delta < -jitter) - adjust = sub_max; - else - adjust = 0; + s64 new_period; + + /* compute new period aligning last timestamp with interrupt timestamp */ + new_period = div_s64(ts->it.up - ts->timestamp, sample_nb); - ts->timestamp += adjust; + /* ensure that period never overflows the jitter */ + return clamp(new_period, period_min, period_max); } void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, @@ -143,6 +129,13 @@ void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, if (sample_nb == 0) return; + /* no previous data, compute theoretical value from interrupt */ + if (ts->timestamp == 0) { + /* elapsed time: sensor period * sensor samples number */ + interval = (s64)ts->period * (s64)sample_nb; + ts->timestamp = timestamp - interval; + } + /* update interrupt timestamp and compute chip and sensor periods */ it = &ts->it; it->lo = it->up; @@ -154,17 +147,11 @@ void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, valid = inv_update_chip_period(ts, period); } - /* no previous data, compute theoretical value from interrupt */ - if (ts->timestamp == 0) { - /* elapsed time: sensor period * sensor samples number */ - interval = (s64)ts->period * (s64)sample_nb; - ts->timestamp = it->up - interval; - return; - } - /* if interrupt interval is valid, sync with interrupt timestamp */ if (valid) - inv_align_timestamp_it(ts); + ts->period = inv_align_timestamp_it(ts, sample_nb); + else + ts->period = ts->mult * ts->chip_period.val; } EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP"); |