All of lore.kernel.org
 help / color / mirror / Atom feed
From: Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
To: "jic23@kernel.org" <jic23@kernel.org>
Cc: "linux-iio@vger.kernel.org" <linux-iio@vger.kernel.org>,
	Jean-Baptiste Maneyrol <JManeyrol@invensense.com>
Subject: [PATCH 8/8] iio: imu: inv_mpu6050: add fifo support for magnetometer data
Date: Thu, 29 Aug 2019 15:18:46 +0000	[thread overview]
Message-ID: <20190829151801.13014-9-jmaneyrol@invensense.com> (raw)
In-Reply-To: <20190829151801.13014-1-jmaneyrol@invensense.com>

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    | 14 ++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 90 +++++++++++++++++--
 4 files changed, 94 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 d08cec6a8a7a..d4a321fd03a9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -105,6 +105,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 cfc11cb0a36c..48ed66e39cda 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..f0a5350c1848 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -14,6 +14,7 @@
 #include <linux/poll.h>
 #include <linux/math64.h>
 #include <asm/unaligned.h>
+
 #include "inv_mpu_iio.h"
 
 /**
@@ -124,7 +125,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 +143,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,17 +194,19 @@ 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)
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
-
 	if (st->chip_config.gyro_fifo_enable)
 		bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
-
 	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
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index dd55e70b6f77..fa4928e589b9 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,52 @@ static void inv_scan_query(struct iio_dev *indio_dev)
 			 indio_dev->active_scan_mask);
 }
 
+#ifdef CONFIG_INV_MPU6050_MAGN
+static void inv_scan_query_mpu9x50(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	st->chip_config.gyro_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_GYRO_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_GYRO_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_GYRO_Z,
+			 indio_dev->active_scan_mask);
+
+	st->chip_config.accl_fifo_enable =
+		test_bit(INV_MPU9X50_SCAN_ACCL_X,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_ACCL_Y,
+			 indio_dev->active_scan_mask) ||
+		test_bit(INV_MPU9X50_SCAN_ACCL_Z,
+			 indio_dev->active_scan_mask);
+
+	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);
+}
+#endif
+
+static void inv_scan_query(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+	switch (st->chip_type) {
+#ifdef CONFIG_INV_MPU6050_MAGN
+	case INV_MPU9250:
+	case INV_MPU9255:
+		return inv_scan_query_mpu9x50(indio_dev);
+#endif
+	default:
+		return inv_scan_query_mpu6050(indio_dev);
+	}
+}
+
 /**
  *  inv_mpu6050_set_enable() - enable chip functions.
  *  @indio_dev:	Device driver instance.
@@ -34,6 +80,11 @@ 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;
+	unsigned int gyro_skip = 0;
+	unsigned int accl_skip = 0;
+	unsigned int magn_skip = 0;
+	unsigned int skip_samples;
 	int result;
 
 	if (enable) {
@@ -41,14 +92,13 @@ 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;
+			gyro_skip = 1;
 		}
 		if (st->chip_config.accl_fifo_enable) {
 			result = inv_mpu6050_switch_engine(st, true,
@@ -56,22 +106,40 @@ 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;
+			/* mag first sample is always not ready, skip it */
+			magn_skip = 1;
+		}
+		/* compute first samples to skip */
+		skip_samples = gyro_skip;
+		if (accl_skip > skip_samples)
+			skip_samples = accl_skip;
+		if (magn_skip > skip_samples)
+			skip_samples = magn_skip;
+		st->skip_samples = skip_samples;
 		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 +158,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


  parent reply	other threads:[~2019-08-29 15:36 UTC|newest]

Thread overview: 17+ messages / expand[flat|nested]  mbox.gz  Atom feed  top
2019-08-29 15:18 [PATCH 0/8] add magnetometer support for MPU925x Jean-Baptiste Maneyrol
2019-08-29 15:18 ` [PATCH 1/8] iio: imu: inv_mpu6050: disable i2c mux for 925x under Kconfig Jean-Baptiste Maneyrol
2019-08-29 15:18 ` [PATCH 2/8] iio: imu: inv_mpu6050: add header include protection macro Jean-Baptiste Maneyrol
2019-08-29 15:18 ` [PATCH 3/8] iio: imu: inv_mpu6050: add defines for supporting 9-axis chips Jean-Baptiste Maneyrol
2019-08-29 15:18 ` [PATCH 4/8] iio: imu: inv_mpu6050: fix objects syntax in Makefile Jean-Baptiste Maneyrol
2019-08-29 15:18 ` [PATCH 5/8] iio: imu: inv_mpu6050: helpers for using i2c master on auxiliary bus Jean-Baptiste Maneyrol
2019-09-08 11:57   ` Jonathan Cameron
2019-08-29 15:18 ` [PATCH 6/8] iio: imu: inv_mpu6050: add magnetometer implementation for MPU925x Jean-Baptiste Maneyrol
2019-09-08 11:57   ` Jonathan Cameron
2019-08-29 15:18 ` [PATCH 7/8] iio: imu: inv_mpu6050: add magnetometer support inside mpu driver Jean-Baptiste Maneyrol
2019-09-08 12:05   ` Jonathan Cameron
2019-09-09 16:18     ` Jean-Baptiste Maneyrol
2019-08-29 15:18 ` Jean-Baptiste Maneyrol [this message]
2019-09-08 12:07   ` [PATCH 8/8] iio: imu: inv_mpu6050: add fifo support for magnetometer data Jonathan Cameron
2019-09-08 11:42 ` [PATCH 0/8] add magnetometer support for MPU925x Jonathan Cameron
2019-09-09  9:55   ` Jean-Baptiste Maneyrol
2019-09-10 13:39     ` Jonathan Cameron

Reply instructions:

You may reply publicly to this message via plain-text email
using any one of the following methods:

* Save the following mbox file, import it into your mail client,
  and reply-to-all from there: mbox

  Avoid top-posting and favor interleaved quoting:
  https://en.wikipedia.org/wiki/Posting_style#Interleaved_style

* Reply using the --to, --cc, and --in-reply-to
  switches of git-send-email(1):

  git send-email \
    --in-reply-to=20190829151801.13014-9-jmaneyrol@invensense.com \
    --to=jmaneyrol@invensense.com \
    --cc=jic23@kernel.org \
    --cc=linux-iio@vger.kernel.org \
    /path/to/YOUR_REPLY

  https://kernel.org/pub/software/scm/git/docs/git-send-email.html

* If your mail client supports setting the In-Reply-To header
  via mailto: links, try the mailto: link
Be sure your reply has a Subject: header at the top and a blank line before the message body. 
This is an external index of several public inboxes,
see mirroring instructions on how to clone and mirror
all data and code used by this external index.