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 v2 7/7] iio: imu: inv_mpu6050: add fifo support for magnetometer data
Date: Thu, 12 Sep 2019 10:58:37 +0000 [thread overview]
Message-ID: <20190912105804.15650-7-jmaneyrol@invensense.com> (raw)
In-Reply-To: <20190912105804.15650-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 | 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
next prev parent reply other threads:[~2019-09-12 10:58 UTC|newest]
Thread overview: 10+ messages / expand[flat|nested] mbox.gz Atom feed top
2019-09-12 10:58 [PATCH v2 1/7] iio: imu: inv_mpu6050: disable i2c mux for MPU925x Jean-Baptiste Maneyrol
2019-09-12 10:58 ` [PATCH v2 2/7] iio: imu: inv_mpu6050: add header include protection macro Jean-Baptiste Maneyrol
2019-09-12 10:58 ` [PATCH v2 3/7] iio: imu: inv_mpu6050: add defines for supporting 9-axis chips Jean-Baptiste Maneyrol
2019-09-12 10:58 ` [PATCH v2 4/7] iio: imu: inv_mpu6050: fix objects syntax in Makefile Jean-Baptiste Maneyrol
2019-09-12 10:58 ` [PATCH v2 5/7] iio: imu: inv_mpu6050: helpers for using i2c master on auxiliary bus Jean-Baptiste Maneyrol
2019-09-15 13:13 ` Jonathan Cameron
2019-09-12 10:58 ` [PATCH v2 6/7] iio: imu: inv_mpu6050: add MPU925x magnetometer support Jean-Baptiste Maneyrol
2019-09-12 10:58 ` Jean-Baptiste Maneyrol [this message]
2019-09-15 13:16 ` [PATCH v2 7/7] iio: imu: inv_mpu6050: add fifo support for magnetometer data Jonathan Cameron
2019-09-15 13:09 ` [PATCH v2 1/7] iio: imu: inv_mpu6050: disable i2c mux for MPU925x 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=20190912105804.15650-7-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).