imu/inv_icm42600/inv_icm42600_timestamp.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Copyright (C) 2020 Invensense, Inc.
0004  */
0005
0006 #include <linux/kernel.h>
0007 #include <linux/regmap.h>
0008 #include <linux/math64.h>
0009
0010 #include "inv_icm42600.h"
0011 #include "inv_icm42600_timestamp.h"
0012
0013 /* internal chip period is 32kHz, 31250ns */
0014 #define INV_ICM42600_TIMESTAMP_PERIOD       31250
0015 /* allow a jitter of +/- 2% */
0016 #define INV_ICM42600_TIMESTAMP_JITTER       2
0017 /* compute min and max periods accepted */
0018 #define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p)       \
0019     (((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
0020 #define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p)       \
0021     (((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
0022
0023 /* Add a new value inside an accumulator and update the estimate value */
0024 static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
0025 {
0026     uint64_t sum = 0;
0027     size_t i;
0028
0029     acc->values[acc->idx++] = val;
0030     if (acc->idx >= ARRAY_SIZE(acc->values))
0031         acc->idx = 0;
0032
0033     /* compute the mean of all stored values, use 0 as empty slot */
0034     for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
0035         if (acc->values[i] == 0)
0036             break;
0037         sum += acc->values[i];
0038     }
0039
0040     acc->val = div_u64(sum, i);
0041 }
0042
0043 void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
0044                  uint32_t period)
0045 {
0046     /* initial odr for sensor after reset is 1kHz */
0047     const uint32_t default_period = 1000000;
0048
0049     /* current multiplier and period values after reset */
0050     ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
0051     ts->period = default_period;
0052     /* new set multiplier is the one from chip initialization */
0053     ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
0054
0055     /* use theoretical value for chip period */
0056     inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
0057 }
0058
0059 int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
0060 {
0061     unsigned int val;
0062
0063     /* enable timestamp register */
0064     val = INV_ICM42600_TMST_CONFIG_TMST_TO_REGS_EN |
0065           INV_ICM42600_TMST_CONFIG_TMST_EN;
0066     return regmap_update_bits(st->map, INV_ICM42600_REG_TMST_CONFIG,
0067                   INV_ICM42600_TMST_CONFIG_MASK, val);
0068 }
0069
0070 int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
0071                       uint32_t period, bool fifo)
0072 {
0073     /* when FIFO is on, prevent odr change if one is already pending */
0074     if (fifo && ts->new_mult != 0)
0075         return -EAGAIN;
0076
0077     ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
0078
0079     return 0;
0080 }
0081
0082 static bool inv_validate_period(uint32_t period, uint32_t mult)
0083 {
0084     const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
0085     uint32_t period_min, period_max;
0086
0087     /* check that period is acceptable */
0088     period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
0089     period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
0090     if (period > period_min && period < period_max)
0091         return true;
0092     else
0093         return false;
0094 }
0095
0096 static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
0097                     uint32_t mult, uint32_t period)
0098 {
0099     uint32_t new_chip_period;
0100
0101     if (!inv_validate_period(period, mult))
0102         return false;
0103
0104     /* update chip internal period estimation */
0105     new_chip_period = period / mult;
0106     inv_update_acc(&ts->chip_period, new_chip_period);
0107
0108     return true;
0109 }
0110
0111 void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
0112                       uint32_t fifo_period, size_t fifo_nb,
0113                       size_t sensor_nb, int64_t timestamp)
0114 {
0115     struct inv_icm42600_timestamp_interval *it;
0116     int64_t delta, interval;
0117     const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
0118     uint32_t period = ts->period;
0119     int32_t m;
0120     bool valid = false;
0121
0122     if (fifo_nb == 0)
0123         return;
0124
0125     /* update interrupt timestamp and compute chip and sensor periods */
0126     it = &ts->it;
0127     it->lo = it->up;
0128     it->up = timestamp;
0129     delta = it->up - it->lo;
0130     if (it->lo != 0) {
0131         /* compute period: delta time divided by number of samples */
0132         period = div_s64(delta, fifo_nb);
0133         valid = inv_compute_chip_period(ts, fifo_mult, period);
0134         /* update sensor period if chip internal period is updated */
0135         if (valid)
0136             ts->period = ts->mult * ts->chip_period.val;
0137     }
0138
0139     /* no previous data, compute theoritical value from interrupt */
0140     if (ts->timestamp == 0) {
0141         /* elapsed time: sensor period * sensor samples number */
0142         interval = (int64_t)ts->period * (int64_t)sensor_nb;
0143         ts->timestamp = it->up - interval;
0144         return;
0145     }
0146
0147     /* if interrupt interval is valid, sync with interrupt timestamp */
0148     if (valid) {
0149         /* compute measured fifo_period */
0150         fifo_period = fifo_mult * ts->chip_period.val;
0151         /* delta time between last sample and last interrupt */
0152         delta = it->lo - ts->timestamp;
0153         /* if there are multiple samples, go back to first one */
0154         while (delta >= (fifo_period * 3 / 2))
0155             delta -= fifo_period;
0156         /* compute maximal adjustment value */
0157         m = INV_ICM42600_TIMESTAMP_MAX_PERIOD(ts->period) - ts->period;
0158         if (delta > m)
0159             delta = m;
0160         else if (delta < -m)
0161             delta = -m;
0162         ts->timestamp += delta;
0163     }
0164 }
0165
0166 void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
0167                       uint32_t fifo_period, size_t fifo_nb,
0168                       unsigned int fifo_no)
0169 {
0170     int64_t interval;
0171     uint32_t fifo_mult;
0172
0173     if (ts->new_mult == 0)
0174         return;
0175
0176     /* update to new multiplier and update period */
0177     ts->mult = ts->new_mult;
0178     ts->new_mult = 0;
0179     ts->period = ts->mult * ts->chip_period.val;
0180
0181     /*
0182      * After ODR change the time interval with the previous sample is
0183      * undertermined (depends when the change occures). So we compute the
0184      * timestamp from the current interrupt using the new FIFO period, the
0185      * total number of samples and the current sample numero.
0186      */
0187     if (ts->timestamp != 0) {
0188         /* compute measured fifo period */
0189         fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
0190         fifo_period = fifo_mult * ts->chip_period.val;
0191         /* computes time interval between interrupt and this sample */
0192         interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
0193         ts->timestamp = ts->it.up - interval;
0194     }
0195 }