linux-kernel/linux-7.2.3/drivers/iio/imu/inv_icm45600/inv_icm45600_buffer.c

// SPDX-License-Identifier: GPL-2.0-or-later
/* Copyright (C) 2025 Invensense, Inc. */

#include <linux/bitfield.h>
#include <linux/delay.h>
#include <linux/device.h>
#include <linux/err.h>
#include <linux/minmax.h>
#include <linux/mutex.h>
#include <linux/pm_runtime.h>
#include <linux/regmap.h>
#include <linux/time.h>
#include <linux/types.h>

#include <asm/byteorder.h>

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

#include "inv_icm45600_buffer.h"
#include "inv_icm45600.h"

/* FIFO header: 1 byte */
#define INV_ICM45600_FIFO_EXT_HEADER		BIT(7)
#define INV_ICM45600_FIFO_HEADER_ACCEL		BIT(6)
#define INV_ICM45600_FIFO_HEADER_GYRO		BIT(5)
#define INV_ICM45600_FIFO_HEADER_HIGH_RES	BIT(4)
#define INV_ICM45600_FIFO_HEADER_TMST_FSYNC	GENMASK(3, 2)
#define INV_ICM45600_FIFO_HEADER_ODR_ACCEL	BIT(1)
#define INV_ICM45600_FIFO_HEADER_ODR_GYRO	BIT(0)

struct inv_icm45600_fifo_1sensor_packet {
	u8 header;
	struct inv_icm45600_fifo_sensor_data data;
	s8 temp;
} __packed;

struct inv_icm45600_fifo_2sensors_packet {
	u8 header;
	struct inv_icm45600_fifo_sensor_data accel;
	struct inv_icm45600_fifo_sensor_data gyro;
	s8 temp;
	__le16 timestamp;
} __packed;

ssize_t inv_icm45600_fifo_decode_packet(const void *packet,
					const struct inv_icm45600_fifo_sensor_data **accel,
					const struct inv_icm45600_fifo_sensor_data **gyro,
					const s8 **temp,
					const __le16 **timestamp, unsigned int *odr)
{
	const struct inv_icm45600_fifo_1sensor_packet *pack1 = packet;
	const struct inv_icm45600_fifo_2sensors_packet *pack2 = packet;
	u8 header = *((const u8 *)packet);

	/* FIFO extended header */
	if (header & INV_ICM45600_FIFO_EXT_HEADER) {
		/* Not yet supported */
		return 0;
	}

	/* handle odr flags. */
	*odr = 0;
	if (header & INV_ICM45600_FIFO_HEADER_ODR_GYRO)
		*odr |= INV_ICM45600_SENSOR_GYRO;
	if (header & INV_ICM45600_FIFO_HEADER_ODR_ACCEL)
		*odr |= INV_ICM45600_SENSOR_ACCEL;

	/* Accel + Gyro data are present. */
	if ((header & INV_ICM45600_FIFO_HEADER_ACCEL) &&
	    (header & INV_ICM45600_FIFO_HEADER_GYRO)) {
		*accel = &pack2->accel;
		*gyro = &pack2->gyro;
		*temp = &pack2->temp;
		*timestamp = &pack2->timestamp;
		return sizeof(*pack2);
	}

	/* Accel data only. */
	if (header & INV_ICM45600_FIFO_HEADER_ACCEL) {
		*accel = &pack1->data;
		*gyro = NULL;
		*temp = &pack1->temp;
		*timestamp = NULL;
		return sizeof(*pack1);
	}

	/* Gyro data only. */
	if (header & INV_ICM45600_FIFO_HEADER_GYRO) {
		*accel = NULL;
		*gyro = &pack1->data;
		*temp = &pack1->temp;
		*timestamp = NULL;
		return sizeof(*pack1);
	}

	/* Invalid packet if here. */
	return -EINVAL;
}

void inv_icm45600_buffer_update_fifo_period(struct inv_icm45600_state *st)
{
	u32 period_gyro, period_accel;

	if (st->fifo.en & INV_ICM45600_SENSOR_GYRO)
		period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr);
	else
		period_gyro = U32_MAX;

	if (st->fifo.en & INV_ICM45600_SENSOR_ACCEL)
		period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr);
	else
		period_accel = U32_MAX;

	st->fifo.period = min(period_gyro, period_accel);
}

int inv_icm45600_buffer_set_fifo_en(struct inv_icm45600_state *st,
				    unsigned int fifo_en)
{
	unsigned int mask;
	int ret;

	mask = INV_ICM45600_FIFO_CONFIG3_GYRO_EN |
	       INV_ICM45600_FIFO_CONFIG3_ACCEL_EN;

	ret = regmap_assign_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3, mask,
				 (fifo_en & INV_ICM45600_SENSOR_GYRO) ||
				 (fifo_en & INV_ICM45600_SENSOR_ACCEL));
	if (ret)
		return ret;

	st->fifo.en = fifo_en;
	inv_icm45600_buffer_update_fifo_period(st);

	return 0;
}

static unsigned int inv_icm45600_wm_truncate(unsigned int watermark, size_t packet_size,
					     unsigned int fifo_period)
{
	size_t watermark_max, grace_samples;

	/* Keep 20ms for processing FIFO.*/
	grace_samples = (20U * NSEC_PER_MSEC) / fifo_period;
	if (grace_samples < 1)
		grace_samples = 1;

	watermark_max = INV_ICM45600_FIFO_SIZE_MAX / packet_size;
	watermark_max -= grace_samples;

	return min(watermark, watermark_max);
}

/**
 * inv_icm45600_buffer_update_watermark - update watermark FIFO threshold
 * @st:	driver internal state
 *
 * FIFO watermark threshold is computed based on the required watermark values
 * set for gyro and accel sensors. Since watermark is all about acceptable data
 * latency, use the smallest setting between the 2. It means choosing the
 * smallest latency but this is not as simple as choosing the smallest watermark
 * value. Latency depends on watermark and ODR. It requires several steps:
 * 1) compute gyro and accel latencies and choose the smallest value.
 * 2) adapt the chosen latency so that it is a multiple of both gyro and accel
 *    ones. Otherwise it is possible that you don't meet a requirement. (for
 *    example with gyro @100Hz wm 4 and accel @100Hz with wm 6, choosing the
 *    value of 4 will not meet accel latency requirement because 6 is not a
 *    multiple of 4. You need to use the value 2.)
 * 3) Since all periods are multiple of each others, watermark is computed by
 *    dividing this computed latency by the smallest period, which corresponds
 *    to the FIFO frequency.
 *
 * Returns: 0 on success, a negative error code otherwise.
 */
int inv_icm45600_buffer_update_watermark(struct inv_icm45600_state *st)
{
	const size_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet);
	unsigned int wm_gyro, wm_accel, watermark;
	u32 period_gyro, period_accel, period;
	u32 latency_gyro, latency_accel, latency;

	/* Compute sensors latency, depending on sensor watermark and odr. */
	wm_gyro = inv_icm45600_wm_truncate(st->fifo.watermark.gyro, packet_size,
					   st->fifo.period);
	wm_accel = inv_icm45600_wm_truncate(st->fifo.watermark.accel, packet_size,
					    st->fifo.period);
	/* Use us for odr to avoid overflow using 32 bits values. */
	period_gyro = inv_icm45600_odr_to_period(st->conf.gyro.odr) / NSEC_PER_USEC;
	period_accel = inv_icm45600_odr_to_period(st->conf.accel.odr) / NSEC_PER_USEC;
	latency_gyro = period_gyro * wm_gyro;
	latency_accel = period_accel * wm_accel;

	/* 0 value for watermark means that the sensor is turned off. */
	if (wm_gyro == 0 && wm_accel == 0)
		return 0;

	if (latency_gyro == 0) {
		watermark = wm_accel;
		st->fifo.watermark.eff_accel = wm_accel;
	} else if (latency_accel == 0) {
		watermark = wm_gyro;
		st->fifo.watermark.eff_gyro = wm_gyro;
	} else {
		/* Compute the smallest latency that is a multiple of both. */
		if (latency_gyro <= latency_accel)
			latency = latency_gyro - (latency_accel % latency_gyro);
		else
			latency = latency_accel - (latency_gyro % latency_accel);
		/* Use the shortest period. */
		period = min(period_gyro, period_accel);
		/* All this works because periods are multiple of each others. */
		watermark = max(latency / period, 1);
		/* Update effective watermark. */
		st->fifo.watermark.eff_gyro = max(latency / period_gyro, 1);
		st->fifo.watermark.eff_accel = max(latency / period_accel, 1);
	}

	st->buffer.u16 = cpu_to_le16(watermark);
	return regmap_bulk_write(st->map, INV_ICM45600_REG_FIFO_WATERMARK,
				 &st->buffer.u16, sizeof(st->buffer.u16));
}

static int inv_icm45600_buffer_preenable(struct iio_dev *indio_dev)
{
	struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
	struct device *dev = regmap_get_device(st->map);
	struct inv_icm45600_sensor_state *sensor_st = iio_priv(indio_dev);
	struct inv_sensors_timestamp *ts = &sensor_st->ts;
	int ret;

	ret = pm_runtime_resume_and_get(dev);
	if (ret)
		return ret;

	guard(mutex)(&st->lock);
	inv_sensors_timestamp_reset(ts);

	return 0;
}

/*
 * Update_scan_mode callback is turning sensors on and setting data FIFO enable
 * bits.
 */
static int inv_icm45600_buffer_postenable(struct iio_dev *indio_dev)
{
	struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
	unsigned int val;
	int ret;

	guard(mutex)(&st->lock);

	/* Exit if FIFO is already on. */
	if (st->fifo.on) {
		st->fifo.on++;
		return 0;
	}

	ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
			      INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH);
	if (ret)
		return ret;

	ret = regmap_set_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0,
			      INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN |
			      INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN);
	if (ret)
		return ret;

	val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
			 INV_ICM45600_FIFO_CONFIG0_MODE_STREAM);
	ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0,
				 INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val);
	if (ret)
		return ret;

	/* Enable writing sensor data to FIFO. */
	ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3,
			      INV_ICM45600_FIFO_CONFIG3_IF_EN);
	if (ret)
		return ret;

	st->fifo.on++;
	return 0;
}

static int inv_icm45600_buffer_predisable(struct iio_dev *indio_dev)
{
	struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
	unsigned int val;
	int ret;

	guard(mutex)(&st->lock);

	/* Exit if there are several sensors using the FIFO. */
	if (st->fifo.on > 1) {
		st->fifo.on--;
		return 0;
	}

	/* Disable writing sensor data to FIFO. */
	ret = regmap_clear_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG3,
				INV_ICM45600_FIFO_CONFIG3_IF_EN);
	if (ret)
		return ret;

	val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
			 INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS);
	ret = regmap_update_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG0,
				 INV_ICM45600_FIFO_CONFIG0_MODE_MASK, val);
	if (ret)
		return ret;

	ret = regmap_clear_bits(st->map, INV_ICM45600_REG_INT1_CONFIG0,
				INV_ICM45600_INT1_CONFIG0_FIFO_THS_EN |
				INV_ICM45600_INT1_CONFIG0_FIFO_FULL_EN);
	if (ret)
		return ret;

	ret = regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
			      INV_ICM45600_REG_FIFO_CONFIG2_FIFO_FLUSH);
	if (ret)
		return ret;

	st->fifo.on--;
	return 0;
}

static int _inv_icm45600_buffer_postdisable(struct inv_icm45600_state *st,
					    unsigned int sensor, unsigned int *watermark,
					    unsigned int *sleep)
{
	struct inv_icm45600_sensor_conf conf = INV_ICM45600_SENSOR_CONF_KEEP_VALUES;
	int ret;

	ret = inv_icm45600_buffer_set_fifo_en(st, st->fifo.en & ~sensor);
	if (ret)
		return ret;

	*watermark = 0;
	ret = inv_icm45600_buffer_update_watermark(st);
	if (ret)
		return ret;

	conf.mode = INV_ICM45600_SENSOR_MODE_OFF;
	if (sensor == INV_ICM45600_SENSOR_GYRO)
		return inv_icm45600_set_gyro_conf(st, &conf, sleep);
	else
		return inv_icm45600_set_accel_conf(st, &conf, sleep);
}

static int inv_icm45600_buffer_postdisable(struct iio_dev *indio_dev)
{
	struct inv_icm45600_state *st = iio_device_get_drvdata(indio_dev);
	struct device *dev = regmap_get_device(st->map);
	unsigned int sensor;
	unsigned int *watermark;
	unsigned int sleep;
	int ret;

	if (indio_dev == st->indio_gyro) {
		sensor = INV_ICM45600_SENSOR_GYRO;
		watermark = &st->fifo.watermark.gyro;
	} else if (indio_dev == st->indio_accel) {
		sensor = INV_ICM45600_SENSOR_ACCEL;
		watermark = &st->fifo.watermark.accel;
	} else {
		return -EINVAL;
	}

	sleep = 0;
	scoped_guard(mutex, &st->lock)
		ret = _inv_icm45600_buffer_postdisable(st, sensor, watermark, &sleep);

	/* Sleep required time. */
	if (sleep)
		msleep(sleep);

	pm_runtime_put_autosuspend(dev);

	return ret;
}

const struct iio_buffer_setup_ops inv_icm45600_buffer_ops = {
	.preenable = inv_icm45600_buffer_preenable,
	.postenable = inv_icm45600_buffer_postenable,
	.predisable = inv_icm45600_buffer_predisable,
	.postdisable = inv_icm45600_buffer_postdisable,
};

int inv_icm45600_buffer_fifo_read(struct inv_icm45600_state *st,
				  unsigned int max)
{
	const ssize_t packet_size = sizeof(struct inv_icm45600_fifo_2sensors_packet);
	__le16 *raw_fifo_count;
	size_t fifo_nb, i;
	ssize_t size;
	const struct inv_icm45600_fifo_sensor_data *accel, *gyro;
	const __le16 *timestamp;
	const s8 *temp;
	unsigned int odr;
	int ret;

	/* Reset all samples counters. */
	st->fifo.count = 0;
	st->fifo.nb.gyro = 0;
	st->fifo.nb.accel = 0;
	st->fifo.nb.total = 0;

	raw_fifo_count = &st->buffer.u16;
	ret = regmap_bulk_read(st->map, INV_ICM45600_REG_FIFO_COUNT,
			       raw_fifo_count, sizeof(*raw_fifo_count));
	if (ret)
		return ret;

	/* Check and limit number of samples if requested. */
	fifo_nb = le16_to_cpup(raw_fifo_count);
	if (fifo_nb == 0)
		return 0;
	if (max > 0 && fifo_nb > max)
		fifo_nb = max;

	/* Try to read all FIFO data in internal buffer. */
	st->fifo.count = fifo_nb * packet_size;
	ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA,
				st->fifo.data, st->fifo.count);
	if (ret == -ENOTSUPP || ret == -EFBIG) {
		/* Read full fifo is not supported, read samples one by one. */
		ret = 0;
		for (i = 0; i < st->fifo.count && ret == 0; i += packet_size)
			ret = regmap_noinc_read(st->map, INV_ICM45600_REG_FIFO_DATA,
						&st->fifo.data[i], packet_size);
	}
	if (ret)
		return ret;

	for (i = 0; i < st->fifo.count; i += size) {
		size = inv_icm45600_fifo_decode_packet(&st->fifo.data[i], &accel, &gyro,
						       &temp, &timestamp, &odr);
		if (size <= 0)
			/* No more sample in buffer */
			break;
		if (gyro && inv_icm45600_fifo_is_data_valid(gyro))
			st->fifo.nb.gyro++;
		if (accel && inv_icm45600_fifo_is_data_valid(accel))
			st->fifo.nb.accel++;
		st->fifo.nb.total++;
	}

	return 0;
}

int inv_icm45600_buffer_fifo_parse(struct inv_icm45600_state *st)
{
	struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
	struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel);
	struct inv_sensors_timestamp *ts;
	int ret;

	if (st->fifo.nb.total == 0)
		return 0;

	/* Handle gyroscope timestamp and FIFO data parsing. */
	if (st->fifo.nb.gyro > 0) {
		ts = &gyro_st->ts;
		inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_gyro,
						st->timestamp.gyro);
		ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro);
		if (ret)
			return ret;
	}

	/* Handle accelerometer timestamp and FIFO data parsing. */
	if (st->fifo.nb.accel > 0) {
		ts = &accel_st->ts;
		inv_sensors_timestamp_interrupt(ts, st->fifo.watermark.eff_accel,
						st->timestamp.accel);
		ret = inv_icm45600_accel_parse_fifo(st->indio_accel);
		if (ret)
			return ret;
	}

	return 0;
}

int inv_icm45600_buffer_hwfifo_flush(struct inv_icm45600_state *st,
				     unsigned int count)
{
	struct inv_icm45600_sensor_state *gyro_st = iio_priv(st->indio_gyro);
	struct inv_icm45600_sensor_state *accel_st = iio_priv(st->indio_accel);
	struct inv_sensors_timestamp *ts;
	s64 gyro_ts, accel_ts;
	int ret;

	gyro_ts = iio_get_time_ns(st->indio_gyro);
	accel_ts = iio_get_time_ns(st->indio_accel);

	ret = inv_icm45600_buffer_fifo_read(st, count);
	if (ret)
		return ret;

	if (st->fifo.nb.total == 0)
		return 0;

	if (st->fifo.nb.gyro > 0) {
		ts = &gyro_st->ts;
		inv_sensors_timestamp_interrupt(ts, st->fifo.nb.gyro, gyro_ts);
		ret = inv_icm45600_gyro_parse_fifo(st->indio_gyro);
		if (ret)
			return ret;
	}

	if (st->fifo.nb.accel > 0) {
		ts = &accel_st->ts;
		inv_sensors_timestamp_interrupt(ts, st->fifo.nb.accel, accel_ts);
		ret = inv_icm45600_accel_parse_fifo(st->indio_accel);
		if (ret)
			return ret;
	}

	return 0;
}

int inv_icm45600_buffer_init(struct inv_icm45600_state *st)
{
	int ret;
	unsigned int val;

	st->fifo.watermark.eff_gyro = 1;
	st->fifo.watermark.eff_accel = 1;

	/* Disable all FIFO EN bits. */
	ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG3, 0);
	if (ret)
		return ret;

	/* Disable FIFO and set depth. */
	val = FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_MODE_MASK,
			 INV_ICM45600_FIFO_CONFIG0_MODE_BYPASS) |
	      FIELD_PREP(INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MASK,
			 INV_ICM45600_FIFO_CONFIG0_FIFO_DEPTH_MAX);

	ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG0, val);
	if (ret)
		return ret;

	/* Enable only timestamp in fifo, disable compression. */
	ret = regmap_write(st->map, INV_ICM45600_REG_FIFO_CONFIG4,
			   INV_ICM45600_FIFO_CONFIG4_TMST_FSYNC_EN);
	if (ret)
		return ret;

	/* Enable FIFO continuous watermark interrupt. */
	return regmap_set_bits(st->map, INV_ICM45600_REG_FIFO_CONFIG2,
			       INV_ICM45600_REG_FIFO_CONFIG2_WM_GT_TH);
}