[PATCH v2 0/7] iio: inv_mpu6050: Support i2c master and external readings

[PATCH v2 0/7] iio: inv_mpu6050: Support i2c master and external readings

[Crestez Dan Leonard]

This series attempts to implement support for external readings in i2c master
mode. Previous version here:
    https://www.spinics.net/lists/linux-iio/msg24502.html

Patches 1 and 6 should be considered bugfixes.
Patches 2,3,4 add regmap support and are mostly unchanged from v2
Patch 5 adds i2c master support
Patch 6 adds external readings (which only works in i2c master mode)

It might make sense to wait for Peter Rosin's patches cleaning up i2c mux
locking before patch 5.

Notable differences since previous versions:
 * Prefer inv_mpu_ over inv_mpu6050_ prefix
 * Remove indirection for SLV4 register names
 * Use the registered vendor prefix 'invensense'
 * Rearrange samples for scan mask instead of validating

For i2c master:
 * Explicit read/write when caching addr
 * Support I2C_FUNC_SMBUS_BYTE (making i2cdetect work)
 * Handle xfer errors as reported in status registers

For external channels support:
 * Only enable i2c slaves when required
 * Also forward write_raw for external channels
 * Drop handling read_raw from EXT_SENS registers
 * List external channels by scan index
 * Allow external channels with arbitrary sizes

Crestez Dan Leonard (7):
  iio: inv_mpu6050: Do burst reads using spi/i2c directly
  iio: inv_mpu6050: Initial regcache support
  iio: inv_mpu6050: Only toggle DATA_RDY_EN in inv_reset_fifo
  iio: inv_mpu6050: Cache non-volatile bits of user_ctrl
  iio: inv_mpu6050: Add support for auxiliary I2C master
  iio: inv_mpu6050: Reformat sample for active scan mask
  iio: inv_mpu6050: Expose channels from slave sensors

 .../devicetree/bindings/iio/imu/inv_mpu6050.txt    | 105 +++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c         | 692 ++++++++++++++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c          |   5 -
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h          | 115 +++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c         | 221 ++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c          |   5 -
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c      |  24 +-
 7 files changed, 1111 insertions(+), 56 deletions(-)

-- 
2.5.5

[PATCH v2 1/7] iio: inv_mpu6050: Do burst reads using spi/i2c directly

[Crestez Dan Leonard]

Using regmap_read_bulk is wrong because it assumes that a range of
registers is being read. In our case reading from the fifo register will
return multiple values but this is *not* auto-increment.

This currently works by accident.

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 33 ++++++++++++++++++++++++++----
 1 file changed, 29 insertions(+), 4 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index d070062..8455af0 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/kfifo.h>
 #include <linux/poll.h>
+#include <linux/spi/spi.h>
 #include "inv_mpu_iio.h"
 
 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
@@ -128,6 +129,13 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	u16 fifo_count;
 	s64 timestamp;
 
+	struct device *regmap_dev = regmap_get_device(st->map);
+	struct i2c_client *i2c;
+	struct spi_device *spi = NULL;
+
+	i2c = i2c_verify_client(regmap_dev);
+	spi = i2c ? NULL: to_spi_device(regmap_dev);
+
 	mutex_lock(&indio_dev->mlock);
 	if (!(st->chip_config.accl_fifo_enable |
 		st->chip_config.gyro_fifo_enable))
@@ -160,10 +168,27 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	    fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
 		goto flush_fifo;
 	while (fifo_count >= bytes_per_datum) {
-		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
-					  data, bytes_per_datum);
-		if (result)
-			goto flush_fifo;
+		/*
+		 * We need to do a large burst read from a single register.
+		 *
+		 * regmap_read_bulk assumes that multiple registers are
+		 * involved but in our case st->reg->fifo_r_w + 1 is something
+		 * completely unrelated.
+		 */
+		if (spi) {
+			u8 cmd = st->reg->fifo_r_w | 0x80;
+			result = spi_write_then_read(spi,
+					&cmd, 1,
+					data, bytes_per_datum);
+			if (result)
+				goto flush_fifo;
+		} else {
+			result = i2c_smbus_read_i2c_block_data(i2c,
+					st->reg->fifo_r_w,
+					bytes_per_datum, data);
+			if (result != bytes_per_datum)
+				goto flush_fifo;
+		}
 
 		result = kfifo_out(&st->timestamps, &timestamp, 1);
 		/* when there is no timestamp, put timestamp as 0 */
-- 
2.5.5

Re: [PATCH v2 1/7] iio: inv_mpu6050: Do burst reads using spi/i2c directly

[Jonathan Cameron]

On 18/05/16 16:00, Crestez Dan Leonard wrote:
> Using regmap_read_bulk is wrong because it assumes that a range of
> registers is being read. In our case reading from the fifo register will
> return multiple values but this is *not* auto-increment.
> 
> This currently works by accident.
> 
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
I'd really prefer to see this in the regmap core.  It really doesn't look like
it will be hard to do.  Basically it's just an cut and paste job from
regmap_bulk_read.

1. Check the register in question is volatile - anything else would be bonkers.
2. The emulation of bulk reads relies on volatile or no caching anyway.


However, that will probably take a while, so I suppose we might want to take this
in the meantime as a fix - be it not a terribly urgent one as you observe it
works by accident at the moment.

Clearly it will be needed for your regcache support though.
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 33 ++++++++++++++++++++++++++----
>  1 file changed, 29 insertions(+), 4 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index d070062..8455af0 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -21,6 +21,7 @@
>  #include <linux/interrupt.h>
>  #include <linux/kfifo.h>
>  #include <linux/poll.h>
> +#include <linux/spi/spi.h>
>  #include "inv_mpu_iio.h"
>  
>  static void inv_clear_kfifo(struct inv_mpu6050_state *st)
> @@ -128,6 +129,13 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	u16 fifo_count;
>  	s64 timestamp;
>  
> +	struct device *regmap_dev = regmap_get_device(st->map);
> +	struct i2c_client *i2c;
> +	struct spi_device *spi = NULL;
> +
> +	i2c = i2c_verify_client(regmap_dev);
> +	spi = i2c ? NULL: to_spi_device(regmap_dev);
> +
>  	mutex_lock(&indio_dev->mlock);
>  	if (!(st->chip_config.accl_fifo_enable |
>  		st->chip_config.gyro_fifo_enable))
> @@ -160,10 +168,27 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	    fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
>  		goto flush_fifo;
>  	while (fifo_count >= bytes_per_datum) {
> -		result = regmap_bulk_read(st->map, st->reg->fifo_r_w,
> -					  data, bytes_per_datum);
> -		if (result)
> -			goto flush_fifo;
> +		/*
> +		 * We need to do a large burst read from a single register.
> +		 *
> +		 * regmap_read_bulk assumes that multiple registers are
> +		 * involved but in our case st->reg->fifo_r_w + 1 is something
> +		 * completely unrelated.
> +		 */
> +		if (spi) {
> +			u8 cmd = st->reg->fifo_r_w | 0x80;
> +			result = spi_write_then_read(spi,
> +					&cmd, 1,
> +					data, bytes_per_datum);
> +			if (result)
> +				goto flush_fifo;
> +		} else {
> +			result = i2c_smbus_read_i2c_block_data(i2c,
> +					st->reg->fifo_r_w,
> +					bytes_per_datum, data);
> +			if (result != bytes_per_datum)
> +				goto flush_fifo;
> +		}
>  
>  		result = kfifo_out(&st->timestamps, &timestamp, 1);
>  		/* when there is no timestamp, put timestamp as 0 */
> 

[PATCH v2 2/7] iio: inv_mpu6050: Initial regcache support

[Crestez Dan Leonard]

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 47 ++++++++++++++++++++++++++++++
 drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  |  5 ----
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  1 +
 drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c  |  5 ----
 4 files changed, 48 insertions(+), 10 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index b269b37..5918c23 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -116,6 +116,53 @@ static const struct inv_mpu6050_hw hw_info[] = {
 	},
 };
 
+static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
+{
+	if (reg >= INV_MPU6050_REG_RAW_ACCEL && reg < INV_MPU6050_REG_RAW_ACCEL + 6)
+		return true;
+	if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
+		return true;
+	switch (reg) {
+	case INV_MPU6050_REG_TEMPERATURE:
+	case INV_MPU6050_REG_TEMPERATURE + 1:
+	case INV_MPU6050_REG_USER_CTRL:
+	case INV_MPU6050_REG_PWR_MGMT_1:
+	case INV_MPU6050_REG_FIFO_COUNT_H:
+	case INV_MPU6050_REG_FIFO_COUNT_H + 1:
+	case INV_MPU6050_REG_FIFO_R_W:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool inv_mpu6050_precious_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case INV_MPU6050_REG_FIFO_R_W:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/*
+ * Common regmap config for inv_mpu devices
+ *
+ * The current volatile/precious registers are common among supported devices.
+ * When that changes the volatile/precious callbacks should go through the
+ * inv_mpu6050_reg_map structs.
+ */
+const struct regmap_config inv_mpu_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = inv_mpu6050_volatile_reg,
+	.precious_reg = inv_mpu6050_precious_reg,
+};
+EXPORT_SYMBOL_GPL(inv_mpu_regmap_config);
+
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
 {
 	unsigned int d, mgmt_1;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
index 1a424a6..1a8d1a5 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
@@ -20,11 +20,6 @@
 #include <linux/module.h>
 #include "inv_mpu_iio.h"
 
-static const struct regmap_config inv_mpu_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-};
-
 /*
  * The i2c read/write needs to happen in unlocked mode. As the parent
  * adapter is common. If we use locked versions, it will fail as
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 47ca25b..297b0ef 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -291,3 +291,4 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 int inv_mpu_core_remove(struct device *dev);
 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
 extern const struct dev_pm_ops inv_mpu_pmops;
+extern const struct regmap_config inv_mpu_regmap_config;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
index 190a4a5..b3bd977 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
@@ -17,11 +17,6 @@
 #include <linux/iio/iio.h>
 #include "inv_mpu_iio.h"
 
-static const struct regmap_config inv_mpu_regmap_config = {
-	.reg_bits = 8,
-	.val_bits = 8,
-};
-
 static int inv_mpu_i2c_disable(struct iio_dev *indio_dev)
 {
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
-- 
2.5.5

Re: [PATCH v2 2/7] iio: inv_mpu6050: Initial regcache support

[Matt Ranostay]

On Wed, May 18, 2016 at 8:00 AM, Crestez Dan Leonard
<leonard.crestez@intel.com> wrote:
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 47 ++++++++++++++++++++++++++++++
>  drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  |  5 ----
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  1 +
>  drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c  |  5 ----
>  4 files changed, 48 insertions(+), 10 deletions(-)
>
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index b269b37..5918c23 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -116,6 +116,53 @@ static const struct inv_mpu6050_hw hw_info[] = {
>         },
>  };
>
> +static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
> +{
> +       if (reg >= INV_MPU6050_REG_RAW_ACCEL && reg < INV_MPU6050_REG_RAW_ACCEL + 6)
> +               return true;
> +       if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
> +               return true;

I think you want to put parenthesis around the addition operations...
the condition check probably don't evaluate to what you are expecting.

> +       switch (reg) {
> +       case INV_MPU6050_REG_TEMPERATURE:
> +       case INV_MPU6050_REG_TEMPERATURE + 1:
> +       case INV_MPU6050_REG_USER_CTRL:
> +       case INV_MPU6050_REG_PWR_MGMT_1:
> +       case INV_MPU6050_REG_FIFO_COUNT_H:
> +       case INV_MPU6050_REG_FIFO_COUNT_H + 1:
> +       case INV_MPU6050_REG_FIFO_R_W:
> +               return true;
> +       default:
> +               return false;
> +       }
> +}
> +
> +static bool inv_mpu6050_precious_reg(struct device *dev, unsigned int reg)
> +{
> +       switch (reg) {
> +       case INV_MPU6050_REG_FIFO_R_W:
> +               return true;
> +       default:
> +               return false;
> +       }
> +}
> +
> +/*
> + * Common regmap config for inv_mpu devices
> + *
> + * The current volatile/precious registers are common among supported devices.
> + * When that changes the volatile/precious callbacks should go through the
> + * inv_mpu6050_reg_map structs.
> + */
> +const struct regmap_config inv_mpu_regmap_config = {
> +       .reg_bits = 8,
> +       .val_bits = 8,
> +
> +       .cache_type = REGCACHE_RBTREE,
> +       .volatile_reg = inv_mpu6050_volatile_reg,
> +       .precious_reg = inv_mpu6050_precious_reg,
> +};
> +EXPORT_SYMBOL_GPL(inv_mpu_regmap_config);
> +
>  int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
>  {
>         unsigned int d, mgmt_1;
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> index 1a424a6..1a8d1a5 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
> @@ -20,11 +20,6 @@
>  #include <linux/module.h>
>  #include "inv_mpu_iio.h"
>
> -static const struct regmap_config inv_mpu_regmap_config = {
> -       .reg_bits = 8,
> -       .val_bits = 8,
> -};
> -
>  /*
>   * The i2c read/write needs to happen in unlocked mode. As the parent
>   * adapter is common. If we use locked versions, it will fail as
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 47ca25b..297b0ef 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -291,3 +291,4 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
>  int inv_mpu_core_remove(struct device *dev);
>  int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
>  extern const struct dev_pm_ops inv_mpu_pmops;
> +extern const struct regmap_config inv_mpu_regmap_config;
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
> index 190a4a5..b3bd977 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
> @@ -17,11 +17,6 @@
>  #include <linux/iio/iio.h>
>  #include "inv_mpu_iio.h"
>
> -static const struct regmap_config inv_mpu_regmap_config = {
> -       .reg_bits = 8,
> -       .val_bits = 8,
> -};
> -
>  static int inv_mpu_i2c_disable(struct iio_dev *indio_dev)
>  {
>         struct inv_mpu6050_state *st = iio_priv(indio_dev);
> --
> 2.5.5
>
> --
> To unsubscribe from this list: send the line "unsubscribe linux-iio" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html

Re: [PATCH v2 2/7] iio: inv_mpu6050: Initial regcache support

[Peter Rosin]

Hi!

On 2016-05-20 04:34, Matt Ranostay wrote:
> On Wed, May 18, 2016 at 8:00 AM, Crestez Dan Leonard
> <leonard.crestez@intel.com> wrote:
>> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
>> ---
>>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 47 ++++++++++++++++++++++++++++++
>>  drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  |  5 ----
>>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  1 +
>>  drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c  |  5 ----
>>  4 files changed, 48 insertions(+), 10 deletions(-)
>>
>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> index b269b37..5918c23 100644
>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>> @@ -116,6 +116,53 @@ static const struct inv_mpu6050_hw hw_info[] = {
>>         },
>>  };
>>
>> +static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
>> +{
>> +       if (reg >= INV_MPU6050_REG_RAW_ACCEL && reg < INV_MPU6050_REG_RAW_ACCEL + 6)
>> +               return true;
>> +       if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
>> +               return true;
> 
> I think you want to put parenthesis around the addition operations...

Maybe.

> the condition check probably don't evaluate to what you are expecting.

Looks sane to me since + has highest precedence, then < and >=, and && comes
in last...

>> +       switch (reg) {
>> +       case INV_MPU6050_REG_TEMPERATURE:
>> +       case INV_MPU6050_REG_TEMPERATURE + 1:
>> +       case INV_MPU6050_REG_USER_CTRL:
>> +       case INV_MPU6050_REG_PWR_MGMT_1:
>> +       case INV_MPU6050_REG_FIFO_COUNT_H:
>> +       case INV_MPU6050_REG_FIFO_COUNT_H + 1:
>> +       case INV_MPU6050_REG_FIFO_R_W:
>> +               return true;
>> +       default:
>> +               return false;
>> +       }
>> +}

...but even so, I think I would use an ellipsis in the switch statement
instead, like so:

static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
{
	switch (reg) {
	case INV_MPU6050_REG_RAW_ACCEL ... INV_MPU6050_REG_RAW_ACCEL + 5:
	case INV_MPU6050_REG_RAW_GYRO ... INV_MPU6050_REG_RAW_GYRO  + 5:
	case INV_MPU6050_REG_TEMPERATURE:
	case INV_MPU6050_REG_TEMPERATURE + 1:
	case INV_MPU6050_REG_USER_CTRL:
	case INV_MPU6050_REG_PWR_MGMT_1:
	case INV_MPU6050_REG_FIFO_COUNT_H:
	case INV_MPU6050_REG_FIFO_COUNT_H + 1:
	case INV_MPU6050_REG_FIFO_R_W:
		return true;
	default:
		return false;
	}
}

Cheers,
Peter

Re: [PATCH v2 2/7] iio: inv_mpu6050: Initial regcache support

[Crestez Dan Leonard]

On 05/20/2016 09:39 AM, Peter Rosin wrote:
> On 2016-05-20 04:34, Matt Ranostay wrote:
>> On Wed, May 18, 2016 at 8:00 AM, Crestez Dan Leonard
>> <leonard.crestez@intel.com> wrote:
>>> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
>>> ---
>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 47 ++++++++++++++++++++++++++++++
>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  |  5 ----
>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  1 +
>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c  |  5 ----
>>>  4 files changed, 48 insertions(+), 10 deletions(-)
>>>
>>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> index b269b37..5918c23 100644
>>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>> @@ -116,6 +116,53 @@ static const struct inv_mpu6050_hw hw_info[] = {
>>>         },
>>>  };
>>>
>>> +static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
>>> +{
>>> +       if (reg >= INV_MPU6050_REG_RAW_ACCEL && reg < INV_MPU6050_REG_RAW_ACCEL + 6)
>>> +               return true;
>>> +       if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
>>> +               return true;
>>
>> I think you want to put parenthesis around the addition operations...
> 
> Maybe.
> 
>> the condition check probably don't evaluate to what you are expecting.
> 
> Looks sane to me since + has highest precedence, then < and >=, and && comes
> in last...
> 
> ...but even so, I think I would use an ellipsis in the switch statement
> instead, like so:
> 
> static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
> {
> 	switch (reg) {
> 	case INV_MPU6050_REG_RAW_ACCEL ... INV_MPU6050_REG_RAW_ACCEL + 5:
> 	case INV_MPU6050_REG_RAW_GYRO ... INV_MPU6050_REG_RAW_GYRO  + 5:
> 	case INV_MPU6050_REG_TEMPERATURE:
> 	case INV_MPU6050_REG_TEMPERATURE + 1:
> 	case INV_MPU6050_REG_USER_CTRL:
> 	case INV_MPU6050_REG_PWR_MGMT_1:
> 	case INV_MPU6050_REG_FIFO_COUNT_H:
> 	case INV_MPU6050_REG_FIFO_COUNT_H + 1:
> 	case INV_MPU6050_REG_FIFO_R_W:
> 		return true;
> 	default:
> 		return false;
> 	}
> }

Neat, I didn't know about this extension. It does look nicer in this
function.

Re: [PATCH v2 2/7] iio: inv_mpu6050: Initial regcache support

[Jonathan Cameron]

On 20/05/16 12:01, Crestez Dan Leonard wrote:
> On 05/20/2016 09:39 AM, Peter Rosin wrote:
>> On 2016-05-20 04:34, Matt Ranostay wrote:
>>> On Wed, May 18, 2016 at 8:00 AM, Crestez Dan Leonard
>>> <leonard.crestez@intel.com> wrote:
>>>> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
>>>> ---
>>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c | 47 ++++++++++++++++++++++++++++++
>>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c  |  5 ----
>>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |  1 +
>>>>  drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c  |  5 ----
>>>>  4 files changed, 48 insertions(+), 10 deletions(-)
>>>>
>>>> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>>> index b269b37..5918c23 100644
>>>> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>>> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
>>>> @@ -116,6 +116,53 @@ static const struct inv_mpu6050_hw hw_info[] = {
>>>>         },
>>>>  };
>>>>
>>>> +static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
>>>> +{
>>>> +       if (reg >= INV_MPU6050_REG_RAW_ACCEL && reg < INV_MPU6050_REG_RAW_ACCEL + 6)
>>>> +               return true;
>>>> +       if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
>>>> +               return true;
>>>
>>> I think you want to put parenthesis around the addition operations...
>>
>> Maybe.
>>
>>> the condition check probably don't evaluate to what you are expecting.
>>
>> Looks sane to me since + has highest precedence, then < and >=, and && comes
>> in last...
>>
>> ...but even so, I think I would use an ellipsis in the switch statement
>> instead, like so:
>>
>> static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
>> {
>> 	switch (reg) {
>> 	case INV_MPU6050_REG_RAW_ACCEL ... INV_MPU6050_REG_RAW_ACCEL + 5:
>> 	case INV_MPU6050_REG_RAW_GYRO ... INV_MPU6050_REG_RAW_GYRO  + 5:
>> 	case INV_MPU6050_REG_TEMPERATURE:
>> 	case INV_MPU6050_REG_TEMPERATURE + 1:
>> 	case INV_MPU6050_REG_USER_CTRL:
>> 	case INV_MPU6050_REG_PWR_MGMT_1:
>> 	case INV_MPU6050_REG_FIFO_COUNT_H:
>> 	case INV_MPU6050_REG_FIFO_COUNT_H + 1:
>> 	case INV_MPU6050_REG_FIFO_R_W:
>> 		return true;
>> 	default:
>> 		return false;
>> 	}
>> }
> 
> Neat, I didn't know about this extension. It does look nicer in this
> function.
Be careful to use this sparingly as it does make review harder in some
cases as we have to care which registers happen to numerically fall in the
range.

It's good for some cases though such as multiple bytes of the same 'real
register'.

Jonathan
> 

[PATCH v2 3/7] iio: inv_mpu6050: Only toggle DATA_RDY_EN in inv_reset_fifo

[Crestez Dan Leonard]

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 13 ++++++++-----
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c |  3 ++-
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 8455af0..3fc0b71 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -41,7 +41,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
 
 	/* disable interrupt */
-	result = regmap_write(st->map, st->reg->int_enable, 0);
+	result = regmap_update_bits(st->map, st->reg->int_enable,
+				    INV_MPU6050_BIT_DATA_RDY_EN, 0);
 	if (result) {
 		dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
 			result);
@@ -68,8 +69,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	/* enable interrupt */
 	if (st->chip_config.accl_fifo_enable ||
 	    st->chip_config.gyro_fifo_enable) {
-		result = regmap_write(st->map, st->reg->int_enable,
-				      INV_MPU6050_BIT_DATA_RDY_EN);
+		result = regmap_update_bits(st->map, st->reg->int_enable,
+					    INV_MPU6050_BIT_DATA_RDY_EN,
+					    INV_MPU6050_BIT_DATA_RDY_EN);
 		if (result)
 			return result;
 	}
@@ -92,8 +94,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 
 reset_fifo_fail:
 	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
-	result = regmap_write(st->map, st->reg->int_enable,
-			      INV_MPU6050_BIT_DATA_RDY_EN);
+	result = regmap_update_bits(st->map, st->reg->int_enable,
+				    INV_MPU6050_BIT_DATA_RDY_EN,
+				    INV_MPU6050_BIT_DATA_RDY_EN);
 
 	return result;
 }
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index e8818d4..1a6bad3 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -69,7 +69,8 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 
-		result = regmap_write(st->map, st->reg->int_enable, 0);
+		result = regmap_update_bits(st->map, st->reg->int_enable,
+					INV_MPU6050_BIT_DATA_RDY_EN, 0);
 		if (result)
 			return result;
 
-- 
2.5.5

Re: [PATCH v2 3/7] iio: inv_mpu6050: Only toggle DATA_RDY_EN in inv_reset_fifo

[Jonathan Cameron]

On 18/05/16 16:00, Crestez Dan Leonard wrote:
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
Looks good to me.

Jonathan
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 13 ++++++++-----
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c |  3 ++-
>  2 files changed, 10 insertions(+), 6 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 8455af0..3fc0b71 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -41,7 +41,8 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
>  
>  	/* disable interrupt */
> -	result = regmap_write(st->map, st->reg->int_enable, 0);
> +	result = regmap_update_bits(st->map, st->reg->int_enable,
> +				    INV_MPU6050_BIT_DATA_RDY_EN, 0);
>  	if (result) {
>  		dev_err(regmap_get_device(st->map), "int_enable failed %d\n",
>  			result);
> @@ -68,8 +69,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	/* enable interrupt */
>  	if (st->chip_config.accl_fifo_enable ||
>  	    st->chip_config.gyro_fifo_enable) {
> -		result = regmap_write(st->map, st->reg->int_enable,
> -				      INV_MPU6050_BIT_DATA_RDY_EN);
> +		result = regmap_update_bits(st->map, st->reg->int_enable,
> +					    INV_MPU6050_BIT_DATA_RDY_EN,
> +					    INV_MPU6050_BIT_DATA_RDY_EN);
>  		if (result)
>  			return result;
>  	}
> @@ -92,8 +94,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  
>  reset_fifo_fail:
>  	dev_err(regmap_get_device(st->map), "reset fifo failed %d\n", result);
> -	result = regmap_write(st->map, st->reg->int_enable,
> -			      INV_MPU6050_BIT_DATA_RDY_EN);
> +	result = regmap_update_bits(st->map, st->reg->int_enable,
> +				    INV_MPU6050_BIT_DATA_RDY_EN,
> +				    INV_MPU6050_BIT_DATA_RDY_EN);
>  
>  	return result;
>  }
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index e8818d4..1a6bad3 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -69,7 +69,8 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  		if (result)
>  			return result;
>  
> -		result = regmap_write(st->map, st->reg->int_enable, 0);
> +		result = regmap_update_bits(st->map, st->reg->int_enable,
> +					INV_MPU6050_BIT_DATA_RDY_EN, 0);
>  		if (result)
>  			return result;
>  
> 

[PATCH v2 4/7] iio: inv_mpu6050: Cache non-volatile bits of user_ctrl

[Crestez Dan Leonard]

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     | 2 ++
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 9 ++++++---
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 4 +++-
 3 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 297b0ef..bd2c0fd 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -80,6 +80,7 @@ enum inv_devices {
  *  @enable:		master enable state.
  *  @accl_fifo_enable:	enable accel data output
  *  @gyro_fifo_enable:	enable gyro data output
+ *  @user_ctrl:		The non-volatile bits of user_ctrl
  *  @fifo_rate:		FIFO update rate.
  */
 struct inv_mpu6050_chip_config {
@@ -89,6 +90,7 @@ struct inv_mpu6050_chip_config {
 	unsigned int enable:1;
 	unsigned int accl_fifo_enable:1;
 	unsigned int gyro_fifo_enable:1;
+	u8 user_ctrl;
 	u16 fifo_rate;
 };
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 3fc0b71..56ee1e2 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -53,13 +53,15 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	if (result)
 		goto reset_fifo_fail;
 	/* disable fifo reading */
-	result = regmap_write(st->map, st->reg->user_ctrl, 0);
+	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
+	result = regmap_write(st->map, st->reg->user_ctrl,
+			      st->chip_config.user_ctrl);
 	if (result)
 		goto reset_fifo_fail;
 
 	/* reset FIFO*/
 	result = regmap_write(st->map, st->reg->user_ctrl,
-			      INV_MPU6050_BIT_FIFO_RST);
+			      st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST);
 	if (result)
 		goto reset_fifo_fail;
 
@@ -76,8 +78,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 			return result;
 	}
 	/* enable FIFO reading and I2C master interface*/
+	st->chip_config.user_ctrl |= INV_MPU6050_BIT_FIFO_EN;
 	result = regmap_write(st->map, st->reg->user_ctrl,
-			      INV_MPU6050_BIT_FIFO_EN);
+			      st->chip_config.user_ctrl);
 	if (result)
 		goto reset_fifo_fail;
 	/* enable sensor output to FIFO */
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 1a6bad3..fc55923 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -74,7 +74,9 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 
-		result = regmap_write(st->map, st->reg->user_ctrl, 0);
+		st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
+		result = regmap_write(st->map, st->reg->user_ctrl,
+				      st->chip_config.user_ctrl);
 		if (result)
 			return result;
 
-- 
2.5.5

Re: [PATCH v2 4/7] iio: inv_mpu6050: Cache non-volatile bits of user_ctrl

[Jonathan Cameron]

On 18/05/16 16:00, Crestez Dan Leonard wrote:
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
Kind of a pity we have to do this, but fair enough!

Jonathan
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h     | 2 ++
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c    | 9 ++++++---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c | 4 +++-
>  3 files changed, 11 insertions(+), 4 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 297b0ef..bd2c0fd 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -80,6 +80,7 @@ enum inv_devices {
>   *  @enable:		master enable state.
>   *  @accl_fifo_enable:	enable accel data output
>   *  @gyro_fifo_enable:	enable gyro data output
> + *  @user_ctrl:		The non-volatile bits of user_ctrl
>   *  @fifo_rate:		FIFO update rate.
>   */
>  struct inv_mpu6050_chip_config {
> @@ -89,6 +90,7 @@ struct inv_mpu6050_chip_config {
>  	unsigned int enable:1;
>  	unsigned int accl_fifo_enable:1;
>  	unsigned int gyro_fifo_enable:1;
> +	u8 user_ctrl;
>  	u16 fifo_rate;
>  };
>  
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 3fc0b71..56ee1e2 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -53,13 +53,15 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	if (result)
>  		goto reset_fifo_fail;
>  	/* disable fifo reading */
> -	result = regmap_write(st->map, st->reg->user_ctrl, 0);
> +	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
> +	result = regmap_write(st->map, st->reg->user_ctrl,
> +			      st->chip_config.user_ctrl);
>  	if (result)
>  		goto reset_fifo_fail;
>  
>  	/* reset FIFO*/
>  	result = regmap_write(st->map, st->reg->user_ctrl,
> -			      INV_MPU6050_BIT_FIFO_RST);
> +			      st->chip_config.user_ctrl | INV_MPU6050_BIT_FIFO_RST);
>  	if (result)
>  		goto reset_fifo_fail;
>  
> @@ -76,8 +78,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  			return result;
>  	}
>  	/* enable FIFO reading and I2C master interface*/
> +	st->chip_config.user_ctrl |= INV_MPU6050_BIT_FIFO_EN;
>  	result = regmap_write(st->map, st->reg->user_ctrl,
> -			      INV_MPU6050_BIT_FIFO_EN);
> +			      st->chip_config.user_ctrl);
>  	if (result)
>  		goto reset_fifo_fail;
>  	/* enable sensor output to FIFO */
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index 1a6bad3..fc55923 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -74,7 +74,9 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  		if (result)
>  			return result;
>  
> -		result = regmap_write(st->map, st->reg->user_ctrl, 0);
> +		st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
> +		result = regmap_write(st->map, st->reg->user_ctrl,
> +				      st->chip_config.user_ctrl);
>  		if (result)
>  			return result;
>  
> 

[RFC v2 5/7] iio: inv_mpu6050: Add support for auxiliary I2C master

[Crestez Dan Leonard]

The MPU has an auxiliary I2C bus for connecting external
sensors. This bus has two operating modes:
* bypasss: which connects the primary and auxiliary busses
together. This is already supported via an i2c mux.
* master: where the MPU acts as a master to any external
connected sensors. This is implemented by this patch.

This I2C master mode also works when the MPU itself is connected via
SPI.

I2C master supports up to 5 slaves. Slaves 0-3 have a common operating
mode while slave 4 is different. This patch implements an i2c adapter
using slave 4.

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 .../devicetree/bindings/iio/imu/inv_mpu6050.txt    |  66 +++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c         | 258 ++++++++++++++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h          |  36 +++
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c      |   8 -
 4 files changed, 355 insertions(+), 13 deletions(-)

diff --git a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
index a9fc11e..778d076 100644
--- a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
+++ b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
@@ -1,16 +1,31 @@
 InvenSense MPU-6050 Six-Axis (Gyro + Accelerometer) MEMS MotionTracking Device
 
-http://www.invensense.com/mems/gyro/mpu6050.html
-
 Required properties:
- - compatible : should be "invensense,mpu6050"
- - reg : the I2C address of the sensor
+ - compatible : one of "invensense,mpu6000", "invensense,mpu6050",
+	"invensense,mpu6000" or "invensense,mpu9150"
+ - reg : the I2C or SPI address of the sensor
  - interrupt-parent : should be the phandle for the interrupt controller
  - interrupts : interrupt mapping for GPIO IRQ
 
 Optional properties:
  - mount-matrix: an optional 3x3 mounting rotation matrix
+ - invensense,i2c-aux-master: operate aux i2c in "master" mode (default is bypass).
+
+The MPU has an auxiliary i2c bus for additional sensors. Devices attached this
+way can be described as for a regular linux i2c bus.
+
+It is possible to interact with aux devices in "bypass" or "master" mode. In
+"bypass" mode the auxiliary SDA/SCL lines are connected directly to the main i2c
+interface. In "master" mode access to aux devices is done by instructing the
+MPU to read or write i2c bytes.
+
+In "bypass" mode aux devices are listed behind a "i2c@0" node with reg = <0>;
+In "master" mode aux devices are listed behind a "i2c@1" node with reg = <1>;
 
+The master and bypass modes are not supported at the same time. The
+"invensense,i2c-aux-master" property must be set to activate master mode.
+Bypass mode is generally faster but master mode also works when the MPU is
+connected via SPI.
 
 Example:
 	mpu6050@68 {
@@ -28,3 +43,46 @@ Example:
 		               "0",                   /* y2 */
 		               "0.984807753012208";   /* z2 */
 	};
+
+Example describing mpu9150 (which includes an ak9875 on chip):
+	mpu9150@68 {
+		compatible = "invensense,mpu9150";
+		reg = <0x68>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <18 1>;
+
+		#address-cells = <1>;
+		#size-cells = <0>;
+		i2c@0 {
+			reg = <0>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+
+			ak8975@0c {
+				compatible = "ak,ak8975";
+				reg = <0x0c>;
+			};
+		};
+	};
+
+Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
+	mpu6500@0 {
+		compatible = "invensense,mpu6500";
+		reg = <0x0>;
+		spi-max-frequency = <1000000>;
+		interrupt-parent = <&gpio1>;
+		interrupts = <31 1>;
+
+		#address-cells = <1>;
+		#size-cells = <0>;
+		i2c@1 {
+			reg = <1>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+
+			hmc5883l@1e {
+				compatible = "honeywell,hmc5883l";
+				reg = <0x1e>;
+			};
+		};
+	};
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 5918c23..76683b8 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -25,6 +25,8 @@
 #include <linux/iio/iio.h>
 #include <linux/i2c-mux.h>
 #include <linux/acpi.h>
+#include <linux/completion.h>
+
 #include "inv_mpu_iio.h"
 
 /*
@@ -57,6 +59,7 @@ static const struct inv_mpu6050_reg_map reg_set_6500 = {
 	.int_pin_cfg		= INV_MPU6050_REG_INT_PIN_CFG,
 	.accl_offset		= INV_MPU6500_REG_ACCEL_OFFSET,
 	.gyro_offset		= INV_MPU6050_REG_GYRO_OFFSET,
+	.mst_status		= INV_MPU6050_REG_I2C_MST_STATUS,
 };
 
 static const struct inv_mpu6050_reg_map reg_set_6050 = {
@@ -77,6 +80,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = {
 	.int_pin_cfg		= INV_MPU6050_REG_INT_PIN_CFG,
 	.accl_offset		= INV_MPU6050_REG_ACCEL_OFFSET,
 	.gyro_offset		= INV_MPU6050_REG_GYRO_OFFSET,
+	.mst_status		= INV_MPU6050_REG_I2C_MST_STATUS,
 };
 
 static const struct inv_mpu6050_chip_config chip_config_6050 = {
@@ -130,6 +134,10 @@ static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
 	case INV_MPU6050_REG_FIFO_COUNT_H:
 	case INV_MPU6050_REG_FIFO_COUNT_H + 1:
 	case INV_MPU6050_REG_FIFO_R_W:
+	case INV_MPU6050_REG_I2C_MST_STATUS:
+	case INV_MPU6050_REG_INT_STATUS:
+	case INV_MPU6050_REG_I2C_SLV4_CTRL:
+	case INV_MPU6050_REG_I2C_SLV4_DI:
 		return true;
 	default:
 		return false;
@@ -140,6 +148,8 @@ static bool inv_mpu6050_precious_reg(struct device *dev, unsigned int reg)
 {
 	switch (reg) {
 	case INV_MPU6050_REG_FIFO_R_W:
+	case INV_MPU6050_REG_I2C_MST_STATUS:
+	case INV_MPU6050_REG_INT_STATUS:
 		return true;
 	default:
 		return false;
@@ -852,6 +862,225 @@ static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
 	return 0;
 }
 
+static irqreturn_t inv_mpu_irq_handler(int irq, void *private)
+{
+	struct inv_mpu6050_state *st = (struct inv_mpu6050_state *)private;
+
+	iio_trigger_poll(st->trig);
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t inv_mpu_irq_thread_handler(int irq, void *private)
+{
+	struct inv_mpu6050_state *st = (struct inv_mpu6050_state *)private;
+	int ret, val;
+
+	ret = regmap_read(st->map, st->reg->mst_status, &val);
+	if (ret < 0)
+		return ret;
+
+#ifdef CONFIG_I2C
+	if (val & INV_MPU6050_BIT_I2C_SLV4_DONE) {
+		st->slv4_done_status = val;
+		complete(&st->slv4_done);
+	}
+#endif
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_I2C
+static u32 inv_mpu_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_BYTE;
+}
+
+static int inv_mpu_i2c_smbus_xfer_real(struct i2c_adapter *adap, u16 addr,
+				       unsigned short flags, char read_write,
+				       u8 command, int size,
+				       union i2c_smbus_data *data)
+{
+	struct inv_mpu6050_state *st = i2c_get_adapdata(adap);
+	unsigned long time_left;
+	unsigned int val;
+	u8 ctrl, status;
+	int result;
+
+	if (read_write == I2C_SMBUS_WRITE)
+		addr |= INV_MPU6050_BIT_I2C_SLV4_W;
+	else
+		addr |= INV_MPU6050_BIT_I2C_SLV4_R;
+
+	/*
+	 * Consecutive operations with the same address are very common.
+	 * Read the old addr from the regmap cache and try to avoid extra
+	 * transfers.
+	 */
+	result = regmap_read(st->map, INV_MPU6050_REG_I2C_SLV4_ADDR, &val);
+	if (result < 0)
+		return result;
+	if (addr != val) {
+		result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_ADDR, addr);
+		if (result < 0)
+			return result;
+	}
+
+	if (size == I2C_SMBUS_BYTE_DATA) {
+		result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_REG, command);
+		if (result < 0)
+			return result;
+	}
+
+	if (read_write == I2C_SMBUS_WRITE) {
+		result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_DO, data->byte);
+		if (result < 0)
+			return result;
+	}
+
+	ctrl = INV_MPU6050_BIT_SLV4_EN | INV_MPU6050_BIT_SLV4_INT_EN;
+	if (size == I2C_SMBUS_BYTE)
+		ctrl |= INV_MPU6050_BIT_SLV4_REG_DIS;
+	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV4_CTRL, ctrl);
+	if (result < 0)
+		return result;
+
+	/* Wait for completion for both reads and writes */
+	time_left = wait_for_completion_timeout(&st->slv4_done, HZ);
+	if (!time_left)
+		return -ETIMEDOUT;
+
+	/* Check status for transfer errors */
+	status = st->slv4_done_status;
+	if (status & INV_MPU6050_BIT_I2C_SLV4_NACK)
+		return -EIO;
+	if (status & INV_MPU6050_BIT_I2C_LOST_ARB)
+		return -EIO;
+
+	if (read_write == I2C_SMBUS_READ) {
+		result = regmap_read(st->map, INV_MPU6050_REG_I2C_SLV4_DI, &val);
+		if (result < 0)
+			return result;
+		data->byte = val;
+	}
+
+	return 0;
+}
+
+static int inv_mpu_i2c_smbus_xfer(struct i2c_adapter *adap, u16 addr,
+				  unsigned short flags, char read_write,
+				  u8 command, int size,
+				  union i2c_smbus_data *data)
+{
+	struct inv_mpu6050_state *st = i2c_get_adapdata(adap);
+	struct iio_dev *indio_dev = iio_priv_to_dev(st);
+	int result, power_result;
+
+	/*
+	 * The i2c adapter we implement is extremely limited.
+	 * Check for callers that don't check functionality bits.
+	 *
+	 * If we don't check we might succesfully return incorrect data.
+	 */
+	if (size != I2C_SMBUS_BYTE_DATA && size != I2C_SMBUS_BYTE) {
+		dev_err(&adap->dev, "unsupported xfer rw=%d size=%d\n",
+			read_write, size);
+		return -EINVAL;
+	}
+
+	mutex_lock(&indio_dev->mlock);
+	power_result = inv_mpu6050_set_power_itg(st, true);
+	mutex_unlock(&indio_dev->mlock);
+	if (power_result < 0)
+		return power_result;
+
+	result = inv_mpu_i2c_smbus_xfer_real(adap, addr, flags, read_write, command, size, data);
+
+	mutex_lock(&indio_dev->mlock);
+	power_result = inv_mpu6050_set_power_itg(st, false);
+	mutex_unlock(&indio_dev->mlock);
+	if (power_result < 0)
+		return power_result;
+
+	return result;
+}
+
+static const struct i2c_algorithm inv_mpu_i2c_algo = {
+	.smbus_xfer	=	inv_mpu_i2c_smbus_xfer,
+	.functionality	=	inv_mpu_i2c_functionality,
+};
+
+static struct device_node *inv_mpu_get_aux_i2c_ofnode(struct device_node *parent)
+{
+	struct device_node *child;
+	int result;
+	u32 reg;
+
+	if (!parent)
+		return NULL;
+	for_each_child_of_node(parent, child) {
+		result = of_property_read_u32(child, "reg", &reg);
+		if (result)
+			continue;
+		if (reg == 1 && !strcmp(child->name, "i2c"))
+			return child;
+	}
+
+	return NULL;
+}
+
+static int inv_mpu_probe_aux_master(struct iio_dev* indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	int result;
+
+	/* First check i2c-aux-master property */
+	st->i2c_aux_master_mode = of_property_read_bool(dev->of_node,
+							"invensense,i2c-aux-master");
+	if (!st->i2c_aux_master_mode)
+		return 0;
+	dev_info(dev, "Configuring aux i2c in master mode\n");
+
+	result = inv_mpu6050_set_power_itg(st, true);
+	if (result < 0)
+		return result;
+
+	/* enable master */
+	st->chip_config.user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN;
+	result = regmap_write(st->map, st->reg->user_ctrl, st->chip_config.user_ctrl);
+	if (result < 0)
+		return result;
+
+	result = regmap_update_bits(st->map, st->reg->int_enable,
+				 INV_MPU6050_BIT_MST_INT_EN,
+				 INV_MPU6050_BIT_MST_INT_EN);
+	if (result < 0)
+		return result;
+
+	result = inv_mpu6050_set_power_itg(st, false);
+	if (result < 0)
+		return result;
+
+	init_completion(&st->slv4_done);
+	st->aux_master_adapter.owner = THIS_MODULE;
+	st->aux_master_adapter.algo = &inv_mpu_i2c_algo;
+	st->aux_master_adapter.dev.parent = dev;
+	snprintf(st->aux_master_adapter.name, sizeof(st->aux_master_adapter.name),
+			"aux-master-%s", indio_dev->name);
+	st->aux_master_adapter.dev.of_node = inv_mpu_get_aux_i2c_ofnode(dev->of_node);
+	i2c_set_adapdata(&st->aux_master_adapter, st);
+	/* This will also probe aux devices so transfers must work now */
+	result = i2c_add_adapter(&st->aux_master_adapter);
+	if (result < 0) {
+		dev_err(dev, "i2x aux master register fail %d\n", result);
+		return result;
+	}
+
+	return 0;
+}
+#endif
+
 int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type)
 {
@@ -931,16 +1160,39 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		goto out_unreg_ring;
 	}
 
+	/* Request interrupt for trigger and i2c master adapter */
+	result = devm_request_threaded_irq(&indio_dev->dev, st->irq,
+					   &inv_mpu_irq_handler,
+					   &inv_mpu_irq_thread_handler,
+					   IRQF_TRIGGER_RISING, "inv_mpu",
+					   st);
+	if (result) {
+		dev_err(dev, "request irq fail %d\n", result);
+		goto out_remove_trigger;
+	}
+
+#ifdef CONFIG_I2C
+	result = inv_mpu_probe_aux_master(indio_dev);
+	if (result < 0) {
+		dev_err(dev, "i2c aux master probe fail %d\n", result);
+		goto out_remove_trigger;
+	}
+#endif
+
 	INIT_KFIFO(st->timestamps);
 	spin_lock_init(&st->time_stamp_lock);
 	result = iio_device_register(indio_dev);
 	if (result) {
 		dev_err(dev, "IIO register fail %d\n", result);
-		goto out_remove_trigger;
+		goto out_del_adapter;
 	}
 
 	return 0;
 
+out_del_adapter:
+#ifdef CONFIG_I2C
+	i2c_del_adapter(&st->aux_master_adapter);
+#endif
 out_remove_trigger:
 	inv_mpu6050_remove_trigger(st);
 out_unreg_ring:
@@ -952,10 +1204,14 @@ EXPORT_SYMBOL_GPL(inv_mpu_core_probe);
 int inv_mpu_core_remove(struct device  *dev)
 {
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 
 	iio_device_unregister(indio_dev);
 	inv_mpu6050_remove_trigger(iio_priv(indio_dev));
 	iio_triggered_buffer_cleanup(indio_dev);
+#ifdef CONFIG_I2C
+	i2c_del_adapter(&st->aux_master_adapter);
+#endif
 
 	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 bd2c0fd..85f9b50 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -42,6 +42,7 @@
  *  @int_pin_cfg;	Controls interrupt pin configuration.
  *  @accl_offset:	Controls the accelerometer calibration offset.
  *  @gyro_offset:	Controls the gyroscope calibration offset.
+ *  @mst_status:	secondary I2C master interrupt source status
  */
 struct inv_mpu6050_reg_map {
 	u8 sample_rate_div;
@@ -61,6 +62,7 @@ struct inv_mpu6050_reg_map {
 	u8 int_pin_cfg;
 	u8 accl_offset;
 	u8 gyro_offset;
+	u8 mst_status;
 };
 
 /*device enum */
@@ -139,6 +141,17 @@ struct inv_mpu6050_state {
 	DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
 	struct regmap *map;
 	int irq;
+
+	/* if the MPU connects to aux devices as a master */
+	bool i2c_aux_master_mode;
+
+#ifdef CONFIG_I2C
+	/* I2C adapter for talking to aux sensors in "master" mode */
+	struct i2c_adapter aux_master_adapter;
+	struct completion slv4_done;
+	/* Value of I2C_MST_STATUS after slv4_done */
+	u8 slv4_done_status;
+#endif
 };
 
 /*register and associated bit definition*/
@@ -154,9 +167,32 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_BIT_ACCEL_OUT           0x08
 #define INV_MPU6050_BITS_GYRO_OUT           0x70
 
+#define INV_MPU6050_REG_I2C_SLV4_ADDR       0x31
+#define INV_MPU6050_BIT_I2C_SLV4_R          0x80
+#define INV_MPU6050_BIT_I2C_SLV4_W          0x00
+
+#define INV_MPU6050_REG_I2C_SLV4_REG        0x32
+#define INV_MPU6050_REG_I2C_SLV4_DO         0x33
+#define INV_MPU6050_REG_I2C_SLV4_CTRL       0x34
+
+#define INV_MPU6050_BIT_SLV4_EN             0x80
+#define INV_MPU6050_BIT_SLV4_INT_EN         0x40
+#define INV_MPU6050_BIT_SLV4_REG_DIS        0x20
+
+#define INV_MPU6050_REG_I2C_SLV4_DI         0x35
+
+#define INV_MPU6050_REG_I2C_MST_STATUS      0x36
+#define INV_MPU6050_BIT_I2C_SLV4_DONE       0x40
+#define INV_MPU6050_BIT_I2C_LOST_ARB        0x20
+#define INV_MPU6050_BIT_I2C_SLV4_NACK       0x10
+
 #define INV_MPU6050_REG_INT_ENABLE          0x38
 #define INV_MPU6050_BIT_DATA_RDY_EN         0x01
 #define INV_MPU6050_BIT_DMP_INT_EN          0x02
+#define INV_MPU6050_BIT_MST_INT_EN          0x08
+
+#define INV_MPU6050_REG_INT_STATUS          0x3A
+#define INV_MPU6050_BIT_MST_INT             0x08
 
 #define INV_MPU6050_REG_RAW_ACCEL           0x3B
 #define INV_MPU6050_REG_TEMPERATURE         0x41
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index fc55923..a334ed9 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -126,14 +126,6 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev)
 	if (!st->trig)
 		return -ENOMEM;
 
-	ret = devm_request_irq(&indio_dev->dev, st->irq,
-			       &iio_trigger_generic_data_rdy_poll,
-			       IRQF_TRIGGER_RISING,
-			       "inv_mpu",
-			       st->trig);
-	if (ret)
-		return ret;
-
 	st->trig->dev.parent = regmap_get_device(st->map);
 	st->trig->ops = &inv_mpu_trigger_ops;
 	iio_trigger_set_drvdata(st->trig, indio_dev);
-- 
2.5.5

Re: [RFC v2 5/7] iio: inv_mpu6050: Add support for auxiliary I2C master

[Rob Herring]

On Wed, May 18, 2016 at 06:00:52PM +0300, Crestez Dan Leonard wrote:
> The MPU has an auxiliary I2C bus for connecting external
> sensors. This bus has two operating modes:
> * bypasss: which connects the primary and auxiliary busses
> together. This is already supported via an i2c mux.
> * master: where the MPU acts as a master to any external
> connected sensors. This is implemented by this patch.
> 
> This I2C master mode also works when the MPU itself is connected via
> SPI.
> 
> I2C master supports up to 5 slaves. Slaves 0-3 have a common operating
> mode while slave 4 is different. This patch implements an i2c adapter
> using slave 4.
> 
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
> ---
>  .../devicetree/bindings/iio/imu/inv_mpu6050.txt    |  66 +++++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c         | 258 ++++++++++++++++++++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h          |  36 +++
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c      |   8 -
>  4 files changed, 355 insertions(+), 13 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> index a9fc11e..778d076 100644
> --- a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> +++ b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> @@ -1,16 +1,31 @@
>  InvenSense MPU-6050 Six-Axis (Gyro + Accelerometer) MEMS MotionTracking Device
>  
> -http://www.invensense.com/mems/gyro/mpu6050.html
> -
>  Required properties:
> - - compatible : should be "invensense,mpu6050"
> - - reg : the I2C address of the sensor
> + - compatible : one of "invensense,mpu6000", "invensense,mpu6050",
> +	"invensense,mpu6000" or "invensense,mpu9150"
> + - reg : the I2C or SPI address of the sensor
>   - interrupt-parent : should be the phandle for the interrupt controller
>   - interrupts : interrupt mapping for GPIO IRQ
>  
>  Optional properties:
>   - mount-matrix: an optional 3x3 mounting rotation matrix
> + - invensense,i2c-aux-master: operate aux i2c in "master" mode (default is bypass).
> +
> +The MPU has an auxiliary i2c bus for additional sensors. Devices attached this
> +way can be described as for a regular linux i2c bus.
> +
> +It is possible to interact with aux devices in "bypass" or "master" mode. In
> +"bypass" mode the auxiliary SDA/SCL lines are connected directly to the main i2c
> +interface. In "master" mode access to aux devices is done by instructing the
> +MPU to read or write i2c bytes.
> +
> +In "bypass" mode aux devices are listed behind a "i2c@0" node with reg = <0>;
> +In "master" mode aux devices are listed behind a "i2c@1" node with reg = <1>;

What is meaning and purpose of 0 and 1? You know which mode based on 
invensense,i2c-aux-master.

I don't see the reason for this intermediate node. In bypass mode, the 
driver should register the child nodes with the parent node's bus. In 
master mode, the mpu driver should register itself as an i2c adapter and 
then probe its child nodes. Maybe I'm missing something from all the 
discussion.

> +The master and bypass modes are not supported at the same time. The
> +"invensense,i2c-aux-master" property must be set to activate master mode.
> +Bypass mode is generally faster but master mode also works when the MPU is
> +connected via SPI.

invensense,i2c-aux-master should only apply if the host interface is 
SPI. Bypass mode is faster, it there any reason to use master mode when 
the host is I2C? A slave address conflict is the only thing I can think 
of.

Rob

[PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Crestez Dan Leonard]

Right now it is possible to only enable some of the x/y/z channels, for
example you can enable accel_z without x or y but if you actually do
that what you get is actually only the x channel.

Fix this by reformatting the hardware sample to only include the
requested channels.

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |   8 +++
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 107 ++++++++++++++++++++++++++++-
 2 files changed, 112 insertions(+), 3 deletions(-)

diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index 85f9b50..ec1401d 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -152,6 +152,11 @@ struct inv_mpu6050_state {
 	/* Value of I2C_MST_STATUS after slv4_done */
 	u8 slv4_done_status;
 #endif
+
+#define INV_MPU6050_MAX_SCAN_ELEMENTS 7
+	unsigned int scan_offsets[INV_MPU6050_MAX_SCAN_ELEMENTS];
+	unsigned int scan_lengths[INV_MPU6050_MAX_SCAN_ELEMENTS];
+	bool realign_required;
 };
 
 /*register and associated bit definition*/
@@ -274,6 +279,9 @@ enum inv_mpu6050_scan {
 	INV_MPU6050_SCAN_TIMESTAMP,
 };
 
+#define INV_MPU6050_SCAN_MASK_ACCEL    0x07
+#define INV_MPU6050_SCAN_MASK_GYRO     0x38
+
 enum inv_mpu6050_filter_e {
 	INV_MPU6050_FILTER_256HZ_NOLPF2 = 0,
 	INV_MPU6050_FILTER_188HZ,
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 56ee1e2..49e503c 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -24,6 +24,90 @@
 #include <linux/spi/spi.h>
 #include "inv_mpu_iio.h"
 
+static void inv_mpu_get_scan_offsets(
+		struct iio_dev *indio_dev,
+		const unsigned long *mask,
+		const unsigned int masklen,
+		unsigned int *scan_offsets)
+{
+	unsigned int pos = 0;
+
+	if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) {
+		scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0;
+		scan_offsets[INV_MPU6050_SCAN_ACCL_Y] = pos + 2;
+		scan_offsets[INV_MPU6050_SCAN_ACCL_Z] = pos + 4;
+		pos += 6;
+	}
+	if (*mask & INV_MPU6050_SCAN_MASK_GYRO) {
+		scan_offsets[INV_MPU6050_SCAN_GYRO_X] = pos + 0;
+		scan_offsets[INV_MPU6050_SCAN_GYRO_Y] = pos + 2;
+		scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4;
+		pos += 6;
+	}
+}
+
+/* This is slowish but relatively common */
+static const struct iio_chan_spec *
+iio_chan_by_scan_index(struct iio_dev *indio_dev, int index)
+{
+	int i;
+
+	for (i = 0; i < indio_dev->num_channels; ++i)
+		if (indio_dev->channels[i].scan_index == index)
+			return &indio_dev->channels[i];
+
+	return NULL;
+}
+
+static int iio_check_scan_offsets_aligned(
+		struct iio_dev *indio_dev,
+		const unsigned long *mask,
+		unsigned int masklen,
+		unsigned int *scan_offsets)
+{
+	int scan_index;
+	unsigned int pos = 0;
+	const struct iio_chan_spec *chan;
+
+	for_each_set_bit(scan_index, mask, masklen) {
+		chan = iio_chan_by_scan_index(indio_dev, scan_index);
+		if (scan_offsets[scan_index] != pos)
+			return false;
+		pos = ALIGN(pos + chan->scan_type.storagebits / 8,
+			    chan->scan_type.storagebits / 8);
+	}
+
+	return true;
+}
+
+static void iio_get_scan_lengths(struct iio_dev *indio_dev, unsigned int *scan_length)
+{
+	int i;
+	const struct iio_chan_spec *chan;
+
+	for (i = 0; i < indio_dev->num_channels; ++i) {
+		chan = &indio_dev->channels[i];
+		if (chan->scan_index < 0)
+			continue;
+		scan_length[chan->scan_index] = chan->scan_type.storagebits / 8;
+	}
+}
+
+static void iio_realign_sample(
+		struct iio_dev *indio_dev,
+		const u8 *ibuf, u8 *obuf,
+		const unsigned int *scan_offset,
+		const unsigned int *scan_lengths)
+{
+	unsigned int pos = 0;
+	int i;
+
+	for_each_set_bit(i, indio_dev->active_scan_mask, indio_dev->masklength) {
+		memcpy(&obuf[pos], &ibuf[scan_offset[i]], scan_lengths[i]);
+		pos = ALIGN(pos + scan_lengths[i], scan_lengths[i]);
+	}
+}
+
 static void inv_clear_kfifo(struct inv_mpu6050_state *st)
 {
 	unsigned long flags;
@@ -38,7 +122,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 {
 	int result;
 	u8 d;
-	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	const unsigned long *mask = indio_dev->active_scan_mask;
+	unsigned int masklen = indio_dev->masklength;
 
 	/* disable interrupt */
 	result = regmap_update_bits(st->map, st->reg->int_enable,
@@ -93,6 +179,13 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	if (result)
 		goto reset_fifo_fail;
 
+	/* check realign required */
+	inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets);
+	st->realign_required = !iio_check_scan_offsets_aligned(
+			indio_dev, mask, masklen, st->scan_offsets);
+	if (st->realign_required)
+		iio_get_scan_lengths(indio_dev, st->scan_lengths);
+
 	return 0;
 
 reset_fifo_fail:
@@ -201,8 +294,16 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 		if (result == 0)
 			timestamp = 0;
 
-		result = iio_push_to_buffers_with_timestamp(indio_dev, data,
-							    timestamp);
+		if (st->realign_required) {
+			u8 adata[INV_MPU6050_OUTPUT_DATA_SIZE];
+			iio_realign_sample(indio_dev, data, adata,
+					    st->scan_offsets, st->scan_lengths);
+			result = iio_push_to_buffers_with_timestamp(
+					indio_dev, adata, timestamp);
+		} else {
+			result = iio_push_to_buffers_with_timestamp(
+					indio_dev, data, timestamp);
+		}
 		if (result)
 			goto flush_fifo;
 		fifo_count -= bytes_per_datum;
-- 
2.5.5

Re: [PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Jonathan Cameron]

On 18/05/16 16:00, Crestez Dan Leonard wrote:
> Right now it is possible to only enable some of the x/y/z channels, for
> example you can enable accel_z without x or y but if you actually do
> that what you get is actually only the x channel.
> 
> Fix this by reformatting the hardware sample to only include the
> requested channels.
As it stands here there is no benefit in doing this over using the core
demux. In fact it's considerably less efficient (fair enough that you
are keeping it simple in the first instance).
The patch description should make that clear.

I'd definitely like to see simple extension of that option to handle
a callback to get the nearest scanmask that is possible (as an alternative
to the static scan_masks_available list.)

This only gets interesting if we are dealing with the unaligned case and for
these parts that only kicks in I think if the slave devices have say 3 bytes in
their data type.  
> 
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
> ---
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h  |   8 +++
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c | 107 ++++++++++++++++++++++++++++-
>  2 files changed, 112 insertions(+), 3 deletions(-)
> 
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index 85f9b50..ec1401d 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -152,6 +152,11 @@ struct inv_mpu6050_state {
>  	/* Value of I2C_MST_STATUS after slv4_done */
>  	u8 slv4_done_status;
>  #endif
> +
> +#define INV_MPU6050_MAX_SCAN_ELEMENTS 7
> +	unsigned int scan_offsets[INV_MPU6050_MAX_SCAN_ELEMENTS];
> +	unsigned int scan_lengths[INV_MPU6050_MAX_SCAN_ELEMENTS];
> +	bool realign_required;
>  };
>  
>  /*register and associated bit definition*/
> @@ -274,6 +279,9 @@ enum inv_mpu6050_scan {
>  	INV_MPU6050_SCAN_TIMESTAMP,
>  };
>  
> +#define INV_MPU6050_SCAN_MASK_ACCEL    0x07
> +#define INV_MPU6050_SCAN_MASK_GYRO     0x38
> +
>  enum inv_mpu6050_filter_e {
>  	INV_MPU6050_FILTER_256HZ_NOLPF2 = 0,
>  	INV_MPU6050_FILTER_188HZ,
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 56ee1e2..49e503c 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -24,6 +24,90 @@
>  #include <linux/spi/spi.h>
>  #include "inv_mpu_iio.h"
>  
> +static void inv_mpu_get_scan_offsets(
> +		struct iio_dev *indio_dev,
> +		const unsigned long *mask,
> +		const unsigned int masklen,
> +		unsigned int *scan_offsets)
> +{
> +	unsigned int pos = 0;
> +
> +	if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) {
> +		scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0;
> +		scan_offsets[INV_MPU6050_SCAN_ACCL_Y] = pos + 2;
> +		scan_offsets[INV_MPU6050_SCAN_ACCL_Z] = pos + 4;
> +		pos += 6;
> +	}
> +	if (*mask & INV_MPU6050_SCAN_MASK_GYRO) {
> +		scan_offsets[INV_MPU6050_SCAN_GYRO_X] = pos + 0;
> +		scan_offsets[INV_MPU6050_SCAN_GYRO_Y] = pos + 2;
> +		scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4;
> +		pos += 6;
> +	}
> +}
> +
> +/* This is slowish but relatively common */
> +static const struct iio_chan_spec *
> +iio_chan_by_scan_index(struct iio_dev *indio_dev, int index)
> +{
> +	int i;
> +
> +	for (i = 0; i < indio_dev->num_channels; ++i)
> +		if (indio_dev->channels[i].scan_index == index)
> +			return &indio_dev->channels[i];
> +
> +	return NULL;
> +}
> +
> +static int iio_check_scan_offsets_aligned(
> +		struct iio_dev *indio_dev,
> +		const unsigned long *mask,
> +		unsigned int masklen,
> +		unsigned int *scan_offsets)
> +{
> +	int scan_index;
> +	unsigned int pos = 0;
> +	const struct iio_chan_spec *chan;
> +
> +	for_each_set_bit(scan_index, mask, masklen) {
> +		chan = iio_chan_by_scan_index(indio_dev, scan_index);
> +		if (scan_offsets[scan_index] != pos)
> +			return false;
> +		pos = ALIGN(pos + chan->scan_type.storagebits / 8,
> +			    chan->scan_type.storagebits / 8);
> +	}
> +
> +	return true;
> +}
> +
> +static void iio_get_scan_lengths(struct iio_dev *indio_dev, unsigned int *scan_length)
> +{
> +	int i;
> +	const struct iio_chan_spec *chan;
> +
> +	for (i = 0; i < indio_dev->num_channels; ++i) {
> +		chan = &indio_dev->channels[i];
> +		if (chan->scan_index < 0)
> +			continue;
> +		scan_length[chan->scan_index] = chan->scan_type.storagebits / 8;
> +	}
> +}
> +
> +static void iio_realign_sample(
> +		struct iio_dev *indio_dev,
> +		const u8 *ibuf, u8 *obuf,
> +		const unsigned int *scan_offset,
> +		const unsigned int *scan_lengths)
> +{
> +	unsigned int pos = 0;
> +	int i;
> +
> +	for_each_set_bit(i, indio_dev->active_scan_mask, indio_dev->masklength) {
> +		memcpy(&obuf[pos], &ibuf[scan_offset[i]], scan_lengths[i]);
> +		pos = ALIGN(pos + scan_lengths[i], scan_lengths[i]);
> +	}
> +}
Either rename these so they aren't so generic or propose adding them to
the core code as helper functions.

> +
>  static void inv_clear_kfifo(struct inv_mpu6050_state *st)
>  {
>  	unsigned long flags;
> @@ -38,7 +122,9 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  {
>  	int result;
>  	u8 d;
> -	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	const unsigned long *mask = indio_dev->active_scan_mask;
> +	unsigned int masklen = indio_dev->masklength;
>  
>  	/* disable interrupt */
>  	result = regmap_update_bits(st->map, st->reg->int_enable,
> @@ -93,6 +179,13 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	if (result)
>  		goto reset_fifo_fail;
>  
> +	/* check realign required */
> +	inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets);
> +	st->realign_required = !iio_check_scan_offsets_aligned(
> +			indio_dev, mask, masklen, st->scan_offsets);
> +	if (st->realign_required)
> +		iio_get_scan_lengths(indio_dev, st->scan_lengths);
> +
>  	return 0;
>  
>  reset_fifo_fail:
> @@ -201,8 +294,16 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  		if (result == 0)
>  			timestamp = 0;
>  
> -		result = iio_push_to_buffers_with_timestamp(indio_dev, data,
> -							    timestamp);
> +		if (st->realign_required) {
> +			u8 adata[INV_MPU6050_OUTPUT_DATA_SIZE];
> +			iio_realign_sample(indio_dev, data, adata,
> +					    st->scan_offsets, st->scan_lengths);
> +			result = iio_push_to_buffers_with_timestamp(
> +					indio_dev, adata, timestamp);
> +		} else {
> +			result = iio_push_to_buffers_with_timestamp(
> +					indio_dev, data, timestamp);
> +		}
>  		if (result)
>  			goto flush_fifo;
>  		fifo_count -= bytes_per_datum;
> 

Re: [PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Crestez Dan Leonard]

On 05/29/2016 06:47 PM, Jonathan Cameron wrote:
> On 18/05/16 16:00, Crestez Dan Leonard wrote:
>> Right now it is possible to only enable some of the x/y/z channels, for
>> example you can enable accel_z without x or y but if you actually do
>> that what you get is actually only the x channel.
>>
>> Fix this by reformatting the hardware sample to only include the
>> requested channels.
> As it stands here there is no benefit in doing this over using the core
> demux. In fact it's considerably less efficient (fair enough that you
> are keeping it simple in the first instance).
> The patch description should make that clear.

Why is it less efficient? All it really does is a bunch of memcpy.

> I'd definitely like to see simple extension of that option to handle
> a callback to get the nearest scanmask that is possible (as an alternative
> to the static scan_masks_available list.)
> 
> This only gets interesting if we are dealing with the unaligned case and for
> these parts that only kicks in I think if the slave devices have say 3 bytes in
> their data type.  

But I want to deal with the unaligned case because it's better than
introducing odd validations on slave channels. If I added an extension
to get the nearest scanmask I would have to remove it in PATCH 7.

-- 
Regards,
Leonard

Re: [PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Jonathan Cameron]

On 30 May 2016 14:44:41 BST, Crestez Dan Leonard <leonard.crestez@intel.com> wrote:
>On 05/29/2016 06:47 PM, Jonathan Cameron wrote:
>> On 18/05/16 16:00, Crestez Dan Leonard wrote:
>>> Right now it is possible to only enable some of the x/y/z channels,
>for
>>> example you can enable accel_z without x or y but if you actually do
>>> that what you get is actually only the x channel.
>>>
>>> Fix this by reformatting the hardware sample to only include the
>>> requested channels.
>> As it stands here there is no benefit in doing this over using the
>core
>> demux. In fact it's considerably less efficient (fair enough that you
>> are keeping it simple in the first instance).
>> The patch description should make that clear.
>
>Why is it less efficient? All it really does is a bunch of memcpy.

Not doing agglomeration of neighbouring copies (iirc) not git either set of code to
hand!

>
>> I'd definitely like to see simple extension of that option to handle
>> a callback to get the nearest scanmask that is possible (as an
>alternative
>> to the static scan_masks_available list.)
>> 
>> This only gets interesting if we are dealing with the unaligned case
>and for
>> these parts that only kicks in I think if the slave devices have say
>3 bytes in
>> their data type.  
>
>But I want to deal with the unaligned case because it's better than
>introducing odd validations on slave channels. If I added an extension
>to get the nearest scanmask I would have to remove it in PATCH 7.
Hmm I must have misread that. Though you were only supporting 16 bit channels
 for aux sensors.

Then for now can we give this a slightly less generic name. I am not happy
 enough that we want this in the core 'yet'. 
Easy to rename later if it makes sense.

Thanks

Jonathan 

-- 
Sent from my Android device with K-9 Mail. Please excuse my brevity.

Re: [PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Crestez Dan Leonard]

On 05/31/2016 12:42 AM, Jonathan Cameron wrote:
> On 30 May 2016 14:44:41 BST, Crestez Dan Leonard <leonard.crestez@intel.com> wrote:
>> On 05/29/2016 06:47 PM, Jonathan Cameron wrote:
>>> On 18/05/16 16:00, Crestez Dan Leonard wrote:
>>>> Right now it is possible to only enable some of the x/y/z channels,
>> for
>>>> example you can enable accel_z without x or y but if you actually do
>>>> that what you get is actually only the x channel.
>>>>
>>>> Fix this by reformatting the hardware sample to only include the
>>>> requested channels.
>>> As it stands here there is no benefit in doing this over using the
>> core
>>> demux. In fact it's considerably less efficient (fair enough that you
>>> are keeping it simple in the first instance).
>>> The patch description should make that clear.
>>
>> Why is it less efficient? All it really does is a bunch of memcpy.
> 
> Not doing agglomeration of neighbouring copies (iirc) not git either set of code to
> hand!

You're right about that. But the total data rate is still very low.

>>> I'd definitely like to see simple extension of that option to handle
>>> a callback to get the nearest scanmask that is possible (as an
>> alternative
>>> to the static scan_masks_available list.)
>>>
>>> This only gets interesting if we are dealing with the unaligned case
>> and for
>>> these parts that only kicks in I think if the slave devices have say
>> 3 bytes in
>>> their data type.  
>>
>> But I want to deal with the unaligned case because it's better than
>> introducing odd validations on slave channels. If I added an extension
>> to get the nearest scanmask I would have to remove it in PATCH 7.
> Hmm I must have misread that. Though you were only supporting 16 bit channels
>  for aux sensors.

That was in a previous version, I dropped that limitation now.

> Then for now can we give this a slightly less generic name. I am not happy
>  enough that we want this in the core 'yet'. 
> Easy to rename later if it makes sense.

Ok, I will rename these functions to start with inv_mpu_* instead of iio_*.

In theory it would be interesting to refactor the iio demuxing code to
support this but then this patch serios would grow even more complicated.

-- 
Regards,
Leonard

Re: [PATCH v2 6/7] iio: inv_mpu6050: Reformat sample for active scan mask

[Jonathan Cameron]

On 31 May 2016 09:56:51 BST, Crestez Dan Leonard <leonard.crestez@intel.com> wrote:
>On 05/31/2016 12:42 AM, Jonathan Cameron wrote:
>> On 30 May 2016 14:44:41 BST, Crestez Dan Leonard
><leonard.crestez@intel.com> wrote:
>>> On 05/29/2016 06:47 PM, Jonathan Cameron wrote:
>>>> On 18/05/16 16:00, Crestez Dan Leonard wrote:
>>>>> Right now it is possible to only enable some of the x/y/z
>channels,
>>> for
>>>>> example you can enable accel_z without x or y but if you actually
>do
>>>>> that what you get is actually only the x channel.
>>>>>
>>>>> Fix this by reformatting the hardware sample to only include the
>>>>> requested channels.
>>>> As it stands here there is no benefit in doing this over using the
>>> core
>>>> demux. In fact it's considerably less efficient (fair enough that
>you
>>>> are keeping it simple in the first instance).
>>>> The patch description should make that clear.
>>>
>>> Why is it less efficient? All it really does is a bunch of memcpy.
>> 
>> Not doing agglomeration of neighbouring copies (iirc) not git either
>set of code to
>> hand!
>
>You're right about that. But the total data rate is still very low.
>
>>>> I'd definitely like to see simple extension of that option to
>handle
>>>> a callback to get the nearest scanmask that is possible (as an
>>> alternative
>>>> to the static scan_masks_available list.)
>>>>
>>>> This only gets interesting if we are dealing with the unaligned
>case
>>> and for
>>>> these parts that only kicks in I think if the slave devices have
>say
>>> 3 bytes in
>>>> their data type.  
>>>
>>> But I want to deal with the unaligned case because it's better than
>>> introducing odd validations on slave channels. If I added an
>extension
>>> to get the nearest scanmask I would have to remove it in PATCH 7.
>> Hmm I must have misread that. Though you were only supporting 16 bit
>channels
>>  for aux sensors.
>
>That was in a previous version, I dropped that limitation now.
>
>> Then for now can we give this a slightly less generic name. I am not
>happy
>>  enough that we want this in the core 'yet'. 
>> Easy to rename later if it makes sense.
>
>Ok, I will rename these functions to start with inv_mpu_* instead of
>iio_*.
>
>In theory it would be interesting to refactor the iio demuxing code to
>support this but then this patch serios would grow even more
>complicated.

Agreed. Let's leave that for a separate effort!

I was vaguely wondering about letting the core demux input be unaligned but
 enforce the output keeping current alignment.  Fiddly even then however.

J

-- 
Sent from my Android device with K-9 Mail. Please excuse my brevity.

[RFC v2 7/7] iio: inv_mpu6050: Expose channels from slave sensors

[Crestez Dan Leonard]

This works by copying the iio_chan_specs from the slave device and
republishing them as if they belonged to the MPU itself. All
read/write_raw operations are forwarded to the other driver.

The original device is still registered with linux as a normal driver
and works normally and you can poke at it to configure stuff like sample
rates and scaling factors.

Buffer values are read from the MPU fifo, allowing a much higher
sampling rate.

Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
---
 .../devicetree/bindings/iio/imu/inv_mpu6050.txt    |  47 ++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_core.c         | 387 ++++++++++++++++++++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h          |  74 +++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c         |  65 +++-
 drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c      |   9 +
 5 files changed, 556 insertions(+), 26 deletions(-)

diff --git a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
index 778d076..07d572a 100644
--- a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
+++ b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
@@ -11,8 +11,8 @@ Optional properties:
  - mount-matrix: an optional 3x3 mounting rotation matrix
  - invensense,i2c-aux-master: operate aux i2c in "master" mode (default is bypass).
 
-The MPU has an auxiliary i2c bus for additional sensors. Devices attached this
-way can be described as for a regular linux i2c bus.
+It is possible to attach auxiliary sensors to the MPU and have them be handled
+by linux. Those auxiliary sensors are described as an i2c bus.
 
 It is possible to interact with aux devices in "bypass" or "master" mode. In
 "bypass" mode the auxiliary SDA/SCL lines are connected directly to the main i2c
@@ -25,7 +25,31 @@ In "master" mode aux devices are listed behind a "i2c@1" node with reg = <1>;
 The master and bypass modes are not supported at the same time. The
 "invensense,i2c-aux-master" property must be set to activate master mode.
 Bypass mode is generally faster but master mode also works when the MPU is
-connected via SPI.
+connected via SPI. Enabling master mode is required for automated external
+readings.
+
+
+It is possible to configure the MPU to automatically fetch reading for
+devices connected on the auxiliary i2c bus without CPU intervention. When this
+is done the driver will present the channels of the slaved devices as if they
+belong to the MPU device itself.
+
+Reads and write to config values (like scaling factors) are forwarded to the
+other driver while data reads are handled from MPU registers. The channels are
+also available through the MPU's iio triggered buffer mechanism. This feature
+can allow sampling up to 24 bytes from 4 slaves at a much higher rate.
+
+This is specified in device tree as an "invensense,external-sensors" node with
+children indexed by slave ids 0 to 3. The child nodes identifying each external
+sensor reading have the following properties:
+ - reg: 0 to 3 slave index
+ - invensense,addr : the I2C address to read from
+ - invensense,reg : the I2C register to read from
+ - invensense,len : read length from the device
+ - invensense,external-channels : list of slaved channels specified by scan_index.
+
+The sum of storagebits for external channels must equal the read length. Only
+16bit channels are currently supported.
 
 Example:
 	mpu6050@68 {
@@ -65,7 +89,8 @@ Example describing mpu9150 (which includes an ak9875 on chip):
 		};
 	};
 
-Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
+Example describing a mpu6500 via SPI with an hmc5883l on aux i2c configured for
+automatic external readings as slave 0:
 	mpu6500@0 {
 		compatible = "invensense,mpu6500";
 		reg = <0x0>;
@@ -73,6 +98,8 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
 		interrupt-parent = <&gpio1>;
 		interrupts = <31 1>;
 
+		invensense,i2c-aux-master;
+
 		#address-cells = <1>;
 		#size-cells = <0>;
 		i2c@1 {
@@ -85,4 +112,16 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
 				reg = <0x1e>;
 			};
 		};
+
+		invensense,external-sensors {
+			#address-cells = <1>;
+			#size-cells = <0>;
+			hmc5833l@0 {
+				reg = <0>;
+				invensense,addr = <0x1e>;
+				invensense,reg = <3>;
+				invensense,len = <6>;
+				invensense,external-channels = <0 1 2>;
+			};
+		};
 	};
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 76683b8..715b2e4 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -126,6 +126,9 @@ static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
 		return true;
 	if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
 		return true;
+	if (reg < INV_MPU6050_REG_EXT_SENS_DATA_00 + INV_MPU6050_CNT_EXT_SENS_DATA &&
+			reg >= INV_MPU6050_REG_EXT_SENS_DATA_00)
+		return true;
 	switch (reg) {
 	case INV_MPU6050_REG_TEMPERATURE:
 	case INV_MPU6050_REG_TEMPERATURE + 1:
@@ -335,8 +338,24 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
 		     struct iio_chan_spec const *chan,
 		     int *val, int *val2, long mask)
 {
-	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 	int ret = 0;
+	int chan_index;
+
+	if (chan > indio_dev->channels + indio_dev->num_channels ||
+			chan < indio_dev->channels)
+		return -EINVAL;
+	chan_index = chan - indio_dev->channels;
+	if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
+		struct inv_mpu_ext_chan_state *ext_chan_state =
+				&st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
+		struct inv_mpu_ext_sens_state *ext_sens_state =
+				&st->ext_sens[ext_chan_state->ext_sens_index];
+		struct iio_dev *orig_dev = ext_sens_state->orig_dev;
+		const struct iio_chan_spec *orig_chan =
+				&orig_dev->channels[ext_chan_state->orig_chan_index];
+		return orig_dev->info->read_raw(orig_dev, orig_chan, val, val2, mask);
+	}
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
@@ -518,9 +537,25 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
 				 struct iio_chan_spec const *chan,
 				 int val, int val2, long mask)
 {
-	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int chan_index;
 	int result;
 
+	if (chan > indio_dev->channels + indio_dev->num_channels ||
+			chan < indio_dev->channels)
+		return -EINVAL;
+	chan_index = chan - indio_dev->channels;
+	if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
+		struct inv_mpu_ext_chan_state *ext_chan_state =
+				&st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
+		struct inv_mpu_ext_sens_state *ext_sens_state =
+				&st->ext_sens[ext_chan_state->ext_sens_index];
+		struct iio_dev *orig_dev = ext_sens_state->orig_dev;
+		const struct iio_chan_spec *orig_chan =
+				&orig_dev->channels[ext_chan_state->orig_chan_index];
+		return orig_dev->info->write_raw(orig_dev, orig_chan, val, val2, mask);
+	}
+
 	mutex_lock(&indio_dev->mlock);
 	/*
 	 * we should only update scale when the chip is disabled, i.e.
@@ -809,6 +844,346 @@ static const struct iio_info mpu_info = {
 	.validate_trigger = inv_mpu6050_validate_trigger,
 };
 
+extern struct device_type iio_device_type;
+
+static int iio_device_from_i2c_client_match(struct device *dev, void *data)
+{
+	return dev->type == &iio_device_type;
+}
+
+static struct iio_dev* iio_device_from_i2c_client(struct i2c_client* i2c)
+{
+	struct device *child;
+
+	child = device_find_child(&i2c->dev, NULL, iio_device_from_i2c_client_match);
+
+	return (child ? dev_to_iio_dev(child) : NULL);
+}
+
+static int inv_mpu_slave_enable(struct inv_mpu6050_state *st, int index, bool state)
+{
+	return regmap_update_bits(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
+				  INV_MPU6050_BIT_I2C_SLV_EN,
+				  state ? INV_MPU6050_BIT_I2C_SLV_EN : 0);
+}
+
+/* Enable slaves based on scan mask */
+int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st,
+			      const unsigned long mask)
+{
+	int i, result;
+
+	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
+		long slave_mask = st->ext_sens[i].scan_mask;
+		result = inv_mpu_slave_enable(st, i, slave_mask & mask);
+		if (result)
+			return result;
+	}
+
+	return 0;
+}
+
+static int inv_mpu_parse_one_ext_sens(struct device *dev,
+				      struct device_node *np,
+				      struct inv_mpu_ext_sens_spec *spec)
+{
+	int result;
+	u32 addr, reg, len;
+
+	result = of_property_read_u32(np, "invensense,addr", &addr);
+	if (result)
+		return result;
+	result = of_property_read_u32(np, "invensense,reg", &reg);
+	if (result)
+		return result;
+	result = of_property_read_u32(np, "invensense,len", &len);
+	if (result)
+		return result;
+
+	spec->addr = addr;
+	spec->reg = reg;
+	spec->len = len;
+
+	result = of_property_count_u32_elems(np, "invensense,external-channels");
+	if (result < 0)
+		return result;
+	spec->num_ext_channels = result;
+	spec->ext_channels = devm_kmalloc(dev, spec->num_ext_channels * sizeof(*spec->ext_channels), GFP_KERNEL);
+	if (!spec->ext_channels)
+		return -ENOMEM;
+	result = of_property_read_u32_array(np, "invensense,external-channels",
+					    spec->ext_channels,
+					    spec->num_ext_channels);
+	if (result)
+		return result;
+
+	return 0;
+}
+
+static int inv_mpu_parse_ext_sens(struct device *dev,
+				  struct device_node *node,
+				  struct inv_mpu_ext_sens_spec *specs)
+{
+	struct device_node *child;
+	int result;
+	u32 reg;
+
+	for_each_available_child_of_node(node, child) {
+		result = of_property_read_u32(child, "reg", &reg);
+		if (result)
+			return result;
+		if (reg > INV_MPU6050_MAX_EXT_SENSORS) {
+			dev_err(dev, "External sensor index %u out of range, max %d\n",
+				reg, INV_MPU6050_MAX_EXT_SENSORS);
+			return -EINVAL;
+		}
+		result = inv_mpu_parse_one_ext_sens(dev, child, &specs[reg]);
+		if (result)
+			return result;
+	}
+
+	return 0;
+}
+
+static int inv_mpu_get_ext_sens_spec(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	struct device_node *node;
+	int result;
+
+	node = of_get_child_by_name(dev->of_node, "invensense,external-sensors");
+	if (node) {
+		result = inv_mpu_parse_ext_sens(dev, node, st->ext_sens_spec);
+		if (result)
+			dev_err(dev, "Failed to parsing external-sensors devicetree data\n");
+		return result;
+	}
+
+	return 0;
+}
+
+/* Struct used while enumerating devices and matching them */
+struct inv_mpu_handle_ext_sensor_fnarg
+{
+	struct iio_dev *indio_dev;
+
+	/* Current scan index */
+	int scan_index;
+	/* Current channel index */
+	int chan_index;
+	/* Non-const pointer to channels */
+	struct iio_chan_spec *channels;
+};
+
+/*
+ * Write initial configuration of a slave at probe time.
+ *
+ * This is mostly fixed except for enabling/disabling individual slaves.
+ */
+static int inv_mpu_config_external_read(struct inv_mpu6050_state *st, int index,
+					const struct inv_mpu_ext_sens_spec *spec)
+{
+	int result;
+
+	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(index),
+			      INV_MPU6050_BIT_I2C_SLV_RW | spec->addr);
+	if (result)
+		return result;
+	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(index), spec->reg);
+	if (result)
+		return result;
+	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
+			      spec->len);
+	if (result)
+		return result;
+
+	return result;
+}
+
+/* Handle one device */
+static int inv_mpu_handle_slave_device(
+		struct inv_mpu_handle_ext_sensor_fnarg *fnarg,
+		int slave_index,
+		struct iio_dev *orig_dev)
+{
+	int i, j;
+	int data_offset;
+	struct iio_dev *indio_dev = fnarg->indio_dev;
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct device *mydev = regmap_get_device(st->map);
+	struct inv_mpu_ext_sens_spec *ext_sens_spec =
+			&st->ext_sens_spec[slave_index];
+	struct inv_mpu_ext_sens_state *ext_sens_state =
+			&st->ext_sens[slave_index];
+
+	dev_info(mydev, "slave %d is device %s\n",
+			slave_index, orig_dev->name ?: dev_name(&orig_dev->dev));
+	ext_sens_state->orig_dev = orig_dev;
+	ext_sens_state->scan_mask = 0;
+	data_offset = 0;
+
+	/* handle channels: */
+	for (i = 0; i < ext_sens_spec->num_ext_channels; ++i) {
+		int orig_chan_index = -1;
+		const struct iio_chan_spec *orig_chan_spec;
+		struct iio_chan_spec *chan_spec;
+		struct inv_mpu_ext_chan_state *chan_state;
+
+		for (j = 0; j < orig_dev->num_channels; ++j)
+			if (orig_dev->channels[j].scan_index == ext_sens_spec->ext_channels[i]) {
+				orig_chan_index = j;
+				break;
+			}
+
+		if (orig_chan_index < 0) {
+			dev_err(mydev, "Could not find slave channel with scan_index %d\n",
+					ext_sens_spec->ext_channels[i]);
+		}
+
+		orig_chan_spec = &orig_dev->channels[orig_chan_index];
+		chan_spec = &fnarg->channels[INV_MPU6050_NUM_INT_CHAN + fnarg->chan_index];
+		chan_state = &st->ext_chan[fnarg->chan_index];
+
+		chan_state->ext_sens_index = slave_index;
+		chan_state->orig_chan_index = orig_chan_index;
+		chan_state->data_offset = data_offset;
+		memcpy(chan_spec, orig_chan_spec, sizeof(struct iio_chan_spec));
+		chan_spec->scan_index = fnarg->scan_index;
+		ext_sens_state->scan_mask |= (1 << chan_spec->scan_index);
+
+		fnarg->scan_index++;
+		fnarg->chan_index++;
+		data_offset += chan_spec->scan_type.storagebits / 8;
+		dev_info(mydev, "Reading external channel #%d scan_index %d data_offset %d"
+				" from original device %s chan #%d scan_index %d\n",
+				fnarg->chan_index, chan_spec->scan_index, chan_state->data_offset,
+				orig_dev->name ?: dev_name(&orig_dev->dev), orig_chan_index, orig_chan_spec->scan_index);
+	}
+	if (ext_sens_spec->len != data_offset) {
+		dev_err(mydev, "slave %d length mismatch between "
+				"i2c slave read length (%d) and "
+				"sum of channel sizes (%d)\n",
+				slave_index, ext_sens_spec->len, data_offset);
+		return -EINVAL;
+	}
+
+	return inv_mpu_config_external_read(st, slave_index, ext_sens_spec);
+}
+
+/* device_for_each_child enum function */
+static int inv_mpu_handle_ext_sensor_fn(struct device *dev, void *data)
+{
+	struct inv_mpu_handle_ext_sensor_fnarg *fnarg = data;
+	struct iio_dev *indio_dev = fnarg->indio_dev;
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct i2c_client *client;
+	struct iio_dev *orig_dev;
+	int i, result;
+
+	client = i2c_verify_client(dev);
+	if (!client)
+		return 0;
+	orig_dev = iio_device_from_i2c_client(client);
+	if (!orig_dev)
+		return 0;
+
+	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
+		if (st->ext_sens_spec[i].addr != client->addr)
+			continue;
+		if (st->ext_sens[i].orig_dev) {
+			dev_warn(&indio_dev->dev, "already found slave with at addr %d\n", client->addr);
+			continue;
+		}
+
+		result = inv_mpu_handle_slave_device(fnarg, i, orig_dev);
+		if (result)
+			return result;
+	}
+	return 0;
+}
+
+static int inv_mpu6050_handle_ext_sensors(struct iio_dev *indio_dev)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	struct device *dev = regmap_get_device(st->map);
+	struct inv_mpu_handle_ext_sensor_fnarg fnarg = {
+		.indio_dev = indio_dev,
+		.chan_index = 0,
+		.scan_index = INV_MPU6050_SCAN_TIMESTAMP,
+	};
+	int i, result;
+	int num_ext_chan = 0, sum_data_len = 0;
+	int num_scan_elements;
+
+	inv_mpu_get_ext_sens_spec(indio_dev);
+	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
+		num_ext_chan += st->ext_sens_spec[i].num_ext_channels;
+		sum_data_len += st->ext_sens_spec[i].len;
+	}
+	if (sum_data_len > INV_MPU6050_CNT_EXT_SENS_DATA) {
+		dev_err(dev, "Too many bytes from external sensors:"
+			      " requested=%d max=%d\n",
+			      sum_data_len, INV_MPU6050_CNT_EXT_SENS_DATA);
+		return -EINVAL;
+	}
+
+	/* Allocate scan_offsets/scan_lengths */
+	num_scan_elements = INV_MPU6050_NUM_INT_SCAN_ELEMENTS + num_ext_chan;
+	st->scan_offsets = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
+	if (!st->scan_offsets)
+		return -ENOMEM;
+	st->scan_lengths = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
+	if (!st->scan_lengths)
+		return -ENOMEM;
+
+	/* Zero length means nothing to do, just publish internal channels */
+	if (!sum_data_len) {
+		indio_dev->channels = inv_mpu_channels;
+		indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN;
+		BUILD_BUG_ON(ARRAY_SIZE(inv_mpu_channels) != INV_MPU6050_NUM_INT_CHAN);
+		return 0;
+	}
+
+	indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN + num_ext_chan;
+	indio_dev->channels = fnarg.channels = devm_kmalloc(dev,
+			indio_dev->num_channels * sizeof(struct iio_chan_spec),
+			GFP_KERNEL);
+	if (!fnarg.channels)
+		return -ENOMEM;
+	memcpy(fnarg.channels, inv_mpu_channels, sizeof(inv_mpu_channels));
+	memset(fnarg.channels + INV_MPU6050_NUM_INT_CHAN, 0,
+	       num_ext_chan * sizeof(struct iio_chan_spec));
+
+	st->ext_chan = devm_kzalloc(dev, num_ext_chan * sizeof(*st->ext_chan), GFP_KERNEL);
+	if (!st->ext_chan)
+		return -ENOMEM;
+
+	result = inv_mpu6050_set_power_itg(st, true);
+	if (result < 0)
+		return result;
+
+	result = device_for_each_child(&st->aux_master_adapter.dev, &fnarg,
+				       inv_mpu_handle_ext_sensor_fn);
+	if (result)
+		goto out_disable;
+	/* Timestamp channel has index 0 and last scan_index */
+	fnarg.channels[0].scan_index = fnarg.scan_index;
+
+	if (fnarg.chan_index != num_ext_chan) {
+		dev_err(&indio_dev->dev, "Failed to match all external channels!\n");
+		result = -EINVAL;
+		goto out_disable;
+	}
+
+	result = inv_mpu6050_set_power_itg(st, false);
+	return result;
+
+out_disable:
+	inv_mpu6050_set_power_itg(st, false);
+	return result;
+}
+
 /**
  *  inv_check_and_setup_chip() - check and setup chip.
  */
@@ -1140,8 +1515,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 		indio_dev->name = name;
 	else
 		indio_dev->name = dev_name(dev);
-	indio_dev->channels = inv_mpu_channels;
-	indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
 
 	indio_dev->info = &mpu_info;
 	indio_dev->modes = INDIO_BUFFER_TRIGGERED;
@@ -1179,6 +1552,12 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
 	}
 #endif
 
+	result = inv_mpu6050_handle_ext_sensors(indio_dev);
+	if (result < 0) {
+		dev_err(dev, "failed to configure channels %d\n", result);
+		goto out_remove_trigger;
+	}
+
 	INIT_KFIFO(st->timestamps);
 	spin_lock_init(&st->time_stamp_lock);
 	result = iio_device_register(indio_dev);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
index ec1401d..56fd943 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -110,6 +110,45 @@ struct inv_mpu6050_hw {
 	const struct inv_mpu6050_chip_config *config;
 };
 
+/* Maximum external sensors */
+/* These are SLV0-3 in HW, SLV4 is reserved for i2c master */
+#define INV_MPU6050_MAX_EXT_SENSORS 4
+
+/* Number of internal channels */
+#define INV_MPU6050_NUM_INT_CHAN 8
+
+/* Number of internal scan elements (channels with scan_index >= 0) */
+#define INV_MPU6050_NUM_INT_SCAN_ELEMENTS 7
+
+/* Specification for an external sensor */
+struct inv_mpu_ext_sens_spec {
+	unsigned short addr;
+	u8 reg, len;
+
+	int num_ext_channels;
+	int *ext_channels;
+};
+
+/* Driver state for each external sensor */
+struct inv_mpu_ext_sens_state {
+	struct iio_dev *orig_dev;
+
+	/* Mask of all channels in this sensor */
+	unsigned long scan_mask;
+};
+
+/* Driver state for each external channel */
+struct inv_mpu_ext_chan_state {
+	/* Index inside state->ext_sens */
+	int ext_sens_index;
+
+	/* Index inside orig_dev->channels */
+	int orig_chan_index;
+
+	/* Relative to first offset for this slave */
+	int data_offset;
+};
+
 /*
  *  struct inv_mpu6050_state - Driver state variables.
  *  @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
@@ -153,10 +192,13 @@ struct inv_mpu6050_state {
 	u8 slv4_done_status;
 #endif
 
-#define INV_MPU6050_MAX_SCAN_ELEMENTS 7
-	unsigned int scan_offsets[INV_MPU6050_MAX_SCAN_ELEMENTS];
-	unsigned int scan_lengths[INV_MPU6050_MAX_SCAN_ELEMENTS];
+	unsigned int *scan_offsets;
+	unsigned int *scan_lengths;
 	bool realign_required;
+
+	struct inv_mpu_ext_sens_spec ext_sens_spec[INV_MPU6050_MAX_EXT_SENSORS];
+	struct inv_mpu_ext_sens_state ext_sens[INV_MPU6050_MAX_EXT_SENSORS];
+	struct inv_mpu_ext_chan_state *ext_chan;
 };
 
 /*register and associated bit definition*/
@@ -171,6 +213,24 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_REG_FIFO_EN             0x23
 #define INV_MPU6050_BIT_ACCEL_OUT           0x08
 #define INV_MPU6050_BITS_GYRO_OUT           0x70
+#define INV_MPU6050_BIT_SLV0_FIFO_EN        0x01
+#define INV_MPU6050_BIT_SLV1_FIFO_EN        0x02
+#define INV_MPU6050_BIT_SLV2_FIFO_EN        0x04
+#define INV_MPU6050_BIT_SLV2_FIFO_EN        0x04
+
+/* SLV3 fifo enabling is not in REG_FIFO_EN */
+#define INV_MPU6050_REG_MST_CTRL	    0x24
+#define INV_MPU6050_BIT_SLV3_FIFO_EN        0x20
+
+/* For slaves 0-3 */
+#define INV_MPU6050_REG_I2C_SLV_ADDR(i)     (0x25 + 3 * (i))
+#define INV_MPU6050_REG_I2C_SLV_REG(i)      (0x26 + 3 * (i))
+#define INV_MPU6050_REG_I2C_SLV_CTRL(i)     (0x27 + 3 * (i))
+#define INV_MPU6050_BIT_I2C_SLV_RW          0x80
+#define INV_MPU6050_BIT_I2C_SLV_EN          0x80
+#define INV_MPU6050_BIT_I2C_SLV_BYTE_SW     0x40
+#define INV_MPU6050_BIT_I2C_SLV_REG_DIS     0x20
+#define INV_MPU6050_BIT_I2C_SLV_REG_GRP     0x10
 
 #define INV_MPU6050_REG_I2C_SLV4_ADDR       0x31
 #define INV_MPU6050_BIT_I2C_SLV4_R          0x80
@@ -247,8 +307,8 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_GYRO_CONFIG_FSR_SHIFT    3
 #define INV_MPU6050_ACCL_CONFIG_FSR_SHIFT    3
 
-/* 6 + 6 round up and plus 8 */
-#define INV_MPU6050_OUTPUT_DATA_SIZE         24
+/* max is 3*2 accel + 3*2 gyro + 24 external + 8 ts */
+#define INV_MPU6050_OUTPUT_DATA_SIZE         44
 
 #define INV_MPU6050_REG_INT_PIN_CFG	0x37
 #define INV_MPU6050_BIT_BYPASS_EN	0x2
@@ -261,6 +321,9 @@ struct inv_mpu6050_state {
 #define INV_MPU6050_MIN_FIFO_RATE            4
 #define INV_MPU6050_ONE_K_HZ                 1000
 
+#define INV_MPU6050_REG_EXT_SENS_DATA_00	0x49
+#define INV_MPU6050_CNT_EXT_SENS_DATA		24
+
 #define INV_MPU6050_REG_WHOAMI			117
 
 #define INV_MPU6000_WHOAMI_VALUE		0x68
@@ -328,6 +391,7 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev);
 void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st);
 int inv_reset_fifo(struct iio_dev *indio_dev);
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask);
+int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st, const unsigned long mask);
 int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
 int inv_mpu_acpi_create_mux_client(struct i2c_client *client);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
index 49e503c..919148a 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
@@ -30,7 +30,9 @@ static void inv_mpu_get_scan_offsets(
 		const unsigned int masklen,
 		unsigned int *scan_offsets)
 {
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
 	unsigned int pos = 0;
+	int i, j;
 
 	if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) {
 		scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0;
@@ -44,6 +46,24 @@ static void inv_mpu_get_scan_offsets(
 		scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4;
 		pos += 6;
 	}
+	/* HW lays out channels in slave order */
+	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
+		struct inv_mpu_ext_sens_spec *ext_sens_spec;
+		struct inv_mpu_ext_sens_state *ext_sens_state;
+		ext_sens_spec = &st->ext_sens_spec[i];
+		ext_sens_state = &st->ext_sens[i];
+
+		if (!(ext_sens_state->scan_mask & *mask))
+			continue;
+		for (j = 0; j + INV_MPU6050_NUM_INT_CHAN < indio_dev->num_channels; ++j) {
+			const struct iio_chan_spec *chan;
+			if (st->ext_chan[j].ext_sens_index != i)
+				continue;
+			chan = &indio_dev->channels[j + INV_MPU6050_NUM_INT_CHAN];
+			scan_offsets[chan->scan_index] = pos + st->ext_chan[j].data_offset;
+		}
+		pos += ext_sens_spec->len;
+	}
 }
 
 /* This is slowish but relatively common */
@@ -138,6 +158,10 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	result = regmap_write(st->map, st->reg->fifo_en, 0);
 	if (result)
 		goto reset_fifo_fail;
+	result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
+				    INV_MPU6050_BIT_SLV3_FIFO_EN, 0);
+	if (result)
+		goto reset_fifo_fail;
 	/* disable fifo reading */
 	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
 	result = regmap_write(st->map, st->reg->user_ctrl,
@@ -155,14 +179,12 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
 	inv_clear_kfifo(st);
 
 	/* enable interrupt */
-	if (st->chip_config.accl_fifo_enable ||
-	    st->chip_config.gyro_fifo_enable) {
-		result = regmap_update_bits(st->map, st->reg->int_enable,
-					    INV_MPU6050_BIT_DATA_RDY_EN,
-					    INV_MPU6050_BIT_DATA_RDY_EN);
-		if (result)
-			return result;
-	}
+	result = regmap_update_bits(st->map, st->reg->int_enable,
+				    INV_MPU6050_BIT_DATA_RDY_EN,
+				    INV_MPU6050_BIT_DATA_RDY_EN);
+	if (result)
+		return result;
+
 	/* enable FIFO reading and I2C master interface*/
 	st->chip_config.user_ctrl |= INV_MPU6050_BIT_FIFO_EN;
 	result = regmap_write(st->map, st->reg->user_ctrl,
@@ -175,9 +197,22 @@ 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 (*indio_dev->active_scan_mask & st->ext_sens[0].scan_mask)
+		d |= INV_MPU6050_BIT_SLV0_FIFO_EN;
+	if (*indio_dev->active_scan_mask & st->ext_sens[1].scan_mask)
+		d |= INV_MPU6050_BIT_SLV1_FIFO_EN;
+	if (*indio_dev->active_scan_mask & st->ext_sens[2].scan_mask)
+		d |= INV_MPU6050_BIT_SLV2_FIFO_EN;
 	result = regmap_write(st->map, st->reg->fifo_en, d);
 	if (result)
 		goto reset_fifo_fail;
+	if (*indio_dev->active_scan_mask & st->ext_sens[3].scan_mask) {
+		result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
+					    INV_MPU6050_BIT_SLV3_FIFO_EN,
+					    INV_MPU6050_BIT_SLV3_FIFO_EN);
+		if (result)
+			goto reset_fifo_fail;
+	}
 
 	/* check realign required */
 	inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets);
@@ -222,8 +257,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	struct iio_poll_func *pf = p;
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct inv_mpu6050_state *st = iio_priv(indio_dev);
-	size_t bytes_per_datum;
+	size_t bytes_per_datum = 0;
 	int result;
+	int i;
 	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
 	u16 fifo_count;
 	s64 timestamp;
@@ -236,15 +272,18 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
 	spi = i2c ? NULL: to_spi_device(regmap_dev);
 
 	mutex_lock(&indio_dev->mlock);
-	if (!(st->chip_config.accl_fifo_enable |
-		st->chip_config.gyro_fifo_enable))
-		goto end_session;
+
+	/* Sample length */
 	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;
+	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i)
+		if (st->ext_sens[i].scan_mask & *indio_dev->active_scan_mask)
+			bytes_per_datum += st->ext_sens_spec[i].len;
+	if (!bytes_per_datum)
+		return 0;
 
 	/*
 	 * read fifo_count register to know how many bytes inside FIFO
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index a334ed9..39e791b 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -61,6 +61,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 			if (result)
 				return result;
 		}
+
+		result = inv_mpu_slave_enable_mask(st, *indio_dev->active_scan_mask);
+		if (result)
+			return result;
+
 		result = inv_reset_fifo(indio_dev);
 		if (result)
 			return result;
@@ -80,6 +85,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
 		if (result)
 			return result;
 
+		result = inv_mpu_slave_enable_mask(st, 0);
+		if (result)
+			return result;
+
 		result = inv_mpu6050_switch_engine(st, false,
 					INV_MPU6050_BIT_PWR_GYRO_STBY);
 		if (result)
-- 
2.5.5

Re: [RFC v2 7/7] iio: inv_mpu6050: Expose channels from slave sensors

[Rob Herring]

On Wed, May 18, 2016 at 06:00:54PM +0300, Crestez Dan Leonard wrote:
> This works by copying the iio_chan_specs from the slave device and
> republishing them as if they belonged to the MPU itself. All
> read/write_raw operations are forwarded to the other driver.
> 
> The original device is still registered with linux as a normal driver
> and works normally and you can poke at it to configure stuff like sample
> rates and scaling factors.
> 
> Buffer values are read from the MPU fifo, allowing a much higher
> sampling rate.
> 
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>
> ---
>  .../devicetree/bindings/iio/imu/inv_mpu6050.txt    |  47 ++-

Please put all the binding changes in 1 separate patch. The h/w is not 
evolving...

Then I would have had the full picture when replying the to first 
change.

Rob

Re: [RFC v2 7/7] iio: inv_mpu6050: Expose channels from slave sensors

[Jonathan Cameron]

On 18/05/16 16:00, Crestez Dan Leonard wrote:
> This works by copying the iio_chan_specs from the slave device and
> republishing them as if they belonged to the MPU itself. All
> read/write_raw operations are forwarded to the other driver.
> 
> The original device is still registered with linux as a normal driver
> and works normally and you can poke at it to configure stuff like sample
> rates and scaling factors.
> 
> Buffer values are read from the MPU fifo, allowing a much higher
> sampling rate.
> 
> Signed-off-by: Crestez Dan Leonard <leonard.crestez@intel.com>

Looks pretty good to me - a few comments inline.

Looking forward to the next revision!

I'm in an odd state of being amazed at how little code it took to support this and
how fiddly some corners of it really are ;)

Jonathan

p.s. I definitely need an Ack from Ge on this btw!
> ---
>  .../devicetree/bindings/iio/imu/inv_mpu6050.txt    |  47 ++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_core.c         | 387 ++++++++++++++++++++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h          |  74 +++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c         |  65 +++-
>  drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c      |   9 +
>  5 files changed, 556 insertions(+), 26 deletions(-)
> 
> diff --git a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> index 778d076..07d572a 100644
> --- a/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> +++ b/Documentation/devicetree/bindings/iio/imu/inv_mpu6050.txt
> @@ -11,8 +11,8 @@ Optional properties:
>   - mount-matrix: an optional 3x3 mounting rotation matrix
>   - invensense,i2c-aux-master: operate aux i2c in "master" mode (default is bypass).
>  
> -The MPU has an auxiliary i2c bus for additional sensors. Devices attached this
> -way can be described as for a regular linux i2c bus.
> +It is possible to attach auxiliary sensors to the MPU and have them be handled
> +by linux. Those auxiliary sensors are described as an i2c bus.
>  
>  It is possible to interact with aux devices in "bypass" or "master" mode. In
>  "bypass" mode the auxiliary SDA/SCL lines are connected directly to the main i2c
> @@ -25,7 +25,31 @@ In "master" mode aux devices are listed behind a "i2c@1" node with reg = <1>;
>  The master and bypass modes are not supported at the same time. The
>  "invensense,i2c-aux-master" property must be set to activate master mode.
>  Bypass mode is generally faster but master mode also works when the MPU is
> -connected via SPI.
> +connected via SPI. Enabling master mode is required for automated external
> +readings.
> +
> +
> +It is possible to configure the MPU to automatically fetch reading for
> +devices connected on the auxiliary i2c bus without CPU intervention. When this
> +is done the driver will present the channels of the slaved devices as if they
> +belong to the MPU device itself.
> +
> +Reads and write to config values (like scaling factors) are forwarded to the
> +other driver while data reads are handled from MPU registers. The channels are
> +also available through the MPU's iio triggered buffer mechanism. This feature
> +can allow sampling up to 24 bytes from 4 slaves at a much higher rate.
> +
> +This is specified in device tree as an "invensense,external-sensors" node with
> +children indexed by slave ids 0 to 3. The child nodes identifying each external
> +sensor reading have the following properties:
> + - reg: 0 to 3 slave index
> + - invensense,addr : the I2C address to read from
> + - invensense,reg : the I2C register to read from
> + - invensense,len : read length from the device
> + - invensense,external-channels : list of slaved channels specified by scan_index.
> +
> +The sum of storagebits for external channels must equal the read length. Only
> +16bit channels are currently supported.
Interesting - so right now the core mangling would work, you just don't want
the complex available_scan_masks list, though to my mind that wouldn't be all that
difficult to autogenerate and if we let each sensor set of channel be represented
by a single letter can be written out as all the values a 7 bit binary number can
take, or 128 entries.

Just about worth the addition of a callback to provide the smallest one currently
relevant to the core.
>  
>  Example:
>  	mpu6050@68 {
> @@ -65,7 +89,8 @@ Example describing mpu9150 (which includes an ak9875 on chip):
>  		};
>  	};
>  
> -Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
> +Example describing a mpu6500 via SPI with an hmc5883l on aux i2c configured for
> +automatic external readings as slave 0:
>  	mpu6500@0 {
>  		compatible = "invensense,mpu6500";
>  		reg = <0x0>;
> @@ -73,6 +98,8 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
>  		interrupt-parent = <&gpio1>;
>  		interrupts = <31 1>;
>  
> +		invensense,i2c-aux-master;
> +
>  		#address-cells = <1>;
>  		#size-cells = <0>;
>  		i2c@1 {
> @@ -85,4 +112,16 @@ Example describing a mpu6500 via SPI with an hmc5883l on auxiliary i2c:
>  				reg = <0x1e>;
>  			};
>  		};
> +
> +		invensense,external-sensors {
> +			#address-cells = <1>;
> +			#size-cells = <0>;
> +			hmc5833l@0 {
> +				reg = <0>;
> +				invensense,addr = <0x1e>;
> +				invensense,reg = <3>;
> +				invensense,len = <6>;
> +				invensense,external-channels = <0 1 2>;
> +			};
> +		};
>  	};
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> index 76683b8..715b2e4 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
> @@ -126,6 +126,9 @@ static bool inv_mpu6050_volatile_reg(struct device *dev, unsigned int reg)
>  		return true;
>  	if (reg >= INV_MPU6050_REG_RAW_GYRO && reg < INV_MPU6050_REG_RAW_GYRO + 6)
>  		return true;
> +	if (reg < INV_MPU6050_REG_EXT_SENS_DATA_00 + INV_MPU6050_CNT_EXT_SENS_DATA &&
> +			reg >= INV_MPU6050_REG_EXT_SENS_DATA_00)
> +		return true;
>  	switch (reg) {
>  	case INV_MPU6050_REG_TEMPERATURE:
>  	case INV_MPU6050_REG_TEMPERATURE + 1:
> @@ -335,8 +338,24 @@ inv_mpu6050_read_raw(struct iio_dev *indio_dev,
>  		     struct iio_chan_spec const *chan,
>  		     int *val, int *val2, long mask)
>  {
> -	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
Minor white space fixup like this should be in a separate patch.  This is a complex
enough beast as it is, don't add padding!
>  	int ret = 0;
> +	int chan_index;
> +
> +	if (chan > indio_dev->channels + indio_dev->num_channels ||
> +			chan < indio_dev->channels)
> +		return -EINVAL;
> +	chan_index = chan - indio_dev->channels;
> +	if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
> +		struct inv_mpu_ext_chan_state *ext_chan_state =
> +				&st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
> +		struct inv_mpu_ext_sens_state *ext_sens_state =
> +				&st->ext_sens[ext_chan_state->ext_sens_index];
> +		struct iio_dev *orig_dev = ext_sens_state->orig_dev;
> +		const struct iio_chan_spec *orig_chan =
> +				&orig_dev->channels[ext_chan_state->orig_chan_index];
> +		return orig_dev->info->read_raw(orig_dev, orig_chan, val, val2, mask);
> +	}
>  
>  	switch (mask) {
>  	case IIO_CHAN_INFO_RAW:
> @@ -518,9 +537,25 @@ static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
>  				 struct iio_chan_spec const *chan,
>  				 int val, int val2, long mask)
>  {
> -	struct inv_mpu6050_state  *st = iio_priv(indio_dev);
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	int chan_index;
>  	int result;
>  
> +	if (chan > indio_dev->channels + indio_dev->num_channels ||
> +			chan < indio_dev->channels)
> +		return -EINVAL;
> +	chan_index = chan - indio_dev->channels;
> +	if (chan_index >= INV_MPU6050_NUM_INT_CHAN) {
> +		struct inv_mpu_ext_chan_state *ext_chan_state =
> +				&st->ext_chan[chan_index - INV_MPU6050_NUM_INT_CHAN];
> +		struct inv_mpu_ext_sens_state *ext_sens_state =
> +				&st->ext_sens[ext_chan_state->ext_sens_index];
> +		struct iio_dev *orig_dev = ext_sens_state->orig_dev;
> +		const struct iio_chan_spec *orig_chan =
> +				&orig_dev->channels[ext_chan_state->orig_chan_index];
> +		return orig_dev->info->write_raw(orig_dev, orig_chan, val, val2, mask);
> +	}
> +
>  	mutex_lock(&indio_dev->mlock);
>  	/*
>  	 * we should only update scale when the chip is disabled, i.e.
> @@ -809,6 +844,346 @@ static const struct iio_info mpu_info = {
>  	.validate_trigger = inv_mpu6050_validate_trigger,
>  };
>  
> +extern struct device_type iio_device_type;
> +
> +static int iio_device_from_i2c_client_match(struct device *dev, void *data)
> +{
> +	return dev->type == &iio_device_type;
> +}
> +
> +static struct iio_dev* iio_device_from_i2c_client(struct i2c_client* i2c)
> +{
> +	struct device *child;
> +
> +	child = device_find_child(&i2c->dev, NULL, iio_device_from_i2c_client_match);
> +
> +	return (child ? dev_to_iio_dev(child) : NULL);
> +}
> +
> +static int inv_mpu_slave_enable(struct inv_mpu6050_state *st, int index, bool state)
> +{
> +	return regmap_update_bits(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
> +				  INV_MPU6050_BIT_I2C_SLV_EN,
> +				  state ? INV_MPU6050_BIT_I2C_SLV_EN : 0);
> +}
> +
> +/* Enable slaves based on scan mask */
> +int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st,
> +			      const unsigned long mask)
> +{
> +	int i, result;
> +
> +	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> +		long slave_mask = st->ext_sens[i].scan_mask;
> +		result = inv_mpu_slave_enable(st, i, slave_mask & mask);
> +		if (result)
> +			return result;
> +	}
> +
> +	return 0;
> +}
> +
> +static int inv_mpu_parse_one_ext_sens(struct device *dev,
> +				      struct device_node *np,
> +				      struct inv_mpu_ext_sens_spec *spec)
> +{
> +	int result;
> +	u32 addr, reg, len;
> +
> +	result = of_property_read_u32(np, "invensense,addr", &addr);
> +	if (result)
> +		return result;
> +	result = of_property_read_u32(np, "invensense,reg", &reg);
> +	if (result)
> +		return result;
> +	result = of_property_read_u32(np, "invensense,len", &len);
> +	if (result)
> +		return result;
> +
> +	spec->addr = addr;
> +	spec->reg = reg;
> +	spec->len = len;
> +
> +	result = of_property_count_u32_elems(np, "invensense,external-channels");
> +	if (result < 0)
> +		return result;
> +	spec->num_ext_channels = result;
> +	spec->ext_channels = devm_kmalloc(dev, spec->num_ext_channels * sizeof(*spec->ext_channels), GFP_KERNEL);
> +	if (!spec->ext_channels)
> +		return -ENOMEM;
> +	result = of_property_read_u32_array(np, "invensense,external-channels",
> +					    spec->ext_channels,
> +					    spec->num_ext_channels);
> +	if (result)
> +		return result;
> +
> +	return 0;
> +}
> +
> +static int inv_mpu_parse_ext_sens(struct device *dev,
> +				  struct device_node *node,
> +				  struct inv_mpu_ext_sens_spec *specs)
> +{
> +	struct device_node *child;
> +	int result;
> +	u32 reg;
> +
> +	for_each_available_child_of_node(node, child) {
> +		result = of_property_read_u32(child, "reg", &reg);
> +		if (result)
> +			return result;
> +		if (reg > INV_MPU6050_MAX_EXT_SENSORS) {
> +			dev_err(dev, "External sensor index %u out of range, max %d\n",
> +				reg, INV_MPU6050_MAX_EXT_SENSORS);
> +			return -EINVAL;
> +		}
> +		result = inv_mpu_parse_one_ext_sens(dev, child, &specs[reg]);
> +		if (result)
> +			return result;
> +	}
> +
> +	return 0;
> +}
> +
> +static int inv_mpu_get_ext_sens_spec(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	struct device *dev = regmap_get_device(st->map);
> +	struct device_node *node;
> +	int result;
> +
> +	node = of_get_child_by_name(dev->of_node, "invensense,external-sensors");
> +	if (node) {
> +		result = inv_mpu_parse_ext_sens(dev, node, st->ext_sens_spec);
> +		if (result)
> +			dev_err(dev, "Failed to parsing external-sensors devicetree data\n");
> +		return result;
> +	}
> +
> +	return 0;
> +}
> +
> +/* Struct used while enumerating devices and matching them */
> +struct inv_mpu_handle_ext_sensor_fnarg
> +{
> +	struct iio_dev *indio_dev;
> +
> +	/* Current scan index */
> +	int scan_index;
> +	/* Current channel index */
> +	int chan_index;
> +	/* Non-const pointer to channels */
> +	struct iio_chan_spec *channels;
> +};
> +
> +/*
> + * Write initial configuration of a slave at probe time.
> + *
> + * This is mostly fixed except for enabling/disabling individual slaves.
> + */
> +static int inv_mpu_config_external_read(struct inv_mpu6050_state *st, int index,
> +					const struct inv_mpu_ext_sens_spec *spec)
> +{
> +	int result;
> +
> +	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_ADDR(index),
> +			      INV_MPU6050_BIT_I2C_SLV_RW | spec->addr);
> +	if (result)
> +		return result;
> +	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_REG(index), spec->reg);
> +	if (result)
> +		return result;
> +	result = regmap_write(st->map, INV_MPU6050_REG_I2C_SLV_CTRL(index),
> +			      spec->len);
> +	if (result)
> +		return result;
> +
> +	return result;
> +}
> +
> +/* Handle one device */
> +static int inv_mpu_handle_slave_device(
> +		struct inv_mpu_handle_ext_sensor_fnarg *fnarg,
> +		int slave_index,
> +		struct iio_dev *orig_dev)
> +{
> +	int i, j;
> +	int data_offset;
> +	struct iio_dev *indio_dev = fnarg->indio_dev;
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	struct device *mydev = regmap_get_device(st->map);
> +	struct inv_mpu_ext_sens_spec *ext_sens_spec =
> +			&st->ext_sens_spec[slave_index];
> +	struct inv_mpu_ext_sens_state *ext_sens_state =
> +			&st->ext_sens[slave_index];
> +
> +	dev_info(mydev, "slave %d is device %s\n",
> +			slave_index, orig_dev->name ?: dev_name(&orig_dev->dev));
> +	ext_sens_state->orig_dev = orig_dev;
> +	ext_sens_state->scan_mask = 0;
> +	data_offset = 0;
> +
> +	/* handle channels: */
> +	for (i = 0; i < ext_sens_spec->num_ext_channels; ++i) {
> +		int orig_chan_index = -1;
> +		const struct iio_chan_spec *orig_chan_spec;
> +		struct iio_chan_spec *chan_spec;
> +		struct inv_mpu_ext_chan_state *chan_state;
> +
> +		for (j = 0; j < orig_dev->num_channels; ++j)
> +			if (orig_dev->channels[j].scan_index == ext_sens_spec->ext_channels[i]) {
> +				orig_chan_index = j;
> +				break;
> +			}
> +
> +		if (orig_chan_index < 0) {
> +			dev_err(mydev, "Could not find slave channel with scan_index %d\n",
> +					ext_sens_spec->ext_channels[i]);
> +		}
> +
> +		orig_chan_spec = &orig_dev->channels[orig_chan_index];
> +		chan_spec = &fnarg->channels[INV_MPU6050_NUM_INT_CHAN + fnarg->chan_index];
> +		chan_state = &st->ext_chan[fnarg->chan_index];
> +
> +		chan_state->ext_sens_index = slave_index;
> +		chan_state->orig_chan_index = orig_chan_index;
> +		chan_state->data_offset = data_offset;
> +		memcpy(chan_spec, orig_chan_spec, sizeof(struct iio_chan_spec));
> +		chan_spec->scan_index = fnarg->scan_index;
> +		ext_sens_state->scan_mask |= (1 << chan_spec->scan_index);
> +
> +		fnarg->scan_index++;
> +		fnarg->chan_index++;
> +		data_offset += chan_spec->scan_type.storagebits / 8;
> +		dev_info(mydev, "Reading external channel #%d scan_index %d data_offset %d"
> +				" from original device %s chan #%d scan_index %d\n",
> +				fnarg->chan_index, chan_spec->scan_index, chan_state->data_offset,
> +				orig_dev->name ?: dev_name(&orig_dev->dev), orig_chan_index, orig_chan_spec->scan_index);
> +	}
> +	if (ext_sens_spec->len != data_offset) {
> +		dev_err(mydev, "slave %d length mismatch between "
> +				"i2c slave read length (%d) and "
> +				"sum of channel sizes (%d)\n",
> +				slave_index, ext_sens_spec->len, data_offset);
> +		return -EINVAL;
> +	}
> +
> +	return inv_mpu_config_external_read(st, slave_index, ext_sens_spec);
> +}
> +
> +/* device_for_each_child enum function */
> +static int inv_mpu_handle_ext_sensor_fn(struct device *dev, void *data)
> +{
> +	struct inv_mpu_handle_ext_sensor_fnarg *fnarg = data;
> +	struct iio_dev *indio_dev = fnarg->indio_dev;
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	struct i2c_client *client;
> +	struct iio_dev *orig_dev;
> +	int i, result;
> +
> +	client = i2c_verify_client(dev);
> +	if (!client)
> +		return 0;
> +	orig_dev = iio_device_from_i2c_client(client);
> +	if (!orig_dev)
> +		return 0;
> +
> +	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> +		if (st->ext_sens_spec[i].addr != client->addr)
> +			continue;
> +		if (st->ext_sens[i].orig_dev) {
> +			dev_warn(&indio_dev->dev, "already found slave with at addr %d\n", client->addr);
> +			continue;
> +		}
> +
> +		result = inv_mpu_handle_slave_device(fnarg, i, orig_dev);
> +		if (result)
> +			return result;
> +	}
> +	return 0;
> +}
> +
> +static int inv_mpu6050_handle_ext_sensors(struct iio_dev *indio_dev)
> +{
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> +	struct device *dev = regmap_get_device(st->map);
> +	struct inv_mpu_handle_ext_sensor_fnarg fnarg = {
> +		.indio_dev = indio_dev,
> +		.chan_index = 0,
> +		.scan_index = INV_MPU6050_SCAN_TIMESTAMP,
> +	};
> +	int i, result;
> +	int num_ext_chan = 0, sum_data_len = 0;
> +	int num_scan_elements;
> +
> +	inv_mpu_get_ext_sens_spec(indio_dev);
> +	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> +		num_ext_chan += st->ext_sens_spec[i].num_ext_channels;
> +		sum_data_len += st->ext_sens_spec[i].len;
> +	}
> +	if (sum_data_len > INV_MPU6050_CNT_EXT_SENS_DATA) {
> +		dev_err(dev, "Too many bytes from external sensors:"
> +			      " requested=%d max=%d\n",
> +			      sum_data_len, INV_MPU6050_CNT_EXT_SENS_DATA);
> +		return -EINVAL;
> +	}
> +
> +	/* Allocate scan_offsets/scan_lengths */
> +	num_scan_elements = INV_MPU6050_NUM_INT_SCAN_ELEMENTS + num_ext_chan;
> +	st->scan_offsets = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
> +	if (!st->scan_offsets)
> +		return -ENOMEM;
> +	st->scan_lengths = devm_kmalloc(dev, num_scan_elements * sizeof(int), GFP_KERNEL);
> +	if (!st->scan_lengths)
> +		return -ENOMEM;
> +
> +	/* Zero length means nothing to do, just publish internal channels */
> +	if (!sum_data_len) {
> +		indio_dev->channels = inv_mpu_channels;
> +		indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN;
> +		BUILD_BUG_ON(ARRAY_SIZE(inv_mpu_channels) != INV_MPU6050_NUM_INT_CHAN);
> +		return 0;
> +	}
> +
> +	indio_dev->num_channels = INV_MPU6050_NUM_INT_CHAN + num_ext_chan;
> +	indio_dev->channels = fnarg.channels = devm_kmalloc(dev,
> +			indio_dev->num_channels * sizeof(struct iio_chan_spec),
> +			GFP_KERNEL);
> +	if (!fnarg.channels)
> +		return -ENOMEM;
> +	memcpy(fnarg.channels, inv_mpu_channels, sizeof(inv_mpu_channels));
> +	memset(fnarg.channels + INV_MPU6050_NUM_INT_CHAN, 0,
> +	       num_ext_chan * sizeof(struct iio_chan_spec));
> +
> +	st->ext_chan = devm_kzalloc(dev, num_ext_chan * sizeof(*st->ext_chan), GFP_KERNEL);
> +	if (!st->ext_chan)
> +		return -ENOMEM;
> +
> +	result = inv_mpu6050_set_power_itg(st, true);
> +	if (result < 0)
> +		return result;
> +
> +	result = device_for_each_child(&st->aux_master_adapter.dev, &fnarg,
> +				       inv_mpu_handle_ext_sensor_fn);
> +	if (result)
> +		goto out_disable;
> +	/* Timestamp channel has index 0 and last scan_index */
> +	fnarg.channels[0].scan_index = fnarg.scan_index;
> +
> +	if (fnarg.chan_index != num_ext_chan) {
> +		dev_err(&indio_dev->dev, "Failed to match all external channels!\n");
> +		result = -EINVAL;
> +		goto out_disable;
> +	}
> +
> +	result = inv_mpu6050_set_power_itg(st, false);
> +	return result;
> +
> +out_disable:
> +	inv_mpu6050_set_power_itg(st, false);
> +	return result;
> +}
> +
>  /**
>   *  inv_check_and_setup_chip() - check and setup chip.
>   */
> @@ -1140,8 +1515,6 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
>  		indio_dev->name = name;
>  	else
>  		indio_dev->name = dev_name(dev);
> -	indio_dev->channels = inv_mpu_channels;
> -	indio_dev->num_channels = ARRAY_SIZE(inv_mpu_channels);
>  
>  	indio_dev->info = &mpu_info;
>  	indio_dev->modes = INDIO_BUFFER_TRIGGERED;
> @@ -1179,6 +1552,12 @@ int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
>  	}
>  #endif
>  
> +	result = inv_mpu6050_handle_ext_sensors(indio_dev);
> +	if (result < 0) {
> +		dev_err(dev, "failed to configure channels %d\n", result);
> +		goto out_remove_trigger;
> +	}
> +
>  	INIT_KFIFO(st->timestamps);
>  	spin_lock_init(&st->time_stamp_lock);
>  	result = iio_device_register(indio_dev);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> index ec1401d..56fd943 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
> @@ -110,6 +110,45 @@ struct inv_mpu6050_hw {
>  	const struct inv_mpu6050_chip_config *config;
>  };
>  
> +/* Maximum external sensors */
> +/* These are SLV0-3 in HW, SLV4 is reserved for i2c master */
> +#define INV_MPU6050_MAX_EXT_SENSORS 4
> +
> +/* Number of internal channels */
> +#define INV_MPU6050_NUM_INT_CHAN 8
> +
> +/* Number of internal scan elements (channels with scan_index >= 0) */
> +#define INV_MPU6050_NUM_INT_SCAN_ELEMENTS 7
> +
> +/* Specification for an external sensor */
> +struct inv_mpu_ext_sens_spec {
> +	unsigned short addr;
> +	u8 reg, len;
> +
> +	int num_ext_channels;
> +	int *ext_channels;
> +};
> +
> +/* Driver state for each external sensor */
> +struct inv_mpu_ext_sens_state {
> +	struct iio_dev *orig_dev;
> +
> +	/* Mask of all channels in this sensor */
> +	unsigned long scan_mask;
> +};
> +
> +/* Driver state for each external channel */
> +struct inv_mpu_ext_chan_state {
> +	/* Index inside state->ext_sens */
> +	int ext_sens_index;
> +
> +	/* Index inside orig_dev->channels */
> +	int orig_chan_index;
> +
> +	/* Relative to first offset for this slave */
> +	int data_offset;
> +};
> +
>  /*
>   *  struct inv_mpu6050_state - Driver state variables.
>   *  @TIMESTAMP_FIFO_SIZE: fifo size for timestamp.
> @@ -153,10 +192,13 @@ struct inv_mpu6050_state {
>  	u8 slv4_done_status;
>  #endif
>  
> -#define INV_MPU6050_MAX_SCAN_ELEMENTS 7
> -	unsigned int scan_offsets[INV_MPU6050_MAX_SCAN_ELEMENTS];
> -	unsigned int scan_lengths[INV_MPU6050_MAX_SCAN_ELEMENTS];
> +	unsigned int *scan_offsets;
> +	unsigned int *scan_lengths;
>  	bool realign_required;
> +
> +	struct inv_mpu_ext_sens_spec ext_sens_spec[INV_MPU6050_MAX_EXT_SENSORS];
> +	struct inv_mpu_ext_sens_state ext_sens[INV_MPU6050_MAX_EXT_SENSORS];
> +	struct inv_mpu_ext_chan_state *ext_chan;
>  };
>  
>  /*register and associated bit definition*/
> @@ -171,6 +213,24 @@ struct inv_mpu6050_state {
>  #define INV_MPU6050_REG_FIFO_EN             0x23
>  #define INV_MPU6050_BIT_ACCEL_OUT           0x08
>  #define INV_MPU6050_BITS_GYRO_OUT           0x70
> +#define INV_MPU6050_BIT_SLV0_FIFO_EN        0x01
> +#define INV_MPU6050_BIT_SLV1_FIFO_EN        0x02
> +#define INV_MPU6050_BIT_SLV2_FIFO_EN        0x04
> +#define INV_MPU6050_BIT_SLV2_FIFO_EN        0x04
> +
> +/* SLV3 fifo enabling is not in REG_FIFO_EN */
> +#define INV_MPU6050_REG_MST_CTRL	    0x24
> +#define INV_MPU6050_BIT_SLV3_FIFO_EN        0x20
> +
> +/* For slaves 0-3 */
> +#define INV_MPU6050_REG_I2C_SLV_ADDR(i)     (0x25 + 3 * (i))
> +#define INV_MPU6050_REG_I2C_SLV_REG(i)      (0x26 + 3 * (i))
> +#define INV_MPU6050_REG_I2C_SLV_CTRL(i)     (0x27 + 3 * (i))
> +#define INV_MPU6050_BIT_I2C_SLV_RW          0x80
> +#define INV_MPU6050_BIT_I2C_SLV_EN          0x80
> +#define INV_MPU6050_BIT_I2C_SLV_BYTE_SW     0x40
> +#define INV_MPU6050_BIT_I2C_SLV_REG_DIS     0x20
> +#define INV_MPU6050_BIT_I2C_SLV_REG_GRP     0x10
>  
>  #define INV_MPU6050_REG_I2C_SLV4_ADDR       0x31
>  #define INV_MPU6050_BIT_I2C_SLV4_R          0x80
> @@ -247,8 +307,8 @@ struct inv_mpu6050_state {
>  #define INV_MPU6050_GYRO_CONFIG_FSR_SHIFT    3
>  #define INV_MPU6050_ACCL_CONFIG_FSR_SHIFT    3
>  
> -/* 6 + 6 round up and plus 8 */
> -#define INV_MPU6050_OUTPUT_DATA_SIZE         24
> +/* max is 3*2 accel + 3*2 gyro + 24 external + 8 ts */
> +#define INV_MPU6050_OUTPUT_DATA_SIZE         44
>  
>  #define INV_MPU6050_REG_INT_PIN_CFG	0x37
>  #define INV_MPU6050_BIT_BYPASS_EN	0x2
> @@ -261,6 +321,9 @@ struct inv_mpu6050_state {
>  #define INV_MPU6050_MIN_FIFO_RATE            4
>  #define INV_MPU6050_ONE_K_HZ                 1000
>  
> +#define INV_MPU6050_REG_EXT_SENS_DATA_00	0x49
> +#define INV_MPU6050_CNT_EXT_SENS_DATA		24
> +
>  #define INV_MPU6050_REG_WHOAMI			117
>  
>  #define INV_MPU6000_WHOAMI_VALUE		0x68
> @@ -328,6 +391,7 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev);
>  void inv_mpu6050_remove_trigger(struct inv_mpu6050_state *st);
>  int inv_reset_fifo(struct iio_dev *indio_dev);
>  int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask);
> +int inv_mpu_slave_enable_mask(struct inv_mpu6050_state *st, const unsigned long mask);
>  int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 val);
>  int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on);
>  int inv_mpu_acpi_create_mux_client(struct i2c_client *client);
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> index 49e503c..919148a 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_ring.c
> @@ -30,7 +30,9 @@ static void inv_mpu_get_scan_offsets(
>  		const unsigned int masklen,
>  		unsigned int *scan_offsets)
>  {
> +	struct inv_mpu6050_state *st = iio_priv(indio_dev);
>  	unsigned int pos = 0;
> +	int i, j;
>  
>  	if (*mask & INV_MPU6050_SCAN_MASK_ACCEL) {
>  		scan_offsets[INV_MPU6050_SCAN_ACCL_X] = pos + 0;
> @@ -44,6 +46,24 @@ static void inv_mpu_get_scan_offsets(
>  		scan_offsets[INV_MPU6050_SCAN_GYRO_Z] = pos + 4;
>  		pos += 6;
>  	}
> +	/* HW lays out channels in slave order */
> +	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i) {
> +		struct inv_mpu_ext_sens_spec *ext_sens_spec;
> +		struct inv_mpu_ext_sens_state *ext_sens_state;
> +		ext_sens_spec = &st->ext_sens_spec[i];
> +		ext_sens_state = &st->ext_sens[i];
> +
> +		if (!(ext_sens_state->scan_mask & *mask))
> +			continue;
> +		for (j = 0; j + INV_MPU6050_NUM_INT_CHAN < indio_dev->num_channels; ++j) {
> +			const struct iio_chan_spec *chan;
> +			if (st->ext_chan[j].ext_sens_index != i)
> +				continue;
> +			chan = &indio_dev->channels[j + INV_MPU6050_NUM_INT_CHAN];
> +			scan_offsets[chan->scan_index] = pos + st->ext_chan[j].data_offset;
> +		}
> +		pos += ext_sens_spec->len;
> +	}
>  }
>  
>  /* This is slowish but relatively common */
> @@ -138,6 +158,10 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	result = regmap_write(st->map, st->reg->fifo_en, 0);
>  	if (result)
>  		goto reset_fifo_fail;
> +	result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
> +				    INV_MPU6050_BIT_SLV3_FIFO_EN, 0);
> +	if (result)
> +		goto reset_fifo_fail;
>  	/* disable fifo reading */
>  	st->chip_config.user_ctrl &= ~INV_MPU6050_BIT_FIFO_EN;
>  	result = regmap_write(st->map, st->reg->user_ctrl,
> @@ -155,14 +179,12 @@ int inv_reset_fifo(struct iio_dev *indio_dev)
>  	inv_clear_kfifo(st);
>  
>  	/* enable interrupt */
> -	if (st->chip_config.accl_fifo_enable ||
> -	    st->chip_config.gyro_fifo_enable) {
> -		result = regmap_update_bits(st->map, st->reg->int_enable,
> -					    INV_MPU6050_BIT_DATA_RDY_EN,
> -					    INV_MPU6050_BIT_DATA_RDY_EN);
> -		if (result)
> -			return result;
> -	}
> +	result = regmap_update_bits(st->map, st->reg->int_enable,
> +				    INV_MPU6050_BIT_DATA_RDY_EN,
> +				    INV_MPU6050_BIT_DATA_RDY_EN);
> +	if (result)
> +		return result;
> +
>  	/* enable FIFO reading and I2C master interface*/
>  	st->chip_config.user_ctrl |= INV_MPU6050_BIT_FIFO_EN;
>  	result = regmap_write(st->map, st->reg->user_ctrl,
> @@ -175,9 +197,22 @@ 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 (*indio_dev->active_scan_mask & st->ext_sens[0].scan_mask)
> +		d |= INV_MPU6050_BIT_SLV0_FIFO_EN;
> +	if (*indio_dev->active_scan_mask & st->ext_sens[1].scan_mask)
> +		d |= INV_MPU6050_BIT_SLV1_FIFO_EN;
> +	if (*indio_dev->active_scan_mask & st->ext_sens[2].scan_mask)
> +		d |= INV_MPU6050_BIT_SLV2_FIFO_EN;
>  	result = regmap_write(st->map, st->reg->fifo_en, d);
>  	if (result)
>  		goto reset_fifo_fail;
> +	if (*indio_dev->active_scan_mask & st->ext_sens[3].scan_mask) {
> +		result = regmap_update_bits(st->map, INV_MPU6050_REG_MST_CTRL,
> +					    INV_MPU6050_BIT_SLV3_FIFO_EN,
> +					    INV_MPU6050_BIT_SLV3_FIFO_EN);
> +		if (result)
> +			goto reset_fifo_fail;
> +	}
>  
>  	/* check realign required */
>  	inv_mpu_get_scan_offsets(indio_dev, mask, masklen, st->scan_offsets);
> @@ -222,8 +257,9 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	struct iio_poll_func *pf = p;
>  	struct iio_dev *indio_dev = pf->indio_dev;
>  	struct inv_mpu6050_state *st = iio_priv(indio_dev);
> -	size_t bytes_per_datum;
> +	size_t bytes_per_datum = 0;
>  	int result;
> +	int i;
>  	u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
>  	u16 fifo_count;
>  	s64 timestamp;
> @@ -236,15 +272,18 @@ irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
>  	spi = i2c ? NULL: to_spi_device(regmap_dev);
>  
>  	mutex_lock(&indio_dev->mlock);
> -	if (!(st->chip_config.accl_fifo_enable |
> -		st->chip_config.gyro_fifo_enable))
> -		goto end_session;
> +
> +	/* Sample length */
>  	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;
> +	for (i = 0; i < INV_MPU6050_MAX_EXT_SENSORS; ++i)
> +		if (st->ext_sens[i].scan_mask & *indio_dev->active_scan_mask)
> +			bytes_per_datum += st->ext_sens_spec[i].len;
> +	if (!bytes_per_datum)
> +		return 0;
>  
>  	/*
>  	 * read fifo_count register to know how many bytes inside FIFO
> diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> index a334ed9..39e791b 100644
> --- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> +++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
> @@ -61,6 +61,11 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  			if (result)
>  				return result;
>  		}
> +
> +		result = inv_mpu_slave_enable_mask(st, *indio_dev->active_scan_mask);
> +		if (result)
> +			return result;
> +
>  		result = inv_reset_fifo(indio_dev);
>  		if (result)
>  			return result;
> @@ -80,6 +85,10 @@ static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
>  		if (result)
>  			return result;
>  
> +		result = inv_mpu_slave_enable_mask(st, 0);
> +		if (result)
> +			return result;
> +
>  		result = inv_mpu6050_switch_engine(st, false,
>  					INV_MPU6050_BIT_PWR_GYRO_STBY);
>  		if (result)
>