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 */ 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 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 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 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 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 1060ecc363cSJean-Baptiste Maneyrol static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) 1070ecc363cSJean-Baptiste Maneyrol { 1080ecc363cSJean-Baptiste Maneyrol int64_t delta, jitter; 1090ecc363cSJean-Baptiste Maneyrol int64_t adjust; 1100ecc363cSJean-Baptiste Maneyrol 1110ecc363cSJean-Baptiste Maneyrol /* delta time between last sample and last interrupt */ 1120ecc363cSJean-Baptiste Maneyrol delta = ts->it.lo - ts->timestamp; 1130ecc363cSJean-Baptiste Maneyrol 1140ecc363cSJean-Baptiste Maneyrol /* adjust timestamp while respecting jitter */ 1150ecc363cSJean-Baptiste Maneyrol jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter); 1160ecc363cSJean-Baptiste Maneyrol if (delta > jitter) 1170ecc363cSJean-Baptiste Maneyrol adjust = jitter; 1180ecc363cSJean-Baptiste Maneyrol else if (delta < -jitter) 1190ecc363cSJean-Baptiste Maneyrol adjust = -jitter; 1200ecc363cSJean-Baptiste Maneyrol else 1210ecc363cSJean-Baptiste Maneyrol adjust = 0; 1220ecc363cSJean-Baptiste Maneyrol 1230ecc363cSJean-Baptiste Maneyrol ts->timestamp += adjust; 1240ecc363cSJean-Baptiste Maneyrol } 1250ecc363cSJean-Baptiste Maneyrol 1260ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, 1270ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb, 1280ecc363cSJean-Baptiste Maneyrol size_t sensor_nb, int64_t timestamp) 1290ecc363cSJean-Baptiste Maneyrol { 1300ecc363cSJean-Baptiste Maneyrol struct inv_sensors_timestamp_interval *it; 1310ecc363cSJean-Baptiste Maneyrol int64_t delta, interval; 1320ecc363cSJean-Baptiste Maneyrol const uint32_t fifo_mult = fifo_period / ts->chip.clock_period; 1330ecc363cSJean-Baptiste Maneyrol uint32_t period = ts->period; 1340ecc363cSJean-Baptiste Maneyrol bool valid = false; 1350ecc363cSJean-Baptiste Maneyrol 1360ecc363cSJean-Baptiste Maneyrol if (fifo_nb == 0) 1370ecc363cSJean-Baptiste Maneyrol return; 1380ecc363cSJean-Baptiste Maneyrol 1390ecc363cSJean-Baptiste Maneyrol /* update interrupt timestamp and compute chip and sensor periods */ 1400ecc363cSJean-Baptiste Maneyrol it = &ts->it; 1410ecc363cSJean-Baptiste Maneyrol it->lo = it->up; 1420ecc363cSJean-Baptiste Maneyrol it->up = timestamp; 1430ecc363cSJean-Baptiste Maneyrol delta = it->up - it->lo; 1440ecc363cSJean-Baptiste Maneyrol if (it->lo != 0) { 1450ecc363cSJean-Baptiste Maneyrol /* compute period: delta time divided by number of samples */ 1460ecc363cSJean-Baptiste Maneyrol period = div_s64(delta, fifo_nb); 1470ecc363cSJean-Baptiste Maneyrol valid = inv_update_chip_period(ts, fifo_mult, period); 1480ecc363cSJean-Baptiste Maneyrol } 1490ecc363cSJean-Baptiste Maneyrol 1500ecc363cSJean-Baptiste Maneyrol /* no previous data, compute theoritical value from interrupt */ 1510ecc363cSJean-Baptiste Maneyrol if (ts->timestamp == 0) { 1520ecc363cSJean-Baptiste Maneyrol /* elapsed time: sensor period * sensor samples number */ 1530ecc363cSJean-Baptiste Maneyrol interval = (int64_t)ts->period * (int64_t)sensor_nb; 1540ecc363cSJean-Baptiste Maneyrol ts->timestamp = it->up - interval; 1550ecc363cSJean-Baptiste Maneyrol return; 1560ecc363cSJean-Baptiste Maneyrol } 1570ecc363cSJean-Baptiste Maneyrol 1580ecc363cSJean-Baptiste Maneyrol /* if interrupt interval is valid, sync with interrupt timestamp */ 1590ecc363cSJean-Baptiste Maneyrol if (valid) 1600ecc363cSJean-Baptiste Maneyrol inv_align_timestamp_it(ts); 1610ecc363cSJean-Baptiste Maneyrol } 1620ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP); 1630ecc363cSJean-Baptiste Maneyrol 1640ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts, 1650ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb, 1660ecc363cSJean-Baptiste Maneyrol unsigned int fifo_no) 1670ecc363cSJean-Baptiste Maneyrol { 1680ecc363cSJean-Baptiste Maneyrol int64_t interval; 1690ecc363cSJean-Baptiste Maneyrol uint32_t fifo_mult; 1700ecc363cSJean-Baptiste Maneyrol 1710ecc363cSJean-Baptiste Maneyrol if (ts->new_mult == 0) 1720ecc363cSJean-Baptiste Maneyrol return; 1730ecc363cSJean-Baptiste Maneyrol 1740ecc363cSJean-Baptiste Maneyrol /* update to new multiplier and update period */ 1750ecc363cSJean-Baptiste Maneyrol ts->mult = ts->new_mult; 1760ecc363cSJean-Baptiste Maneyrol ts->new_mult = 0; 1770ecc363cSJean-Baptiste Maneyrol ts->period = ts->mult * ts->chip_period.val; 1780ecc363cSJean-Baptiste Maneyrol 1790ecc363cSJean-Baptiste Maneyrol /* 1800ecc363cSJean-Baptiste Maneyrol * After ODR change the time interval with the previous sample is 1810ecc363cSJean-Baptiste Maneyrol * undertermined (depends when the change occures). So we compute the 1820ecc363cSJean-Baptiste Maneyrol * timestamp from the current interrupt using the new FIFO period, the 1830ecc363cSJean-Baptiste Maneyrol * total number of samples and the current sample numero. 1840ecc363cSJean-Baptiste Maneyrol */ 1850ecc363cSJean-Baptiste Maneyrol if (ts->timestamp != 0) { 1860ecc363cSJean-Baptiste Maneyrol /* compute measured fifo period */ 1870ecc363cSJean-Baptiste Maneyrol fifo_mult = fifo_period / ts->chip.clock_period; 1880ecc363cSJean-Baptiste Maneyrol fifo_period = fifo_mult * ts->chip_period.val; 1890ecc363cSJean-Baptiste Maneyrol /* computes time interval between interrupt and this sample */ 1900ecc363cSJean-Baptiste Maneyrol interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period; 1910ecc363cSJean-Baptiste Maneyrol ts->timestamp = ts->it.up - interval; 1920ecc363cSJean-Baptiste Maneyrol } 1930ecc363cSJean-Baptiste Maneyrol } 1940ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP); 1950ecc363cSJean-Baptiste Maneyrol 1960ecc363cSJean-Baptiste Maneyrol MODULE_AUTHOR("InvenSense, Inc."); 1970ecc363cSJean-Baptiste Maneyrol MODULE_DESCRIPTION("InvenSense sensors timestamp module"); 1980ecc363cSJean-Baptiste Maneyrol MODULE_LICENSE("GPL"); 199