3 files changed, 116 insertions, 6 deletions

diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig
index 2d0608ba88d7..48fbc0bc7e2a 100644
--- a/drivers/iio/imu/inv_mpu6050/Kconfig
+++ b/drivers/iio/imu/inv_mpu6050/Kconfig
@@ -7,6 +7,7 @@ config INV_MPU6050_IIO
         depends on I2C && SYSFS
         select IIO_BUFFER
         select IIO_TRIGGERED_BUFFER
+        select I2C_MUX
         help
           This driver supports the Invensense MPU6050 devices.
           This driver can also support MPU6500 in MPU6050 compatibility mode
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index b75519deac1a..6d2c115f39b4 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -23,6 +23,7 @@
 #include <linux/kfifo.h>
 #include <linux/spinlock.h>
 #include <linux/iio/iio.h>
+#include <linux/i2c-mux.h>
 #include "inv_mpu_iio.h"
 
 /*
@@ -52,6 +53,7 @@ static const struct inv_mpu6050_reg_map reg_set_6050 = {
         .int_enable             = INV_MPU6050_REG_INT_ENABLE,
         .pwr_mgmt_1             = INV_MPU6050_REG_PWR_MGMT_1,
         .pwr_mgmt_2             = INV_MPU6050_REG_PWR_MGMT_2,
+        .int_pin_cfg            = INV_MPU6050_REG_INT_PIN_CFG,
 };
 
 static const struct inv_mpu6050_chip_config chip_config_6050 = {
@@ -77,6 +79,83 @@ int inv_mpu6050_write_reg(struct inv_mpu6050_state *st, int reg, u8 d)
         return i2c_smbus_write_i2c_block_data(st->client, reg, 1, &d);
 }
 
+/*
+ * 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
+ * the mux adapter will lock the parent i2c adapter, while calling
+ * select/deselect functions.
+ */
+static int inv_mpu6050_write_reg_unlocked(struct inv_mpu6050_state *st,
+                                          u8 reg, u8 d)
+{
+        int ret;
+        u8 buf[2];
+        struct i2c_msg msg[1] = {
+                {
+                        .addr = st->client->addr,
+                        .flags = 0,
+                        .len = sizeof(buf),
+                        .buf = buf,
+                }
+        };
+
+        buf[0] = reg;
+        buf[1] = d;
+        ret = __i2c_transfer(st->client->adapter, msg, 1);
+        if (ret != 1)
+                return ret;
+
+        return 0;
+}
+
+static int inv_mpu6050_select_bypass(struct i2c_adapter *adap, void *mux_priv,
+                                     u32 chan_id)
+{
+        struct iio_dev *indio_dev = mux_priv;
+        struct inv_mpu6050_state *st = iio_priv(indio_dev);
+        int ret = 0;
+
+        /* Use the same mutex which was used everywhere to protect power-op */
+        mutex_lock(&indio_dev->mlock);
+        if (!st->powerup_count) {
+                ret = inv_mpu6050_write_reg_unlocked(st, st->reg->pwr_mgmt_1,
+                                                     0);
+                if (ret)
+                        goto write_error;
+
+                msleep(INV_MPU6050_REG_UP_TIME);
+        }
+        if (!ret) {
+                st->powerup_count++;
+                ret = inv_mpu6050_write_reg_unlocked(st, st->reg->int_pin_cfg,
+                                                     st->client->irq |
+                                                     INV_MPU6050_BIT_BYPASS_EN);
+        }
+write_error:
+        mutex_unlock(&indio_dev->mlock);
+
+        return ret;
+}
+
+static int inv_mpu6050_deselect_bypass(struct i2c_adapter *adap,
+                                       void *mux_priv, u32 chan_id)
+{
+        struct iio_dev *indio_dev = mux_priv;
+        struct inv_mpu6050_state *st = iio_priv(indio_dev);
+
+        mutex_lock(&indio_dev->mlock);
+        /* It doesn't really mattter, if any of the calls fails */
+        inv_mpu6050_write_reg_unlocked(st, st->reg->int_pin_cfg,
+                                       st->client->irq);
+        st->powerup_count--;
+        if (!st->powerup_count)
+                inv_mpu6050_write_reg_unlocked(st, st->reg->pwr_mgmt_1,
+                                               INV_MPU6050_BIT_SLEEP);
+        mutex_unlock(&indio_dev->mlock);
+
+        return 0;
+}
+
 int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
 {
         u8 d, mgmt_1;
@@ -133,13 +212,22 @@ int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en, u32 mask)
 
 int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st, bool power_on)
 {
-        int result;
+        int result = 0;
+
+        if (power_on) {
+                /* Already under indio-dev->mlock mutex */
+                if (!st->powerup_count)
+                        result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1,
+                                                       0);
+                if (!result)
+                        st->powerup_count++;
+        } else {
+                st->powerup_count--;
+                if (!st->powerup_count)
+                        result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1,
+                                                       INV_MPU6050_BIT_SLEEP);
+        }
 
-        if (power_on)
-                result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1, 0);
-        else
-                result = inv_mpu6050_write_reg(st, st->reg->pwr_mgmt_1,
-                                                INV_MPU6050_BIT_SLEEP);
         if (result)
                 return result;
 
@@ -673,6 +761,7 @@ static int inv_mpu_probe(struct i2c_client *client,
 
         st = iio_priv(indio_dev);
         st->client = client;
+        st->powerup_count = 0;
         pdata = dev_get_platdata(&client->dev);
         if (pdata)
                 st->plat_data = *pdata;
@@ -720,8 +809,21 @@ static int inv_mpu_probe(struct i2c_client *client,
                 goto out_remove_trigger;
         }
 
+        st->mux_adapter = i2c_add_mux_adapter(client->adapter,
+                                              &client->dev,
+                                              indio_dev,
+                                              0, 0, 0,
+                                              inv_mpu6050_select_bypass,
+                                              inv_mpu6050_deselect_bypass);
+        if (!st->mux_adapter) {
+                result = -ENODEV;
+                goto out_unreg_device;
+        }
+
         return 0;
 
+out_unreg_device:
+        iio_device_unregister(indio_dev);
 out_remove_trigger:
         inv_mpu6050_remove_trigger(st);
 out_unreg_ring:
@@ -734,6 +836,7 @@ static int inv_mpu_remove(struct i2c_client *client)
         struct iio_dev *indio_dev = i2c_get_clientdata(client);
         struct inv_mpu6050_state *st = iio_priv(indio_dev);
 
+        i2c_del_mux_adapter(st->mux_adapter);
         iio_device_unregister(indio_dev);
         inv_mpu6050_remove_trigger(st);
         iio_triggered_buffer_cleanup(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 e7799315d4dc..aa837de57079 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_iio.h
@@ -54,6 +54,7 @@ struct inv_mpu6050_reg_map {
         u8 int_enable;
         u8 pwr_mgmt_1;
         u8 pwr_mgmt_2;
+        u8 int_pin_cfg;
 };
 
 /*device enum */
@@ -119,6 +120,8 @@ struct inv_mpu6050_state {
         enum   inv_devices chip_type;
         spinlock_t time_stamp_lock;
         struct i2c_client *client;
+        struct i2c_adapter *mux_adapter;
+        unsigned int powerup_count;
         struct inv_mpu6050_platform_data plat_data;
         DECLARE_KFIFO(timestamps, long long, TIMESTAMP_FIFO_SIZE);
 };
@@ -179,6 +182,9 @@ struct inv_mpu6050_state {
 /* 6 + 6 round up and plus 8 */
 #define INV_MPU6050_OUTPUT_DATA_SIZE         24
 
+#define INV_MPU6050_REG_INT_PIN_CFG     0x37
+#define INV_MPU6050_BIT_BYPASS_EN       0x2
+
 /* init parameters */
 #define INV_MPU6050_INIT_FIFO_RATE           50
 #define INV_MPU6050_TIME_STAMP_TOR           5