10ecc363cSJean-Baptiste Maneyrol // SPDX-License-Identifier: GPL-2.0-or-later
20ecc363cSJean-Baptiste Maneyrol /*
30ecc363cSJean-Baptiste Maneyrol * Copyright (C) 2020 Invensense, Inc.
40ecc363cSJean-Baptiste Maneyrol */
50ecc363cSJean-Baptiste Maneyrol
60ecc363cSJean-Baptiste Maneyrol #include <linux/errno.h>
70ecc363cSJean-Baptiste Maneyrol #include <linux/kernel.h>
80ecc363cSJean-Baptiste Maneyrol #include <linux/math64.h>
90ecc363cSJean-Baptiste Maneyrol #include <linux/module.h>
100ecc363cSJean-Baptiste Maneyrol
110ecc363cSJean-Baptiste Maneyrol #include <linux/iio/common/inv_sensors_timestamp.h>
120ecc363cSJean-Baptiste Maneyrol
130ecc363cSJean-Baptiste Maneyrol /* compute jitter, min and max following jitter in per mille */
140ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \
150ecc363cSJean-Baptiste Maneyrol (div_s64((_val) * (_jitter), 1000))
160ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
170ecc363cSJean-Baptiste Maneyrol (((_val) * (1000 - (_jitter))) / 1000)
180ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
190ecc363cSJean-Baptiste Maneyrol (((_val) * (1000 + (_jitter))) / 1000)
200ecc363cSJean-Baptiste Maneyrol
210ecc363cSJean-Baptiste Maneyrol /* Add a new value inside an accumulator and update the estimate value */
inv_update_acc(struct inv_sensors_timestamp_acc * acc,uint32_t val)220ecc363cSJean-Baptiste Maneyrol static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, uint32_t val)
230ecc363cSJean-Baptiste Maneyrol {
240ecc363cSJean-Baptiste Maneyrol uint64_t sum = 0;
250ecc363cSJean-Baptiste Maneyrol size_t i;
260ecc363cSJean-Baptiste Maneyrol
270ecc363cSJean-Baptiste Maneyrol acc->values[acc->idx++] = val;
280ecc363cSJean-Baptiste Maneyrol if (acc->idx >= ARRAY_SIZE(acc->values))
290ecc363cSJean-Baptiste Maneyrol acc->idx = 0;
300ecc363cSJean-Baptiste Maneyrol
310ecc363cSJean-Baptiste Maneyrol /* compute the mean of all stored values, use 0 as empty slot */
320ecc363cSJean-Baptiste Maneyrol for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
330ecc363cSJean-Baptiste Maneyrol if (acc->values[i] == 0)
340ecc363cSJean-Baptiste Maneyrol break;
350ecc363cSJean-Baptiste Maneyrol sum += acc->values[i];
360ecc363cSJean-Baptiste Maneyrol }
370ecc363cSJean-Baptiste Maneyrol
380ecc363cSJean-Baptiste Maneyrol acc->val = div_u64(sum, i);
390ecc363cSJean-Baptiste Maneyrol }
400ecc363cSJean-Baptiste Maneyrol
inv_sensors_timestamp_init(struct inv_sensors_timestamp * ts,const struct inv_sensors_timestamp_chip * chip)410ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
420ecc363cSJean-Baptiste Maneyrol const struct inv_sensors_timestamp_chip *chip)
430ecc363cSJean-Baptiste Maneyrol {
440ecc363cSJean-Baptiste Maneyrol memset(ts, 0, sizeof(*ts));
450ecc363cSJean-Baptiste Maneyrol
460ecc363cSJean-Baptiste Maneyrol /* save chip parameters and compute min and max clock period */
470ecc363cSJean-Baptiste Maneyrol ts->chip = *chip;
480ecc363cSJean-Baptiste Maneyrol ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
490ecc363cSJean-Baptiste Maneyrol ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
500ecc363cSJean-Baptiste Maneyrol
510ecc363cSJean-Baptiste Maneyrol /* current multiplier and period values after reset */
520ecc363cSJean-Baptiste Maneyrol ts->mult = chip->init_period / chip->clock_period;
530ecc363cSJean-Baptiste Maneyrol ts->period = chip->init_period;
540ecc363cSJean-Baptiste Maneyrol
550ecc363cSJean-Baptiste Maneyrol /* use theoretical value for chip period */
560ecc363cSJean-Baptiste Maneyrol inv_update_acc(&ts->chip_period, chip->clock_period);
570ecc363cSJean-Baptiste Maneyrol }
580ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
590ecc363cSJean-Baptiste Maneyrol
inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp * ts,uint32_t period,bool fifo)600ecc363cSJean-Baptiste Maneyrol int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
610ecc363cSJean-Baptiste Maneyrol uint32_t period, bool fifo)
620ecc363cSJean-Baptiste Maneyrol {
63aaf6b327SJean-Baptiste Maneyrol uint32_t mult;
64aaf6b327SJean-Baptiste Maneyrol
650ecc363cSJean-Baptiste Maneyrol /* when FIFO is on, prevent odr change if one is already pending */
660ecc363cSJean-Baptiste Maneyrol if (fifo && ts->new_mult != 0)
670ecc363cSJean-Baptiste Maneyrol return -EAGAIN;
680ecc363cSJean-Baptiste Maneyrol
69aaf6b327SJean-Baptiste Maneyrol mult = period / ts->chip.clock_period;
70aaf6b327SJean-Baptiste Maneyrol if (mult != ts->mult)
71aaf6b327SJean-Baptiste Maneyrol ts->new_mult = mult;
720ecc363cSJean-Baptiste Maneyrol
730ecc363cSJean-Baptiste Maneyrol return 0;
740ecc363cSJean-Baptiste Maneyrol }
750ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
760ecc363cSJean-Baptiste Maneyrol
inv_validate_period(struct inv_sensors_timestamp * ts,uint32_t period,uint32_t mult)770ecc363cSJean-Baptiste Maneyrol static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult)
780ecc363cSJean-Baptiste Maneyrol {
790ecc363cSJean-Baptiste Maneyrol uint32_t period_min, period_max;
800ecc363cSJean-Baptiste Maneyrol
810ecc363cSJean-Baptiste Maneyrol /* check that period is acceptable */
820ecc363cSJean-Baptiste Maneyrol period_min = ts->min_period * mult;
830ecc363cSJean-Baptiste Maneyrol period_max = ts->max_period * mult;
840ecc363cSJean-Baptiste Maneyrol if (period > period_min && period < period_max)
850ecc363cSJean-Baptiste Maneyrol return true;
860ecc363cSJean-Baptiste Maneyrol else
870ecc363cSJean-Baptiste Maneyrol return false;
880ecc363cSJean-Baptiste Maneyrol }
890ecc363cSJean-Baptiste Maneyrol
inv_update_chip_period(struct inv_sensors_timestamp * ts,uint32_t mult,uint32_t period)900ecc363cSJean-Baptiste Maneyrol static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
910ecc363cSJean-Baptiste Maneyrol uint32_t mult, uint32_t period)
920ecc363cSJean-Baptiste Maneyrol {
930ecc363cSJean-Baptiste Maneyrol uint32_t new_chip_period;
940ecc363cSJean-Baptiste Maneyrol
950ecc363cSJean-Baptiste Maneyrol if (!inv_validate_period(ts, period, mult))
960ecc363cSJean-Baptiste Maneyrol return false;
970ecc363cSJean-Baptiste Maneyrol
980ecc363cSJean-Baptiste Maneyrol /* update chip internal period estimation */
990ecc363cSJean-Baptiste Maneyrol new_chip_period = period / mult;
1000ecc363cSJean-Baptiste Maneyrol inv_update_acc(&ts->chip_period, new_chip_period);
1010ecc363cSJean-Baptiste Maneyrol ts->period = ts->mult * ts->chip_period.val;
1020ecc363cSJean-Baptiste Maneyrol
1030ecc363cSJean-Baptiste Maneyrol return true;
1040ecc363cSJean-Baptiste Maneyrol }
1050ecc363cSJean-Baptiste Maneyrol
inv_align_timestamp_it(struct inv_sensors_timestamp * ts)1060ecc363cSJean-Baptiste Maneyrol static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
1070ecc363cSJean-Baptiste Maneyrol {
1086ee0c842SJean-Baptiste Maneyrol const int64_t period_min = ts->min_period * ts->mult;
1096ee0c842SJean-Baptiste Maneyrol const int64_t period_max = ts->max_period * ts->mult;
1106ee0c842SJean-Baptiste Maneyrol int64_t add_max, sub_max;
1110ecc363cSJean-Baptiste Maneyrol int64_t delta, jitter;
1120ecc363cSJean-Baptiste Maneyrol int64_t adjust;
1130ecc363cSJean-Baptiste Maneyrol
1140ecc363cSJean-Baptiste Maneyrol /* delta time between last sample and last interrupt */
1150ecc363cSJean-Baptiste Maneyrol delta = ts->it.lo - ts->timestamp;
1160ecc363cSJean-Baptiste Maneyrol
1170ecc363cSJean-Baptiste Maneyrol /* adjust timestamp while respecting jitter */
1186ee0c842SJean-Baptiste Maneyrol add_max = period_max - (int64_t)ts->period;
1196ee0c842SJean-Baptiste Maneyrol sub_max = period_min - (int64_t)ts->period;
1200ecc363cSJean-Baptiste Maneyrol jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
1210ecc363cSJean-Baptiste Maneyrol if (delta > jitter)
1226ee0c842SJean-Baptiste Maneyrol adjust = add_max;
1230ecc363cSJean-Baptiste Maneyrol else if (delta < -jitter)
1246ee0c842SJean-Baptiste Maneyrol adjust = sub_max;
1250ecc363cSJean-Baptiste Maneyrol else
1260ecc363cSJean-Baptiste Maneyrol adjust = 0;
1270ecc363cSJean-Baptiste Maneyrol
1280ecc363cSJean-Baptiste Maneyrol ts->timestamp += adjust;
1290ecc363cSJean-Baptiste Maneyrol }
1300ecc363cSJean-Baptiste Maneyrol
inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp * ts,uint32_t fifo_period,size_t fifo_nb,size_t sensor_nb,int64_t timestamp)1310ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
1320ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb,
1330ecc363cSJean-Baptiste Maneyrol size_t sensor_nb, int64_t timestamp)
1340ecc363cSJean-Baptiste Maneyrol {
1350ecc363cSJean-Baptiste Maneyrol struct inv_sensors_timestamp_interval *it;
1360ecc363cSJean-Baptiste Maneyrol int64_t delta, interval;
1370ecc363cSJean-Baptiste Maneyrol const uint32_t fifo_mult = fifo_period / ts->chip.clock_period;
1380ecc363cSJean-Baptiste Maneyrol uint32_t period = ts->period;
1390ecc363cSJean-Baptiste Maneyrol bool valid = false;
1400ecc363cSJean-Baptiste Maneyrol
1410ecc363cSJean-Baptiste Maneyrol if (fifo_nb == 0)
1420ecc363cSJean-Baptiste Maneyrol return;
1430ecc363cSJean-Baptiste Maneyrol
1440ecc363cSJean-Baptiste Maneyrol /* update interrupt timestamp and compute chip and sensor periods */
1450ecc363cSJean-Baptiste Maneyrol it = &ts->it;
1460ecc363cSJean-Baptiste Maneyrol it->lo = it->up;
1470ecc363cSJean-Baptiste Maneyrol it->up = timestamp;
1480ecc363cSJean-Baptiste Maneyrol delta = it->up - it->lo;
1490ecc363cSJean-Baptiste Maneyrol if (it->lo != 0) {
1500ecc363cSJean-Baptiste Maneyrol /* compute period: delta time divided by number of samples */
1510ecc363cSJean-Baptiste Maneyrol period = div_s64(delta, fifo_nb);
1520ecc363cSJean-Baptiste Maneyrol valid = inv_update_chip_period(ts, fifo_mult, period);
1530ecc363cSJean-Baptiste Maneyrol }
1540ecc363cSJean-Baptiste Maneyrol
1550ecc363cSJean-Baptiste Maneyrol /* no previous data, compute theoritical value from interrupt */
1560ecc363cSJean-Baptiste Maneyrol if (ts->timestamp == 0) {
1570ecc363cSJean-Baptiste Maneyrol /* elapsed time: sensor period * sensor samples number */
1580ecc363cSJean-Baptiste Maneyrol interval = (int64_t)ts->period * (int64_t)sensor_nb;
1590ecc363cSJean-Baptiste Maneyrol ts->timestamp = it->up - interval;
1600ecc363cSJean-Baptiste Maneyrol return;
1610ecc363cSJean-Baptiste Maneyrol }
1620ecc363cSJean-Baptiste Maneyrol
1630ecc363cSJean-Baptiste Maneyrol /* if interrupt interval is valid, sync with interrupt timestamp */
1640ecc363cSJean-Baptiste Maneyrol if (valid)
1650ecc363cSJean-Baptiste Maneyrol inv_align_timestamp_it(ts);
1660ecc363cSJean-Baptiste Maneyrol }
1670ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
1680ecc363cSJean-Baptiste Maneyrol
inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp * ts,uint32_t fifo_period,size_t fifo_nb,unsigned int fifo_no)1690ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
1700ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb,
1710ecc363cSJean-Baptiste Maneyrol unsigned int fifo_no)
1720ecc363cSJean-Baptiste Maneyrol {
1730ecc363cSJean-Baptiste Maneyrol int64_t interval;
1740ecc363cSJean-Baptiste Maneyrol uint32_t fifo_mult;
1750ecc363cSJean-Baptiste Maneyrol
1760ecc363cSJean-Baptiste Maneyrol if (ts->new_mult == 0)
1770ecc363cSJean-Baptiste Maneyrol return;
1780ecc363cSJean-Baptiste Maneyrol
1790ecc363cSJean-Baptiste Maneyrol /* update to new multiplier and update period */
1800ecc363cSJean-Baptiste Maneyrol ts->mult = ts->new_mult;
1810ecc363cSJean-Baptiste Maneyrol ts->new_mult = 0;
1820ecc363cSJean-Baptiste Maneyrol ts->period = ts->mult * ts->chip_period.val;
1830ecc363cSJean-Baptiste Maneyrol
1840ecc363cSJean-Baptiste Maneyrol /*
1850ecc363cSJean-Baptiste Maneyrol * After ODR change the time interval with the previous sample is
1860ecc363cSJean-Baptiste Maneyrol * undertermined (depends when the change occures). So we compute the
1870ecc363cSJean-Baptiste Maneyrol * timestamp from the current interrupt using the new FIFO period, the
1880ecc363cSJean-Baptiste Maneyrol * total number of samples and the current sample numero.
1890ecc363cSJean-Baptiste Maneyrol */
1900ecc363cSJean-Baptiste Maneyrol if (ts->timestamp != 0) {
1910ecc363cSJean-Baptiste Maneyrol /* compute measured fifo period */
1920ecc363cSJean-Baptiste Maneyrol fifo_mult = fifo_period / ts->chip.clock_period;
1930ecc363cSJean-Baptiste Maneyrol fifo_period = fifo_mult * ts->chip_period.val;
1940ecc363cSJean-Baptiste Maneyrol /* computes time interval between interrupt and this sample */
1950ecc363cSJean-Baptiste Maneyrol interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
1960ecc363cSJean-Baptiste Maneyrol ts->timestamp = ts->it.up - interval;
1970ecc363cSJean-Baptiste Maneyrol }
1980ecc363cSJean-Baptiste Maneyrol }
1990ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
2000ecc363cSJean-Baptiste Maneyrol
2010ecc363cSJean-Baptiste Maneyrol MODULE_AUTHOR("InvenSense, Inc.");
2020ecc363cSJean-Baptiste Maneyrol MODULE_DESCRIPTION("InvenSense sensors timestamp module");
2030ecc363cSJean-Baptiste Maneyrol MODULE_LICENSE("GPL");
204