linux-kernel/linux-7.2.3/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_fusion.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * STMicroelectronics st_lsm6dsx IMU sensor fusion
 *
 * Copyright 2026 BayLibre, SAS
 */

#include <linux/cleanup.h>
#include <linux/errno.h>
#include <linux/iio/iio.h>
#include <linux/iio/sysfs.h>
#include <linux/mutex.h>
#include <linux/regmap.h>
#include <linux/sprintf.h>
#include <linux/types.h>
#include <linux/units.h>

#include "st_lsm6dsx.h"

static int
st_lsm6dsx_fusion_get_odr_val(const struct st_lsm6dsx_fusion_settings *settings,
			      u32 odr_mHz, u8 *val)
{
	int odr_hz = odr_mHz / MILLI;
	int i;

	for (i = 0; i < settings->odr_len; i++) {
		if (settings->odr_hz[i] == odr_hz)
			break;
	}
	if (i == settings->odr_len)
		return -EINVAL;

	*val = i;
	return 0;
}

/**
 * st_lsm6dsx_fusion_page_enable - Enable access to sensor fusion configuration
 * registers.
 * @hw: Sensor hardware instance.
 *
 * Return: 0 on success, negative value on error.
 */
static int st_lsm6dsx_fusion_page_enable(struct st_lsm6dsx_hw *hw)
{
	const struct st_lsm6dsx_reg *mux;

	mux = &hw->settings->fusion_settings.page_mux;

	return regmap_set_bits(hw->regmap, mux->addr, mux->mask);
}

/**
 * st_lsm6dsx_fusion_page_disable - Disable access to sensor fusion
 * configuration registers.
 * @hw: Sensor hardware instance.
 *
 * Return: 0 on success, negative value on error.
 */
static int st_lsm6dsx_fusion_page_disable(struct st_lsm6dsx_hw *hw)
{
	const struct st_lsm6dsx_reg *mux;

	mux = &hw->settings->fusion_settings.page_mux;

	return regmap_clear_bits(hw->regmap, mux->addr, mux->mask);
}

static int st_lsm6dsx_fusion_set_odr_locked(struct st_lsm6dsx_sensor *sensor,
					    bool enable)
{
	const struct st_lsm6dsx_fusion_settings *settings;
	struct st_lsm6dsx_hw *hw = sensor->hw;
	int err;

	settings = &hw->settings->fusion_settings;
	if (enable) {
		const struct st_lsm6dsx_reg *reg = &settings->odr_reg;
		u8 odr_val;
		u8 data;

		st_lsm6dsx_fusion_get_odr_val(settings, sensor->hwfifo_odr_mHz,
					      &odr_val);
		data = ST_LSM6DSX_SHIFT_VAL(odr_val, reg->mask);
		err = regmap_update_bits(hw->regmap, reg->addr, reg->mask,
					 data);
		if (err)
			return err;
	}

	return regmap_assign_bits(hw->regmap, settings->fifo_enable.addr,
				  settings->fifo_enable.mask, enable);
}

int st_lsm6dsx_fusion_set_enable(struct st_lsm6dsx_sensor *sensor, bool enable)
{
	struct st_lsm6dsx_hw *hw = sensor->hw;
	const struct st_lsm6dsx_reg *en_reg;
	int err;

	guard(mutex)(&hw->page_lock);

	en_reg = &hw->settings->fusion_settings.enable;
	err = st_lsm6dsx_fusion_page_enable(hw);
	if (err)
		return err;

	err = regmap_assign_bits(hw->regmap, en_reg->addr, en_reg->mask, enable);
	if (err) {
		st_lsm6dsx_fusion_page_disable(hw);
		return err;
	}

	return st_lsm6dsx_fusion_page_disable(hw);
}

int st_lsm6dsx_fusion_set_odr(struct st_lsm6dsx_sensor *sensor, bool enable)
{
	struct st_lsm6dsx_hw *hw = sensor->hw;
	int err;

	guard(mutex)(&hw->page_lock);

	err = st_lsm6dsx_fusion_page_enable(hw);
	if (err)
		return err;

	err = st_lsm6dsx_fusion_set_odr_locked(sensor, enable);
	if (err) {
		st_lsm6dsx_fusion_page_disable(hw);
		return err;
	}

	return st_lsm6dsx_fusion_page_disable(hw);
}

static int st_lsm6dsx_fusion_read_raw(struct iio_dev *iio_dev,
				      struct iio_chan_spec const *ch,
				      int *val, int *val2, long mask)
{
	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);

	switch (mask) {
	case IIO_CHAN_INFO_SAMP_FREQ:
		*val = sensor->hwfifo_odr_mHz / MILLI;
		*val2 = (sensor->hwfifo_odr_mHz % MILLI) * (MICRO / MILLI);
		return IIO_VAL_INT_PLUS_MICRO;
	default:
		return -EINVAL;
	}
}

static int st_lsm6dsx_fusion_write_raw(struct iio_dev *iio_dev,
				       struct iio_chan_spec const *chan,
				       int val, int val2, long mask)
{
	struct st_lsm6dsx_sensor *sensor = iio_priv(iio_dev);
	const struct st_lsm6dsx_fusion_settings *settings;
	int err;

	settings = &sensor->hw->settings->fusion_settings;
	switch (mask) {
	case IIO_CHAN_INFO_SAMP_FREQ: {
		u32 odr_mHz = val * MILLI + val2 * (MILLI / MICRO);
		u8 odr_val;

		/* check that the requested frequency is supported */
		err = st_lsm6dsx_fusion_get_odr_val(settings, odr_mHz, &odr_val);
		if (err)
			return err;

		sensor->hwfifo_odr_mHz = odr_mHz;
		return 0;
	}
	default:
		return -EINVAL;
	}
}

static int st_lsm6dsx_fusion_read_avail(struct iio_dev *indio_dev,
					struct iio_chan_spec const *chan,
					const int **vals, int *type,
					int *length, long mask)
{
	struct st_lsm6dsx_sensor *sensor = iio_priv(indio_dev);
	const struct st_lsm6dsx_fusion_settings *settings;

	settings = &sensor->hw->settings->fusion_settings;
	switch (mask) {
	case IIO_CHAN_INFO_SAMP_FREQ:
		*vals = settings->odr_hz;
		*type = IIO_VAL_INT;
		*length = settings->odr_len;
		return IIO_AVAIL_LIST;
	default:
		return -EINVAL;
	}
}

static const struct iio_info st_lsm6dsx_fusion_info = {
	.read_raw = st_lsm6dsx_fusion_read_raw,
	.read_avail = st_lsm6dsx_fusion_read_avail,
	.write_raw = st_lsm6dsx_fusion_write_raw,
	.hwfifo_set_watermark = st_lsm6dsx_set_watermark,
};

int st_lsm6dsx_fusion_probe(struct st_lsm6dsx_hw *hw, const char *name)
{
	const struct st_lsm6dsx_fusion_settings *settings;
	struct st_lsm6dsx_sensor *sensor;
	struct iio_dev *iio_dev;
	int ret;

	iio_dev = devm_iio_device_alloc(hw->dev, sizeof(*sensor));
	if (!iio_dev)
		return -ENOMEM;

	settings = &hw->settings->fusion_settings;
	sensor = iio_priv(iio_dev);
	sensor->id = ST_LSM6DSX_ID_FUSION;
	sensor->hw = hw;
	sensor->hwfifo_odr_mHz = settings->odr_hz[0] * MILLI;
	sensor->watermark = 1;
	iio_dev->modes = INDIO_DIRECT_MODE;
	iio_dev->info = &st_lsm6dsx_fusion_info;
	iio_dev->channels = settings->chan;
	iio_dev->num_channels = settings->chan_len;
	ret = snprintf(sensor->name, sizeof(sensor->name), "%s_fusion", name);
	if (ret >= sizeof(sensor->name))
		return -E2BIG;
	iio_dev->name = sensor->name;

	/*
	 * Put the IIO device pointer in the iio_devs array so that the caller
	 * can set up a buffer and register this IIO device.
	 */
	hw->iio_devs[ST_LSM6DSX_ID_FUSION] = iio_dev;

	return 0;
}