1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2020 Invensense, Inc. |
4 | */ |
5 | |
6 | #include <linux/errno.h> |
7 | #include <linux/kernel.h> |
8 | #include <linux/math64.h> |
9 | #include <linux/module.h> |
10 | |
11 | #include <linux/iio/common/inv_sensors_timestamp.h> |
12 | |
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) |
20 | |
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) |
23 | { |
24 | uint64_t sum = 0; |
25 | size_t i; |
26 | |
27 | acc->values[acc->idx++] = val; |
28 | if (acc->idx >= ARRAY_SIZE(acc->values)) |
29 | acc->idx = 0; |
30 | |
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) |
34 | break; |
35 | sum += acc->values[i]; |
36 | } |
37 | |
38 | acc->val = div_u64(dividend: sum, divisor: i); |
39 | } |
40 | |
41 | void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts, |
42 | const struct inv_sensors_timestamp_chip *chip) |
43 | { |
44 | memset(ts, 0, sizeof(*ts)); |
45 | |
46 | /* save chip parameters and compute min and max clock period */ |
47 | ts->chip = *chip; |
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); |
50 | |
51 | /* current multiplier and period values after reset */ |
52 | ts->mult = chip->init_period / chip->clock_period; |
53 | ts->period = chip->init_period; |
54 | |
55 | /* use theoretical value for chip period */ |
56 | inv_update_acc(acc: &ts->chip_period, val: chip->clock_period); |
57 | } |
58 | EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP); |
59 | |
60 | int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts, |
61 | uint32_t period, bool fifo) |
62 | { |
63 | /* when FIFO is on, prevent odr change if one is already pending */ |
64 | if (fifo && ts->new_mult != 0) |
65 | return -EAGAIN; |
66 | |
67 | ts->new_mult = period / ts->chip.clock_period; |
68 | |
69 | return 0; |
70 | } |
71 | EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP); |
72 | |
73 | static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult) |
74 | { |
75 | uint32_t period_min, period_max; |
76 | |
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) |
81 | return true; |
82 | else |
83 | return false; |
84 | } |
85 | |
86 | static bool inv_update_chip_period(struct inv_sensors_timestamp *ts, |
87 | uint32_t mult, uint32_t period) |
88 | { |
89 | uint32_t new_chip_period; |
90 | |
91 | if (!inv_validate_period(ts, period, mult)) |
92 | return false; |
93 | |
94 | /* update chip internal period estimation */ |
95 | new_chip_period = period / mult; |
96 | inv_update_acc(acc: &ts->chip_period, val: new_chip_period); |
97 | ts->period = ts->mult * ts->chip_period.val; |
98 | |
99 | return true; |
100 | } |
101 | |
102 | static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts) |
103 | { |
104 | int64_t delta, jitter; |
105 | int64_t adjust; |
106 | |
107 | /* delta time between last sample and last interrupt */ |
108 | delta = ts->it.lo - ts->timestamp; |
109 | |
110 | /* adjust timestamp while respecting jitter */ |
111 | jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter); |
112 | if (delta > jitter) |
113 | adjust = jitter; |
114 | else if (delta < -jitter) |
115 | adjust = -jitter; |
116 | else |
117 | adjust = 0; |
118 | |
119 | ts->timestamp += adjust; |
120 | } |
121 | |
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) |
125 | { |
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; |
130 | bool valid = false; |
131 | |
132 | if (fifo_nb == 0) |
133 | return; |
134 | |
135 | /* update interrupt timestamp and compute chip and sensor periods */ |
136 | it = &ts->it; |
137 | it->lo = it->up; |
138 | it->up = timestamp; |
139 | delta = it->up - it->lo; |
140 | if (it->lo != 0) { |
141 | /* compute period: delta time divided by number of samples */ |
142 | period = div_s64(dividend: delta, divisor: fifo_nb); |
143 | valid = inv_update_chip_period(ts, mult: fifo_mult, period); |
144 | } |
145 | |
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; |
151 | return; |
152 | } |
153 | |
154 | /* if interrupt interval is valid, sync with interrupt timestamp */ |
155 | if (valid) |
156 | inv_align_timestamp_it(ts); |
157 | } |
158 | EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP); |
159 | |
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) |
163 | { |
164 | int64_t interval; |
165 | uint32_t fifo_mult; |
166 | |
167 | if (ts->new_mult == 0) |
168 | return; |
169 | |
170 | /* update to new multiplier and update period */ |
171 | ts->mult = ts->new_mult; |
172 | ts->new_mult = 0; |
173 | ts->period = ts->mult * ts->chip_period.val; |
174 | |
175 | /* |
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. |
180 | */ |
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; |
188 | } |
189 | } |
190 | EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP); |
191 | |
192 | MODULE_AUTHOR("InvenSense, Inc." ); |
193 | MODULE_DESCRIPTION("InvenSense sensors timestamp module" ); |
194 | MODULE_LICENSE("GPL" ); |
195 | |