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 = (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 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"); 206