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 */
22static 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
41void 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}
58EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
59
60int 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}
71EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
72
73static 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
86static 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(&ts->chip_period, new_chip_period);
97	ts->period = ts->mult * ts->chip_period.val;
98
99	return true;
100}
101
102static 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
122void 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(delta, fifo_nb);
143		valid = inv_update_chip_period(ts, 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}
158EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
159
160void 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}
190EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
191
192MODULE_AUTHOR("InvenSense, Inc.");
193MODULE_DESCRIPTION("InvenSense sensors timestamp module");
194MODULE_LICENSE("GPL");
195