diff options
Diffstat (limited to 'drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c')
| -rw-r--r-- | drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c | 195 |
1 files changed, 0 insertions, 195 deletions
diff --git a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c b/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c deleted file mode 100644 index 7f2dc41f807b..000000000000 --- a/drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c +++ /dev/null @@ -1,195 +0,0 @@ -// SPDX-License-Identifier: GPL-2.0-or-later -/* - * Copyright (C) 2020 Invensense, Inc. - */ - -#include <linux/kernel.h> -#include <linux/regmap.h> -#include <linux/math64.h> - -#include "inv_icm42600.h" -#include "inv_icm42600_timestamp.h" - -/* internal chip period is 32kHz, 31250ns */ -#define INV_ICM42600_TIMESTAMP_PERIOD 31250 -/* allow a jitter of +/- 2% */ -#define INV_ICM42600_TIMESTAMP_JITTER 2 -/* compute min and max periods accepted */ -#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \ - (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100) -#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \ - (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100) - -/* Add a new value inside an accumulator and update the estimate value */ -static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val) -{ - uint64_t sum = 0; - size_t i; - - acc->values[acc->idx++] = val; - if (acc->idx >= ARRAY_SIZE(acc->values)) - acc->idx = 0; - - /* compute the mean of all stored values, use 0 as empty slot */ - for (i = 0; i < ARRAY_SIZE(acc->values); ++i) { - if (acc->values[i] == 0) - break; - sum += acc->values[i]; - } - - acc->val = div_u64(sum, i); -} - -void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts, - uint32_t period) -{ - /* initial odr for sensor after reset is 1kHz */ - const uint32_t default_period = 1000000; - - /* current multiplier and period values after reset */ - ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD; - ts->period = default_period; - /* new set multiplier is the one from chip initialization */ - ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD; - - /* use theoretical value for chip period */ - inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD); -} - -int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st) -{ - unsigned int val; - - /* enable timestamp register */ - val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN | - INV_ICM42600_TMST_CONFIG_TMST_EN; - return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG, - INV_ICM42600_TMST_CONFIG_MASK, val); -} - -int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts, - uint32_t period, bool fifo) -{ - /* when FIFO is on, prevent odr change if one is already pending */ - if (fifo && ts->new_mult != 0) - return -EAGAIN; - - ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD; - - return 0; -} - -static bool inv_validate_period(uint32_t period, uint32_t mult) -{ - const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD; - uint32_t period_min, period_max; - - /* check that period is acceptable */ - period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult; - period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult; - if (period > period_min && period < period_max) - return true; - else - return false; -} - -static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts, - uint32_t mult, uint32_t period) -{ - uint32_t new_chip_period; - - if (!inv_validate_period(period, mult)) - return false; - - /* update chip internal period estimation */ - new_chip_period = period / mult; - inv_update_acc(&ts->chip_period, new_chip_period); - - return true; -} - -void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts, - uint32_t fifo_period, size_t fifo_nb, - size_t sensor_nb, int64_t timestamp) -{ - struct inv_icm42600_timestamp_interval *it; - int64_t delta, interval; - const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD; - uint32_t period = ts->period; - int32_t m; - bool valid = false; - - if (fifo_nb == 0) - return; - - /* update interrupt timestamp and compute chip and sensor periods */ - it = &ts->it; - it->lo = it->up; - it->up = timestamp; - delta = it->up - it->lo; - if (it->lo != 0) { - /* compute period: delta time divided by number of samples */ - period = div_s64(delta, fifo_nb); - valid = inv_compute_chip_period(ts, fifo_mult, period); - /* update sensor period if chip internal period is updated */ - if (valid) - ts->period = ts->mult * ts->chip_period.val; - } - - /* no previous data, compute theoritical value from interrupt */ - if (ts->timestamp == 0) { - /* elapsed time: sensor period * sensor samples number */ - interval = (int64_t)ts->period * (int64_t)sensor_nb; - ts->timestamp = it->up - interval; - return; - } - - /* if interrupt interval is valid, sync with interrupt timestamp */ - if (valid) { - /* compute measured fifo_period */ - fifo_period = fifo_mult * ts->chip_period.val; - /* delta time between last sample and last interrupt */ - delta = it->lo - ts->timestamp; - /* if there are multiple samples, go back to first one */ - while (delta >= (fifo_period * 3 / 2)) - delta -= fifo_period; - /* compute maximal adjustment value */ - m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period; - if (delta > m) - delta = m; - else if (delta < -m) - delta = -m; - ts->timestamp += delta; - } -} - -void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts, - uint32_t fifo_period, size_t fifo_nb, - unsigned int fifo_no) -{ - int64_t interval; - uint32_t fifo_mult; - - if (ts->new_mult == 0) - return; - - /* update to new multiplier and update period */ - ts->mult = ts->new_mult; - ts->new_mult = 0; - ts->period = ts->mult * ts->chip_period.val; - - /* - * After ODR change the time interval with the previous sample is - * undertermined (depends when the change occures). So we compute the - * timestamp from the current interrupt using the new FIFO period, the - * total number of samples and the current sample numero. - */ - if (ts->timestamp != 0) { - /* compute measured fifo period */ - fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD; - fifo_period = fifo_mult * ts->chip_period.val; - /* computes time interval between interrupt and this sample */ - interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period; - ts->timestamp = ts->it.up - interval; - } -} |