drivers/iio/imu/inv_icm42600/inv_icm42600_timestamp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2020 Invensense, Inc.
   4  */
   5
   6 #include <linux/kernel.h>
   7 #include <linux/regmap.h>
   8 #include <linux/math64.h>
   9
  10 #include "inv_icm42600.h"
  11 #include "inv_icm42600_timestamp.h"
  12
  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)
  22
  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)
  25 {
  26         uint64_t sum = 0;
  27         size_t i;
  28
  29         acc->values[acc->idx++] = val;
  30         if (acc->idx >= ARRAY_SIZE(acc->values))
  31                 acc->idx = 0;
  32
  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)
  36                         break;
  37                 sum += acc->values[i];
  38         }
  39
  40         acc->val = div_u64(sum, i);
  41 }
  42
  43 void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
  44                                  uint32_t period)
  45 {
  46         /* initial odr for sensor after reset is 1kHz */
  47         const uint32_t default_period = 1000000;
  48
  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;
  54
  55         /* use theoretical value for chip period */
  56         inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
  57 }
  58
  59 int inv_icm42600_timestamp_setup(struct inv_icm42600_state *st)
  60 {
  61         unsigned int val;
  62
  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);
  68 }
  69
  70 int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
  71                                       uint32_t period, bool fifo)
  72 {
  73         /* when FIFO is on, prevent odr change if one is already pending */
  74         if (fifo && ts->new_mult != 0)
  75                 return -EAGAIN;
  76
  77         ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
  78
  79         return 0;
  80 }
  81
  82 static bool inv_validate_period(uint32_t period, uint32_t mult)
  83 {
  84         const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
  85         uint32_t period_min, period_max;
  86
  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)
  91                 return true;
  92         else
  93                 return false;
  94 }
  95
  96 static bool inv_compute_chip_period(struct inv_icm42600_timestamp *ts,
  97                                     uint32_t mult, uint32_t period)
  98 {
  99         uint32_t new_chip_period;
 100
 101         if (!inv_validate_period(period, mult))
 102                 return false;
 103
 104         /* update chip internal period estimation */
 105         new_chip_period = period / mult;
 106         inv_update_acc(&ts->chip_period, new_chip_period);
 107
 108         return true;
 109 }
 110
 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)
 114 {
 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;
 119         int32_t m;
 120         bool valid = false;
 121
 122         if (fifo_nb == 0)
 123                 return;
 124
 125         /* update interrupt timestamp and compute chip and sensor periods */
 126         it = &ts->it;
 127         it->lo = it->up;
 128         it->up = timestamp;
 129         delta = it->up - it->lo;
 130         if (it->lo != 0) {
 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 */
 135                 if (valid)
 136                         ts->period = ts->mult * ts->chip_period.val;
 137         }
 138
 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;
 144                 return;
 145         }
 146
 147         /* if interrupt interval is valid, sync with interrupt timestamp */
 148         if (valid) {
 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;
 158                 if (delta > m)
 159                         delta = m;
 160                 else if (delta < -m)
 161                         delta = -m;
 162                 ts->timestamp += delta;
 163         }
 164 }
 165
 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)
 169 {
 170         int64_t interval;
 171         uint32_t fifo_mult;
 172
 173         if (ts->new_mult == 0)
 174                 return;
 175
 176         /* update to new multiplier and update period */
 177         ts->mult = ts->new_mult;
 178         ts->new_mult = 0;
 179         ts->period = ts->mult * ts->chip_period.val;
 180
 181         /*
 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.
 186          */
 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;
 194         }
 195 }