mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-08-22 17:01:14 +00:00
iio: make invensense timestamp module generic
Rename common module to inv_sensors_timestamp, add configuration at init (chip internal clock, acceptable jitter, ...) and update inv_icm42600 driver integration. Signed-off-by: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com> Reviewed-by: Andy Shevchenko <andy.shevchenko@gmail.com> Link: https://lore.kernel.org/r/20230606162147.79667-4-inv.git-commit@tdk.com Signed-off-by: Jonathan Cameron <Jonathan.Cameron@huawei.com>
This commit is contained in:
Jean-Baptiste Maneyrol
Jonathan Cameron
parent
d99ff463ec
commit
0ecc363cce
8 changed files with 193 additions and 163 deletions
|
|
@ -3,4 +3,4 @@
|
|||
# Makefile for TDK-InvenSense sensors module.
|
||||
#
|
||||
|
||||
obj-$(CONFIG_IIO_INV_SENSORS_TIMESTAMP) += inv_icm42600_timestamp.o
|
||||
obj-$(CONFIG_IIO_INV_SENSORS_TIMESTAMP) += inv_sensors_timestamp.o
|
||||
|
|
|
|||
|
|
@ -8,20 +8,18 @@
|
|||
#include <linux/math64.h>
|
||||
#include <linux/module.h>
|
||||
|
||||
#include <linux/iio/common/inv_icm42600_timestamp.h>
|
||||
#include <linux/iio/common/inv_sensors_timestamp.h>
|
||||
|
||||
/* internal chip period is 32kHz, 31250ns */
|
||||
#define INV_ICM42600_TIMESTAMP_PERIOD 31250
|
||||
/* allow a jitter of +/- 2% */
|
||||
#define INV_ICM42600_TIMESTAMP_JITTER 2
|
||||
/* compute min and max periods accepted */
|
||||
#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \
|
||||
(((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100)
|
||||
#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
|
||||
(((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
|
||||
/* compute jitter, min and max following jitter in per mille */
|
||||
#define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \
|
||||
(div_s64((_val) * (_jitter), 1000))
|
||||
#define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
|
||||
(((_val) * (1000 - (_jitter))) / 1000)
|
||||
#define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
|
||||
(((_val) * (1000 + (_jitter))) / 1000)
|
||||
|
||||
/* Add a new value inside an accumulator and update the estimate value */
|
||||
static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
|
||||
static void inv_update_acc(struct inv_sensors_timestamp_acc *acc, uint32_t val)
|
||||
{
|
||||
uint64_t sum = 0;
|
||||
size_t i;
|
||||
|
|
@ -40,56 +38,57 @@ static void inv_update_acc(struct inv_icm42600_timestamp_acc *acc, uint32_t val)
|
|||
acc->val = div_u64(sum, i);
|
||||
}
|
||||
|
||||
void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t period)
|
||||
void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
|
||||
const struct inv_sensors_timestamp_chip *chip)
|
||||
{
|
||||
/* initial odr for sensor after reset is 1kHz */
|
||||
const uint32_t default_period = 1000000;
|
||||
memset(ts, 0, sizeof(*ts));
|
||||
|
||||
/* save chip parameters and compute min and max clock period */
|
||||
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);
|
||||
|
||||
/* current multiplier and period values after reset */
|
||||
ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
ts->period = default_period;
|
||||
/* new set multiplier is the one from chip initialization */
|
||||
ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
ts->mult = chip->init_period / chip->clock_period;
|
||||
ts->period = chip->init_period;
|
||||
|
||||
/* use theoretical value for chip period */
|
||||
inv_update_acc(&ts->chip_period, INV_ICM42600_TIMESTAMP_PERIOD);
|
||||
inv_update_acc(&ts->chip_period, chip->clock_period);
|
||||
}
|
||||
EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
|
||||
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_init, IIO_INV_SENSORS_TIMESTAMP);
|
||||
|
||||
int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t period, bool fifo)
|
||||
int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
|
||||
uint32_t period, bool fifo)
|
||||
{
|
||||
/* when FIFO is on, prevent odr change if one is already pending */
|
||||
if (fifo && ts->new_mult != 0)
|
||||
return -EAGAIN;
|
||||
|
||||
ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
ts->new_mult = period / ts->chip.clock_period;
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
|
||||
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_update_odr, IIO_INV_SENSORS_TIMESTAMP);
|
||||
|
||||
static bool inv_validate_period(uint32_t period, uint32_t mult)
|
||||
static bool inv_validate_period(struct inv_sensors_timestamp *ts, uint32_t period, uint32_t mult)
|
||||
{
|
||||
const uint32_t chip_period = INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
uint32_t period_min, period_max;
|
||||
|
||||
/* check that period is acceptable */
|
||||
period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult;
|
||||
period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult;
|
||||
period_min = ts->min_period * mult;
|
||||
period_max = ts->max_period * mult;
|
||||
if (period > period_min && period < period_max)
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t mult, uint32_t period)
|
||||
static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
|
||||
uint32_t mult, uint32_t period)
|
||||
{
|
||||
uint32_t new_chip_period;
|
||||
|
||||
if (!inv_validate_period(period, mult))
|
||||
if (!inv_validate_period(ts, period, mult))
|
||||
return false;
|
||||
|
||||
/* update chip internal period estimation */
|
||||
|
|
@ -100,7 +99,7 @@ static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts,
|
|||
return true;
|
||||
}
|
||||
|
||||
static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
|
||||
static void inv_align_timestamp_it(struct inv_sensors_timestamp *ts)
|
||||
{
|
||||
int64_t delta, jitter;
|
||||
int64_t adjust;
|
||||
|
|
@ -109,7 +108,7 @@ static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
|
|||
delta = ts->it.lo - ts->timestamp;
|
||||
|
||||
/* adjust timestamp while respecting jitter */
|
||||
jitter = div_s64((int64_t)ts->period * INV_ICM42600_TIMESTAMP_JITTER, 100);
|
||||
jitter = INV_SENSORS_TIMESTAMP_JITTER((int64_t)ts->period, ts->chip.jitter);
|
||||
if (delta > jitter)
|
||||
adjust = jitter;
|
||||
else if (delta < -jitter)
|
||||
|
|
@ -120,13 +119,13 @@ static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
|
|||
ts->timestamp += adjust;
|
||||
}
|
||||
|
||||
void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
|
||||
void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
size_t sensor_nb, int64_t timestamp)
|
||||
{
|
||||
struct inv_icm42600_timestamp_interval *it;
|
||||
struct inv_sensors_timestamp_interval *it;
|
||||
int64_t delta, interval;
|
||||
const uint32_t fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
const uint32_t fifo_mult = fifo_period / ts->chip.clock_period;
|
||||
uint32_t period = ts->period;
|
||||
bool valid = false;
|
||||
|
||||
|
|
@ -156,11 +155,11 @@ void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
|
|||
if (valid)
|
||||
inv_align_timestamp_it(ts);
|
||||
}
|
||||
EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
|
||||
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_interrupt, IIO_INV_SENSORS_TIMESTAMP);
|
||||
|
||||
void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
unsigned int fifo_no)
|
||||
void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
unsigned int fifo_no)
|
||||
{
|
||||
int64_t interval;
|
||||
uint32_t fifo_mult;
|
||||
|
|
@ -181,14 +180,14 @@ void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
|
|||
*/
|
||||
if (ts->timestamp != 0) {
|
||||
/* compute measured fifo period */
|
||||
fifo_mult = fifo_period / INV_ICM42600_TIMESTAMP_PERIOD;
|
||||
fifo_mult = fifo_period / ts->chip.clock_period;
|
||||
fifo_period = fifo_mult * ts->chip_period.val;
|
||||
/* computes time interval between interrupt and this sample */
|
||||
interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
|
||||
ts->timestamp = ts->it.up - interval;
|
||||
}
|
||||
}
|
||||
EXPORT_SYMBOL_NS_GPL(inv_icm42600_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
|
||||
EXPORT_SYMBOL_NS_GPL(inv_sensors_timestamp_apply_odr, IIO_INV_SENSORS_TIMESTAMP);
|
||||
|
||||
MODULE_AUTHOR("InvenSense, Inc.");
|
||||
MODULE_DESCRIPTION("InvenSense sensors timestamp module");
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
#include <linux/math64.h>
|
||||
|
||||
#include <linux/iio/buffer.h>
|
||||
#include <linux/iio/common/inv_icm42600_timestamp.h>
|
||||
#include <linux/iio/common/inv_sensors_timestamp.h>
|
||||
#include <linux/iio/iio.h>
|
||||
#include <linux/iio/kfifo_buf.h>
|
||||
|
||||
|
|
@ -99,7 +99,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
|
|||
const unsigned long *scan_mask)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
|
||||
unsigned int fifo_en = 0;
|
||||
unsigned int sleep_temp = 0;
|
||||
|
|
@ -127,7 +127,7 @@ static int inv_icm42600_accel_update_scan_mode(struct iio_dev *indio_dev,
|
|||
}
|
||||
|
||||
/* update data FIFO write */
|
||||
inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
|
||||
inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
|
||||
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
|
||||
if (ret)
|
||||
goto out_unlock;
|
||||
|
|
@ -312,7 +312,7 @@ static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
|
|||
int val, int val2)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
struct device *dev = regmap_get_device(st->map);
|
||||
unsigned int idx;
|
||||
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
|
||||
|
|
@ -331,8 +331,8 @@ static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
|
|||
pm_runtime_get_sync(dev);
|
||||
mutex_lock(&st->lock);
|
||||
|
||||
ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
|
||||
iio_buffer_enabled(indio_dev));
|
||||
ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
|
||||
iio_buffer_enabled(indio_dev));
|
||||
if (ret)
|
||||
goto out_unlock;
|
||||
|
||||
|
|
@ -708,7 +708,8 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
|
|||
{
|
||||
struct device *dev = regmap_get_device(st->map);
|
||||
const char *name;
|
||||
struct inv_icm42600_timestamp *ts;
|
||||
struct inv_sensors_timestamp_chip ts_chip;
|
||||
struct inv_sensors_timestamp *ts;
|
||||
struct iio_dev *indio_dev;
|
||||
int ret;
|
||||
|
||||
|
|
@ -720,8 +721,15 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
|
|||
if (!indio_dev)
|
||||
return ERR_PTR(-ENOMEM);
|
||||
|
||||
/*
|
||||
* clock period is 32kHz (31250ns)
|
||||
* jitter is +/- 2% (20 per mille)
|
||||
*/
|
||||
ts_chip.clock_period = 31250;
|
||||
ts_chip.jitter = 20;
|
||||
ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
|
||||
ts = iio_priv(indio_dev);
|
||||
inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.accel.odr));
|
||||
inv_sensors_timestamp_init(ts, &ts_chip);
|
||||
|
||||
iio_device_set_drvdata(indio_dev, st);
|
||||
indio_dev->name = name;
|
||||
|
|
@ -746,7 +754,7 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
|
|||
int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
ssize_t i, size;
|
||||
unsigned int no;
|
||||
const void *accel, *gyro, *timestamp;
|
||||
|
|
@ -769,15 +777,15 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
|
|||
|
||||
/* update odr */
|
||||
if (odr & INV_ICM42600_SENSOR_ACCEL)
|
||||
inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
|
||||
st->fifo.nb.total, no);
|
||||
inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
|
||||
st->fifo.nb.total, no);
|
||||
|
||||
/* buffer is copied to userspace, zeroing it to avoid any data leak */
|
||||
memset(&buffer, 0, sizeof(buffer));
|
||||
memcpy(&buffer.accel, accel, sizeof(buffer.accel));
|
||||
/* convert 8 bits FIFO temperature in high resolution format */
|
||||
buffer.temp = temp ? (*temp * 64) : 0;
|
||||
ts_val = inv_icm42600_timestamp_pop(ts);
|
||||
ts_val = inv_sensors_timestamp_pop(ts);
|
||||
iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
#include <linux/delay.h>
|
||||
|
||||
#include <linux/iio/buffer.h>
|
||||
#include <linux/iio/common/inv_icm42600_timestamp.h>
|
||||
#include <linux/iio/common/inv_sensors_timestamp.h>
|
||||
#include <linux/iio/iio.h>
|
||||
|
||||
#include "inv_icm42600.h"
|
||||
|
|
@ -276,12 +276,12 @@ static int inv_icm42600_buffer_preenable(struct iio_dev *indio_dev)
|
|||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct device *dev = regmap_get_device(st->map);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
|
||||
pm_runtime_get_sync(dev);
|
||||
|
||||
mutex_lock(&st->lock);
|
||||
inv_icm42600_timestamp_reset(ts);
|
||||
inv_sensors_timestamp_reset(ts);
|
||||
mutex_unlock(&st->lock);
|
||||
|
||||
return 0;
|
||||
|
|
@ -505,7 +505,7 @@ int inv_icm42600_buffer_fifo_read(struct inv_icm42600_state *st,
|
|||
|
||||
int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
|
||||
{
|
||||
struct inv_icm42600_timestamp *ts;
|
||||
struct inv_sensors_timestamp *ts;
|
||||
int ret;
|
||||
|
||||
if (st->fifo.nb.total == 0)
|
||||
|
|
@ -513,8 +513,8 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
|
|||
|
||||
/* handle gyroscope timestamp and FIFO data parsing */
|
||||
ts = iio_priv(st->indio_gyro);
|
||||
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
|
||||
st->fifo.nb.gyro, st->timestamp.gyro);
|
||||
inv_sensors_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
|
||||
st->fifo.nb.gyro, st->timestamp.gyro);
|
||||
if (st->fifo.nb.gyro > 0) {
|
||||
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
|
||||
if (ret)
|
||||
|
|
@ -523,8 +523,8 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
|
|||
|
||||
/* handle accelerometer timestamp and FIFO data parsing */
|
||||
ts = iio_priv(st->indio_accel);
|
||||
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
|
||||
st->fifo.nb.accel, st->timestamp.accel);
|
||||
inv_sensors_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
|
||||
st->fifo.nb.accel, st->timestamp.accel);
|
||||
if (st->fifo.nb.accel > 0) {
|
||||
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
|
||||
if (ret)
|
||||
|
|
@ -537,7 +537,7 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
|
|||
int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
|
||||
unsigned int count)
|
||||
{
|
||||
struct inv_icm42600_timestamp *ts;
|
||||
struct inv_sensors_timestamp *ts;
|
||||
int64_t gyro_ts, accel_ts;
|
||||
int ret;
|
||||
|
||||
|
|
@ -553,9 +553,9 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
|
|||
|
||||
if (st->fifo.nb.gyro > 0) {
|
||||
ts = iio_priv(st->indio_gyro);
|
||||
inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
|
||||
st->fifo.nb.total, st->fifo.nb.gyro,
|
||||
gyro_ts);
|
||||
inv_sensors_timestamp_interrupt(ts, st->fifo.period,
|
||||
st->fifo.nb.total, st->fifo.nb.gyro,
|
||||
gyro_ts);
|
||||
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
|
@ -563,9 +563,9 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
|
|||
|
||||
if (st->fifo.nb.accel > 0) {
|
||||
ts = iio_priv(st->indio_accel);
|
||||
inv_icm42600_timestamp_interrupt(ts, st->fifo.period,
|
||||
st->fifo.nb.total, st->fifo.nb.accel,
|
||||
accel_ts);
|
||||
inv_sensors_timestamp_interrupt(ts, st->fifo.period,
|
||||
st->fifo.nb.total, st->fifo.nb.accel,
|
||||
accel_ts);
|
||||
ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
|
|
|||
|
|
@ -16,7 +16,6 @@
|
|||
#include <linux/property.h>
|
||||
#include <linux/regmap.h>
|
||||
|
||||
#include <linux/iio/common/inv_icm42600_timestamp.h>
|
||||
#include <linux/iio/iio.h>
|
||||
|
||||
#include "inv_icm42600.h"
|
||||
|
|
|
|||
|
|
@ -12,7 +12,7 @@
|
|||
#include <linux/math64.h>
|
||||
|
||||
#include <linux/iio/buffer.h>
|
||||
#include <linux/iio/common/inv_icm42600_timestamp.h>
|
||||
#include <linux/iio/common/inv_sensors_timestamp.h>
|
||||
#include <linux/iio/iio.h>
|
||||
#include <linux/iio/kfifo_buf.h>
|
||||
|
||||
|
|
@ -99,7 +99,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
|
|||
const unsigned long *scan_mask)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
|
||||
unsigned int fifo_en = 0;
|
||||
unsigned int sleep_gyro = 0;
|
||||
|
|
@ -127,7 +127,7 @@ static int inv_icm42600_gyro_update_scan_mode(struct iio_dev *indio_dev,
|
|||
}
|
||||
|
||||
/* update data FIFO write */
|
||||
inv_icm42600_timestamp_apply_odr(ts, 0, 0, 0);
|
||||
inv_sensors_timestamp_apply_odr(ts, 0, 0, 0);
|
||||
ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
|
||||
if (ret)
|
||||
goto out_unlock;
|
||||
|
|
@ -324,7 +324,7 @@ static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
|
|||
int val, int val2)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
struct device *dev = regmap_get_device(st->map);
|
||||
unsigned int idx;
|
||||
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
|
||||
|
|
@ -343,8 +343,8 @@ static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
|
|||
pm_runtime_get_sync(dev);
|
||||
mutex_lock(&st->lock);
|
||||
|
||||
ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
|
||||
iio_buffer_enabled(indio_dev));
|
||||
ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
|
||||
iio_buffer_enabled(indio_dev));
|
||||
if (ret)
|
||||
goto out_unlock;
|
||||
|
||||
|
|
@ -719,7 +719,8 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
|
|||
{
|
||||
struct device *dev = regmap_get_device(st->map);
|
||||
const char *name;
|
||||
struct inv_icm42600_timestamp *ts;
|
||||
struct inv_sensors_timestamp_chip ts_chip;
|
||||
struct inv_sensors_timestamp *ts;
|
||||
struct iio_dev *indio_dev;
|
||||
int ret;
|
||||
|
||||
|
|
@ -731,8 +732,15 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
|
|||
if (!indio_dev)
|
||||
return ERR_PTR(-ENOMEM);
|
||||
|
||||
/*
|
||||
* clock period is 32kHz (31250ns)
|
||||
* jitter is +/- 2% (20 per mille)
|
||||
*/
|
||||
ts_chip.clock_period = 31250;
|
||||
ts_chip.jitter = 20;
|
||||
ts_chip.init_period = inv_icm42600_odr_to_period(st->conf.accel.odr);
|
||||
ts = iio_priv(indio_dev);
|
||||
inv_icm42600_timestamp_init(ts, inv_icm42600_odr_to_period(st->conf.gyro.odr));
|
||||
inv_sensors_timestamp_init(ts, &ts_chip);
|
||||
|
||||
iio_device_set_drvdata(indio_dev, st);
|
||||
indio_dev->name = name;
|
||||
|
|
@ -758,7 +766,7 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
|
|||
int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
|
||||
{
|
||||
struct inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
|
||||
struct inv_icm42600_timestamp *ts = iio_priv(indio_dev);
|
||||
struct inv_sensors_timestamp *ts = iio_priv(indio_dev);
|
||||
ssize_t i, size;
|
||||
unsigned int no;
|
||||
const void *accel, *gyro, *timestamp;
|
||||
|
|
@ -781,15 +789,15 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
|
|||
|
||||
/* update odr */
|
||||
if (odr & INV_ICM42600_SENSOR_GYRO)
|
||||
inv_icm42600_timestamp_apply_odr(ts, st->fifo.period,
|
||||
st->fifo.nb.total, no);
|
||||
inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
|
||||
st->fifo.nb.total, no);
|
||||
|
||||
/* buffer is copied to userspace, zeroing it to avoid any data leak */
|
||||
memset(&buffer, 0, sizeof(buffer));
|
||||
memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
|
||||
/* convert 8 bits FIFO temperature in high resolution format */
|
||||
buffer.temp = temp ? (*temp * 64) : 0;
|
||||
ts_val = inv_icm42600_timestamp_pop(ts);
|
||||
ts_val = inv_sensors_timestamp_pop(ts);
|
||||
iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,79 +0,0 @@
|
|||
/* SPDX-License-Identifier: GPL-2.0-or-later */
|
||||
/*
|
||||
* Copyright (C) 2020 Invensense, Inc.
|
||||
*/
|
||||
|
||||
#ifndef INV_ICM42600_TIMESTAMP_H_
|
||||
#define INV_ICM42600_TIMESTAMP_H_
|
||||
|
||||
/**
|
||||
* struct inv_icm42600_timestamp_interval - timestamps interval
|
||||
* @lo: interval lower bound
|
||||
* @up: interval upper bound
|
||||
*/
|
||||
struct inv_icm42600_timestamp_interval {
|
||||
int64_t lo;
|
||||
int64_t up;
|
||||
};
|
||||
|
||||
/**
|
||||
* struct inv_icm42600_timestamp_acc - accumulator for computing an estimation
|
||||
* @val: current estimation of the value, the mean of all values
|
||||
* @idx: current index of the next free place in values table
|
||||
* @values: table of all measured values, use for computing the mean
|
||||
*/
|
||||
struct inv_icm42600_timestamp_acc {
|
||||
uint32_t val;
|
||||
size_t idx;
|
||||
uint32_t values[32];
|
||||
};
|
||||
|
||||
/**
|
||||
* struct inv_icm42600_timestamp - timestamp management states
|
||||
* @it: interrupts interval timestamps
|
||||
* @timestamp: store last timestamp for computing next data timestamp
|
||||
* @mult: current internal period multiplier
|
||||
* @new_mult: new set internal period multiplier (not yet effective)
|
||||
* @period: measured current period of the sensor
|
||||
* @chip_period: accumulator for computing internal chip period
|
||||
*/
|
||||
struct inv_icm42600_timestamp {
|
||||
struct inv_icm42600_timestamp_interval it;
|
||||
int64_t timestamp;
|
||||
uint32_t mult;
|
||||
uint32_t new_mult;
|
||||
uint32_t period;
|
||||
struct inv_icm42600_timestamp_acc chip_period;
|
||||
};
|
||||
|
||||
void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t period);
|
||||
|
||||
int inv_icm42600_timestamp_update_odr(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t period, bool fifo);
|
||||
|
||||
void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
size_t sensor_nb, int64_t timestamp);
|
||||
|
||||
static inline int64_t
|
||||
inv_icm42600_timestamp_pop(struct inv_icm42600_timestamp *ts)
|
||||
{
|
||||
ts->timestamp += ts->period;
|
||||
return ts->timestamp;
|
||||
}
|
||||
|
||||
void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
unsigned int fifo_no);
|
||||
|
||||
static inline void
|
||||
inv_icm42600_timestamp_reset(struct inv_icm42600_timestamp *ts)
|
||||
{
|
||||
const struct inv_icm42600_timestamp_interval interval_init = {0LL, 0LL};
|
||||
|
||||
ts->it = interval_init;
|
||||
ts->timestamp = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
95
include/linux/iio/common/inv_sensors_timestamp.h
Normal file
95
include/linux/iio/common/inv_sensors_timestamp.h
Normal file
|
|
@ -0,0 +1,95 @@
|
|||
/* SPDX-License-Identifier: GPL-2.0-or-later */
|
||||
/*
|
||||
* Copyright (C) 2020 Invensense, Inc.
|
||||
*/
|
||||
|
||||
#ifndef INV_SENSORS_TIMESTAMP_H_
|
||||
#define INV_SENSORS_TIMESTAMP_H_
|
||||
|
||||
/**
|
||||
* struct inv_sensors_timestamp_chip - chip internal properties
|
||||
* @clock_period: internal clock period in ns
|
||||
* @jitter: acceptable jitter in per-mille
|
||||
* @init_period: chip initial period at reset in ns
|
||||
*/
|
||||
struct inv_sensors_timestamp_chip {
|
||||
uint32_t clock_period;
|
||||
uint32_t jitter;
|
||||
uint32_t init_period;
|
||||
};
|
||||
|
||||
/**
|
||||
* struct inv_sensors_timestamp_interval - timestamps interval
|
||||
* @lo: interval lower bound
|
||||
* @up: interval upper bound
|
||||
*/
|
||||
struct inv_sensors_timestamp_interval {
|
||||
int64_t lo;
|
||||
int64_t up;
|
||||
};
|
||||
|
||||
/**
|
||||
* struct inv_sensors_timestamp_acc - accumulator for computing an estimation
|
||||
* @val: current estimation of the value, the mean of all values
|
||||
* @idx: current index of the next free place in values table
|
||||
* @values: table of all measured values, use for computing the mean
|
||||
*/
|
||||
struct inv_sensors_timestamp_acc {
|
||||
uint32_t val;
|
||||
size_t idx;
|
||||
uint32_t values[32];
|
||||
};
|
||||
|
||||
/**
|
||||
* struct inv_sensors_timestamp - timestamp management states
|
||||
* @chip: chip internal characteristics
|
||||
* @min_period: minimal acceptable clock period
|
||||
* @max_period: maximal acceptable clock period
|
||||
* @it: interrupts interval timestamps
|
||||
* @timestamp: store last timestamp for computing next data timestamp
|
||||
* @mult: current internal period multiplier
|
||||
* @new_mult: new set internal period multiplier (not yet effective)
|
||||
* @period: measured current period of the sensor
|
||||
* @chip_period: accumulator for computing internal chip period
|
||||
*/
|
||||
struct inv_sensors_timestamp {
|
||||
struct inv_sensors_timestamp_chip chip;
|
||||
uint32_t min_period;
|
||||
uint32_t max_period;
|
||||
struct inv_sensors_timestamp_interval it;
|
||||
int64_t timestamp;
|
||||
uint32_t mult;
|
||||
uint32_t new_mult;
|
||||
uint32_t period;
|
||||
struct inv_sensors_timestamp_acc chip_period;
|
||||
};
|
||||
|
||||
void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
|
||||
const struct inv_sensors_timestamp_chip *chip);
|
||||
|
||||
int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
|
||||
uint32_t period, bool fifo);
|
||||
|
||||
void inv_sensors_timestamp_interrupt(struct inv_sensors_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
size_t sensor_nb, int64_t timestamp);
|
||||
|
||||
static inline int64_t inv_sensors_timestamp_pop(struct inv_sensors_timestamp *ts)
|
||||
{
|
||||
ts->timestamp += ts->period;
|
||||
return ts->timestamp;
|
||||
}
|
||||
|
||||
void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
|
||||
uint32_t fifo_period, size_t fifo_nb,
|
||||
unsigned int fifo_no);
|
||||
|
||||
static inline void inv_sensors_timestamp_reset(struct inv_sensors_timestamp *ts)
|
||||
{
|
||||
const struct inv_sensors_timestamp_interval interval_init = {0LL, 0LL};
|
||||
|
||||
ts->it = interval_init;
|
||||
ts->timestamp = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Reference in a new issue