#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/math64.h>
#include <linux/module.h>
#include <linux/iio/common/inv_sensors_timestamp.h>
#define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
(((_val) * (1000 - (_jitter))) / 1000)
#define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
(((_val) * (1000 + (_jitter))) / 1000)
#define INV_SENSORS_MIN_IT_DELTA (20 * NSEC_PER_MSEC)
static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, u32 val)
{
u64 sum = 0;
size_t i;
acc->values[acc->idx++] = val;
if (acc->idx >= ARRAY_SIZE(acc->values))
acc->idx = 0;
for (i = 0; i < ARRAY_SIZE(acc->values); ++i) {
if (acc->values[i] == 0)
break;
sum += acc->values[i];
}
acc->val = div_u64(sum, i);
}
void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
const struct inv_sensors_timestamp_chip *chip)
{
memset(ts, 0, sizeof(*ts));
ts->chip = *chip;
ts->min_period = INV_SENSORS_TIMESTAMP_MIN(chip->clock_period, chip->jitter);
ts->max_period = INV_SENSORS_TIMESTAMP_MAX(chip->clock_period, chip->jitter);
ts->mult = chip->init_period / chip->clock_period;
ts->period = chip->init_period;
inv_update_acc(&ts->chip_period, chip->clock_period);
}
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, "IIO_INV_SENSORS_TIMESTAMP");
int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
u32 period, bool fifo)
{
u32 mult;
if (fifo && ts->new_mult != 0)
return -EAGAIN;
mult = period / ts->chip.clock_period;
if (mult != ts->mult)
ts->new_mult = mult;
if (!fifo)
inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
return 0;
}
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, "IIO_INV_SENSORS_TIMESTAMP");
static bool inv_validate_period(struct inv_sensors_timestamp *ts, u32 period)
{
u32 period_min, period_max;
period_min = ts->min_period * ts->mult;
period_max = ts->max_period * ts->mult;
if (period > period_min && period < period_max)
return true;
else
return false;
}
static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
u32 period)
{
u32 new_chip_period;
if (!inv_validate_period(ts, period))
return false;
new_chip_period = period / ts->mult;
inv_update_acc(&ts->chip_period, new_chip_period);
return true;
}
static u32 inv_align_timestamp_it(struct inv_sensors_timestamp *ts,
unsigned int sample_nb)
{
const s64 period_min = (s64)ts->min_period * ts->mult;
const s64 period_max = (s64)ts->max_period * ts->mult;
s64 new_period;
new_period = div_s64(ts->it.up - ts->timestamp, sample_nb);
return clamp(new_period, period_min, period_max);
}
void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
size_t sample_nb, s64 timestamp)
{
struct inv_sensors_timestamp_interval *it;
s64 delta, delta_threshold, interval;
u32 period;
bool valid = false;
if (sample_nb == 0)
return;
if (ts->timestamp == 0) {
interval = (s64)ts->period * (s64)sample_nb;
ts->timestamp = timestamp - interval;
}
it = &ts->delta;
ts->delta_counter += sample_nb;
delta = timestamp - it->up;
delta_threshold = INV_SENSORS_TIMESTAMP_MIN(INV_SENSORS_MIN_IT_DELTA, ts->chip.jitter);
if (delta >= delta_threshold) {
it->lo = it->up;
it->up = timestamp;
if (it->lo != 0) {
delta = it->up - it->lo;
period = div_s64(delta, ts->delta_counter);
inv_update_chip_period(ts, period);
}
ts->delta_counter = 0;
}
it = &ts->it;
it->lo = it->up;
it->up = timestamp;
delta = it->up - it->lo;
if (it->lo != 0) {
period = div_s64(delta, sample_nb);
valid = inv_validate_period(ts, period);
}
period = inv_align_timestamp_it(ts, sample_nb);
if (valid)
ts->period = period;
else
ts->period = min(ts->mult * ts->chip_period.val, period);
}
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, "IIO_INV_SENSORS_TIMESTAMP");
void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
u32 fifo_period, size_t fifo_nb,
unsigned int fifo_no)
{
s64 interval;
u32 fifo_mult;
if (ts->new_mult == 0)
return;
ts->mult = ts->new_mult;
ts->new_mult = 0;
ts->period = ts->mult * ts->chip_period.val;
if (ts->timestamp != 0) {
fifo_mult = fifo_period / ts->chip.clock_period;
fifo_period = fifo_mult * ts->chip_period.val;
interval = (s64)(fifo_nb - fifo_no) * (s64)fifo_period;
ts->timestamp = ts->it.up - interval;
}
}
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, "IIO_INV_SENSORS_TIMESTAMP");
MODULE_AUTHOR("InvenSense, Inc.");
MODULE_DESCRIPTION("InvenSense sensors timestamp module");
MODULE_LICENSE("GPL"