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

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

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;

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))

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);

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))