driver, inv_mpu: store register suspend and recover register resume, if hardware...

Change-Id: I758be101acfd7a3a756c75bd4f13542daef850d1
Signed-off-by: Zorro Liu <lyx@rock-chips.com>

diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
index 58dcb08236ec96610ea2156b25fc3a9971f2e02a..471317f7b59c86a9aa9995e1af54ce0e3f53ece6 100644 (file)
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_core.c
@@ -77,6 +77,57 @@ static const struct inv_hw_s hw_info[INV_NUM_PARTS] = {
        {128, "MPU6515"},
 };
 
+/* define INV_REG_NUM_MAX the max register unmbers of all sensor type */
+#define INV_REG_NUM_MAX 128
+static u8 inv_reg_data[128] = {0};
+/**
+ *  inv_reg_store() - Register store for hw poweroff.
+ */
+int inv_reg_store(struct inv_mpu_iio_s *st)
+{
+       int ii;
+       char data;
+
+       if (!st->chip_config.enable)
+               st->set_power_state(st, true);
+       for (ii = 0; ii < st->hw->num_reg; ii++) {
+               /* don't read fifo r/w register */
+               if (ii == st->reg.fifo_r_w)
+                       data = 0;
+               else
+                       inv_plat_read(st, ii, 1, &data);
+               inv_reg_data[ii] = data;
+       }
+       if (!st->chip_config.enable)
+               st->set_power_state(st, false);
+
+       return 0;
+}
+
+/**
+ *  inv_reg_recover() - Register recover for hw poweroff.
+ */
+int inv_reg_recover(struct inv_mpu_iio_s *st)
+{
+       int ii;
+       char data;
+
+       if (!st->chip_config.enable)
+               st->set_power_state(st, true);
+       for (ii = 0; ii < st->hw->num_reg; ii++) {
+               data = inv_reg_data[ii];
+               /* don't write fifo r/w register */
+               if (ii == st->reg.fifo_r_w)
+                       continue;
+               else
+                       inv_plat_single_write(st, ii, data);
+       }
+       if (!st->chip_config.enable)
+               st->set_power_state(st, false);
+
+       return 0;
+}
+
 static void inv_setup_reg(struct inv_reg_map_s *reg)
 {
        reg->sample_rate_div    = REG_SAMPLE_RATE_DIV;

diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
index 95232da1b80385fdb85c59c5748619462e75e890..38bd151a3e6f90b5e7f1d914c7ad82eb6ad5b874 100644 (file)
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
@@ -342,10 +342,13 @@ static int of_inv_parse_platform_data(struct i2c_client *client,
        int orig_x, orig_y, orig_z;
        int i;
        struct device_node *np = client->dev.of_node;
+       struct iio_dev *indio_dev = i2c_get_clientdata(client);
+       struct inv_mpu_iio_s *st = iio_priv(indio_dev);
        unsigned long irq_flags;
        int irq_pin;
        int gpio_pin;
        int debug;
+       int hw_pwoff;
 
        gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags);
        gpio_request(gpio_pin, "mpu6500");
@@ -417,6 +420,12 @@ static int of_inv_parse_platform_data(struct i2c_client *client,
                                mpu_data.orientation[i] = -1;
        }
 
+       ret = of_property_read_u32(np, "support-hw-poweroff", &hw_pwoff);
+       if (ret != 0) {
+               st->support_hw_poweroff = 0;
+       }
+       st->support_hw_poweroff = hw_pwoff;
+
        ret = of_property_read_u32(np, "mpu-debug", &debug);
        if (ret != 0) {
                dev_err(&client->dev, "get mpu-debug error\n");
@@ -465,6 +474,7 @@ static int inv_mpu_probe(struct i2c_client *client,
        st->client = client;
        st->sl_handle = client->adapter;
        st->i2c_addr = client->addr;
+       i2c_set_clientdata(client, indio_dev);
        if (client->dev.of_node) {
                result = of_inv_parse_platform_data(client, &st->plat_data);
                if (result)
@@ -502,7 +512,6 @@ static int inv_mpu_probe(struct i2c_client *client,
        }
 
        /* Make state variables available to all _show and _store functions. */
-       i2c_set_clientdata(client, indio_dev);
        indio_dev->dev.parent = &client->dev;
        if (!strcmp(id->name, "mpu6xxx"))
                indio_dev->name = st->name;
@@ -615,21 +624,60 @@ static int inv_mpu_remove(struct i2c_client *client)
 #ifdef CONFIG_PM
 static int inv_mpu_resume(struct device *dev)
 {
-       struct inv_mpu_iio_s *st =
-                       iio_priv(i2c_get_clientdata(to_i2c_client(dev)));
+       struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+       struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+       int result;
+
        pr_debug("%s inv_mpu_resume\n", st->hw->name);
-       return st->set_power_state(st, true);
+
+       if (st->support_hw_poweroff) {
+               mutex_lock(&indio_dev->mlock);
+               /* reset to make sure previous state are not there */
+               result = inv_plat_single_write(st, st->reg.pwr_mgmt_1, BIT_H_RESET);
+               if (result) {
+                       pr_err("%s, reset failed\n", __func__);
+                       goto rw_err;
+               }
+               msleep(POWER_UP_TIME);
+               /* toggle power state */
+               result = st->set_power_state(st, false);
+               if (result) {
+                       pr_err("%s, set_power_state false failed\n", __func__);
+                       goto rw_err;
+               }
+               result = st->set_power_state(st, true);
+               if (result) {
+                       pr_err("%s, set_power_state true failed\n", __func__);
+                       goto rw_err;
+               }
+               result = inv_plat_single_write(st, st->reg.user_ctrl, st->i2c_dis);
+               if (result) {
+                       pr_err("%s, set user_ctrl failed\n", __func__);
+                       goto rw_err;
+               }
+               inv_reg_recover(st);
+               mutex_unlock(&indio_dev->mlock);
+       } else {
+               result = st->set_power_state(st, true);
+       }
+       return result;
+
+rw_err:
+       mutex_unlock(&indio_dev->mlock);
+       return result;
 }
 
 static int inv_mpu_suspend(struct device *dev)
 {
        struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
        struct inv_mpu_iio_s *st = iio_priv(indio_dev);
-       int result;
+       int result = 0;
 
        pr_debug("%s inv_mpu_suspend\n", st->hw->name);
+
        mutex_lock(&indio_dev->mlock);
-       result = 0;
+       if (st->support_hw_poweroff)
+               inv_reg_store(st);
        if ((!st->chip_config.dmp_on) ||
                (!st->chip_config.enable) ||
                (!st->chip_config.dmp_event_int_on))

diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
index 0722c6397ab01bfbaeca6ca765e798beb352b975..aa3335320bccb39a4116d35a014b5369fec685c4 100644 (file)
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
@@ -365,6 +365,7 @@ struct inv_mpu_iio_s {
        signed short hid_temperature;
        u64 hid_timestamp;
        int use_hid;
+       int support_hw_poweroff;
        int accel_bias[3];
        int gyro_bias[3];
        short raw_gyro[3];
@@ -859,7 +860,8 @@ ssize_t inv_dmp_firmware_read(struct file *filp,
                                struct kobject *kobj,
                                struct bin_attribute *bin_attr,
                                char *buf, loff_t off, size_t count);
-
+int inv_reg_store(struct inv_mpu_iio_s *st);
+int inv_reg_recover(struct inv_mpu_iio_s *st);
 int inv_mpu_configure_ring(struct iio_dev *indio_dev);
 int inv_mpu_probe_trigger(struct iio_dev *indio_dev);
 void inv_mpu_unconfigure_ring(struct iio_dev *indio_dev);

diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c
index e95802f0be406ed021708c9b02bd24d4c52d53ca..21c80cdeb3f6e0aba6d4ba1b96b290940c987b32 100644 (file)
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c
@@ -190,11 +190,14 @@ static int of_inv_parse_platform_data(struct spi_device *spi,
        u32 orientation[9];
        int orig_x, orig_y, orig_z;
        int i;
+       struct iio_dev *indio_dev = spi_get_drvdata(spi);
+       struct inv_mpu_iio_s *st = iio_priv(indio_dev);
        struct device_node *np = spi->dev.of_node;
        unsigned long irq_flags;
        int irq_pin;
        int gpio_pin;
        int debug;
+       int hw_pwoff;
 
        gpio_pin = of_get_named_gpio_flags(np, "irq-gpio", 0, (enum of_gpio_flags *)&irq_flags);
        gpio_request(gpio_pin, "mpu6500");
@@ -264,6 +267,12 @@ static int of_inv_parse_platform_data(struct spi_device *spi,
                                mpu_data.orientation[i] = -1;
        }
 
+       ret = of_property_read_u32(np, "support-hw-poweroff", &hw_pwoff);
+       if (ret != 0) {
+               st->support_hw_poweroff = 0;
+       }
+       st->support_hw_poweroff = hw_pwoff;
+
        ret = of_property_read_u32(np, "mpu-debug", &debug);
        if (ret != 0) {
                dev_err(&spi->dev, "get mpu-debug error\n");
@@ -312,6 +321,7 @@ static int inv_mpu_probe(struct spi_device *spi)
                goto out_no_free;
        }
        st = iio_priv(indio_dev);
+       spi_set_drvdata(spi, indio_dev);
        if (spi->dev.of_node) {
                result = of_inv_parse_platform_data(spi, &st->plat_data);
                if (result)
@@ -323,7 +333,6 @@ static int inv_mpu_probe(struct spi_device *spi)
                        *(struct mpu_platform_data *)dev_get_platdata(&spi->dev);
 
        /* Make state variables available to all _show and _store functions. */
-       spi_set_drvdata(spi, indio_dev);
        indio_dev->dev.parent = &spi->dev;
        st->dev = &spi->dev;
        st->irq = spi->irq;
@@ -461,21 +470,60 @@ static int inv_mpu_remove(struct spi_device *spi)
 #ifdef CONFIG_PM
 static int inv_mpu_resume(struct device *dev)
 {
-       struct inv_mpu_iio_s *st =
-                       iio_priv(spi_get_drvdata(to_spi_device(dev)));
+       struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev));
+       struct inv_mpu_iio_s *st = iio_priv(indio_dev);
+       int result;
+
        pr_debug("%s inv_mpu_resume\n", st->hw->name);
-       return st->set_power_state(st, true);
+
+       if (st->support_hw_poweroff) {
+               mutex_lock(&indio_dev->mlock);
+               /* reset to make sure previous state are not there */
+               result = inv_plat_single_write(st, st->reg.pwr_mgmt_1, BIT_H_RESET);
+               if (result) {
+                       pr_err("%s, reset failed\n", __func__);
+                       goto rw_err;
+               }
+               msleep(POWER_UP_TIME);
+               /* toggle power state */
+               result = st->set_power_state(st, false);
+               if (result) {
+                       pr_err("%s, set_power_state false failed\n", __func__);
+                       goto rw_err;
+               }
+               result = st->set_power_state(st, true);
+               if (result) {
+                       pr_err("%s, set_power_state true failed\n", __func__);
+                       goto rw_err;
+               }
+               result = inv_plat_single_write(st, st->reg.user_ctrl, st->i2c_dis);
+               if (result) {
+                       pr_err("%s, set user_ctrl failed\n", __func__);
+                       goto rw_err;
+               }
+               inv_reg_recover(st);
+               mutex_unlock(&indio_dev->mlock);
+       } else {
+               result = st->set_power_state(st, true);
+       }
+       return result;
+
+rw_err:
+       mutex_unlock(&indio_dev->mlock);
+       return result;
 }
 
 static int inv_mpu_suspend(struct device *dev)
 {
        struct iio_dev *indio_dev = spi_get_drvdata(to_spi_device(dev));
        struct inv_mpu_iio_s *st = iio_priv(indio_dev);
-       int result;
+       int result = 0;
 
        pr_debug("%s inv_mpu_suspend\n", st->hw->name);
+
        mutex_lock(&indio_dev->mlock);
-       result = 0;
+       if (st->support_hw_poweroff)
+               inv_reg_store(st);
        if ((!st->chip_config.dmp_on) ||
                (!st->chip_config.enable) ||
                (!st->chip_config.dmp_event_int_on))