1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2020 Invensense, Inc.
4 */
5
6 #include <linux/kernel.h>
7 #include <linux/regmap.h>
8 #include <linux/math64.h>
9
10 #include "inv_icm42600.h"
11 #include "inv_icm42600_timestamp.h"
12
13 /* internal chip period is 32kHz, 31250ns */
14 #define INV_ICM42600_TIMESTAMP_PERIOD 31250
15 /* allow a jitter of +/- 2% */
16 #define INV_ICM42600_TIMESTAMP_JITTER 2
17 /* compute min and max periods accepted */
18 #define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \
19 (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
20 #define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
21 (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
22
23 /* Add a new value inside an accumulator and update the estimate value */
inv_update_acc(struct inv_icm42600_timestamp_acc * acc,uint32_t val)24 static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
25 {
26 uint64_t sum = 0;
27 size_t i;
28
29 acc->values[acc->idx++] = val;
30 if (acc->idx >= ARRAY_SIZE(acc->values))
31 acc->idx = 0;
32
33 /* compute the mean of all stored values, use 0 as empty slot */
34 for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
35 if (acc->values[i] == 0)
36 break;
37 sum += acc->values[i];
38 }
39
40 acc->val = div_u64(sum, i);
41 }
42
inv_icm42600_timestamp_init(struct inv_icm42600_timestamp * ts,uint32_t period)43 void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
44 uint32_t period)
45 {
46 /* initial odr for sensor after reset is 1kHz */
47 const uint32_t default_period = 1000000;
48
49 /* current multiplier and period values after reset */
50 ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
51 ts->period = default_period;
52 /* new set multiplier is the one from chip initialization */
53 ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
54
55 /* use theoretical value for chip period */
56 inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
57 }
58
inv_icm42600_timestamp_setup(struct inv_icm42600_state * st)59 int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
60 {
61 unsigned int val;
62
63 /* enable timestamp register */
64 val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
65 INV_ICM42600_TMST_CONFIG_TMST_EN;
66 return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
67 INV_ICM42600_TMST_CONFIG_MASK, val);
68 }
69
inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp * ts,uint32_t period,bool fifo)70 int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
71 uint32_t period, bool fifo)
72 {
73 /* when FIFO is on, prevent odr change if one is already pending */
74 if (fifo && ts->new_mult != 0)
75 return -EAGAIN;
76
77 ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
78
79 return 0;
80 }
81
inv_validate_period(uint32_t period,uint32_t mult)82 static bool inv_validate_period(uint32_t period, uint32_t mult)
83 {
84 const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
85 uint32_t period_min, period_max;
86
87 /* check that period is acceptable */
88 period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
89 period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
90 if (period > period_min && period < period_max)
91 return true;
92 else
93 return false;
94 }
95
inv_compute_chip_period(struct inv_icm42600_timestamp * ts,uint32_t mult,uint32_t period)96 static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
97 uint32_t mult, uint32_t period)
98 {
99 uint32_t new_chip_period;
100
101 if (!inv_validate_period(period, mult))
102 return false;
103
104 /* update chip internal period estimation */
105 new_chip_period = period / mult;
106 inv_update_acc(&ts->chip_period, new_chip_period);
107
108 return true;
109 }
110
inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp * ts,uint32_t fifo_period,size_t fifo_nb,size_t sensor_nb,int64_t timestamp)111 void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
112 uint32_t fifo_period, size_t fifo_nb,
113 size_t sensor_nb, int64_t timestamp)
114 {
115 struct inv_icm42600_timestamp_interval *it;
116 int64_t delta, interval;
117 const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
118 uint32_t period = ts->period;
119 int32_t m;
120 bool valid = false;
121
122 if (fifo_nb == 0)
123 return;
124
125 /* update interrupt timestamp and compute chip and sensor periods */
126 it = &ts->it;
127 it->lo = it->up;
128 it->up = timestamp;
129 delta = it->up - it->lo;
130 if (it->lo != 0) {
131 /* compute period: delta time divided by number of samples */
132 period = div_s64(delta, fifo_nb);
133 valid = inv_compute_chip_period(ts, fifo_mult, period);
134 /* update sensor period if chip internal period is updated */
135 if (valid)
136 ts->period = ts->mult * ts->chip_period.val;
137 }
138
139 /* no previous data, compute theoritical value from interrupt */
140 if (ts->timestamp == 0) {
141 /* elapsed time: sensor period * sensor samples number */
142 interval = (int64_t)ts->period * (int64_t)sensor_nb;
143 ts->timestamp = it->up - interval;
144 return;
145 }
146
147 /* if interrupt interval is valid, sync with interrupt timestamp */
148 if (valid) {
149 /* compute measured fifo_period */
150 fifo_period = fifo_mult * ts->chip_period.val;
151 /* delta time between last sample and last interrupt */
152 delta = it->lo - ts->timestamp;
153 /* if there are multiple samples, go back to first one */
154 while (delta >= (fifo_period * 3 / 2))
155 delta -= fifo_period;
156 /* compute maximal adjustment value */
157 m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
158 if (delta > m)
159 delta = m;
160 else if (delta < -m)
161 delta = -m;
162 ts->timestamp += delta;
163 }
164 }
165
inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp * ts,uint32_t fifo_period,size_t fifo_nb,unsigned int fifo_no)166 void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
167 uint32_t fifo_period, size_t fifo_nb,
168 unsigned int fifo_no)
169 {
170 int64_t interval;
171 uint32_t fifo_mult;
172
173 if (ts->new_mult == 0)
174 return;
175
176 /* update to new multiplier and update period */
177 ts->mult = ts->new_mult;
178 ts->new_mult = 0;
179 ts->period = ts->mult * ts->chip_period.val;
180
181 /*
182 * After ODR change the time interval with the previous sample is
183 * undertermined (depends when the change occures). So we compute the
184 * timestamp from the current interrupt using the new FIFO period, the
185 * total number of samples and the current sample numero.
186 */
187 if (ts->timestamp != 0) {
188 /* compute measured fifo period */
189 fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
190 fifo_period = fifo_mult * ts->chip_period.val;
191 /* computes time interval between interrupt and this sample */
192 interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
193 ts->timestamp = ts->it.up - interval;
194 }
195 }
196