[PATCH v2 1/7] iio: imu: inv_mpu6050: disable i2c mux for MPU925x

[PATCH v2 1/7] iio: imu: inv_mpu6050: disable i2c mux for MPU925x

[Jean-Baptiste Maneyrol]

Disable i2c mux for supported 9xxx chips. This is a
pre-requesite for controling 9xxx magnetometer using the
i2c master of the chip.

Check in device-tree that there is no i2c-gate device declared
for ensuring backward compatibility with existing setups.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c |  7 +--
 drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  | 60 +++++++++++++++++++---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  2 +
 3 files changed, 58 insertions(+), 11 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index b17f060b52fc..7b2e4d81bbba 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -1156,9 +1156,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		return result;
 	}
 
-	if (inv_mpu_bus_setup)
-		inv_mpu_bus_setup(indio_dev);
-
 	dev_set_drvdata(dev, indio_dev);
 	indio_dev->dev.parent = dev;
 	/* name will be NULL when enumerated via ACPI */
@@ -1167,6 +1164,10 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 	else
 		indio_dev->name = dev_name(dev);
 
+	/* requires parent device set in indio_dev */
+	if (inv_mpu_bus_setup)
+		inv_mpu_bus_setup(indio_dev);
+
 	if (chip_type == INV_ICM20602) {
 		indio_dev->channels = inv_icm20602_channels;
 		indio_dev->num_channels = ARRAY_SIZE(inv_icm20602_channels);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
index 4b8b5a87398c..389cc8505e0e 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
@@ -68,6 +68,56 @@ static const char *inv_mpu_match_acpi_device(struct device *dev,
 	return dev_name(dev);
 }
 
+static bool inv_mpu_i2c_aux_bus(struct device *dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
+
+	switch (st->chip_type) {
+	case INV_ICM20608:
+	case INV_ICM20602:
+		/* no i2c auxiliary bus on the chip */
+		return false;
+	case INV_MPU9250:
+	case INV_MPU9255:
+		if (st->magn_disabled)
+			return true;
+		else
+			return false;
+	default:
+		return true;
+	}
+}
+
+/*
+ * MPU9xxx magnetometer support requires to disable i2c auxiliary bus support.
+ * To ensure backward compatibility with existing setups, do not disable
+ * i2c auxiliary bus if it used.
+ * Check for i2c-gate node in devicetree and set magnetometer disabled.
+ * Only MPU6500 is supported by ACPI, no need to check.
+ */
+static int inv_mpu_magn_disable(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct device *dev = indio_dev->dev.parent;
+	struct device_node *mux_node;
+
+	switch (st->chip_type) {
+	case INV_MPU9250:
+	case INV_MPU9255:
+		mux_node = of_get_child_by_name(dev->of_node, "i2c-gate");
+		if (mux_node != NULL) {
+			st->magn_disabled = true;
+			dev_warn(dev, "disable internal use of magnetometer\n");
+		}
+		of_node_put(mux_node);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
 /**
  *  inv_mpu_probe() - probe function.
  *  @client:          i2c client.
@@ -112,17 +162,12 @@ static int inv_mpu_probe(struct i2c_client *client,
 	}
 
 	result = inv_mpu_core_probe(regmap, client->irq, name,
-				    NULL, chip_type);
+				    inv_mpu_magn_disable, chip_type);
 	if (result < 0)
 		return result;
 
 	st = iio_priv(dev_get_drvdata(&client->dev));
-	switch (st->chip_type) {
-	case INV_ICM20608:
-	case INV_ICM20602:
-		/* no i2c auxiliary bus on the chip */
-		break;
-	default:
+	if (inv_mpu_i2c_aux_bus(&client->dev)) {
 		/* declare i2c auxiliary bus */
 		st->muxc = i2c_mux_alloc(client->adapter, &client->dev,
 					 1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
@@ -137,7 +182,6 @@ static int inv_mpu_probe(struct i2c_client *client,
 		result = inv_mpu_acpi_create_mux_client(client);
 		if (result)
 			goto out_del_mux;
-		break;
 	}
 
 	return 0;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index db1c6904388b..cbbb2fb8949a 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -125,6 +125,7 @@ struct inv_mpu6050_hw {
  *  @it_timestamp:	timestamp from previous interrupt.
  *  @data_timestamp:	timestamp for next data sample.
  *  @vddio_supply	voltage regulator for the chip.
+ *  @magn_disabled:     magnetometer disabled for backward compatibility reason.
  */
 struct inv_mpu6050_state {
 	struct mutex lock;
@@ -146,6 +147,7 @@ struct inv_mpu6050_state {
 	s64 it_timestamp;
 	s64 data_timestamp;
 	struct regulator *vddio_supply;
+	bool magn_disabled;
 };
 
 /*register and associated bit definition*/
-- 
2.17.1

[PATCH v2 2/7] iio: imu: inv_mpu6050: add header include protection macro

[Jean-Baptiste Maneyrol]

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index cbbb2fb8949a..7cfd3a05c144 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -2,6 +2,10 @@
 /*
 * Copyright (C) 2012 Invensense, Inc.
 */
+
+#ifndef INV_MPU_IIO_H_
+#define INV_MPU_IIO_H_
+
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
 #include <linux/mutex.h>
@@ -344,3 +348,5 @@ void inv_mpu_acpi_delete_mux_client(struct i2c_client *client);
 int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type);
 extern const struct dev_pm_ops inv_mpu_pmops;
+
+#endif
-- 
2.17.1

[PATCH v2 3/7] iio: imu: inv_mpu6050: add defines for supporting 9-axis chips

[Jean-Baptiste Maneyrol]

Add registers defines required for driving chip i2c master ip.
Add MPU9xxx magnetometer scan elements and update data bytes size.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h | 56 ++++++++++++++++++++++-
 1 file changed, 54 insertions(+), 2 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 7cfd3a05c144..04215bc6e8ab 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -164,9 +164,41 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_REG_ACCEL_CONFIG        0x1C
 
 #define INV_MPU6050_REG_FIFO_EN             0x23
+#define INV_MPU6050_BIT_SLAVE_0             0x01
+#define INV_MPU6050_BIT_SLAVE_1             0x02
+#define INV_MPU6050_BIT_SLAVE_2             0x04
 #define INV_MPU6050_BIT_ACCEL_OUT           0x08
 #define INV_MPU6050_BITS_GYRO_OUT           0x70
 
+#define INV_MPU6050_REG_I2C_MST_CTRL        0x24
+#define INV_MPU6050_BITS_I2C_MST_CLK_400KHZ 0x0D
+#define INV_MPU6050_BIT_I2C_MST_P_NSR       0x10
+#define INV_MPU6050_BIT_SLV3_FIFO_EN        0x20
+#define INV_MPU6050_BIT_WAIT_FOR_ES         0x40
+#define INV_MPU6050_BIT_MULT_MST_EN         0x80
+
+/* control I2C slaves from 0 to 3 */
+#define INV_MPU6050_REG_I2C_SLV_ADDR(_x)    (0x25 + 3 * (_x))
+#define INV_MPU6050_BIT_I2C_SLV_RNW         0x80
+
+#define INV_MPU6050_REG_I2C_SLV_REG(_x)     (0x26 + 3 * (_x))
+
+#define INV_MPU6050_REG_I2C_SLV_CTRL(_x)    (0x27 + 3 * (_x))
+#define INV_MPU6050_BIT_SLV_GRP             0x10
+#define INV_MPU6050_BIT_SLV_REG_DIS         0x20
+#define INV_MPU6050_BIT_SLV_BYTE_SW         0x40
+#define INV_MPU6050_BIT_SLV_EN              0x80
+
+/* I2C master delay register */
+#define INV_MPU6050_REG_I2C_SLV4_CTRL       0x34
+#define INV_MPU6050_BITS_I2C_MST_DLY(_x)    ((_x) & 0x1F)
+
+#define INV_MPU6050_REG_I2C_MST_STATUS      0x36
+#define INV_MPU6050_BIT_I2C_SLV0_NACK       0x01
+#define INV_MPU6050_BIT_I2C_SLV1_NACK       0x02
+#define INV_MPU6050_BIT_I2C_SLV2_NACK       0x04
+#define INV_MPU6050_BIT_I2C_SLV3_NACK       0x08
+
 #define INV_MPU6050_REG_INT_ENABLE          0x38
 #define INV_MPU6050_BIT_DATA_RDY_EN         0x01
 #define INV_MPU6050_BIT_DMP_INT_EN          0x02
@@ -179,6 +211,18 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_BIT_FIFO_OVERFLOW_INT   0x10
 #define INV_MPU6050_BIT_RAW_DATA_RDY_INT    0x01
 
+#define INV_MPU6050_REG_EXT_SENS_DATA       0x49
+
+/* I2C slaves data output from 0 to 3 */
+#define INV_MPU6050_REG_I2C_SLV_DO(_x)      (0x63 + (_x))
+
+#define INV_MPU6050_REG_I2C_MST_DELAY_CTRL  0x67
+#define INV_MPU6050_BIT_I2C_SLV0_DLY_EN     0x01
+#define INV_MPU6050_BIT_I2C_SLV1_DLY_EN     0x02
+#define INV_MPU6050_BIT_I2C_SLV2_DLY_EN     0x04
+#define INV_MPU6050_BIT_I2C_SLV3_DLY_EN     0x08
+#define INV_MPU6050_BIT_DELAY_ES_SHADOW     0x80
+
 #define INV_MPU6050_REG_USER_CTRL           0x6A
 #define INV_MPU6050_BIT_FIFO_RST            0x04
 #define INV_MPU6050_BIT_DMP_RST             0x08
@@ -206,6 +250,9 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_BYTES_PER_3AXIS_SENSOR   6
 #define INV_MPU6050_FIFO_COUNT_BYTE          2
 
+/* MPU9X50 9-axis magnetometer */
+#define INV_MPU9X50_BYTES_MAGN               7
+
 /* ICM20602 FIFO samples include temperature readings */
 #define INV_ICM20602_BYTES_PER_TEMP_SENSOR   2
 
@@ -233,8 +280,8 @@ struct inv_mpu6050_state {
 #define INV_ICM20602_TEMP_OFFSET	     8170
 #define INV_ICM20602_TEMP_SCALE		     3060
 
-/* 6 + 6 round up and plus 8 */
-#define INV_MPU6050_OUTPUT_DATA_SIZE         24
+/* 6 + 6 + 7 (for MPU9x50) = 19 round up to 24 and plus 8 */
+#define INV_MPU6050_OUTPUT_DATA_SIZE         32
 
 #define INV_MPU6050_REG_INT_PIN_CFG	0x37
 #define INV_MPU6050_ACTIVE_HIGH		0x00
@@ -283,6 +330,11 @@ enum inv_mpu6050_scan {
 	INV_MPU6050_SCAN_GYRO_Y,
 	INV_MPU6050_SCAN_GYRO_Z,
 	INV_MPU6050_SCAN_TIMESTAMP,
+
+	INV_MPU9X50_SCAN_MAGN_X = INV_MPU6050_SCAN_GYRO_Z + 1,
+	INV_MPU9X50_SCAN_MAGN_Y,
+	INV_MPU9X50_SCAN_MAGN_Z,
+	INV_MPU9X50_SCAN_TIMESTAMP,
 };
 
 /* scan element definition for ICM20602, which includes temperature */
-- 
2.17.1

[PATCH v2 4/7] iio: imu: inv_mpu6050: fix objects syntax in Makefile

[Jean-Baptiste Maneyrol]

Use the correct syntax *-y for declaring object files.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/Makefile | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile
index 70ffe0d13d8c..33bec09fee9b 100644
--- a/drivers/iio/imu/inv_mpu6050/Makefile
+++ b/drivers/iio/imu/inv_mpu6050/Makefile
@@ -4,10 +4,10 @@
 #
 
 obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o
-inv-mpu6050-objs := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o
+inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o
 
 obj-$(CONFIG_INV_MPU6050_I2C) += inv-mpu6050-i2c.o
-inv-mpu6050-i2c-objs := inv_mpu_i2c.o inv_mpu_acpi.o
+inv-mpu6050-i2c-y := inv_mpu_i2c.o inv_mpu_acpi.o
 
 obj-$(CONFIG_INV_MPU6050_SPI) += inv-mpu6050-spi.o
-inv-mpu6050-spi-objs := inv_mpu_spi.o
+inv-mpu6050-spi-y := inv_mpu_spi.o
-- 
2.17.1

[PATCH v2 5/7] iio: imu: inv_mpu6050: helpers for using i2c master on auxiliary bus

[Jean-Baptiste Maneyrol]

Add helper functions to use the i2c auxiliary bus with the MPU i2c
master block.

Support only register based chip, reading and 1 byte writing. These
will be useful for initializing magnetometers inside MPU9x50 chips.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/Makefile      |   3 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c | 184 ++++++++++++++++++++++
 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h |  46 ++++++
 3 files changed, 232 insertions(+), 1 deletion(-)
 create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
 create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h

diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile
index 33bec09fee9b..2cfbd926522f 100644
--- a/drivers/iio/imu/inv_mpu6050/Makefile
+++ b/drivers/iio/imu/inv_mpu6050/Makefile
@@ -4,7 +4,8 @@
 #
 
 obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o
-inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o
+inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o \
+		 inv_mpu_aux.o
 
 obj-$(CONFIG_INV_MPU6050_I2C) += inv-mpu6050-i2c.o
 inv-mpu6050-i2c-y := inv_mpu_i2c.o inv_mpu_acpi.o
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
new file mode 100644
index 000000000000..b4907c1a8bb6
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
@@ -0,0 +1,184 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 TDK-InvenSense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+
+#include "inv_mpu_iio.h"
+
+/*
+ * i2c master auxiliary bus transfer function.
+ * Requires the i2c operations to be correctly setup before.
+ */
+static int inv_mpu_i2c_master_xfer(const struct inv_mpu6050_state *st)
+{
+	/* use 50hz frequency for xfer */
+	const unsigned int freq = 50;
+	const unsigned int period_ms = 1000 / freq;
+	uint8_t d;
+	unsigned int user_ctrl;
+	int ret;
+
+	/* set sample rate */
+	d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(freq);
+	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
+	if (ret)
+		return ret;
+
+	/* start i2c master */
+	user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN;
+	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
+	if (ret)
+		goto error_restore_rate;
+
+	/* wait for xfer: 1 period + half-period margin */
+	msleep(period_ms + period_ms / 2);
+
+	/* stop i2c master */
+	user_ctrl = st->chip_config.user_ctrl;
+	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
+	if (ret)
+		goto error_stop_i2c;
+
+	/* restore sample rate */
+	d = st->chip_config.divider;
+	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
+	if (ret)
+		goto error_restore_rate;
+
+	return 0;
+
+error_stop_i2c:
+	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
+error_restore_rate:
+	regmap_write(st->map, st->reg->sample_rate_div, st->chip_config.divider);
+	return ret;
+}
+
+int inv_mpu_aux_init(const struct inv_mpu6050_state *st)
+{
+	unsigned int val;
+	int ret;
+
+	/* configure i2c master */
+	val = INV_MPU6050_BITS_I2C_MST_CLK_400KHZ |
+			INV_MPU6050_BIT_WAIT_FOR_ES;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_MST_CTRL, val);
+	if (ret)
+		return ret;
+
+	/* configure i2c master delay */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, 0);
+	if (ret)
+		return ret;
+
+	val = INV_MPU6050_BIT_I2C_SLV0_DLY_EN |
+			INV_MPU6050_BIT_I2C_SLV1_DLY_EN |
+			INV_MPU6050_BIT_I2C_SLV2_DLY_EN |
+			INV_MPU6050_BIT_I2C_SLV3_DLY_EN |
+			INV_MPU6050_BIT_DELAY_ES_SHADOW;
+	return regmap_write(st->map, INV_MPU6050_REG_I2C_MST_DELAY_CTRL, val);
+}
+
+/*
+ * i2c slave reading using SLV0
+ */
+int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr,
+		     uint8_t reg, uint8_t *val, size_t size)
+{
+	unsigned int status;
+	int ret;
+
+	if (size > 0x0F)
+		return -EINVAL;
+
+	/* setup i2c SLV0 control: i2c addr, register, enable + size */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0),
+			   INV_MPU6050_BIT_I2C_SLV_RNW | addr);
+	if (ret)
+		return ret;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg);
+	if (ret)
+		return ret;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
+			   INV_MPU6050_BIT_SLV_EN | size);
+	if (ret)
+		return ret;
+
+	/* do i2c xfer */
+	ret = inv_mpu_i2c_master_xfer(st);
+	if (ret)
+		goto error_disable_i2c;
+
+	/* disable i2c slave */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
+	if (ret)
+		goto error_disable_i2c;
+
+	/* check i2c status */
+	ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
+	if (ret)
+		return ret;
+	if (status & INV_MPU6050_BIT_I2C_SLV0_NACK)
+		return -EIO;
+
+	/* read data in registers */
+	return regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA,
+				val, size);
+
+error_disable_i2c:
+	regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
+	return ret;
+}
+
+/*
+ * i2c slave writing 1 byte using SLV0
+ */
+int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr,
+		      uint8_t reg, uint8_t val)
+{
+	unsigned int status;
+	int ret;
+
+	/* setup i2c SLV0 control: i2c addr, register, value, enable + size */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), addr);
+	if (ret)
+		return ret;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg);
+	if (ret)
+		return ret;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(0), val);
+	if (ret)
+		return ret;
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
+			   INV_MPU6050_BIT_SLV_EN | 1);
+	if (ret)
+		return ret;
+
+	/* do i2c xfer */
+	ret = inv_mpu_i2c_master_xfer(st);
+	if (ret)
+		goto error_disable_i2c;
+
+	/* disable i2c slave */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
+	if (ret)
+		goto error_disable_i2c;
+
+	/* check i2c status */
+	ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
+	if (ret)
+		return ret;
+	if (status & INV_MPU6050_BIT_I2C_SLV0_NACK)
+		return -EIO;
+
+	return 0;
+
+error_disable_i2c:
+	regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
+	return ret;
+}
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h
new file mode 100644
index 000000000000..bae4eab58e6c
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 TDK-InvenSense, Inc.
+ */
+
+#ifndef INV_MPU_AUX_H_
+#define INV_MPU_AUX_H_
+
+#include <linux/kernel.h>
+
+#include "inv_mpu_iio.h"
+
+/**
+ * inv_mpu_aux_init() - init i2c auxiliary bus
+ * @st: driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ */
+int inv_mpu_aux_init(const struct inv_mpu6050_state *st);
+
+/**
+ * inv_mpu_aux_read() - read register function for i2c auxiliary bus
+ * @st: driver internal state.
+ * @addr: chip i2c Address
+ * @reg: chip register address
+ * @val: buffer for storing read bytes
+ * @size: number of bytes to read
+ *
+ *  Returns 0 on success, a negative error code otherwise.
+ */
+int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr,
+		     uint8_t reg, uint8_t *val, size_t size);
+
+/**
+ * inv_mpu_aux_write() - write register function for i2c auxiliary bus
+ * @st: driver internal state.
+ * @addr: chip i2c Address
+ * @reg: chip register address
+ * @val: 1 byte value to write
+ *
+ *  Returns 0 on success, a negative error code otherwise.
+ */
+int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr,
+		      uint8_t reg, uint8_t val);
+
+#endif		/* INV_MPU_AUX_H_ */
-- 
2.17.1

[PATCH v2 6/7] iio: imu: inv_mpu6050: add MPU925x magnetometer support

[Jean-Baptiste Maneyrol]

Add support of driving MPU9250 magnetometer connected on i2c
auxiliary bus using the MPU i2c master.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/Makefile       |   2 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 144 ++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |   4 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c | 355 +++++++++++++++++++++
 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h |  36 +++
 5 files changed, 537 insertions(+), 4 deletions(-)
 create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
 create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h

diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile
index 2cfbd926522f..c103441a906b 100644
--- a/drivers/iio/imu/inv_mpu6050/Makefile
+++ b/drivers/iio/imu/inv_mpu6050/Makefile
@@ -5,7 +5,7 @@
 
 obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o
 inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o \
-		 inv_mpu_aux.o
+		 inv_mpu_aux.o inv_mpu_magn.o
 
 obj-$(CONFIG_INV_MPU6050_I2C) += inv-mpu6050-i2c.o
 inv-mpu6050-i2c-y := inv_mpu_i2c.o inv_mpu_acpi.o
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 7b2e4d81bbba..f1c65e0dd1a5 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
 #include "inv_mpu_iio.h"
+#include "inv_mpu_magn.h"
 
 /*
  * this is the gyro scale translated from dynamic range plus/minus
@@ -332,6 +333,11 @@ static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
 	 */
 	st->chip_period = NSEC_PER_MSEC;
 
+	/* magn chip init, noop if not present in the chip */
+	result = inv_mpu_magn_probe(st);
+	if (result)
+		goto error_power_off;
+
 	return inv_mpu6050_set_power_itg(st, false);
 
 error_power_off:
@@ -411,6 +417,9 @@ static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
 		ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
 					      IIO_MOD_X, val);
 		break;
+	case IIO_MAGN:
+		ret = inv_mpu_magn_read(st, chan->channel2, val);
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -469,6 +478,8 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 				*val2 = INV_MPU6050_TEMP_SCALE;
 
 			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_MAGN:
+			return inv_mpu_magn_get_scale(st, chan, val, val2);
 		default:
 			return -EINVAL;
 		}
@@ -710,6 +721,11 @@ inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
 	if (result)
 		goto fifo_rate_fail_power_off;
 
+	/* update rate for magn, noop if not present in chip */
+	result = inv_mpu_magn_set_rate(st, fifo_rate);
+	if (result)
+		goto fifo_rate_fail_power_off;
+
 fifo_rate_fail_power_off:
 	result |= inv_mpu6050_set_power_itg(st, false);
 fifo_rate_fail_unlock:
@@ -795,8 +811,14 @@ inv_get_mount_matrix(const struct iio_dev *indio_dev,
 		     const struct iio_chan_spec *chan)
 {
 	struct inv_mpu6050_state *data = iio_priv(indio_dev);
+	const struct iio_mount_matrix *matrix;
+
+	if (chan->type == IIO_MAGN)
+		matrix = &data->magn_orient;
+	else
+		matrix = &data->orientation;
 
-	return &data->orientation;
+	return matrix;
 }
 
 static const struct iio_chan_spec_ext_info inv_ext_info[] = {
@@ -864,6 +886,98 @@ static const unsigned long inv_mpu_scan_masks[] = {
 	0,
 };
 
+#define INV_MPU9X50_MAGN_CHAN(_chan2, _bits, _index)			\
+	{								\
+		.type = IIO_MAGN,					\
+		.modified = 1,						\
+		.channel2 = _chan2,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_SCALE) |	\
+				      BIT(IIO_CHAN_INFO_RAW),		\
+		.scan_index = _index,					\
+		.scan_type = {						\
+			.sign = 's',					\
+			.realbits = _bits,				\
+			.storagebits = 16,				\
+			.shift = 0,					\
+			.endianness = IIO_BE,				\
+		},							\
+		.ext_info = inv_ext_info,				\
+	}
+
+static const struct iio_chan_spec inv_mpu9250_channels[] = {
+	IIO_CHAN_SOFT_TIMESTAMP(INV_MPU9X50_SCAN_TIMESTAMP),
+	/*
+	 * Note that temperature should only be via polled reading only,
+	 * not the final scan elements output.
+	 */
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW)
+				| BIT(IIO_CHAN_INFO_OFFSET)
+				| BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = -1,
+	},
+	INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
+	INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
+	INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
+
+	INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
+	INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
+	INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
+
+	/* Magnetometer resolution is 16 bits */
+	INV_MPU9X50_MAGN_CHAN(IIO_MOD_X, 16, INV_MPU9X50_SCAN_MAGN_X),
+	INV_MPU9X50_MAGN_CHAN(IIO_MOD_Y, 16, INV_MPU9X50_SCAN_MAGN_Y),
+	INV_MPU9X50_MAGN_CHAN(IIO_MOD_Z, 16, INV_MPU9X50_SCAN_MAGN_Z),
+};
+
+static const unsigned long inv_mpu9x50_scan_masks[] = {
+	/* 3-axis accel */
+	BIT(INV_MPU6050_SCAN_ACCL_X)
+		| BIT(INV_MPU6050_SCAN_ACCL_Y)
+		| BIT(INV_MPU6050_SCAN_ACCL_Z),
+	/* 3-axis gyro */
+	BIT(INV_MPU6050_SCAN_GYRO_X)
+		| BIT(INV_MPU6050_SCAN_GYRO_Y)
+		| BIT(INV_MPU6050_SCAN_GYRO_Z),
+	/* 3-axis magn */
+	BIT(INV_MPU9X50_SCAN_MAGN_X)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Y)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Z),
+	/* 6-axis accel + gyro */
+	BIT(INV_MPU6050_SCAN_ACCL_X)
+		| BIT(INV_MPU6050_SCAN_ACCL_Y)
+		| BIT(INV_MPU6050_SCAN_ACCL_Z)
+		| BIT(INV_MPU6050_SCAN_GYRO_X)
+		| BIT(INV_MPU6050_SCAN_GYRO_Y)
+		| BIT(INV_MPU6050_SCAN_GYRO_Z),
+	/* 6-axis accel + magn */
+	BIT(INV_MPU6050_SCAN_ACCL_X)
+		| BIT(INV_MPU6050_SCAN_ACCL_Y)
+		| BIT(INV_MPU6050_SCAN_ACCL_Z)
+		| BIT(INV_MPU9X50_SCAN_MAGN_X)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Y)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Z),
+	/* 6-axis gyro + magn */
+	BIT(INV_MPU6050_SCAN_GYRO_X)
+		| BIT(INV_MPU6050_SCAN_GYRO_Y)
+		| BIT(INV_MPU6050_SCAN_GYRO_Z)
+		| BIT(INV_MPU9X50_SCAN_MAGN_X)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Y)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Z),
+	/* 9-axis accel + gyro + magn */
+	BIT(INV_MPU6050_SCAN_ACCL_X)
+		| BIT(INV_MPU6050_SCAN_ACCL_Y)
+		| BIT(INV_MPU6050_SCAN_ACCL_Z)
+		| BIT(INV_MPU6050_SCAN_GYRO_X)
+		| BIT(INV_MPU6050_SCAN_GYRO_Y)
+		| BIT(INV_MPU6050_SCAN_GYRO_Z)
+		| BIT(INV_MPU9X50_SCAN_MAGN_X)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Y)
+		| BIT(INV_MPU9X50_SCAN_MAGN_Z),
+	0,
+};
+
 static const struct iio_chan_spec inv_icm20602_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(INV_ICM20602_SCAN_TIMESTAMP),
 	{
@@ -1145,6 +1259,11 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		return result;
 	}
 
+	/* fill magnetometer orientation */
+	result = inv_mpu_magn_set_orient(st);
+	if (result)
+		return result;
+
 	/* power is turned on inside check chip type*/
 	result = inv_check_and_setup_chip(st);
 	if (result)
@@ -1168,14 +1287,33 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 	if (inv_mpu_bus_setup)
 		inv_mpu_bus_setup(indio_dev);
 
-	if (chip_type == INV_ICM20602) {
+	switch (chip_type) {
+	case INV_MPU9250:
+	case INV_MPU9255:
+		/*
+		 * Use magnetometer inside the chip only if there is no i2c
+		 * auxiliary device in use.
+		 */
+		if (!st->magn_disabled) {
+			indio_dev->channels = inv_mpu9250_channels;
+			indio_dev->num_channels = ARRAY_SIZE(inv_mpu9250_channels);
+			indio_dev->available_scan_masks = inv_mpu9x50_scan_masks;
+		} else {
+			indio_dev->channels = inv_mpu_channels;
+			indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
+			indio_dev->available_scan_masks = inv_mpu_scan_masks;
+		}
+		break;
+	case INV_ICM20602:
 		indio_dev->channels = inv_icm20602_channels;
 		indio_dev->num_channels = ARRAY_SIZE(inv_icm20602_channels);
 		indio_dev->available_scan_masks = inv_icm20602_scan_masks;
-	} else {
+		break;
+	default:
 		indio_dev->channels = inv_mpu_channels;
 		indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
 		indio_dev->available_scan_masks = inv_mpu_scan_masks;
+		break;
 	}
 
 	indio_dev->info = &mpu_info;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 04215bc6e8ab..953f85618199 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -130,6 +130,8 @@ struct inv_mpu6050_hw {
  *  @data_timestamp:	timestamp for next data sample.
  *  @vddio_supply	voltage regulator for the chip.
  *  @magn_disabled:     magnetometer disabled for backward compatibility reason.
+ *  @magn_raw_to_gauss:	coefficient to convert mag raw value to Gauss.
+ *  @magn_orient:       magnetometer sensor chip orientation if available.
  */
 struct inv_mpu6050_state {
 	struct mutex lock;
@@ -152,6 +154,8 @@ struct inv_mpu6050_state {
 	s64 data_timestamp;
 	struct regulator *vddio_supply;
 	bool magn_disabled;
+	s32 magn_raw_to_gauss[3];
+	struct iio_mount_matrix magn_orient;
 };
 
 /*register and associated bit definition*/
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
new file mode 100644
index 000000000000..415d8acece54
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.c
@@ -0,0 +1,355 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 TDK-InvenSense, Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/string.h>
+
+#include "inv_mpu_iio.h"
+#include "inv_mpu_aux.h"
+
+/*
+ * MPU9250 magnetometer is an AKM AK8963 chip on I2C aux bus
+ */
+#define INV_MPU_MAGN_I2C_ADDR		0x0C
+
+#define INV_MPU_MAGN_REG_WIA		0x00
+#define INV_MPU_MAGN_BITS_WIA		0x48
+
+#define INV_MPU_MAGN_REG_ST1		0x02
+#define INV_MPU_MAGN_BIT_DRDY		0x01
+#define INV_MPU_MAGN_BIT_DOR		0x02
+
+#define INV_MPU_MAGN_REG_DATA		0x03
+
+#define INV_MPU_MAGN_REG_ST2		0x09
+#define INV_MPU_MAGN_BIT_HOFL		0x08
+#define INV_MPU_MAGN_BIT_BITM		0x10
+
+#define INV_MPU_MAGN_REG_CNTL1		0x0A
+#define INV_MPU_MAGN_BITS_MODE_PWDN	0x00
+#define INV_MPU_MAGN_BITS_MODE_SINGLE	0x01
+#define INV_MPU_MAGN_BITS_MODE_FUSE	0x0F
+#define INV_MPU_MAGN_BIT_OUTPUT_BIT	0x10
+
+#define INV_MPU_MAGN_REG_CNTL2		0x0B
+#define INV_MPU_MAGN_BIT_SRST		0x01
+
+#define INV_MPU_MAGN_REG_ASAX		0x10
+#define INV_MPU_MAGN_REG_ASAY		0x11
+#define INV_MPU_MAGN_REG_ASAZ		0x12
+
+/* Magnetometer maximum frequency */
+#define INV_MPU_MAGN_FREQ_HZ_MAX	50
+
+static bool inv_magn_supported(const struct inv_mpu6050_state *st)
+{
+	switch (st->chip_type) {
+	case INV_MPU9250:
+	case INV_MPU9255:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/* init magnetometer chip */
+static int inv_magn_init(struct inv_mpu6050_state *st)
+{
+	uint8_t val;
+	uint8_t asa[3];
+	int ret;
+
+	/* check whoami */
+	ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_WIA,
+			       &val, sizeof(val));
+	if (ret)
+		return ret;
+	if (val != INV_MPU_MAGN_BITS_WIA)
+		return -ENODEV;
+
+	/* reset chip */
+	ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
+				INV_MPU_MAGN_REG_CNTL2,
+				INV_MPU_MAGN_BIT_SRST);
+	if (ret)
+		return ret;
+
+	/* read fuse ROM data */
+	ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
+				INV_MPU_MAGN_REG_CNTL1,
+				INV_MPU_MAGN_BITS_MODE_FUSE);
+	if (ret)
+		return ret;
+
+	ret = inv_mpu_aux_read(st, INV_MPU_MAGN_I2C_ADDR, INV_MPU_MAGN_REG_ASAX,
+			       asa, sizeof(asa));
+	if (ret)
+		return ret;
+
+	/* switch back to power-down */
+	ret = inv_mpu_aux_write(st, INV_MPU_MAGN_I2C_ADDR,
+				INV_MPU_MAGN_REG_CNTL1,
+				INV_MPU_MAGN_BITS_MODE_PWDN);
+	if (ret)
+		return ret;
+
+	/*
+	 * Sensitivity adjustement and scale to Gauss
+	 *
+	 * Hadj = H * (((ASA - 128) * 0.5 / 128) + 1)
+	 * Factor simplification:
+	 * Hadj = H * ((ASA + 128) / 256)
+	 *
+	 * Sensor sentivity
+	 * 0.15 uT in 16 bits mode
+	 * 1 uT = 0.01 G and value is in micron (1e6)
+	 * sensitvity = 0.15 uT * 0.01 * 1e6
+	 *
+	 * raw_to_gauss = Hadj * 1500
+	 */
+	st->magn_raw_to_gauss[0] = (((int32_t)asa[0] + 128) * 1500) / 256;
+	st->magn_raw_to_gauss[1] = (((int32_t)asa[1] + 128) * 1500) / 256;
+	st->magn_raw_to_gauss[2] = (((int32_t)asa[2] + 128) * 1500) / 256;
+
+	return 0;
+}
+
+/**
+ * inv_mpu_magn_probe() - probe and setup magnetometer chip
+ * @st: driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise
+ *
+ * It is probing the chip and setting up all needed i2c transfers.
+ * Noop if there is no magnetometer in the chip.
+ */
+int inv_mpu_magn_probe(struct inv_mpu6050_state *st)
+{
+	int ret;
+
+	/* quit if chip is not supported */
+	if (!inv_magn_supported(st))
+		return 0;
+
+	/* configure i2c master aux port */
+	ret = inv_mpu_aux_init(st);
+	if (ret)
+		return ret;
+
+	/* check and init mag chip */
+	ret = inv_magn_init(st);
+	if (ret)
+		return ret;
+
+	/*
+	 * configure mpu i2c master accesses
+	 * i2c SLV0: read sensor data, 7 bytes data(6)-ST2
+	 * Byte swap data to store them in big-endian in impair address groups
+	 */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0),
+			   INV_MPU6050_BIT_I2C_SLV_RNW | INV_MPU_MAGN_I2C_ADDR);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0),
+			   INV_MPU_MAGN_REG_DATA);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
+			   INV_MPU6050_BIT_SLV_EN |
+			   INV_MPU6050_BIT_SLV_BYTE_SW |
+			   INV_MPU6050_BIT_SLV_GRP |
+			   INV_MPU9X50_BYTES_MAGN);
+	if (ret)
+		return ret;
+
+	/* i2c SLV1: launch single measurement */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(1),
+			   INV_MPU_MAGN_I2C_ADDR);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(1),
+			   INV_MPU_MAGN_REG_CNTL1);
+	if (ret)
+		return ret;
+
+	/* add 16 bits mode */
+	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(1),
+			   INV_MPU_MAGN_BITS_MODE_SINGLE |
+			   INV_MPU_MAGN_BIT_OUTPUT_BIT);
+	if (ret)
+		return ret;
+
+	return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(1),
+			    INV_MPU6050_BIT_SLV_EN | 1);
+}
+
+/**
+ * inv_mpu_magn_set_rate() - set magnetometer sampling rate
+ * @st: driver internal state
+ * @fifo_rate: mpu set fifo rate
+ *
+ * Returns 0 on success, a negative error code otherwise
+ *
+ * Limit sampling frequency to the maximum value supported by the
+ * magnetometer chip. Resulting in duplicated data for higher frequencies.
+ * Noop if there is no magnetometer in the chip.
+ */
+int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate)
+{
+	uint8_t d;
+
+	/* quit if chip is not supported */
+	if (!inv_magn_supported(st))
+		return 0;
+
+	/*
+	 * update i2c master delay to limit mag sampling to max frequency
+	 * compute fifo_rate divider d: rate = fifo_rate / (d + 1)
+	 */
+	if (fifo_rate > INV_MPU_MAGN_FREQ_HZ_MAX)
+		d = fifo_rate / INV_MPU_MAGN_FREQ_HZ_MAX - 1;
+	else
+		d = 0;
+
+	return regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, d);
+}
+
+/**
+ * inv_mpu_magn_set_orient() - fill magnetometer mounting matrix
+ * @st: driver internal state
+ *
+ * Returns 0 on success, a negative error code otherwise
+ *
+ * Fill magnetometer mounting matrix using the provided chip matrix.
+ */
+int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st)
+{
+	const char *orient;
+	char *str;
+	int i;
+
+	/* fill magnetometer orientation */
+	switch (st->chip_type) {
+	case INV_MPU9250:
+	case INV_MPU9255:
+		/* x <- y */
+		st->magn_orient.rotation[0] = st->orientation.rotation[3];
+		st->magn_orient.rotation[1] = st->orientation.rotation[4];
+		st->magn_orient.rotation[2] = st->orientation.rotation[5];
+		/* y <- x */
+		st->magn_orient.rotation[3] = st->orientation.rotation[0];
+		st->magn_orient.rotation[4] = st->orientation.rotation[1];
+		st->magn_orient.rotation[5] = st->orientation.rotation[2];
+		/* z <- -z */
+		for (i = 0; i < 3; ++i) {
+			orient = st->orientation.rotation[6 + i];
+			/* use length + 2 for adding minus sign if needed */
+			str = devm_kzalloc(regmap_get_device(st->map),
+					   strlen(orient) + 2, GFP_KERNEL);
+			if (str == NULL)
+				return -ENOMEM;
+			if (strcmp(orient, "0") == 0) {
+				strcpy(str, orient);
+			} else if (orient[0] == '-') {
+				strcpy(str, &orient[1]);
+			} else {
+				str[0] = '-';
+				strcpy(&str[1], orient);
+			}
+			st->magn_orient.rotation[6 + i] = str;
+		}
+		break;
+	default:
+		st->magn_orient = st->orientation;
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * inv_mpu_magn_read() - read magnetometer data
+ * @st: driver internal state
+ * @axis: IIO modifier axis value
+ * @val: store corresponding axis value
+ *
+ * Returns 0 on success, a negative error code otherwise
+ */
+int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val)
+{
+	unsigned int user_ctrl, status;
+	__be16 data[3];
+	uint8_t addr;
+	uint8_t d;
+	unsigned int period_ms;
+	int ret;
+
+	/* quit if chip is not supported */
+	if (!inv_magn_supported(st))
+		return -ENODEV;
+
+	/* Mag data: X - Y - Z */
+	switch (axis) {
+	case IIO_MOD_X:
+		addr = 0;
+		break;
+	case IIO_MOD_Y:
+		addr = 1;
+		break;
+	case IIO_MOD_Z:
+		addr = 2;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set sample rate to max mag freq */
+	d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU_MAGN_FREQ_HZ_MAX);
+	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
+	if (ret)
+		return ret;
+
+	/* start i2c master, wait for xfer, stop */
+	user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN;
+	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
+	if (ret)
+		return ret;
+
+	/* need to wait 2 periods + half-period margin */
+	period_ms = 1000 / INV_MPU_MAGN_FREQ_HZ_MAX;
+	msleep(period_ms * 2 + period_ms / 2);
+	user_ctrl = st->chip_config.user_ctrl;
+	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
+	if (ret)
+		return ret;
+
+	/* restore sample rate */
+	d = st->chip_config.divider;
+	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
+	if (ret)
+		return ret;
+
+	/* check i2c status and read raw data */
+	ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
+	if (ret)
+		return ret;
+
+	if (status & INV_MPU6050_BIT_I2C_SLV0_NACK ||
+			status & INV_MPU6050_BIT_I2C_SLV1_NACK)
+		return -EIO;
+
+	ret = regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA,
+			       data, sizeof(data));
+	if (ret)
+		return ret;
+
+	*val = (int16_t)be16_to_cpu(data[addr]);
+
+	return IIO_VAL_INT;
+}
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
new file mode 100644
index 000000000000..b41bd0578478
--- /dev/null
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_magn.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 TDK-InvenSense, Inc.
+ */
+
+#ifndef INV_MPU_MAGN_H_
+#define INV_MPU_MAGN_H_
+
+#include <linux/kernel.h>
+
+#include "inv_mpu_iio.h"
+
+int inv_mpu_magn_probe(struct inv_mpu6050_state *st);
+
+/**
+ * inv_mpu_magn_get_scale() - get magnetometer scale value
+ * @st: driver internal state
+ *
+ * Returns IIO data format.
+ */
+static inline int inv_mpu_magn_get_scale(const struct inv_mpu6050_state *st,
+					 const struct iio_chan_spec *chan,
+					 int *val, int *val2)
+{
+	*val = 0;
+	*val2 = st->magn_raw_to_gauss[chan->address];
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+int inv_mpu_magn_set_rate(const struct inv_mpu6050_state *st, int fifo_rate);
+
+int inv_mpu_magn_set_orient(struct inv_mpu6050_state *st);
+
+int inv_mpu_magn_read(const struct inv_mpu6050_state *st, int axis, int *val);
+
+#endif		/* INV_MPU_MAGN_H_ */
-- 
2.17.1

[PATCH v2 7/7] iio: imu: inv_mpu6050: add fifo support for magnetometer data

[Jean-Baptiste Maneyrol]

Put read magnetometer data by mpu inside the fifo.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    |  1 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 11 ++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 86 ++++++++++++++++---
 4 files changed, 88 insertions(+), 12 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index f1c65e0dd1a5..354030e9bed5 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -104,6 +104,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = {
 	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
 	.gyro_fifo_enable = false,
 	.accl_fifo_enable = false,
+	.magn_fifo_enable = false,
 	.accl_fs = INV_MPU6050_FS_02G,
 	.user_ctrl = 0,
 };
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 953f85618199..52fcf45050a5 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -86,6 +86,7 @@ enum inv_devices {
  *  @accl_fs:		accel full scale range.
  *  @accl_fifo_enable:	enable accel data output
  *  @gyro_fifo_enable:	enable gyro data output
+ *  @magn_fifo_enable:	enable magn data output
  *  @divider:		chip sample rate divider (sample rate divider - 1)
  */
 struct inv_mpu6050_chip_config {
@@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config {
 	unsigned int accl_fs:2;
 	unsigned int accl_fifo_enable:1;
 	unsigned int gyro_fifo_enable:1;
+	unsigned int magn_fifo_enable:1;
 	u8 divider;
 	u8 user_ctrl;
 };
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 5f9a5de0bab4..bbf68b474556 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -124,7 +124,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 
 	/* enable interrupt */
 	if (st->chip_config.accl_fifo_enable ||
-	    st->chip_config.gyro_fifo_enable) {
+	    st->chip_config.gyro_fifo_enable ||
+	    st->chip_config.magn_fifo_enable) {
 		result = regmap_write(st->map, st->reg->int_enable,
 				      INV_MPU6050_BIT_DATA_RDY_EN);
 		if (result)
@@ -141,6 +142,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 		d |= INV_MPU6050_BITS_GYRO_OUT;
 	if (st->chip_config.accl_fifo_enable)
 		d |= INV_MPU6050_BIT_ACCEL_OUT;
+	if (st->chip_config.magn_fifo_enable)
+		d |= INV_MPU6050_BIT_SLAVE_0;
 	result = regmap_write(st->map, st->reg->fifo_en, d);
 	if (result)
 		goto reset_fifo_fail;
@@ -190,7 +193,8 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	}
 
 	if (!(st->chip_config.accl_fifo_enable |
-		st->chip_config.gyro_fifo_enable))
+		st->chip_config.gyro_fifo_enable |
+		st->chip_config.magn_fifo_enable))
 		goto end_session;
 	bytes_per_datum = 0;
 	if (st->chip_config.accl_fifo_enable)
@@ -202,6 +206,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	if (st->chip_type == INV_ICM20602)
 		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
 
+	if (st->chip_config.magn_fifo_enable)
+		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
+
 	/*
 	 * read fifo_count register to know how many bytes are inside the FIFO
 	 * right now
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index dd55e70b6f77..d7d951927a44 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -5,7 +5,7 @@
 
 #include "inv_mpu_iio.h"
 
-static void inv_scan_query(struct iio_dev *indio_dev)
+static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
@@ -26,6 +26,60 @@ static void inv_scan_query(struct iio_dev *indio_dev)
 			 indio_dev->active_scan_mask);
 }
 
+static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	inv_scan_query_mpu6050(indio_dev);
+
+	/* no magnetometer if i2c auxiliary bus is used */
+	if (st->magn_disabled)
+		return;
+
+	st->chip_config.magn_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_MAGN_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_MAGN_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
+			 indio_dev->active_scan_mask);
+}
+
+static void inv_scan_query(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	switch (st->chip_type) {
+	case INV_MPU9250:
+	case INV_MPU9255:
+		return inv_scan_query_mpu9x50(indio_dev);
+	default:
+		return inv_scan_query_mpu6050(indio_dev);
+	}
+}
+
+static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
+{
+	unsigned int gyro_skip = 0;
+	unsigned int magn_skip = 0;
+	unsigned int skip_samples;
+
+	/* gyro first sample is out of specs, skip it */
+	if (st->chip_config.gyro_fifo_enable)
+		gyro_skip = 1;
+
+	/* mag first sample is always not ready, skip it */
+	if (st->chip_config.magn_fifo_enable)
+		magn_skip = 1;
+
+	/* compute first samples to skip */
+	skip_samples = gyro_skip;
+	if (magn_skip > skip_samples)
+		skip_samples = magn_skip;
+
+	return skip_samples;
+}
+
 /**
  *  inv_mpu6050_set_enable() - enable chip functions.
  *  @indio_dev:	Device driver instance.
@@ -34,6 +88,7 @@ static void inv_scan_query(struct iio_dev *indio_dev)
 static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 {
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	uint8_t d;
 	int result;
 
 	if (enable) {
@@ -41,14 +96,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 		inv_scan_query(indio_dev);
-		st->skip_samples = 0;
 		if (st->chip_config.gyro_fifo_enable) {
 			result = inv_mpu6050_switch_engine(st, true,
 					INV_MPU6050_BIT_PWR_GYRO_STBY);
 			if (result)
 				goto error_power_off;
-			/* gyro first sample is out of specs, skip it */
-			st->skip_samples = 1;
 		}
 		if (st->chip_config.accl_fifo_enable) {
 			result = inv_mpu6050_switch_engine(st, true,
@@ -56,22 +108,32 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 			if (result)
 				goto error_gyro_off;
 		}
+		if (st->chip_config.magn_fifo_enable) {
+			d = st->chip_config.user_ctrl |
+					INV_MPU6050_BIT_I2C_MST_EN;
+			result = regmap_write(st->map, st->reg->user_ctrl, d);
+			if (result)
+				goto error_accl_off;
+			st->chip_config.user_ctrl = d;
+		}
+		st->skip_samples = inv_compute_skip_samples(st);
 		result = inv_reset_fifo(indio_dev);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 	} else {
 		result = regmap_write(st->map, st->reg->fifo_en, 0);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 
 		result = regmap_write(st->map, st->reg->int_enable, 0);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
 
-		result = regmap_write(st->map, st->reg->user_ctrl,
-				      st->chip_config.user_ctrl);
+		d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
+		result = regmap_write(st->map, st->reg->user_ctrl, d);
 		if (result)
-			goto error_accl_off;
+			goto error_magn_off;
+		st->chip_config.user_ctrl = d;
 
 		result = inv_mpu6050_switch_engine(st, false,
 					INV_MPU6050_BIT_PWR_ACCL_STBY);
@@ -90,6 +152,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 
 	return 0;
 
+error_magn_off:
+	/* always restore user_ctrl to disable fifo properly */
+	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
+	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
 error_accl_off:
 	if (st->chip_config.accl_fifo_enable)
 		inv_mpu6050_switch_engine(st, false,
-- 
2.17.1

Re: [PATCH v2 1/7] iio: imu: inv_mpu6050: disable i2c mux for MPU925x

[Jonathan Cameron]

On Thu, 12 Sep 2019 10:58:24 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:

> Disable i2c mux for supported 9xxx chips. This is a
> pre-requesite for controling 9xxx magnetometer using the
> i2c master of the chip.
> 
> Check in device-tree that there is no i2c-gate device declared
> for ensuring backward compatibility with existing setups.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>

One trivial inline.

Jonathan

> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c |  7 +--
>  drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  | 60 +++++++++++++++++++---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  2 +
>  3 files changed, 58 insertions(+), 11 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index b17f060b52fc..7b2e4d81bbba 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -1156,9 +1156,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
>  		return result;
>  	}
>  
> -	if (inv_mpu_bus_setup)
> -		inv_mpu_bus_setup(indio_dev);
> -
>  	dev_set_drvdata(dev, indio_dev);
>  	indio_dev->dev.parent = dev;
>  	/* name will be NULL when enumerated via ACPI */
> @@ -1167,6 +1164,10 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
>  	else
>  		indio_dev->name = dev_name(dev);
>  
> +	/* requires parent device set in indio_dev */
> +	if (inv_mpu_bus_setup)
> +		inv_mpu_bus_setup(indio_dev);
> +
>  	if (chip_type == INV_ICM20602) {
>  		indio_dev->channels = inv_icm20602_channels;
>  		indio_dev->num_channels = ARRAY_SIZE(inv_icm20602_channels);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> index 4b8b5a87398c..389cc8505e0e 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> @@ -68,6 +68,56 @@ static const char *inv_mpu_match_acpi_device(struct device *dev,
>  	return dev_name(dev);
>  }
>  
> +static bool inv_mpu_i2c_aux_bus(struct device *dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
> +
> +	switch (st->chip_type) {
> +	case INV_ICM20608:
> +	case INV_ICM20602:
> +		/* no i2c auxiliary bus on the chip */
> +		return false;
> +	case INV_MPU9250:
> +	case INV_MPU9255:

	return st->magn_disabled; ?

> +		if (st->magn_disabled)
> +			return true;
> +		else
> +			return false;
> +	default:
> +		return true;
> +	}
> +}
> +
> +/*
> + * MPU9xxx magnetometer support requires to disable i2c auxiliary bus support.
> + * To ensure backward compatibility with existing setups, do not disable
> + * i2c auxiliary bus if it used.
> + * Check for i2c-gate node in devicetree and set magnetometer disabled.
> + * Only MPU6500 is supported by ACPI, no need to check.
> + */
> +static int inv_mpu_magn_disable(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	struct device *dev = indio_dev->dev.parent;
> +	struct device_node *mux_node;
> +
> +	switch (st->chip_type) {
> +	case INV_MPU9250:
> +	case INV_MPU9255:
> +		mux_node = of_get_child_by_name(dev->of_node, "i2c-gate");
> +		if (mux_node != NULL) {
> +			st->magn_disabled = true;
> +			dev_warn(dev, "disable internal use of magnetometer\n");
> +		}
> +		of_node_put(mux_node);
> +		break;
> +	default:
> +		break;
> +	}
> +
> +	return 0;
> +}
> +
>  /**
>   *  inv_mpu_probe() - probe function.
>   *  @client:          i2c client.
> @@ -112,17 +162,12 @@ static int inv_mpu_probe(struct i2c_client *client,
>  	}
>  
>  	result = inv_mpu_core_probe(regmap, client->irq, name,
> -				    NULL, chip_type);
> +				    inv_mpu_magn_disable, chip_type);
>  	if (result < 0)
>  		return result;
>  
>  	st = iio_priv(dev_get_drvdata(&client->dev));
> -	switch (st->chip_type) {
> -	case INV_ICM20608:
> -	case INV_ICM20602:
> -		/* no i2c auxiliary bus on the chip */
> -		break;
> -	default:
> +	if (inv_mpu_i2c_aux_bus(&client->dev)) {
>  		/* declare i2c auxiliary bus */
>  		st->muxc = i2c_mux_alloc(client->adapter, &client->dev,
>  					 1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
> @@ -137,7 +182,6 @@ static int inv_mpu_probe(struct i2c_client *client,
>  		result = inv_mpu_acpi_create_mux_client(client);
>  		if (result)
>  			goto out_del_mux;
> -		break;
>  	}
>  
>  	return 0;
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index db1c6904388b..cbbb2fb8949a 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -125,6 +125,7 @@ struct inv_mpu6050_hw {
>   *  @it_timestamp:	timestamp from previous interrupt.
>   *  @data_timestamp:	timestamp for next data sample.
>   *  @vddio_supply	voltage regulator for the chip.
> + *  @magn_disabled:     magnetometer disabled for backward compatibility reason.
>   */
>  struct inv_mpu6050_state {
>  	struct mutex lock;
> @@ -146,6 +147,7 @@ struct inv_mpu6050_state {
>  	s64 it_timestamp;
>  	s64 data_timestamp;
>  	struct regulator *vddio_supply;
> +	bool magn_disabled;
>  };
>  
>  /*register and associated bit definition*/

Re: [PATCH v2 5/7] iio: imu: inv_mpu6050: helpers for using i2c master on auxiliary bus

[Jonathan Cameron]

On Thu, 12 Sep 2019 10:58:34 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:

> Add helper functions to use the i2c auxiliary bus with the MPU i2c
> master block.
> 
> Support only register based chip, reading and 1 byte writing. These
> will be useful for initializing magnetometers inside MPU9x50 chips.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Docs should be by the implementation not in the header.

Otherwise, this looks fine I think. I'm not planning to plough through
datasheets for this set so mostly assuming you got it right ;)

Jonathan



> ---
>  drivers/iio/imu/inv_mpu6050/Makefile      |   3 +-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c | 184 ++++++++++++++++++++++
>  drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h |  46 ++++++
>  3 files changed, 232 insertions(+), 1 deletion(-)
>  create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
>  create mode 100644 drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/Makefile b/drivers/iio/imu/inv_mpu6050/Makefile
> index 33bec09fee9b..2cfbd926522f 100644
> --- a/drivers/iio/imu/inv_mpu6050/Makefile
> +++ b/drivers/iio/imu/inv_mpu6050/Makefile
> @@ -4,7 +4,8 @@
>  #
>  
>  obj-$(CONFIG_INV_MPU6050_IIO) += inv-mpu6050.o
> -inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o
> +inv-mpu6050-y := inv_mpu_core.o inv_mpu_ring.o inv_mpu_trigger.o \
> +		 inv_mpu_aux.o
>  
>  obj-$(CONFIG_INV_MPU6050_I2C) += inv-mpu6050-i2c.o
>  inv-mpu6050-i2c-y := inv_mpu_i2c.o inv_mpu_acpi.o
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
> new file mode 100644
> index 000000000000..b4907c1a8bb6
> --- /dev/null
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.c
> @@ -0,0 +1,184 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Copyright (C) 2019 TDK-InvenSense, Inc.
> + */
> +
> +#include <linux/kernel.h>
> +#include <linux/device.h>
> +#include <linux/regmap.h>
> +#include <linux/delay.h>
> +
> +#include "inv_mpu_iio.h"
> +
> +/*
> + * i2c master auxiliary bus transfer function.
> + * Requires the i2c operations to be correctly setup before.
> + */
> +static int inv_mpu_i2c_master_xfer(const struct inv_mpu6050_state *st)
> +{
> +	/* use 50hz frequency for xfer */
> +	const unsigned int freq = 50;
> +	const unsigned int period_ms = 1000 / freq;
> +	uint8_t d;
> +	unsigned int user_ctrl;
> +	int ret;
> +
> +	/* set sample rate */
> +	d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(freq);
> +	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
> +	if (ret)
> +		return ret;
> +
> +	/* start i2c master */
> +	user_ctrl = st->chip_config.user_ctrl | INV_MPU6050_BIT_I2C_MST_EN;
> +	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
> +	if (ret)
> +		goto error_restore_rate;
> +
> +	/* wait for xfer: 1 period + half-period margin */
> +	msleep(period_ms + period_ms / 2);
> +
> +	/* stop i2c master */
> +	user_ctrl = st->chip_config.user_ctrl;
> +	ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
> +	if (ret)
> +		goto error_stop_i2c;
> +
> +	/* restore sample rate */
> +	d = st->chip_config.divider;
> +	ret = regmap_write(st->map, st->reg->sample_rate_div, d);
> +	if (ret)
> +		goto error_restore_rate;
> +
> +	return 0;
> +
> +error_stop_i2c:
> +	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
> +error_restore_rate:
> +	regmap_write(st->map, st->reg->sample_rate_div, st->chip_config.divider);
> +	return ret;
> +}
> +
> +int inv_mpu_aux_init(const struct inv_mpu6050_state *st)
> +{
> +	unsigned int val;
> +	int ret;
> +
> +	/* configure i2c master */
> +	val = INV_MPU6050_BITS_I2C_MST_CLK_400KHZ |
> +			INV_MPU6050_BIT_WAIT_FOR_ES;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_MST_CTRL, val);
> +	if (ret)
> +		return ret;
> +
> +	/* configure i2c master delay */
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, 0);
> +	if (ret)
> +		return ret;
> +
> +	val = INV_MPU6050_BIT_I2C_SLV0_DLY_EN |
> +			INV_MPU6050_BIT_I2C_SLV1_DLY_EN |
> +			INV_MPU6050_BIT_I2C_SLV2_DLY_EN |
> +			INV_MPU6050_BIT_I2C_SLV3_DLY_EN |
> +			INV_MPU6050_BIT_DELAY_ES_SHADOW;
> +	return regmap_write(st->map, INV_MPU6050_REG_I2C_MST_DELAY_CTRL, val);
> +}
> +
> +/*
> + * i2c slave reading using SLV0
> + */
> +int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr,
> +		     uint8_t reg, uint8_t *val, size_t size)
> +{
> +	unsigned int status;
> +	int ret;
> +
> +	if (size > 0x0F)
> +		return -EINVAL;
> +
> +	/* setup i2c SLV0 control: i2c addr, register, enable + size */
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0),
> +			   INV_MPU6050_BIT_I2C_SLV_RNW | addr);
> +	if (ret)
> +		return ret;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg);
> +	if (ret)
> +		return ret;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
> +			   INV_MPU6050_BIT_SLV_EN | size);
> +	if (ret)
> +		return ret;
> +
> +	/* do i2c xfer */
> +	ret = inv_mpu_i2c_master_xfer(st);
> +	if (ret)
> +		goto error_disable_i2c;
> +
> +	/* disable i2c slave */
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
> +	if (ret)
> +		goto error_disable_i2c;
> +
> +	/* check i2c status */
> +	ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
> +	if (ret)
> +		return ret;
> +	if (status & INV_MPU6050_BIT_I2C_SLV0_NACK)
> +		return -EIO;
> +
> +	/* read data in registers */
> +	return regmap_bulk_read(st->map, INV_MPU6050_REG_EXT_SENS_DATA,
> +				val, size);
> +
> +error_disable_i2c:
> +	regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
> +	return ret;
> +}
> +
> +/*
> + * i2c slave writing 1 byte using SLV0
> + */
> +int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr,
> +		      uint8_t reg, uint8_t val)
> +{
> +	unsigned int status;
> +	int ret;
> +
> +	/* setup i2c SLV0 control: i2c addr, register, value, enable + size */
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(0), addr);
> +	if (ret)
> +		return ret;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(0), reg);
> +	if (ret)
> +		return ret;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_DO(0), val);
> +	if (ret)
> +		return ret;
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0),
> +			   INV_MPU6050_BIT_SLV_EN | 1);
> +	if (ret)
> +		return ret;
> +
> +	/* do i2c xfer */
> +	ret = inv_mpu_i2c_master_xfer(st);
> +	if (ret)
> +		goto error_disable_i2c;
> +
> +	/* disable i2c slave */
> +	ret = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
> +	if (ret)
> +		goto error_disable_i2c;
> +
> +	/* check i2c status */
> +	ret = regmap_read(st->map, INV_MPU6050_REG_I2C_MST_STATUS, &status);
> +	if (ret)
> +		return ret;
> +	if (status & INV_MPU6050_BIT_I2C_SLV0_NACK)
> +		return -EIO;
> +
> +	return 0;
> +
> +error_disable_i2c:
> +	regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(0), 0);
> +	return ret;
> +}
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h
> new file mode 100644
> index 000000000000..bae4eab58e6c
> --- /dev/null
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_aux.h
> @@ -0,0 +1,46 @@
> +/* SPDX-License-Identifier: GPL-2.0 */
> +/*
> + * Copyright (C) 2019 TDK-InvenSense, Inc.
> + */
> +
> +#ifndef INV_MPU_AUX_H_
> +#define INV_MPU_AUX_H_
> +
> +#include <linux/kernel.h>
> +
> +#include "inv_mpu_iio.h"
> +
> +/**
> + * inv_mpu_aux_init() - init i2c auxiliary bus
> + * @st: driver internal state
> + *
> + * Returns 0 on success, a negative error code otherwise.
> + */
> +int inv_mpu_aux_init(const struct inv_mpu6050_state *st);
> +
> +/**
> + * inv_mpu_aux_read() - read register function for i2c auxiliary bus
> + * @st: driver internal state.
> + * @addr: chip i2c Address
> + * @reg: chip register address
> + * @val: buffer for storing read bytes
> + * @size: number of bytes to read
> + *
> + *  Returns 0 on success, a negative error code otherwise.

Documentation at implementation, not in the header.
Tends to rot when hidden away from the code.

> + */
> +int inv_mpu_aux_read(const struct inv_mpu6050_state *st, uint8_t addr,
> +		     uint8_t reg, uint8_t *val, size_t size);
> +
> +/**
> + * inv_mpu_aux_write() - write register function for i2c auxiliary bus
> + * @st: driver internal state.
> + * @addr: chip i2c Address
> + * @reg: chip register address
> + * @val: 1 byte value to write
> + *
> + *  Returns 0 on success, a negative error code otherwise.
> + */
> +int inv_mpu_aux_write(const struct inv_mpu6050_state *st, uint8_t addr,
> +		      uint8_t reg, uint8_t val);
> +
> +#endif		/* INV_MPU_AUX_H_ */

Re: [PATCH v2 7/7] iio: imu: inv_mpu6050: add fifo support for magnetometer data

[Jonathan Cameron]

On Thu, 12 Sep 2019 10:58:37 +0000
Jean-Baptiste Maneyrol <JManeyrol@invensense.com> wrote:

> Put read magnetometer data by mpu inside the fifo.
> 
> Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>

I'm fine with the rest of this series, so tidy up those two little bits
and unless other comments come in I'll hopefully pick up v3.

Thanks,

Jonathan

> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c    |  1 +
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 11 ++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 86 ++++++++++++++++---
>  4 files changed, 88 insertions(+), 12 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index f1c65e0dd1a5..354030e9bed5 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -104,6 +104,7 @@ static const struct inv_mpu6050_chip_config chip_config_6050 = {
>  	.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(INV_MPU6050_INIT_FIFO_RATE),
>  	.gyro_fifo_enable = false,
>  	.accl_fifo_enable = false,
> +	.magn_fifo_enable = false,
>  	.accl_fs = INV_MPU6050_FS_02G,
>  	.user_ctrl = 0,
>  };
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 953f85618199..52fcf45050a5 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -86,6 +86,7 @@ enum inv_devices {
>   *  @accl_fs:		accel full scale range.
>   *  @accl_fifo_enable:	enable accel data output
>   *  @gyro_fifo_enable:	enable gyro data output
> + *  @magn_fifo_enable:	enable magn data output
>   *  @divider:		chip sample rate divider (sample rate divider - 1)
>   */
>  struct inv_mpu6050_chip_config {
> @@ -94,6 +95,7 @@ struct inv_mpu6050_chip_config {
>  	unsigned int accl_fs:2;
>  	unsigned int accl_fifo_enable:1;
>  	unsigned int gyro_fifo_enable:1;
> +	unsigned int magn_fifo_enable:1;
>  	u8 divider;
>  	u8 user_ctrl;
>  };
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 5f9a5de0bab4..bbf68b474556 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -124,7 +124,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  
>  	/* enable interrupt */
>  	if (st->chip_config.accl_fifo_enable ||
> -	    st->chip_config.gyro_fifo_enable) {
> +	    st->chip_config.gyro_fifo_enable ||
> +	    st->chip_config.magn_fifo_enable) {
>  		result = regmap_write(st->map, st->reg->int_enable,
>  				      INV_MPU6050_BIT_DATA_RDY_EN);
>  		if (result)
> @@ -141,6 +142,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  		d |= INV_MPU6050_BITS_GYRO_OUT;
>  	if (st->chip_config.accl_fifo_enable)
>  		d |= INV_MPU6050_BIT_ACCEL_OUT;
> +	if (st->chip_config.magn_fifo_enable)
> +		d |= INV_MPU6050_BIT_SLAVE_0;
>  	result = regmap_write(st->map, st->reg->fifo_en, d);
>  	if (result)
>  		goto reset_fifo_fail;
> @@ -190,7 +193,8 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	}
>  
>  	if (!(st->chip_config.accl_fifo_enable |
> -		st->chip_config.gyro_fifo_enable))
> +		st->chip_config.gyro_fifo_enable |
> +		st->chip_config.magn_fifo_enable))
>  		goto end_session;
>  	bytes_per_datum = 0;
>  	if (st->chip_config.accl_fifo_enable)
> @@ -202,6 +206,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	if (st->chip_type == INV_ICM20602)
>  		bytes_per_datum += INV_ICM20602_BYTES_PER_TEMP_SENSOR;
>  
> +	if (st->chip_config.magn_fifo_enable)
> +		bytes_per_datum += INV_MPU9X50_BYTES_MAGN;
> +
>  	/*
>  	 * read fifo_count register to know how many bytes are inside the FIFO
>  	 * right now
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index dd55e70b6f77..d7d951927a44 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -5,7 +5,7 @@
>  
>  #include "inv_mpu_iio.h"
>  
> -static void inv_scan_query(struct iio_dev *indio_dev)
> +static void inv_scan_query_mpu6050(struct iio_dev *indio_dev)
>  {
>  	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
>  
> @@ -26,6 +26,60 @@ static void inv_scan_query(struct iio_dev *indio_dev)
>  			 indio_dev->active_scan_mask);
>  }
>  
> +static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +
> +	inv_scan_query_mpu6050(indio_dev);
> +
> +	/* no magnetometer if i2c auxiliary bus is used */
> +	if (st->magn_disabled)
> +		return;
> +
> +	st->chip_config.magn_fifo_enable =
> +		test_bit(INV_MPU9X50_SCAN_MAGN_X,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_MAGN_Y,
> +			 indio_dev->active_scan_mask) ||
> +		test_bit(INV_MPU9X50_SCAN_MAGN_Z,
> +			 indio_dev->active_scan_mask);
> +}
> +
> +static void inv_scan_query(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +
> +	switch (st->chip_type) {
> +	case INV_MPU9250:
> +	case INV_MPU9255:
> +		return inv_scan_query_mpu9x50(indio_dev);
> +	default:
> +		return inv_scan_query_mpu6050(indio_dev);
> +	}
> +}
> +
> +static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
> +{
> +	unsigned int gyro_skip = 0;
> +	unsigned int magn_skip = 0;
> +	unsigned int skip_samples;
> +
> +	/* gyro first sample is out of specs, skip it */
> +	if (st->chip_config.gyro_fifo_enable)
> +		gyro_skip = 1;
> +
> +	/* mag first sample is always not ready, skip it */
> +	if (st->chip_config.magn_fifo_enable)
> +		magn_skip = 1;
> +
> +	/* compute first samples to skip */
> +	skip_samples = gyro_skip;
> +	if (magn_skip > skip_samples)
> +		skip_samples = magn_skip;
> +
> +	return skip_samples;
> +}
> +
>  /**
>   *  inv_mpu6050_set_enable() - enable chip functions.
>   *  @indio_dev:	Device driver instance.
> @@ -34,6 +88,7 @@ static void inv_scan_query(struct iio_dev *indio_dev)
>  static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  {
>  	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	uint8_t d;
>  	int result;
>  
>  	if (enable) {
> @@ -41,14 +96,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  		if (result)
>  			return result;
>  		inv_scan_query(indio_dev);
> -		st->skip_samples = 0;
>  		if (st->chip_config.gyro_fifo_enable) {
>  			result = inv_mpu6050_switch_engine(st, true,
>  					INV_MPU6050_BIT_PWR_GYRO_STBY);
>  			if (result)
>  				goto error_power_off;
> -			/* gyro first sample is out of specs, skip it */
> -			st->skip_samples = 1;
>  		}
>  		if (st->chip_config.accl_fifo_enable) {
>  			result = inv_mpu6050_switch_engine(st, true,
> @@ -56,22 +108,32 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  			if (result)
>  				goto error_gyro_off;
>  		}
> +		if (st->chip_config.magn_fifo_enable) {
> +			d = st->chip_config.user_ctrl |
> +					INV_MPU6050_BIT_I2C_MST_EN;
> +			result = regmap_write(st->map, st->reg->user_ctrl, d);
> +			if (result)
> +				goto error_accl_off;
> +			st->chip_config.user_ctrl = d;
> +		}
> +		st->skip_samples = inv_compute_skip_samples(st);
>  		result = inv_reset_fifo(indio_dev);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  	} else {
>  		result = regmap_write(st->map, st->reg->fifo_en, 0);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  
>  		result = regmap_write(st->map, st->reg->int_enable, 0);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
>  
> -		result = regmap_write(st->map, st->reg->user_ctrl,
> -				      st->chip_config.user_ctrl);
> +		d = st->chip_config.user_ctrl & ~INV_MPU6050_BIT_I2C_MST_EN;
> +		result = regmap_write(st->map, st->reg->user_ctrl, d);
>  		if (result)
> -			goto error_accl_off;
> +			goto error_magn_off;
> +		st->chip_config.user_ctrl = d;
>  
>  		result = inv_mpu6050_switch_engine(st, false,
>  					INV_MPU6050_BIT_PWR_ACCL_STBY);
> @@ -90,6 +152,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  
>  	return 0;
>  
> +error_magn_off:
> +	/* always restore user_ctrl to disable fifo properly */
> +	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
> +	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
>  error_accl_off:
>  	if (st->chip_config.accl_fifo_enable)
>  		inv_mpu6050_switch_engine(st, false,