1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * Copyright (C) 2020 Invensense, Inc.
6 #include <linux/kernel.h>
7 #include <linux/regmap.h>
8 #include <linux/math64.h>
10 #include "inv_icm42600.h"
11 #include "inv_icm42600_timestamp.h"
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)
23 /* Add a new value inside an accumulator and update the estimate value */
24 static void inv_update_acc(struct inv_icm42600_timestamp_acc
*acc
, uint32_t val
)
29 acc
->values
[acc
->idx
++] = val
;
30 if (acc
->idx
>= ARRAY_SIZE(acc
->values
))
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)
37 sum
+= acc
->values
[i
];
40 acc
->val
= div_u64(sum
, i
);
43 void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp
*ts
,
46 /* initial odr for sensor after reset is 1kHz */
47 const uint32_t default_period
= 1000000;
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
;
55 /* use theoretical value for chip period */
56 inv_update_acc(&ts
->chip_period
, INV_ICM42600_TIMESTAMP_PERIOD
);
59 int inv_icm42600_timestamp_setup(struct inv_icm42600_state
*st
)
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
);
70 int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp
*ts
,
71 uint32_t period
, bool fifo
)
73 /* when FIFO is on, prevent odr change if one is already pending */
74 if (fifo
&& ts
->new_mult
!= 0)
77 ts
->new_mult
= period
/ INV_ICM42600_TIMESTAMP_PERIOD
;
82 static bool inv_validate_period(uint32_t period
, uint32_t mult
)
84 const uint32_t chip_period
= INV_ICM42600_TIMESTAMP_PERIOD
;
85 uint32_t period_min
, period_max
;
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
)
96 static bool inv_compute_chip_period(struct inv_icm42600_timestamp
*ts
,
97 uint32_t mult
, uint32_t period
)
99 uint32_t new_chip_period
;
101 if (!inv_validate_period(period
, mult
))
104 /* update chip internal period estimation */
105 new_chip_period
= period
/ mult
;
106 inv_update_acc(&ts
->chip_period
, new_chip_period
);
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
)
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
;
125 /* update interrupt timestamp and compute chip and sensor periods */
129 delta
= it
->up
- it
->lo
;
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 */
136 ts
->period
= ts
->mult
* ts
->chip_period
.val
;
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
;
147 /* if interrupt interval is valid, sync with interrupt timestamp */
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
;
162 ts
->timestamp
+= delta
;
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
)
173 if (ts
->new_mult
== 0)
176 /* update to new multiplier and update period */
177 ts
->mult
= ts
->new_mult
;
179 ts
->period
= ts
->mult
* ts
->chip_period
.val
;
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.
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
;