1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * Copyright (C) 2020 Invensense, Inc.
6 #include <linux/errno.h>
7 #include <linux/kernel.h>
8 #include <linux/math64.h>
9 #include <linux/module.h>
11 #include <linux/iio/common/inv_sensors_timestamp.h>
13 /* compute jitter, min and max following jitter in per mille */
14 #define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \
15 (div_s64((_val) * (_jitter), 1000))
16 #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
17 (((_val) * (1000 - (_jitter))) / 1000)
18 #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
19 (((_val) * (1000 + (_jitter))) / 1000)
21 /* Add a new value inside an accumulator and update the estimate value */
22 static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, uint32_t val)
27 acc->values[acc->idx++] = val;
28 if (acc->idx >= ARRAY_SIZE(acc->values))
31 /* compute the mean of all stored values, use 0 as empty slot */
32 for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
33 if (acc->values[i] == 0)
35 sum += acc->values[i];
38 acc->val = div_u64(sum, i);
41 void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
42 const struct inv_sensors_timestamp_chip *chip)
44 memset(ts, 0, sizeof(*ts));
46 /* save chip parameters and compute min and max clock period */
48 ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
49 ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
51 /* current multiplier and period values after reset */
52 ts->mult = chip->init_period / chip->clock_period;
53 ts->period = chip->init_period;
55 /* use theoretical value for chip period */
56 inv_update_acc(&ts->chip_period, chip->clock_period);
58 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
60 int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
61 uint32_t period, bool fifo)
63 /* when FIFO is on, prevent odr change if one is already pending */
64 if (fifo && ts->new_mult != 0)
67 ts->new_mult = period / ts->chip.clock_period;
71 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
73 static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult)
75 uint32_t period_min, period_max;
77 /* check that period is acceptable */
78 period_min = ts->min_period * mult;
79 period_max = ts->max_period * mult;
80 if (period > period_min && period < period_max)
86 static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
87 uint32_t mult, uint32_t period)
89 uint32_t new_chip_period;
91 if (!inv_validate_period(ts, period, mult))
94 /* update chip internal period estimation */
95 new_chip_period = period / mult;
96 inv_update_acc(&ts->chip_period, new_chip_period);
97 ts->period = ts->mult * ts->chip_period.val;
102 static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
104 int64_t delta, jitter;
107 /* delta time between last sample and last interrupt */
108 delta = ts->it.lo - ts->timestamp;
110 /* adjust timestamp while respecting jitter */
111 jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
114 else if (delta < -jitter)
119 ts->timestamp += adjust;
122 void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
123 uint32_t fifo_period, size_t fifo_nb,
124 size_t sensor_nb, int64_t timestamp)
126 struct inv_sensors_timestamp_interval *it;
127 int64_t delta, interval;
128 const uint32_t fifo_mult = fifo_period / ts->chip.clock_period;
129 uint32_t period = ts->period;
135 /* update interrupt timestamp and compute chip and sensor periods */
139 delta = it->up - it->lo;
141 /* compute period: delta time divided by number of samples */
142 period = div_s64(delta, fifo_nb);
143 valid = inv_update_chip_period(ts, fifo_mult, period);
146 /* no previous data, compute theoritical value from interrupt */
147 if (ts->timestamp == 0) {
148 /* elapsed time: sensor period * sensor samples number */
149 interval = (int64_t)ts->period * (int64_t)sensor_nb;
150 ts->timestamp = it->up - interval;
154 /* if interrupt interval is valid, sync with interrupt timestamp */
156 inv_align_timestamp_it(ts);
158 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
160 void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
161 uint32_t fifo_period, size_t fifo_nb,
162 unsigned int fifo_no)
167 if (ts->new_mult == 0)
170 /* update to new multiplier and update period */
171 ts->mult = ts->new_mult;
173 ts->period = ts->mult * ts->chip_period.val;
176 * After ODR change the time interval with the previous sample is
177 * undertermined (depends when the change occures). So we compute the
178 * timestamp from the current interrupt using the new FIFO period, the
179 * total number of samples and the current sample numero.
181 if (ts->timestamp != 0) {
182 /* compute measured fifo period */
183 fifo_mult = fifo_period / ts->chip.clock_period;
184 fifo_period = fifo_mult * ts->chip_period.val;
185 /* computes time interval between interrupt and this sample */
186 interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
187 ts->timestamp = ts->it.up - interval;
190 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
192 MODULE_AUTHOR("InvenSense, Inc.");
193 MODULE_DESCRIPTION("InvenSense sensors timestamp module");
194 MODULE_LICENSE("GPL");