202 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*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 */22static 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 41void 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}58EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);59 60int 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 return 0;74}75EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);76 77static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period)78{79 uint32_t period_min, period_max;80 81 /* check that period is acceptable */82 period_min = ts->min_period * ts->mult;83 period_max = ts->max_period * ts->mult;84 if (period > period_min && period < period_max)85 return true;86 else87 return false;88}89 90static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,91 uint32_t period)92{93 uint32_t new_chip_period;94 95 if (!inv_validate_period(ts, period))96 return false;97 98 /* update chip internal period estimation */99 new_chip_period = period / ts->mult;100 inv_update_acc(&ts->chip_period, new_chip_period);101 ts->period = ts->mult * ts->chip_period.val;102 103 return true;104}105 106static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)107{108 const int64_t period_min = ts->min_period * ts->mult;109 const int64_t period_max = ts->max_period * ts->mult;110 int64_t add_max, sub_max;111 int64_t delta, jitter;112 int64_t adjust;113 114 /* delta time between last sample and last interrupt */115 delta = ts->it.lo - ts->timestamp;116 117 /* adjust timestamp while respecting jitter */118 add_max = period_max - (int64_t)ts->period;119 sub_max = period_min - (int64_t)ts->period;120 jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);121 if (delta > jitter)122 adjust = add_max;123 else if (delta < -jitter)124 adjust = sub_max;125 else126 adjust = 0;127 128 ts->timestamp += adjust;129}130 131void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,132 size_t sample_nb, int64_t timestamp)133{134 struct inv_sensors_timestamp_interval *it;135 int64_t delta, interval;136 uint32_t period;137 bool valid = false;138 139 if (sample_nb == 0)140 return;141 142 /* update interrupt timestamp and compute chip and sensor periods */143 it = &ts->it;144 it->lo = it->up;145 it->up = timestamp;146 delta = it->up - it->lo;147 if (it->lo != 0) {148 /* compute period: delta time divided by number of samples */149 period = div_s64(delta, sample_nb);150 valid = inv_update_chip_period(ts, period);151 }152 153 /* no previous data, compute theoritical value from interrupt */154 if (ts->timestamp == 0) {155 /* elapsed time: sensor period * sensor samples number */156 interval = (int64_t)ts->period * (int64_t)sample_nb;157 ts->timestamp = it->up - interval;158 return;159 }160 161 /* if interrupt interval is valid, sync with interrupt timestamp */162 if (valid)163 inv_align_timestamp_it(ts);164}165EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);166 167void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,168 uint32_t fifo_period, size_t fifo_nb,169 unsigned int fifo_no)170{171 int64_t interval;172 uint32_t fifo_mult;173 174 if (ts->new_mult == 0)175 return;176 177 /* update to new multiplier and update period */178 ts->mult = ts->new_mult;179 ts->new_mult = 0;180 ts->period = ts->mult * ts->chip_period.val;181 182 /*183 * After ODR change the time interval with the previous sample is184 * undertermined (depends when the change occures). So we compute the185 * timestamp from the current interrupt using the new FIFO period, the186 * total number of samples and the current sample numero.187 */188 if (ts->timestamp != 0) {189 /* compute measured fifo period */190 fifo_mult = fifo_period / ts->chip.clock_period;191 fifo_period = fifo_mult * ts->chip_period.val;192 /* computes time interval between interrupt and this sample */193 interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;194 ts->timestamp = ts->it.up - interval;195 }196}197EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);198 199MODULE_AUTHOR("InvenSense, Inc.");200MODULE_DESCRIPTION("InvenSense sensors timestamp module");201MODULE_LICENSE("GPL");202