iv/linux
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

iio: make invensense timestamp module generic

Browse Source 
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 2023-06-06 16:21:46 +00:00 committed by Jonathan Cameron
parent d99ff463ec
commit 0ecc363cce
8 changed files with 193 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
drivers/iio/common/inv_sensors/Makefile
View File

@ -3,4 +3,4 @@
# Makefile for TDK-InvenSense sensors module. # 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

85
drivers/iio/common/inv_sensors/inv_icm42600_timestamp.c → drivers/iio/common/inv_sensors/inv_sensors_timestamp.c
View File

@ -8,20 +8,18 @@
#include <linux/math64.h> #include <linux/math64.h>
#include <linux/module.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 */ /* compute jitter, min and max following jitter in per mille */
#define INV_ICM42600_TIMESTAMP_PERIOD 31250 #define INV_SENSORS_TIMESTAMP_JITTER(_val, _jitter) \
/* allow a jitter of +/- 2% */ (div_s64((_val) * (_jitter), 1000))
#define INV_ICM42600_TIMESTAMP_JITTER 2 #define INV_SENSORS_TIMESTAMP_MIN(_val, _jitter) \
/* compute min and max periods accepted */ (((_val) * (1000 - (_jitter))) / 1000)
#define INV_ICM42600_TIMESTAMP_MIN_PERIOD(_p) \ #define INV_SENSORS_TIMESTAMP_MAX(_val, _jitter) \
(((_p) * (100 - INV_ICM42600_TIMESTAMP_JITTER)) / 100) (((_val) * (1000 + (_jitter))) / 1000)
#define INV_ICM42600_TIMESTAMP_MAX_PERIOD(_p) \
(((_p) * (100 + INV_ICM42600_TIMESTAMP_JITTER)) / 100)
/* Add a new value inside an accumulator and update the estimate value */ /* 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; uint64_t sum = 0;
size_t i; 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); acc->val = div_u64(sum, i);
} }
void inv_icm42600_timestamp_init(struct inv_icm42600_timestamp *ts, void inv_sensors_timestamp_init(struct inv_sensors_timestamp *ts,
uint32_t period) const struct inv_sensors_timestamp_chip *chip)
{ {
/* initial odr for sensor after reset is 1kHz */ memset(ts, 0, sizeof(*ts));
const uint32_t default_period = 1000000;
/* 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 */ /* current multiplier and period values after reset */
ts->mult = default_period / INV_ICM42600_TIMESTAMP_PERIOD; ts->mult = chip->init_period / chip->clock_period;
ts->period = default_period; ts->period = chip->init_period;
/* new set multiplier is the one from chip initialization */
ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD;
/* use theoretical value for chip 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, int inv_sensors_timestamp_update_odr(struct inv_sensors_timestamp *ts,
uint32_t period, bool fifo) uint32_t period, bool fifo)
{ {
/* when FIFO is on, prevent odr change if one is already pending */ /* when FIFO is on, prevent odr change if one is already pending */
if (fifo && ts->new_mult != 0) if (fifo && ts->new_mult != 0)
return -EAGAIN; return -EAGAIN;
ts->new_mult = period / INV_ICM42600_TIMESTAMP_PERIOD; ts->new_mult = period / ts->chip.clock_period;
return 0; 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; uint32_t period_min, period_max;
/* check that period is acceptable */ /* check that period is acceptable */
period_min = INV_ICM42600_TIMESTAMP_MIN_PERIOD(chip_period) * mult; period_min = ts->min_period * mult;
period_max = INV_ICM42600_TIMESTAMP_MAX_PERIOD(chip_period) * mult; period_max = ts->max_period * mult;
if (period > period_min && period < period_max) if (period > period_min && period < period_max)
return true; return true;
else else
return false; return false;
} }
static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts, static bool inv_update_chip_period(struct inv_sensors_timestamp *ts,
uint32_t mult, uint32_t period) uint32_t mult, uint32_t period)
{ {
uint32_t new_chip_period; uint32_t new_chip_period;
if (!inv_validate_period(period, mult)) if (!inv_validate_period(ts, period, mult))
return false; return false;
/* update chip internal period estimation */ /* update chip internal period estimation */
@ -100,7 +99,7 @@ static bool inv_update_chip_period(struct inv_icm42600_timestamp *ts,
return true; 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 delta, jitter;
int64_t adjust; int64_t adjust;
@ -109,7 +108,7 @@ static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
delta = ts->it.lo - ts->timestamp; delta = ts->it.lo - ts->timestamp;
/* adjust timestamp while respecting jitter */ /* 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) if (delta > jitter)
adjust = jitter; adjust = jitter;
else if (delta < -jitter) else if (delta < -jitter)
@ -120,13 +119,13 @@ static void inv_align_timestamp_it(struct inv_icm42600_timestamp *ts)
ts->timestamp += adjust; 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, uint32_t fifo_period, size_t fifo_nb,
size_t sensor_nb, int64_t timestamp) size_t sensor_nb, int64_t timestamp)
{ {
struct inv_icm42600_timestamp_interval *it; struct inv_sensors_timestamp_interval *it;
int64_t delta, interval; 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; uint32_t period = ts->period;
bool valid = false; bool valid = false;
@ -156,11 +155,11 @@ void inv_icm42600_timestamp_interrupt(struct inv_icm42600_timestamp *ts,
if (valid) if (valid)
inv_align_timestamp_it(ts); 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, void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,
uint32_t fifo_period, size_t fifo_nb, uint32_t fifo_period, size_t fifo_nb,
unsigned int fifo_no) unsigned int fifo_no)
{ {
int64_t interval; int64_t interval;
uint32_t fifo_mult; uint32_t fifo_mult;
@ -181,14 +180,14 @@ void inv_icm42600_timestamp_apply_odr(struct inv_icm42600_timestamp *ts,
*/ */
if (ts->timestamp != 0) { if (ts->timestamp != 0) {
/* compute measured fifo period */ /* 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; fifo_period = fifo_mult * ts->chip_period.val;
/* computes time interval between interrupt and this sample */ /* computes time interval between interrupt and this sample */
interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period; interval = (int64_t)(fifo_nb - fifo_no) * (int64_t)fifo_period;
ts->timestamp = ts->it.up - interval; 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_AUTHOR("InvenSense, Inc.");
MODULE_DESCRIPTION("InvenSense sensors timestamp module"); MODULE_DESCRIPTION("InvenSense sensors timestamp module");

32
drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c
View File

@ -12,7 +12,7 @@
#include <linux/math64.h> #include <linux/math64.h>
#include <linux/iio/buffer.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/iio.h>
#include <linux/iio/kfifo_buf.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) const unsigned long *scan_mask)
{ {
struct inv_icm42600_state *st = iio_device_get_drvdata(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);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0; unsigned int fifo_en = 0;
unsigned int sleep_temp = 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 */ /* 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); ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret) if (ret)
goto out_unlock; goto out_unlock;
@ -312,7 +312,7 @@ static int inv_icm42600_accel_write_odr(struct iio_dev *indio_dev,
int val, int val2) int val, int val2)
{ {
struct inv_icm42600_state *st = iio_device_get_drvdata(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);
struct device *dev = regmap_get_device(st->map); struct device *dev = regmap_get_device(st->map);
unsigned int idx; unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 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); pm_runtime_get_sync(dev);
mutex_lock(&st->lock); mutex_lock(&st->lock);
ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
iio_buffer_enabled(indio_dev)); iio_buffer_enabled(indio_dev));
if (ret) if (ret)
goto out_unlock; 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); struct device *dev = regmap_get_device(st->map);
const char *name; 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; struct iio_dev *indio_dev;
int ret; int ret;
@ -720,8 +721,15 @@ struct iio_dev *inv_icm42600_accel_init(struct inv_icm42600_state *st)
if (!indio_dev) if (!indio_dev)
return ERR_PTR(-ENOMEM); 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); 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); iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name; 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) 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_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; ssize_t i, size;
unsigned int no; unsigned int no;
const void *accel, *gyro, *timestamp; const void *accel, *gyro, *timestamp;
@ -769,15 +777,15 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev)
/* update odr */ /* update odr */
if (odr & INV_ICM42600_SENSOR_ACCEL) if (odr & INV_ICM42600_SENSOR_ACCEL)
inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
st->fifo.nb.total, no); st->fifo.nb.total, no);
/* buffer is copied to userspace, zeroing it to avoid any data leak */ /* buffer is copied to userspace, zeroing it to avoid any data leak */
memset(&buffer, 0, sizeof(buffer)); memset(&buffer, 0, sizeof(buffer));
memcpy(&buffer.accel, accel, sizeof(buffer.accel)); memcpy(&buffer.accel, accel, sizeof(buffer.accel));
/* convert 8 bits FIFO temperature in high resolution format */ /* convert 8 bits FIFO temperature in high resolution format */
buffer.temp = temp ? (*temp * 64) : 0; 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); iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
} }

30
drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c
View File

@ -11,7 +11,7 @@
#include <linux/delay.h> #include <linux/delay.h>
#include <linux/iio/buffer.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/iio.h>
#include "inv_icm42600.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 inv_icm42600_state *st = iio_device_get_drvdata(indio_dev);
struct device *dev = regmap_get_device(st->map); 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); pm_runtime_get_sync(dev);
mutex_lock(&st->lock); mutex_lock(&st->lock);
inv_icm42600_timestamp_reset(ts); inv_sensors_timestamp_reset(ts);
mutex_unlock(&st->lock); mutex_unlock(&st->lock);
return 0; 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) int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
{ {
struct inv_icm42600_timestamp *ts; struct inv_sensors_timestamp *ts;
int ret; int ret;
if (st->fifo.nb.total == 0) 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 */ /* handle gyroscope timestamp and FIFO data parsing */
ts = iio_priv(st->indio_gyro); ts = iio_priv(st->indio_gyro);
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, inv_sensors_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
st->fifo.nb.gyro, st->timestamp.gyro); st->fifo.nb.gyro, st->timestamp.gyro);
if (st->fifo.nb.gyro > 0) { if (st->fifo.nb.gyro > 0) {
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret) if (ret)
@ -523,8 +523,8 @@ int inv_icm42600_buffer_fifo_parse(struct inv_icm42600_state *st)
/* handle accelerometer timestamp and FIFO data parsing */ /* handle accelerometer timestamp and FIFO data parsing */
ts = iio_priv(st->indio_accel); ts = iio_priv(st->indio_accel);
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total, inv_sensors_timestamp_interrupt(ts, st->fifo.period, st->fifo.nb.total,
st->fifo.nb.accel, st->timestamp.accel); st->fifo.nb.accel, st->timestamp.accel);
if (st->fifo.nb.accel > 0) { if (st->fifo.nb.accel > 0) {
ret = inv_icm42600_accel_parse_fifo(st->indio_accel); ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret) 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, int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
unsigned int count) unsigned int count)
{ {
struct inv_icm42600_timestamp *ts; struct inv_sensors_timestamp *ts;
int64_t gyro_ts, accel_ts; int64_t gyro_ts, accel_ts;
int ret; int ret;
@ -553,9 +553,9 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
if (st->fifo.nb.gyro > 0) { if (st->fifo.nb.gyro > 0) {
ts = iio_priv(st->indio_gyro); ts = iio_priv(st->indio_gyro);
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, inv_sensors_timestamp_interrupt(ts, st->fifo.period,
st->fifo.nb.total, st->fifo.nb.gyro, st->fifo.nb.total, st->fifo.nb.gyro,
gyro_ts); gyro_ts);
ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro); ret = inv_icm42600_gyro_parse_fifo(st->indio_gyro);
if (ret) if (ret)
return ret; return ret;
@ -563,9 +563,9 @@ int inv_icm42600_buffer_hwfifo_flush(struct inv_icm42600_state *st,
if (st->fifo.nb.accel > 0) { if (st->fifo.nb.accel > 0) {
ts = iio_priv(st->indio_accel); ts = iio_priv(st->indio_accel);
inv_icm42600_timestamp_interrupt(ts, st->fifo.period, inv_sensors_timestamp_interrupt(ts, st->fifo.period,
st->fifo.nb.total, st->fifo.nb.accel, st->fifo.nb.total, st->fifo.nb.accel,
accel_ts); accel_ts);
ret = inv_icm42600_accel_parse_fifo(st->indio_accel); ret = inv_icm42600_accel_parse_fifo(st->indio_accel);
if (ret) if (ret)
return ret; return ret;

1
drivers/iio/imu/inv_icm42600/inv_icm42600_core.c
View File

@ -16,7 +16,6 @@
#include <linux/property.h> #include <linux/property.h>
#include <linux/regmap.h> #include <linux/regmap.h>
#include <linux/iio/common/inv_icm42600_timestamp.h>
#include <linux/iio/iio.h> #include <linux/iio/iio.h>
#include "inv_icm42600.h" #include "inv_icm42600.h"

32
drivers/iio/imu/inv_icm42600/inv_icm42600_gyro.c
View File

@ -12,7 +12,7 @@
#include <linux/math64.h> #include <linux/math64.h>
#include <linux/iio/buffer.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/iio.h>
#include <linux/iio/kfifo_buf.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) const unsigned long *scan_mask)
{ {
struct inv_icm42600_state *st = iio_device_get_drvdata(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);
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT;
unsigned int fifo_en = 0; unsigned int fifo_en = 0;
unsigned int sleep_gyro = 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 */ /* 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); ret = inv_icm42600_buffer_set_fifo_en(st, fifo_en | st->fifo.en);
if (ret) if (ret)
goto out_unlock; goto out_unlock;
@ -324,7 +324,7 @@ static int inv_icm42600_gyro_write_odr(struct iio_dev *indio_dev,
int val, int val2) int val, int val2)
{ {
struct inv_icm42600_state *st = iio_device_get_drvdata(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);
struct device *dev = regmap_get_device(st->map); struct device *dev = regmap_get_device(st->map);
unsigned int idx; unsigned int idx;
struct inv_icm42600_sensor_conf conf = INV_ICM42600_SENSOR_CONF_INIT; 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); pm_runtime_get_sync(dev);
mutex_lock(&st->lock); mutex_lock(&st->lock);
ret = inv_icm42600_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr), ret = inv_sensors_timestamp_update_odr(ts, inv_icm42600_odr_to_period(conf.odr),
iio_buffer_enabled(indio_dev)); iio_buffer_enabled(indio_dev));
if (ret) if (ret)
goto out_unlock; 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); struct device *dev = regmap_get_device(st->map);
const char *name; 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; struct iio_dev *indio_dev;
int ret; int ret;
@ -731,8 +732,15 @@ struct iio_dev *inv_icm42600_gyro_init(struct inv_icm42600_state *st)
if (!indio_dev) if (!indio_dev)
return ERR_PTR(-ENOMEM); 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); 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); iio_device_set_drvdata(indio_dev, st);
indio_dev->name = name; 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) 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_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; ssize_t i, size;
unsigned int no; unsigned int no;
const void *accel, *gyro, *timestamp; const void *accel, *gyro, *timestamp;
@ -781,15 +789,15 @@ int inv_icm42600_gyro_parse_fifo(struct iio_dev *indio_dev)
/* update odr */ /* update odr */
if (odr & INV_ICM42600_SENSOR_GYRO) if (odr & INV_ICM42600_SENSOR_GYRO)
inv_icm42600_timestamp_apply_odr(ts, st->fifo.period, inv_sensors_timestamp_apply_odr(ts, st->fifo.period,
st->fifo.nb.total, no); st->fifo.nb.total, no);
/* buffer is copied to userspace, zeroing it to avoid any data leak */ /* buffer is copied to userspace, zeroing it to avoid any data leak */
memset(&buffer, 0, sizeof(buffer)); memset(&buffer, 0, sizeof(buffer));
memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro)); memcpy(&buffer.gyro, gyro, sizeof(buffer.gyro));
/* convert 8 bits FIFO temperature in high resolution format */ /* convert 8 bits FIFO temperature in high resolution format */
buffer.temp = temp ? (*temp * 64) : 0; 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); iio_push_to_buffers_with_timestamp(indio_dev, &buffer, ts_val);
} }

79
include/linux/iio/common/inv_icm42600_timestamp.h
View File

@ -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
View 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