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

iio: imu: inv_mpu6050: add WoM (Wake-on-Motion) sensor 



WoM is a threshold test on accel value comparing actual sample with
previous one. It maps best to roc rising event.

Add support of a new WOM sensor and functions for handling the associated
roc_rising event. The event value is in SI units. Ensure WOM is stopped and
restarted at suspend-resume, handle usage with buffer data ready interrupt,
and handle change in sampling rate impacting already set roc value.

Signed-off-by: default avatarJean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
Link: https://lore.kernel.org/r/20240311160557.437337-2-inv.git-commit@tdk.com


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

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c

+280 −2
Original line number Diff line number Diff line
@@ -15,6 +15,8 @@ 
#include <linux/acpi.h>

#include <linux/platform_device.h>

#include <linux/regulator/consumer.h>

#include <linux/math64.h>

#include <linux/minmax.h>

#include <linux/pm.h>

#include <linux/pm_runtime.h>

#include <linux/property.h>
@@ -332,7 +334,7 @@ static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st, 
int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,

			      unsigned int mask)

{

	unsigned int sleep;

	unsigned int sleep, val;

	u8 pwr_mgmt2, user_ctrl;

	int ret;
@@ -345,6 +347,14 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, 
		mask &= ~INV_MPU6050_SENSOR_TEMP;

	if (mask & INV_MPU6050_SENSOR_MAGN && en == st->chip_config.magn_en)

		mask &= ~INV_MPU6050_SENSOR_MAGN;

	if (mask & INV_MPU6050_SENSOR_WOM && en == st->chip_config.wom_en)

		mask &= ~INV_MPU6050_SENSOR_WOM;



	/* force accel on if WoM is on and not going off */

	if (!en && (mask & INV_MPU6050_SENSOR_ACCL) && st->chip_config.wom_en &&

			!(mask & INV_MPU6050_SENSOR_WOM))

		mask &= ~INV_MPU6050_SENSOR_ACCL;



	if (mask == 0)

		return 0;
@@ -439,6 +449,16 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, 
		}

	}



	/* enable/disable accel intelligence control */

	if (mask & INV_MPU6050_SENSOR_WOM) {

		val = en ? INV_MPU6500_BIT_ACCEL_INTEL_EN |

			   INV_MPU6500_BIT_ACCEL_INTEL_MODE : 0;

		ret = regmap_write(st->map, INV_MPU6500_REG_ACCEL_INTEL_CTRL, val);

		if (ret)

			return ret;

		st->chip_config.wom_en = en;

	}



	return 0;

}
@@ -893,6 +913,239 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev, 
	return result;

}



static u64 inv_mpu6050_convert_wom_to_roc(unsigned int threshold, unsigned int freq_div)

{

	/* 4mg per LSB converted in m/s² in micro (1000000) */

	const unsigned int convert = 4U * 9807U;

	u64 value;



	value = threshold * convert;



	/* compute the differential by multiplying by the frequency */

	return div_u64(value * INV_MPU6050_INTERNAL_FREQ_HZ, freq_div);

}



static unsigned int inv_mpu6050_convert_roc_to_wom(u64 roc, unsigned int freq_div)

{

	/* 4mg per LSB converted in m/s² in micro (1000000) */

	const unsigned int convert = 4U * 9807U;

	u64 value;



	/* return 0 only if roc is 0 */

	if (roc == 0)

		return 0;



	value = div_u64(roc * freq_div, convert * INV_MPU6050_INTERNAL_FREQ_HZ);



	/* limit value to 8 bits and prevent 0 */

	return min(255, max(1, value));

}



static int inv_mpu6050_set_wom_int(struct inv_mpu6050_state *st, bool on)

{

	unsigned int reg_val, val;



	switch (st->chip_type) {

	case INV_MPU6050:

	case INV_MPU6500:

	case INV_MPU6515:

	case INV_MPU6880:

	case INV_MPU6000:

	case INV_MPU9150:

	case INV_MPU9250:

	case INV_MPU9255:

		reg_val = INV_MPU6500_BIT_WOM_INT_EN;

		break;

	default:

		reg_val = INV_ICM20608_BIT_WOM_INT_EN;

		break;

	}



	val = on ? reg_val : 0;



	return regmap_update_bits(st->map, st->reg->int_enable, reg_val, val);

}



static int inv_mpu6050_set_wom_threshold(struct inv_mpu6050_state *st, u64 value,

					 unsigned int freq_div)

{

	unsigned int threshold;

	int result;



	/* convert roc to wom threshold and convert back to handle clipping */

	threshold = inv_mpu6050_convert_roc_to_wom(value, freq_div);

	value = inv_mpu6050_convert_wom_to_roc(threshold, freq_div);



	dev_dbg(regmap_get_device(st->map), "wom_threshold: 0x%x\n", threshold);



	switch (st->chip_type) {

	case INV_ICM20609:

	case INV_ICM20689:

	case INV_ICM20600:

	case INV_ICM20602:

	case INV_ICM20690:

		st->data[0] = threshold;

		st->data[1] = threshold;

		st->data[2] = threshold;

		result = regmap_bulk_write(st->map, INV_ICM20609_REG_ACCEL_WOM_X_THR,

					   st->data, 3);

		break;

	default:

		result = regmap_write(st->map, INV_MPU6500_REG_WOM_THRESHOLD, threshold);

		break;

	}

	if (result)

		return result;



	st->chip_config.roc_threshold = value;



	return 0;

}



static int inv_mpu6050_enable_wom(struct inv_mpu6050_state *st, bool en)

{

	struct device *pdev = regmap_get_device(st->map);

	unsigned int mask;

	int result;



	if (en) {

		result = pm_runtime_resume_and_get(pdev);

		if (result)

			return result;



		mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_WOM;

		result = inv_mpu6050_switch_engine(st, true, mask);

		if (result)

			goto error_suspend;



		result = inv_mpu6050_set_wom_int(st, true);

		if (result)

			goto error_suspend;

	} else {

		result = inv_mpu6050_set_wom_int(st, false);

		if (result)

			dev_err(pdev, "error %d disabling WoM interrupt bit", result);



		/* disable only WoM and let accel be disabled by autosuspend */

		result = inv_mpu6050_switch_engine(st, false, INV_MPU6050_SENSOR_WOM);

		if (result) {

			dev_err(pdev, "error %d disabling WoM force off", result);

			/* force WoM off */

			st->chip_config.wom_en = false;

		}



		pm_runtime_mark_last_busy(pdev);

		pm_runtime_put_autosuspend(pdev);

	}



	return result;



error_suspend:

	pm_runtime_mark_last_busy(pdev);

	pm_runtime_put_autosuspend(pdev);

	return result;

}



static int inv_mpu6050_read_event_config(struct iio_dev *indio_dev,

					 const struct iio_chan_spec *chan,

					 enum iio_event_type type,

					 enum iio_event_direction dir)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);



	/* support only WoM (accel roc rising) event */

	if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||

	    dir != IIO_EV_DIR_RISING)

		return -EINVAL;



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



	return st->chip_config.wom_en ? 1 : 0;

}



static int inv_mpu6050_write_event_config(struct iio_dev *indio_dev,

					  const struct iio_chan_spec *chan,

					  enum iio_event_type type,

					  enum iio_event_direction dir,

					  int state)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);

	int enable;



	/* support only WoM (accel roc rising) event */

	if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||

	    dir != IIO_EV_DIR_RISING)

		return -EINVAL;



	enable = !!state;



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



	if (st->chip_config.wom_en == enable)

		return 0;



	return inv_mpu6050_enable_wom(st, enable);

}



static int inv_mpu6050_read_event_value(struct iio_dev *indio_dev,

					const struct iio_chan_spec *chan,

					enum iio_event_type type,

					enum iio_event_direction dir,

					enum iio_event_info info,

					int *val, int *val2)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);

	u32 rem;



	/* support only WoM (accel roc rising) event value */

	if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||

	    dir != IIO_EV_DIR_RISING || info != IIO_EV_INFO_VALUE)

		return -EINVAL;



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



	/* return value in micro */

	*val = div_u64_rem(st->chip_config.roc_threshold, 1000000U, &rem);

	*val2 = rem;



	return IIO_VAL_INT_PLUS_MICRO;

}



static int inv_mpu6050_write_event_value(struct iio_dev *indio_dev,

					 const struct iio_chan_spec *chan,

					 enum iio_event_type type,

					 enum iio_event_direction dir,

					 enum iio_event_info info,

					 int val, int val2)

{

	struct inv_mpu6050_state *st = iio_priv(indio_dev);

	struct device *pdev = regmap_get_device(st->map);

	u64 value;

	int result;



	/* support only WoM (accel roc rising) event value */

	if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||

	    dir != IIO_EV_DIR_RISING || info != IIO_EV_INFO_VALUE)

		return -EINVAL;



	if (val < 0 || val2 < 0)

		return -EINVAL;



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



	result = pm_runtime_resume_and_get(pdev);

	if (result)

		return result;



	value = (u64)val * 1000000ULL + (u64)val2;

	result = inv_mpu6050_set_wom_threshold(st, value, INV_MPU6050_FREQ_DIVIDER(st));



	pm_runtime_mark_last_busy(pdev);

	pm_runtime_put_autosuspend(pdev);



	return result;

}



/*

 *  inv_mpu6050_set_lpf() - set low pass filer based on fifo rate.

 *
@@ -989,6 +1242,12 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr, 
	if (result)

		goto fifo_rate_fail_power_off;



	/* update wom threshold since roc is dependent on sampling frequency */

	result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold,

					       INV_MPU6050_FREQ_DIVIDER(st));

	if (result)

		goto fifo_rate_fail_power_off;



	pm_runtime_mark_last_busy(pdev);

fifo_rate_fail_power_off:

	pm_runtime_put_autosuspend(pdev);
@@ -1326,6 +1585,10 @@ static const struct iio_info mpu_info = { 
	.write_raw = &inv_mpu6050_write_raw,

	.write_raw_get_fmt = &inv_write_raw_get_fmt,

	.attrs = &inv_attribute_group,

	.read_event_config = inv_mpu6050_read_event_config,

	.write_event_config = inv_mpu6050_write_event_config,

	.read_event_value = inv_mpu6050_read_event_value,

	.write_event_value = inv_mpu6050_write_event_value,

	.validate_trigger = inv_mpu6050_validate_trigger,

	.debugfs_reg_access = &inv_mpu6050_reg_access,

};
@@ -1706,6 +1969,12 @@ static int inv_mpu_resume(struct device *dev) 
	if (result)

		goto out_unlock;



	if (st->chip_config.wom_en) {

		result = inv_mpu6050_set_wom_int(st, true);

		if (result)

			goto out_unlock;

	}



	if (iio_buffer_enabled(indio_dev))

		result = inv_mpu6050_prepare_fifo(st, true);
@@ -1735,6 +2004,12 @@ static int inv_mpu_suspend(struct device *dev) 
			goto out_unlock;

	}



	if (st->chip_config.wom_en) {

		result = inv_mpu6050_set_wom_int(st, false);

		if (result)

			goto out_unlock;

	}



	if (st->chip_config.accl_en)

		st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL;

	if (st->chip_config.gyro_en)
@@ -1743,6 +2018,8 @@ static int inv_mpu_suspend(struct device *dev) 
		st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP;

	if (st->chip_config.magn_en)

		st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN;

	if (st->chip_config.wom_en)

		st->suspended_sensors |= INV_MPU6050_SENSOR_WOM;

	result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors);

	if (result)

		goto out_unlock;
@@ -1767,7 +2044,8 @@ static int inv_mpu_runtime_suspend(struct device *dev) 
	mutex_lock(&st->lock);



	sensors = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO |

			INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN;

			INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN |

			INV_MPU6050_SENSOR_WOM;

	ret = inv_mpu6050_switch_engine(st, false, sensors);

	if (ret)

		goto out_unlock;

drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h

+19 −1
Original line number Diff line number Diff line
@@ -88,11 +88,12 @@ enum inv_devices { 
	INV_NUM_PARTS

};



/* chip sensors mask: accelerometer, gyroscope, temperature, magnetometer */

/* chip sensors mask: accelerometer, gyroscope, temperature, magnetometer, WoM */

#define INV_MPU6050_SENSOR_ACCL		BIT(0)

#define INV_MPU6050_SENSOR_GYRO		BIT(1)

#define INV_MPU6050_SENSOR_TEMP		BIT(2)

#define INV_MPU6050_SENSOR_MAGN		BIT(3)

#define INV_MPU6050_SENSOR_WOM		BIT(4)



/**

 *  struct inv_mpu6050_chip_config - Cached chip configuration data.
@@ -104,11 +105,13 @@ enum inv_devices { 
 *  @gyro_en:		gyro engine enabled

 *  @temp_en:		temperature sensor enabled

 *  @magn_en:		magn engine (i2c master) enabled

 *  @wom_en:		Wake-on-Motion enabled

 *  @accl_fifo_enable:	enable accel data output

 *  @gyro_fifo_enable:	enable gyro data output

 *  @temp_fifo_enable:	enable temp data output

 *  @magn_fifo_enable:	enable magn data output

 *  @divider:		chip sample rate divider (sample rate divider - 1)

 *  @roc_threshold:	save ROC threshold (WoM) set value

 */

struct inv_mpu6050_chip_config {

	unsigned int clk:3;
@@ -119,12 +122,14 @@ struct inv_mpu6050_chip_config { 
	unsigned int gyro_en:1;

	unsigned int temp_en:1;

	unsigned int magn_en:1;

	unsigned int wom_en:1;

	unsigned int accl_fifo_enable:1;

	unsigned int gyro_fifo_enable:1;

	unsigned int temp_fifo_enable:1;

	unsigned int magn_fifo_enable:1;

	u8 divider;

	u8 user_ctrl;

	u64 roc_threshold;

};



/*
@@ -256,12 +261,16 @@ struct inv_mpu6050_state { 
#define INV_MPU6050_REG_INT_ENABLE          0x38

#define INV_MPU6050_BIT_DATA_RDY_EN         0x01

#define INV_MPU6050_BIT_DMP_INT_EN          0x02

#define INV_MPU6500_BIT_WOM_INT_EN          BIT(6)

#define INV_ICM20608_BIT_WOM_INT_EN         GENMASK(7, 5)



#define INV_MPU6050_REG_RAW_ACCEL           0x3B

#define INV_MPU6050_REG_TEMPERATURE         0x41

#define INV_MPU6050_REG_RAW_GYRO            0x43



#define INV_MPU6050_REG_INT_STATUS          0x3A

#define INV_MPU6500_BIT_WOM_INT             BIT(6)

#define INV_ICM20608_BIT_WOM_INT            GENMASK(7, 5)

#define INV_MPU6050_BIT_FIFO_OVERFLOW_INT   0x10

#define INV_MPU6050_BIT_RAW_DATA_RDY_INT    0x01
@@ -301,6 +310,11 @@ struct inv_mpu6050_state { 
#define INV_MPU6050_BIT_PWR_ACCL_STBY       0x38

#define INV_MPU6050_BIT_PWR_GYRO_STBY       0x07



/* ICM20609 registers */

#define INV_ICM20609_REG_ACCEL_WOM_X_THR    0x20

#define INV_ICM20609_REG_ACCEL_WOM_Y_THR    0x21

#define INV_ICM20609_REG_ACCEL_WOM_Z_THR    0x22



/* ICM20602 register */

#define INV_ICM20602_REG_I2C_IF             0x70

#define INV_ICM20602_BIT_I2C_IF_DIS         0x40
@@ -320,6 +334,10 @@ struct inv_mpu6050_state { 
/* mpu6500 registers */

#define INV_MPU6500_REG_ACCEL_CONFIG_2      0x1D

#define INV_ICM20689_BITS_FIFO_SIZE_MAX     0xC0

#define INV_MPU6500_REG_WOM_THRESHOLD       0x1F

#define INV_MPU6500_REG_ACCEL_INTEL_CTRL    0x69

#define INV_MPU6500_BIT_ACCEL_INTEL_EN      BIT(7)

#define INV_MPU6500_BIT_ACCEL_INTEL_MODE    BIT(6)

#define INV_MPU6500_REG_ACCEL_OFFSET        0x77



/* delay time in milliseconds */

drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c

+2 −4
Original line number Diff line number Diff line
@@ -33,10 +33,8 @@ static int inv_reset_fifo(struct iio_dev *indio_dev) 


reset_fifo_fail:

	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);

	result = regmap_write(st->map, st->reg->int_enable,

			      INV_MPU6050_BIT_DATA_RDY_EN);



	return result;

	return regmap_update_bits(st->map, st->reg->int_enable,

			INV_MPU6050_BIT_DATA_RDY_EN, INV_MPU6050_BIT_DATA_RDY_EN);

}



/*

drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c

+8 −6
Original line number Diff line number Diff line
@@ -135,11 +135,13 @@ int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable) 
		ret = regmap_write(st->map, st->reg->user_ctrl, d);

		if (ret)

			return ret;

		/* enable interrupt */

		ret = regmap_write(st->map, st->reg->int_enable,

				   INV_MPU6050_BIT_DATA_RDY_EN);

		/* enable data interrupt */

		ret = regmap_update_bits(st->map, st->reg->int_enable,

				INV_MPU6050_BIT_DATA_RDY_EN, INV_MPU6050_BIT_DATA_RDY_EN);

	} else {

		ret = regmap_write(st->map, st->reg->int_enable, 0);

		/* disable data interrupt */

		ret = regmap_update_bits(st->map, st->reg->int_enable,

				INV_MPU6050_BIT_DATA_RDY_EN, 0);

		if (ret)

			return ret;

		ret = regmap_write(st->map, st->reg->fifo_en, 0);
@@ -172,9 +174,9 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable) 
			return result;

		/*

		 * In case autosuspend didn't trigger, turn off first not

		 * required sensors.

		 * required sensors excepted WoM

		 */

		result = inv_mpu6050_switch_engine(st, false, ~scan);

		result = inv_mpu6050_switch_engine(st, false, ~scan & ~INV_MPU6050_SENSOR_WOM);

		if (result)

			goto error_power_off;

		result = inv_mpu6050_switch_engine(st, true, scan);