linux-iio.vger.kernel.org archive mirror
 help / color / mirror / Atom feed
From: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
To: jic23@kernel.org, linux-iio@vger.kernel.org
Cc: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Subject: [PATCH 10/13] iio: imu: inv_mpu6050: factorize fifo enable/disable
Date: Wed, 12 Feb 2020 18:40:45 +0100	[thread overview]
Message-ID: <20200212174048.1034-11-jmaneyrol@invensense.com> (raw)
In-Reply-To: <20200212174048.1034-1-jmaneyrol@invensense.com>

Rework fifo enable/disable in a separate function.

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     |  2 +-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 54 ++-------------
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 66 ++++++++++++++-----
 3 files changed, 55 insertions(+), 67 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index a578789c9210..e328c98e362c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -435,7 +435,7 @@ enum inv_mpu6050_clock_sel_e {
 
 irqreturn_t inv_mpu6050_read_fifo(int irq, void *p);
 int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev, int irq_type);
-int inv_reset_fifo(struct iio_dev *indio_dev);
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable);
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
 			      unsigned int mask);
 int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index d7397705974e..9511e4715e2c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -90,60 +90,14 @@ static s64 inv_mpu6050_get_timestamp(struct inv_mpu6050_state *st)
 	return ts;
 }
 
-int inv_reset_fifo(struct iio_dev *indio_dev)
+static int inv_reset_fifo(struct iio_dev *indio_dev)
 {
 	int result;
-	u8 d;
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
-	/* reset it timestamp validation */
-	st->it_timestamp = 0;
-
-	/* disable interrupt */
-	result = regmap_write(st->map, st->reg->int_enable, 0);
-	if (result) {
-		dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
-			result);
-		return result;
-	}
-	/* disable the sensor output to FIFO */
-	result = regmap_write(st->map, st->reg->fifo_en, 0);
-	if (result)
-		goto reset_fifo_fail;
-	/* disable fifo reading */
-	result = regmap_write(st->map, st->reg->user_ctrl,
-			      st->chip_config.user_ctrl);
-	if (result)
-		goto reset_fifo_fail;
-
-	/* reset FIFO*/
-	d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
-	result = regmap_write(st->map, st->reg->user_ctrl, d);
-	if (result)
-		goto reset_fifo_fail;
-
-	/* enable interrupt */
-	result = regmap_write(st->map, st->reg->int_enable,
-			      INV_MPU6050_BIT_DATA_RDY_EN);
-	if (result)
-		return result;
-
-	/* enable FIFO reading */
-	d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
-	result = regmap_write(st->map, st->reg->user_ctrl, d);
-	if (result)
-		goto reset_fifo_fail;
-	/* enable sensor output to FIFO */
-	d = 0;
-	if (st->chip_config.gyro_fifo_enable)
-		d |= INV_MPU6050_BITS_GYRO_OUT;
-	if (st->chip_config.accl_fifo_enable)
-		d |= INV_MPU6050_BIT_ACCEL_OUT;
-	if (st->chip_config.temp_fifo_enable)
-		d |= INV_MPU6050_BIT_TEMP_OUT;
-	if (st->chip_config.magn_fifo_enable)
-		d |= INV_MPU6050_BIT_SLAVE_0;
-	result = regmap_write(st->map, st->reg->fifo_en, d);
+	/* disable fifo and reenable it */
+	inv_mpu6050_prepare_fifo(st, false);
+	result = inv_mpu6050_prepare_fifo(st, true);
 	if (result)
 		goto reset_fifo_fail;
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index cfd7243159f6..f53f50d08b9e 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -100,6 +100,54 @@ static unsigned int inv_compute_skip_samples(const struct inv_mpu6050_state *st)
 	return skip_samples;
 }
 
+int inv_mpu6050_prepare_fifo(struct inv_mpu6050_state *st, bool enable)
+{
+	uint8_t d;
+	int ret;
+
+	if (enable) {
+		st->it_timestamp = 0;
+		/* reset FIFO */
+		d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST;
+		ret = regmap_write(st->map, st->reg->user_ctrl, d);
+		if (ret)
+			return ret;
+		/* enable sensor output to FIFO */
+		d = 0;
+		if (st->chip_config.gyro_fifo_enable)
+			d |= INV_MPU6050_BITS_GYRO_OUT;
+		if (st->chip_config.accl_fifo_enable)
+			d |= INV_MPU6050_BIT_ACCEL_OUT;
+		if (st->chip_config.temp_fifo_enable)
+			d |= INV_MPU6050_BIT_TEMP_OUT;
+		if (st->chip_config.magn_fifo_enable)
+			d |= INV_MPU6050_BIT_SLAVE_0;
+		ret = regmap_write(st->map, st->reg->fifo_en, d);
+		if (ret)
+			return ret;
+		/* enable FIFO reading */
+		d = st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_EN;
+		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);
+	} else {
+		ret = regmap_write(st->map, st->reg->int_enable, 0);
+		if (ret)
+			return ret;
+		ret = regmap_write(st->map, st->reg->fifo_en, 0);
+		if (ret)
+			return ret;
+		/* restore user_ctrl for disabling FIFO reading */
+		ret = regmap_write(st->map, st->reg->user_ctrl,
+				   st->chip_config.user_ctrl);
+	}
+
+	return ret;
+}
+
 /**
  *  inv_mpu6050_set_enable() - enable chip functions.
  *  @indio_dev:	Device driver instance.
@@ -121,24 +169,13 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			goto error_power_off;
 		st->skip_samples = inv_compute_skip_samples(st);
-		result = inv_reset_fifo(indio_dev);
+		result = inv_mpu6050_prepare_fifo(st, true);
 		if (result)
 			goto error_sensors_off;
 	} else {
-		result = regmap_write(st->map, st->reg->fifo_en, 0);
-		if (result)
-			goto error_fifo_off;
-
-		result = regmap_write(st->map, st->reg->int_enable, 0);
-		if (result)
-			goto error_fifo_off;
-
-		/* restore user_ctrl for disabling FIFO reading */
-		result = regmap_write(st->map, st->reg->user_ctrl,
-				      st->chip_config.user_ctrl);
+		result = inv_mpu6050_prepare_fifo(st, false);
 		if (result)
 			goto error_sensors_off;
-
 		result = inv_mpu6050_switch_engine(st, false, scan);
 		if (result)
 			goto error_power_off;
@@ -150,9 +187,6 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 
 	return 0;
 
-error_fifo_off:
-	/* always restore user_ctrl to disable fifo properly */
-	regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
 error_sensors_off:
 	inv_mpu6050_switch_engine(st, false, scan);
 error_power_off:
-- 
2.17.1


  parent reply	other threads:[~2020-02-12 17:41 UTC|newest]

Thread overview: 21+ messages / expand[flat|nested]  mbox.gz  Atom feed  top
2020-02-12 17:40 [PATCH 00/13] Rework sensors engines and power management Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 01/13] iio: imu: inv_mpu6050: enable i2c aux mux bypass only once Jean-Baptiste Maneyrol
2020-02-15 17:30   ` Jonathan Cameron
2020-02-18 16:03     ` Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 02/13] iio: imu: inv_mpu6050: delete useless check Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 03/13] iio: imu: inv_mpu6050: set power on/off only once during all init Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 04/13] iio: imu: inv_mpu6050: simplify polling magnetometer Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 05/13] iio: imu: inv_mpu6050: early init of chip_config for use at setup Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 06/13] iio: imu: inv_mpu6050: add all signal path resets at init Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 07/13] iio: imu: inv_mpu6050: fix sleep time when turning regulators on Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 08/13] iio: imu: inv_mpu6050: rewrite power and engine management Jean-Baptiste Maneyrol
2020-02-15 18:09   ` Jonathan Cameron
2020-02-12 17:40 ` [PATCH 09/13] iio: imu: inv_mpu6050: fix data polling interface Jean-Baptiste Maneyrol
2020-02-12 17:40 ` Jean-Baptiste Maneyrol [this message]
2020-02-12 17:40 ` [PATCH 11/13] iio: imu: inv_mpu6050: dynamic sampling rate change Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 12/13] iio: imu: inv_mpu6050: use runtime pm with autosuspend Jean-Baptiste Maneyrol
2020-02-15 18:22   ` Jonathan Cameron
2020-02-18 16:44     ` Jean-Baptiste Maneyrol
2020-02-12 17:40 ` [PATCH 13/13] iio: imu: inv_mpu6050: temperature only work with accel/gyro Jean-Baptiste Maneyrol
2020-02-15 18:23   ` Jonathan Cameron
2020-02-18 16:46     ` Jean-Baptiste Maneyrol

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=20200212174048.1034-11-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 a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for NNTP newsgroup(s).