diff options
Diffstat (limited to 'drivers/iio/common')
-rw-r--r-- | drivers/iio/common/inv_sensors/inv_sensors_timestamp.c | 35 |
1 files changed, 19 insertions, 16 deletions
diff --git a/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c index 03823ee57f59..fa205f17bd90 100644 --- a/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c +++ b/drivers/iio/common/inv_sensors/inv_sensors_timestamp.c @@ -70,13 +70,13 @@ int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts, } EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP); -static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult) +static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period) { uint32_t period_min, period_max; /* check that period is acceptable */ - period_min = ts->min_period * mult; - period_max = ts->max_period * mult; + period_min = ts->min_period * ts->mult; + period_max = ts->max_period * ts->mult; if (period > period_min && period < period_max) return true; else @@ -84,15 +84,15 @@ static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t perio } static bool inv_update_chip_period(struct inv_sensors_timestamp *ts, - uint32_t mult, uint32_t period) + uint32_t period) { uint32_t new_chip_period; - if (!inv_validate_period(ts, period, mult)) + if (!inv_validate_period(ts, period)) return false; /* update chip internal period estimation */ - new_chip_period = period / mult; + new_chip_period = period / ts->mult; inv_update_acc(&ts->chip_period, new_chip_period); ts->period = ts->mult * ts->chip_period.val; @@ -101,6 +101,9 @@ static bool inv_update_chip_period(struct inv_sensors_timestamp *ts, static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) { + const int64_t period_min = ts->min_period * ts->mult; + const int64_t period_max = ts->max_period * ts->mult; + int64_t add_max, sub_max; int64_t delta, jitter; int64_t adjust; @@ -108,11 +111,13 @@ static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) delta = ts->it.lo - ts->timestamp; /* adjust timestamp while respecting jitter */ + add_max = period_max - (int64_t)ts->period; + sub_max = period_min - (int64_t)ts->period; jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter); if (delta > jitter) - adjust = jitter; + adjust = add_max; else if (delta < -jitter) - adjust = -jitter; + adjust = sub_max; else adjust = 0; @@ -120,16 +125,14 @@ static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) } void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, - uint32_t fifo_period, size_t fifo_nb, - size_t sensor_nb, int64_t timestamp) + size_t sample_nb, int64_t timestamp) { struct inv_sensors_timestamp_interval *it; int64_t delta, interval; - const uint32_t fifo_mult = fifo_period / ts->chip.clock_period; - uint32_t period = ts->period; + uint32_t period; bool valid = false; - if (fifo_nb == 0) + if (sample_nb == 0) return; /* update interrupt timestamp and compute chip and sensor periods */ @@ -139,14 +142,14 @@ void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, delta = it->up - it->lo; if (it->lo != 0) { /* compute period: delta time divided by number of samples */ - period = div_s64(delta, fifo_nb); - valid = inv_update_chip_period(ts, fifo_mult, period); + period = div_s64(delta, sample_nb); + valid = inv_update_chip_period(ts, period); } /* no previous data, compute theoritical value from interrupt */ if (ts->timestamp == 0) { /* elapsed time: sensor period * sensor samples number */ - interval = (int64_t)ts->period * (int64_t)sensor_nb; + interval = (int64_t)ts->period * (int64_t)sample_nb; ts->timestamp = it->up - interval; return; } |