1*0ecc363cSJean-Baptiste Maneyrol // SPDX-License-Identifier: GPL-2.0-or-later 2*0ecc363cSJean-Baptiste Maneyrol /* 3*0ecc363cSJean-Baptiste Maneyrol * Copyright (C) 2020 Invensense, Inc. 4*0ecc363cSJean-Baptiste Maneyrol */ 5*0ecc363cSJean-Baptiste Maneyrol 6*0ecc363cSJean-Baptiste Maneyrol #include <linux/errno.h> 7*0ecc363cSJean-Baptiste Maneyrol #include <linux/kernel.h> 8*0ecc363cSJean-Baptiste Maneyrol #include <linux/math64.h> 9*0ecc363cSJean-Baptiste Maneyrol #include <linux/module.h> 10*0ecc363cSJean-Baptiste Maneyrol 11*0ecc363cSJean-Baptiste Maneyrol #include <linux/iio/common/inv_sensors_timestamp.h> 12*0ecc363cSJean-Baptiste Maneyrol 13*0ecc363cSJean-Baptiste Maneyrol /* compute jitter, min and max following jitter in per mille */ 14*0ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \ 15*0ecc363cSJean-Baptiste Maneyrol (div_s64((_val) * (_jitter), 1000)) 16*0ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \ 17*0ecc363cSJean-Baptiste Maneyrol (((_val) * (1000 - (_jitter))) / 1000) 18*0ecc363cSJean-Baptiste Maneyrol #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \ 19*0ecc363cSJean-Baptiste Maneyrol (((_val) * (1000 + (_jitter))) / 1000) 20*0ecc363cSJean-Baptiste Maneyrol 21*0ecc363cSJean-Baptiste Maneyrol /* Add a new value inside an accumulator and update the estimate value */ 22*0ecc363cSJean-Baptiste Maneyrol static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, uint32_t val) 23*0ecc363cSJean-Baptiste Maneyrol { 24*0ecc363cSJean-Baptiste Maneyrol uint64_t sum = 0; 25*0ecc363cSJean-Baptiste Maneyrol size_t i; 26*0ecc363cSJean-Baptiste Maneyrol 27*0ecc363cSJean-Baptiste Maneyrol acc->values[acc->idx++] = val; 28*0ecc363cSJean-Baptiste Maneyrol if (acc->idx >= ARRAY_SIZE(acc->values)) 29*0ecc363cSJean-Baptiste Maneyrol acc->idx = 0; 30*0ecc363cSJean-Baptiste Maneyrol 31*0ecc363cSJean-Baptiste Maneyrol /* compute the mean of all stored values, use 0 as empty slot */ 32*0ecc363cSJean-Baptiste Maneyrol for (i = 0; i < ARRAY_SIZE(acc->values); ++i) { 33*0ecc363cSJean-Baptiste Maneyrol if (acc->values[i] == 0) 34*0ecc363cSJean-Baptiste Maneyrol break; 35*0ecc363cSJean-Baptiste Maneyrol sum += acc->values[i]; 36*0ecc363cSJean-Baptiste Maneyrol } 37*0ecc363cSJean-Baptiste Maneyrol 38*0ecc363cSJean-Baptiste Maneyrol acc->val = div_u64(sum, i); 39*0ecc363cSJean-Baptiste Maneyrol } 40*0ecc363cSJean-Baptiste Maneyrol 41*0ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts, 42*0ecc363cSJean-Baptiste Maneyrol const struct inv_sensors_timestamp_chip *chip) 43*0ecc363cSJean-Baptiste Maneyrol { 44*0ecc363cSJean-Baptiste Maneyrol memset(ts, 0, sizeof(*ts)); 45*0ecc363cSJean-Baptiste Maneyrol 46*0ecc363cSJean-Baptiste Maneyrol /* save chip parameters and compute min and max clock period */ 47*0ecc363cSJean-Baptiste Maneyrol ts->chip = *chip; 48*0ecc363cSJean-Baptiste Maneyrol ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter); 49*0ecc363cSJean-Baptiste Maneyrol ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter); 50*0ecc363cSJean-Baptiste Maneyrol 51*0ecc363cSJean-Baptiste Maneyrol /* current multiplier and period values after reset */ 52*0ecc363cSJean-Baptiste Maneyrol ts->mult = chip->init_period / chip->clock_period; 53*0ecc363cSJean-Baptiste Maneyrol ts->period = chip->init_period; 54*0ecc363cSJean-Baptiste Maneyrol 55*0ecc363cSJean-Baptiste Maneyrol /* use theoretical value for chip period */ 56*0ecc363cSJean-Baptiste Maneyrol inv_update_acc(&ts->chip_period, chip->clock_period); 57*0ecc363cSJean-Baptiste Maneyrol } 58*0ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP); 59*0ecc363cSJean-Baptiste Maneyrol 60*0ecc363cSJean-Baptiste Maneyrol int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts, 61*0ecc363cSJean-Baptiste Maneyrol uint32_t period, bool fifo) 62*0ecc363cSJean-Baptiste Maneyrol { 63*0ecc363cSJean-Baptiste Maneyrol /* when FIFO is on, prevent odr change if one is already pending */ 64*0ecc363cSJean-Baptiste Maneyrol if (fifo && ts->new_mult != 0) 65*0ecc363cSJean-Baptiste Maneyrol return -EAGAIN; 66*0ecc363cSJean-Baptiste Maneyrol 67*0ecc363cSJean-Baptiste Maneyrol ts->new_mult = period / ts->chip.clock_period; 68*0ecc363cSJean-Baptiste Maneyrol 69*0ecc363cSJean-Baptiste Maneyrol return 0; 70*0ecc363cSJean-Baptiste Maneyrol } 71*0ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP); 72*0ecc363cSJean-Baptiste Maneyrol 73*0ecc363cSJean-Baptiste Maneyrol static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult) 74*0ecc363cSJean-Baptiste Maneyrol { 75*0ecc363cSJean-Baptiste Maneyrol uint32_t period_min, period_max; 76*0ecc363cSJean-Baptiste Maneyrol 77*0ecc363cSJean-Baptiste Maneyrol /* check that period is acceptable */ 78*0ecc363cSJean-Baptiste Maneyrol period_min = ts->min_period * mult; 79*0ecc363cSJean-Baptiste Maneyrol period_max = ts->max_period * mult; 80*0ecc363cSJean-Baptiste Maneyrol if (period > period_min && period < period_max) 81*0ecc363cSJean-Baptiste Maneyrol return true; 82*0ecc363cSJean-Baptiste Maneyrol else 83*0ecc363cSJean-Baptiste Maneyrol return false; 84*0ecc363cSJean-Baptiste Maneyrol } 85*0ecc363cSJean-Baptiste Maneyrol 86*0ecc363cSJean-Baptiste Maneyrol static bool inv_update_chip_period(struct inv_sensors_timestamp *ts, 87*0ecc363cSJean-Baptiste Maneyrol uint32_t mult, uint32_t period) 88*0ecc363cSJean-Baptiste Maneyrol { 89*0ecc363cSJean-Baptiste Maneyrol uint32_t new_chip_period; 90*0ecc363cSJean-Baptiste Maneyrol 91*0ecc363cSJean-Baptiste Maneyrol if (!inv_validate_period(ts, period, mult)) 92*0ecc363cSJean-Baptiste Maneyrol return false; 93*0ecc363cSJean-Baptiste Maneyrol 94*0ecc363cSJean-Baptiste Maneyrol /* update chip internal period estimation */ 95*0ecc363cSJean-Baptiste Maneyrol new_chip_period = period / mult; 96*0ecc363cSJean-Baptiste Maneyrol inv_update_acc(&ts->chip_period, new_chip_period); 97*0ecc363cSJean-Baptiste Maneyrol ts->period = ts->mult * ts->chip_period.val; 98*0ecc363cSJean-Baptiste Maneyrol 99*0ecc363cSJean-Baptiste Maneyrol return true; 100*0ecc363cSJean-Baptiste Maneyrol } 101*0ecc363cSJean-Baptiste Maneyrol 102*0ecc363cSJean-Baptiste Maneyrol static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) 103*0ecc363cSJean-Baptiste Maneyrol { 104*0ecc363cSJean-Baptiste Maneyrol int64_t delta, jitter; 105*0ecc363cSJean-Baptiste Maneyrol int64_t adjust; 106*0ecc363cSJean-Baptiste Maneyrol 107*0ecc363cSJean-Baptiste Maneyrol /* delta time between last sample and last interrupt */ 108*0ecc363cSJean-Baptiste Maneyrol delta = ts->it.lo - ts->timestamp; 109*0ecc363cSJean-Baptiste Maneyrol 110*0ecc363cSJean-Baptiste Maneyrol /* adjust timestamp while respecting jitter */ 111*0ecc363cSJean-Baptiste Maneyrol jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter); 112*0ecc363cSJean-Baptiste Maneyrol if (delta > jitter) 113*0ecc363cSJean-Baptiste Maneyrol adjust = jitter; 114*0ecc363cSJean-Baptiste Maneyrol else if (delta < -jitter) 115*0ecc363cSJean-Baptiste Maneyrol adjust = -jitter; 116*0ecc363cSJean-Baptiste Maneyrol else 117*0ecc363cSJean-Baptiste Maneyrol adjust = 0; 118*0ecc363cSJean-Baptiste Maneyrol 119*0ecc363cSJean-Baptiste Maneyrol ts->timestamp += adjust; 120*0ecc363cSJean-Baptiste Maneyrol } 121*0ecc363cSJean-Baptiste Maneyrol 122*0ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts, 123*0ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb, 124*0ecc363cSJean-Baptiste Maneyrol size_t sensor_nb, int64_t timestamp) 125*0ecc363cSJean-Baptiste Maneyrol { 126*0ecc363cSJean-Baptiste Maneyrol struct inv_sensors_timestamp_interval *it; 127*0ecc363cSJean-Baptiste Maneyrol int64_t delta, interval; 128*0ecc363cSJean-Baptiste Maneyrol const uint32_t fifo_mult = fifo_period / ts->chip.clock_period; 129*0ecc363cSJean-Baptiste Maneyrol uint32_t period = ts->period; 130*0ecc363cSJean-Baptiste Maneyrol bool valid = false; 131*0ecc363cSJean-Baptiste Maneyrol 132*0ecc363cSJean-Baptiste Maneyrol if (fifo_nb == 0) 133*0ecc363cSJean-Baptiste Maneyrol return; 134*0ecc363cSJean-Baptiste Maneyrol 135*0ecc363cSJean-Baptiste Maneyrol /* update interrupt timestamp and compute chip and sensor periods */ 136*0ecc363cSJean-Baptiste Maneyrol it = &ts->it; 137*0ecc363cSJean-Baptiste Maneyrol it->lo = it->up; 138*0ecc363cSJean-Baptiste Maneyrol it->up = timestamp; 139*0ecc363cSJean-Baptiste Maneyrol delta = it->up - it->lo; 140*0ecc363cSJean-Baptiste Maneyrol if (it->lo != 0) { 141*0ecc363cSJean-Baptiste Maneyrol /* compute period: delta time divided by number of samples */ 142*0ecc363cSJean-Baptiste Maneyrol period = div_s64(delta, fifo_nb); 143*0ecc363cSJean-Baptiste Maneyrol valid = inv_update_chip_period(ts, fifo_mult, period); 144*0ecc363cSJean-Baptiste Maneyrol } 145*0ecc363cSJean-Baptiste Maneyrol 146*0ecc363cSJean-Baptiste Maneyrol /* no previous data, compute theoritical value from interrupt */ 147*0ecc363cSJean-Baptiste Maneyrol if (ts->timestamp == 0) { 148*0ecc363cSJean-Baptiste Maneyrol /* elapsed time: sensor period * sensor samples number */ 149*0ecc363cSJean-Baptiste Maneyrol interval = (int64_t)ts->period * (int64_t)sensor_nb; 150*0ecc363cSJean-Baptiste Maneyrol ts->timestamp = it->up - interval; 151*0ecc363cSJean-Baptiste Maneyrol return; 152*0ecc363cSJean-Baptiste Maneyrol } 153*0ecc363cSJean-Baptiste Maneyrol 154*0ecc363cSJean-Baptiste Maneyrol /* if interrupt interval is valid, sync with interrupt timestamp */ 155*0ecc363cSJean-Baptiste Maneyrol if (valid) 156*0ecc363cSJean-Baptiste Maneyrol inv_align_timestamp_it(ts); 157*0ecc363cSJean-Baptiste Maneyrol } 158*0ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP); 159*0ecc363cSJean-Baptiste Maneyrol 160*0ecc363cSJean-Baptiste Maneyrol void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts, 161*0ecc363cSJean-Baptiste Maneyrol uint32_t fifo_period, size_t fifo_nb, 162*0ecc363cSJean-Baptiste Maneyrol unsigned int fifo_no) 163*0ecc363cSJean-Baptiste Maneyrol { 164*0ecc363cSJean-Baptiste Maneyrol int64_t interval; 165*0ecc363cSJean-Baptiste Maneyrol uint32_t fifo_mult; 166*0ecc363cSJean-Baptiste Maneyrol 167*0ecc363cSJean-Baptiste Maneyrol if (ts->new_mult == 0) 168*0ecc363cSJean-Baptiste Maneyrol return; 169*0ecc363cSJean-Baptiste Maneyrol 170*0ecc363cSJean-Baptiste Maneyrol /* update to new multiplier and update period */ 171*0ecc363cSJean-Baptiste Maneyrol ts->mult = ts->new_mult; 172*0ecc363cSJean-Baptiste Maneyrol ts->new_mult = 0; 173*0ecc363cSJean-Baptiste Maneyrol ts->period = ts->mult * ts->chip_period.val; 174*0ecc363cSJean-Baptiste Maneyrol 175*0ecc363cSJean-Baptiste Maneyrol /* 176*0ecc363cSJean-Baptiste Maneyrol * After ODR change the time interval with the previous sample is 177*0ecc363cSJean-Baptiste Maneyrol * undertermined (depends when the change occures). So we compute the 178*0ecc363cSJean-Baptiste Maneyrol * timestamp from the current interrupt using the new FIFO period, the 179*0ecc363cSJean-Baptiste Maneyrol * total number of samples and the current sample numero. 180*0ecc363cSJean-Baptiste Maneyrol */ 181*0ecc363cSJean-Baptiste Maneyrol if (ts->timestamp != 0) { 182*0ecc363cSJean-Baptiste Maneyrol /* compute measured fifo period */ 183*0ecc363cSJean-Baptiste Maneyrol fifo_mult = fifo_period / ts->chip.clock_period; 184*0ecc363cSJean-Baptiste Maneyrol fifo_period = fifo_mult * ts->chip_period.val; 185*0ecc363cSJean-Baptiste Maneyrol /* computes time interval between interrupt and this sample */ 186*0ecc363cSJean-Baptiste Maneyrol interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period; 187*0ecc363cSJean-Baptiste Maneyrol ts->timestamp = ts->it.up - interval; 188*0ecc363cSJean-Baptiste Maneyrol } 189*0ecc363cSJean-Baptiste Maneyrol } 190*0ecc363cSJean-Baptiste Maneyrol EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP); 191*0ecc363cSJean-Baptiste Maneyrol 192*0ecc363cSJean-Baptiste Maneyrol MODULE_AUTHOR("InvenSense, Inc."); 193*0ecc363cSJean-Baptiste Maneyrol MODULE_DESCRIPTION("InvenSense sensors timestamp module"); 194*0ecc363cSJean-Baptiste Maneyrol MODULE_LICENSE("GPL"); 195