inv_sensors_timestamp.c

  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	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}
 79EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, "IIO_INV_SENSORS_TIMESTAMP");
 80
 81static 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
 94static 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
110static 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
135void 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}
169EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP");
170
171void 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}
201EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, "IIO_INV_SENSORS_TIMESTAMP");
202
203MODULE_AUTHOR("InvenSense, Inc.");
204MODULE_DESCRIPTION("InvenSense sensors timestamp module");
205MODULE_LICENSE("GPL");