Commit 0ecc363c authored by Jean-Baptiste Maneyrol's avatar Jean-Baptiste Maneyrol Committed by Jonathan Cameron
Browse files

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: default avatarJean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
Reviewed-by: default avatarAndy Shevchenko <andy.shevchenko@gmail.com>
Link: https://lore.kernel.org/r/20230606162147.79667-4-inv.git-commit@tdk.com


Signed-off-by: default avatarJonathan Cameron <Jonathan.Cameron@huawei.com>
parent d99ff463

drivers/iio/common/inv_sensors/Makefile

+1 −1
Original line number Diff line number Diff line
@@ -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

drivers/iio/common/inv_sensors/inv_icm42600_timestamp.c→drivers/iio/common/inv_sensors/inv_sensors_timestamp.c

+43 −44
Original line number Diff line number Diff line
@@ -8,20 +8,18 @@ 
#include <linux/math64.h>

#include <linux/module.h>



#include <linux/iio/common/inv_icm42600_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)

#include <linux/iio/common/inv_sensors_timestamp.h>



/* 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,

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,

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,9 +155,9 @@ 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,

void inv_sensors_timestamp_apply_odr(struct inv_sensors_timestamp *ts,

				     uint32_t fifo_period, size_t fifo_nb,

				     unsigned int fifo_no)

{
@@ -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");

drivers/iio/imu/inv_icm42600/inv_icm42600_accel.c

+20 −12
Original line number Diff line number Diff line
@@ -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,7 +331,7 @@ 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),

	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,7 +777,7 @@ 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,

			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 */
@@ -777,7 +785,7 @@ int inv_icm42600_accel_parse_fifo(struct iio_dev *indio_dev) 
		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);

	}

drivers/iio/imu/inv_icm42600/inv_icm42600_buffer.c

+15 −15
Original line number Diff line number Diff line
@@ -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,7 +513,7 @@ 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,

	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);
@@ -523,7 +523,7 @@ 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,

	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);
@@ -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,7 +553,7 @@ 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,

		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);
@@ -563,7 +563,7 @@ 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,

		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);

drivers/iio/imu/inv_icm42600/inv_icm42600_core.c

+0 −1
Original line number Diff line number Diff line
@@ -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"