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 */
inv_update_acc(struct inv_sensors_timestamp_acc * acc,uint32_t val)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(sum, i);
39 }
40
inv_sensors_timestamp_init(struct inv_sensors_timestamp * ts,const struct inv_sensors_timestamp_chip * chip)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(&ts->chip_period, chip->clock_period);
57 }
58 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, "IIO_INV_SENSORS_TIMESTAMP");
59
inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp * ts,uint32_t period,bool fifo)60 int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
61 uint32_t period, bool fifo)
62 {
63 uint32_t mult;
64
65 /* when FIFO is on, prevent odr change if one is already pending */
66 if (fifo && ts->new_mult != 0)
67 return -EAGAIN;
68
69 mult = period / ts->chip.clock_period;
70 if (mult != ts->mult)
71 ts->new_mult = mult;
72
73 /* When FIFO is off, directly apply the new ODR */
74 if (!fifo)
75 inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
76
77 return 0;
78 }
79 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, "IIO_INV_SENSORS_TIMESTAMP");
80
inv_validate_period(struct inv_sensors_timestamp * ts,uint32_t period)81 static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period)
82 {
83 uint32_t period_min, period_max;
84
85 /* check that period is acceptable */
86 period_min = ts->min_period * ts->mult;
87 period_max = ts->max_period * ts->mult;
88 if (period > period_min && period < period_max)
89 return true;
90 else
91 return false;
92 }
93
inv_update_chip_period(struct inv_sensors_timestamp * ts,uint32_t period)94 static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
95 uint32_t period)
96 {
97 uint32_t new_chip_period;
98
99 if (!inv_validate_period(ts, period))
100 return false;
101
102 /* update chip internal period estimation */
103 new_chip_period = period / ts->mult;
104 inv_update_acc(&ts->chip_period, new_chip_period);
105 ts->period = ts->mult * ts->chip_period.val;
106
107 return true;
108 }
109
inv_align_timestamp_it(struct inv_sensors_timestamp * ts)110 static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
111 {
112 const int64_t period_min = (int64_t)ts->min_period * ts->mult;
113 const int64_t period_max = (int64_t)ts->max_period * ts->mult;
114 int64_t add_max, sub_max;
115 int64_t delta, jitter;
116 int64_t adjust;
117
118 /* delta time between last sample and last interrupt */
119 delta = ts->it.lo - ts->timestamp;
120
121 /* adjust timestamp while respecting jitter */
122 add_max = period_max - (int64_t)ts->period;
123 sub_max = period_min - (int64_t)ts->period;
124 jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
125 if (delta > jitter)
126 adjust = add_max;
127 else if (delta < -jitter)
128 adjust = sub_max;
129 else
130 adjust = 0;
131
132 ts->timestamp += adjust;
133 }
134
inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp * ts,size_t sample_nb,int64_t timestamp)135 void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
136 size_t sample_nb, int64_t timestamp)
137 {
138 struct inv_sensors_timestamp_interval *it;
139 int64_t delta, interval;
140 uint32_t period;
141 bool valid = false;
142
143 if (sample_nb == 0)
144 return;
145
146 /* update interrupt timestamp and compute chip and sensor periods */
147 it = &ts->it;
148 it->lo = it->up;
149 it->up = timestamp;
150 delta = it->up - it->lo;
151 if (it->lo != 0) {
152 /* compute period: delta time divided by number of samples */
153 period = div_s64(delta, sample_nb);
154 valid = inv_update_chip_period(ts, period);
155 }
156
157 /* no previous data, compute theoritical value from interrupt */
158 if (ts->timestamp == 0) {
159 /* elapsed time: sensor period * sensor samples number */
160 interval = (int64_t)ts->period * (int64_t)sample_nb;
161 ts->timestamp = it->up - interval;
162 return;
163 }
164
165 /* if interrupt interval is valid, sync with interrupt timestamp */
166 if (valid)
167 inv_align_timestamp_it(ts);
168 }
169 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP");
170
inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp * ts,uint32_t fifo_period,size_t fifo_nb,unsigned int fifo_no)171 void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
172 uint32_t fifo_period, size_t fifo_nb,
173 unsigned int fifo_no)
174 {
175 int64_t interval;
176 uint32_t fifo_mult;
177
178 if (ts->new_mult == 0)
179 return;
180
181 /* update to new multiplier and update period */
182 ts->mult = ts->new_mult;
183 ts->new_mult = 0;
184 ts->period = ts->mult * ts->chip_period.val;
185
186 /*
187 * After ODR change the time interval with the previous sample is
188 * undertermined (depends when the change occures). So we compute the
189 * timestamp from the current interrupt using the new FIFO period, the
190 * total number of samples and the current sample numero.
191 */
192 if (ts->timestamp != 0) {
193 /* compute measured fifo period */
194 fifo_mult = fifo_period / ts->chip.clock_period;
195 fifo_period = fifo_mult * ts->chip_period.val;
196 /* computes time interval between interrupt and this sample */
197 interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
198 ts->timestamp = ts->it.up - interval;
199 }
200 }
201 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, "IIO_INV_SENSORS_TIMESTAMP");
202
203 MODULE_AUTHOR("InvenSense, Inc.");
204 MODULE_DESCRIPTION("InvenSense sensors timestamp module");
205 MODULE_LICENSE("GPL");
206