drivers/iio/common/inv_sensors/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 */
  22 static 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
  41 void 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 }
  58 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, "IIO_INV_SENSORS_TIMESTAMP");
  59
  60 int 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 }
  79 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, "IIO_INV_SENSORS_TIMESTAMP");
  80
  81 static 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
  94 static 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
 110 static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
 111 {
 112         const int64_t period_min = ts->min_period * ts->mult;
 113         const int64_t period_max = 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
 135 void 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 }
 169 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP");
 170
 171 void 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 }
 201 EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, "IIO_INV_SENSORS_TIMESTAMP");
 202
 203 MODULE_AUTHOR("InvenSense, Inc.");
 204 MODULE_DESCRIPTION("InvenSense sensors timestamp module");
 205 MODULE_LICENSE("GPL");