[PATCH v3 1/4] iio: imu: inv_mpu6050: add WoM (Wake-on-Motion) sensor

[inv.git-commit@tdk.com]

From: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>

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: Jean-Baptiste Maneyrol <jean-baptiste.maneyrol@tdk.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    | 282 +++++++++++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  20 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    |   6 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c |  14 +-
 4 files changed, 309 insertions(+), 13 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 0e94e5335e93..d49c0ac91a59 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -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;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 5950e2419ebb..d5b0465d1f74 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -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 */
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 66d4ba088e70..13da6f523ca2 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -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);
 }
 
 /*
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 676704f9151f..ec2398a87f45 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -134,11 +134,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);
@@ -171,9 +173,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);
-- 
2.34.1