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 min and max following jitter in per mille */
14 #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
15 (((_val) * (1000 - (_jitter))) / 1000)
16 #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
17 (((_val) * (1000 + (_jitter))) / 1000)
18
19 /* minimum timestamp delta between 2 interrupts for measuring period (20ms) */
20 #define INV_SENSORS_MIN_IT_DELTA (20 * NSEC_PER_MSEC)
21
22 /* Add a new value inside an accumulator and update the estimate value */
inv_update_acc(struct inv_sensors_timestamp_acc * acc,u32 val)23 static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, u32 val)
24 {
25 u64 sum = 0;
26 size_t i;
27
28 acc->values[acc->idx++] = val;
29 if (acc->idx >= ARRAY_SIZE(acc->values))
30 acc->idx = 0;
31
32 /* compute the mean of all stored values, use 0 as empty slot */
33 for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
34 if (acc->values[i] == 0)
35 break;
36 sum += acc->values[i];
37 }
38
39 acc->val = div_u64(sum, i);
40 }
41
inv_sensors_timestamp_init(struct inv_sensors_timestamp * ts,const struct inv_sensors_timestamp_chip * chip)42 void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
43 const struct inv_sensors_timestamp_chip *chip)
44 {
45 memset(ts, 0, sizeof(*ts));
46
47 /* save chip parameters and compute min and max clock period */
48 ts->chip = *chip;
49 ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
50 ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
51
52 /* current multiplier and period values after reset */
53 ts->mult = chip->init_period / chip->clock_period;
54 ts->period = chip->init_period;
55
56 /* use theoretical value for chip period */
57 inv_update_acc(&ts->chip_period, chip->clock_period);
58 }
59 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, "IIO_INV_SENSORS_TIMESTAMP");
60
inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp * ts,u32 period,bool fifo)61 int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
62 u32 period, bool fifo)
63 {
64 u32 mult;
65
66 /* when FIFO is on, prevent odr change if one is already pending */
67 if (fifo && ts->new_mult != 0)
68 return -EAGAIN;
69
70 mult = period / ts->chip.clock_period;
71 if (mult != ts->mult)
72 ts->new_mult = mult;
73
74 /* When FIFO is off, directly apply the new ODR */
75 if (!fifo)
76 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
77
78 return 0;
79 }
80 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, "IIO_INV_SENSORS_TIMESTAMP");
81
inv_validate_period(struct inv_sensors_timestamp * ts,u32 period)82 static bool inv_validate_period(struct inv_sensors_timestamp *ts, u32 period)
83 {
84 u32 period_min, period_max;
85
86 /* check that period is acceptable */
87 period_min = ts->min_period * ts->mult;
88 period_max = ts->max_period * ts->mult;
89 if (period > period_min && period < period_max)
90 return true;
91 else
92 return false;
93 }
94
inv_update_chip_period(struct inv_sensors_timestamp * ts,u32 period)95 static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
96 u32 period)
97 {
98 u32 new_chip_period;
99
100 if (!inv_validate_period(ts, period))
101 return false;
102
103 /* update chip internal period estimation */
104 new_chip_period = period / ts->mult;
105 inv_update_acc(&ts->chip_period, new_chip_period);
106
107 return true;
108 }
109
inv_align_timestamp_it(struct inv_sensors_timestamp * ts,unsigned int sample_nb)110 static u32 inv_align_timestamp_it(struct inv_sensors_timestamp *ts,
111 unsigned int sample_nb)
112 {
113 const s64 period_min = (s64)ts->min_period * ts->mult;
114 const s64 period_max = (s64)ts->max_period * ts->mult;
115 s64 new_period;
116
117 /* compute new period aligning last timestamp with interrupt timestamp */
118 new_period = div_s64(ts->it.up - ts->timestamp, sample_nb);
119
120 /* ensure that period never overflows the jitter */
121 return clamp(new_period, period_min, period_max);
122 }
123
inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp * ts,size_t sample_nb,s64 timestamp)124 void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
125 size_t sample_nb, s64 timestamp)
126 {
127 struct inv_sensors_timestamp_interval *it;
128 s64 delta, delta_threshold, interval;
129 u32 period;
130 bool valid = false;
131
132 if (sample_nb == 0)
133 return;
134
135 /* no previous data, compute theoretical value from interrupt */
136 if (ts->timestamp == 0) {
137 /* elapsed time: sensor period * sensor samples number */
138 interval = (s64)ts->period * (s64)sample_nb;
139 ts->timestamp = timestamp - interval;
140 }
141
142 /* update delta timestamps and estimated period */
143 it = &ts->delta;
144 ts->delta_counter += sample_nb;
145 delta = timestamp - it->up;
146 delta_threshold = INV_SENSORS_TIMESTAMP_MIN(INV_SENSORS_MIN_IT_DELTA, ts->chip.jitter);
147 if (delta >= delta_threshold) {
148 it->lo = it->up;
149 it->up = timestamp;
150 if (it->lo != 0) {
151 /* compute period: delta time divided by number of samples */
152 delta = it->up - it->lo;
153 period = div_s64(delta, ts->delta_counter);
154 inv_update_chip_period(ts, period);
155 }
156 ts->delta_counter = 0;
157 }
158
159 /* update interrupt timestamp and compute chip and sensor periods */
160 it = &ts->it;
161 it->lo = it->up;
162 it->up = timestamp;
163 delta = it->up - it->lo;
164 if (it->lo != 0) {
165 /* compute period and check validity */
166 period = div_s64(delta, sample_nb);
167 valid = inv_validate_period(ts, period);
168 }
169
170 /*
171 * If interrupt interval is valid, sync with interrupt timestamp.
172 * Otherwise, use estimated value while ensuring interrupt timestamp
173 * remains the maximum possible value.
174 */
175 period = inv_align_timestamp_it(ts, sample_nb);
176 if (valid)
177 ts->period = period;
178 else
179 ts->period = min(ts->mult * ts->chip_period.val, period);
180 }
181 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP");
182
inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp * ts,u32 fifo_period,size_t fifo_nb,unsigned int fifo_no)183 void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
184 u32 fifo_period, size_t fifo_nb,
185 unsigned int fifo_no)
186 {
187 s64 interval;
188 u32 fifo_mult;
189
190 if (ts->new_mult == 0)
191 return;
192
193 /* update to new multiplier and update period */
194 ts->mult = ts->new_mult;
195 ts->new_mult = 0;
196 ts->period = ts->mult * ts->chip_period.val;
197
198 /*
199 * After ODR change the time interval with the previous sample is
200 * undertermined (depends when the change occurs). So we compute the
201 * timestamp from the current interrupt using the new FIFO period, the
202 * total number of samples and the current sample numero.
203 */
204 if (ts->timestamp != 0) {
205 /* compute measured fifo period */
206 fifo_mult = fifo_period / ts->chip.clock_period;
207 fifo_period = fifo_mult * ts->chip_period.val;
208 /* computes time interval between interrupt and this sample */
209 interval = (s64)(fifo_nb - fifo_no) * (s64)fifo_period;
210 ts->timestamp = ts->it.up - interval;
211 }
212 }
213 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, "IIO_INV_SENSORS_TIMESTAMP");
214
215 MODULE_AUTHOR("InvenSense, Inc.");
216 MODULE_DESCRIPTION("InvenSense sensors timestamp module");
217 MODULE_LICENSE("GPL");
218