From: Zorro Liu <lyx@rock-chips.com>
Date: Thu, 8 Sep 2016 09:30:33 +0000 (+0800)
Subject: driver, mpu: Increase the speed of wake up after hardware power off
X-Git-Tag: firefly_0821_release~1565
X-Git-Url: http://demsky.eecs.uci.edu/git/?a=commitdiff_plain;h=f2eda34716fca575d066ac762b5034d660865ad6;p=firefly-linux-kernel-4.4.55.git

driver, mpu: Increase the speed of wake up after hardware power off

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

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 38bd151a3e6f..f1b864324d69 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c
@@ -630,8 +630,8 @@ static int inv_mpu_resume(struct device *dev)
 
 	pr_debug("%s inv_mpu_resume\n", st->hw->name);
 
+	mutex_lock(&indio_dev->mlock);
 	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) {
@@ -655,12 +655,10 @@ static int inv_mpu_resume(struct device *dev)
 			pr_err("%s, set user_ctrl failed\n", __func__);
 			goto rw_err;
 		}
-		inv_reg_recover(st);
-		mutex_unlock(&indio_dev->mlock);
+		inv_resume_recover_setting(st);
 	} else {
 		result = st->set_power_state(st, true);
 	}
-	return result;
 
 rw_err:
 	mutex_unlock(&indio_dev->mlock);
@@ -676,8 +674,6 @@ static int inv_mpu_suspend(struct device *dev)
 	pr_debug("%s inv_mpu_suspend\n", st->hw->name);
 
 	mutex_lock(&indio_dev->mlock);
-	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 aa3335320bcc..99f3ba689355 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h
@@ -893,6 +893,7 @@ int inv_do_test(struct inv_mpu_iio_s *st, int self_test_flag,
 		int *gyro_result, int *accl_result);
 int inv_hw_self_test(struct inv_mpu_iio_s *st);
 void inv_recover_setting(struct inv_mpu_iio_s *st);
+void inv_resume_recover_setting(struct inv_mpu_iio_s *st);
 int inv_power_up_self_test(struct inv_mpu_iio_s *st);
 s64 get_time_ns(void);
 int write_be32_key_to_mem(struct inv_mpu_iio_s *st,
diff --git a/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c b/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
index 93cdba6acdf1..3f285e4f33a0 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_misc.c
@@ -968,6 +968,27 @@ void inv_recover_setting(struct inv_mpu_iio_s *st)
 	st->set_power_state(st, false);
 }
 
+/*
+ *  inv_resume_recover_setting() recover the old settings after from hw powerdown
+ */
+void inv_resume_recover_setting(struct inv_mpu_iio_s *st)
+{
+	struct inv_reg_map_s *reg;
+	int data;
+
+	reg = &st->reg;
+	inv_plat_single_write(st, reg->gyro_config,
+				st->chip_config.fsr << GYRO_CONFIG_FSR_SHIFT);
+	inv_plat_single_write(st, reg->lpf, st->chip_config.lpf);
+	data = ONE_K_HZ / st->chip_config.new_fifo_rate - 1;
+	inv_plat_single_write(st, reg->sample_rate_div, data);
+	if (INV_ITG3500 != st->chip_type) {
+		inv_plat_single_write(st, reg->accl_config,
+					(st->chip_config.accl_fs <<
+					ACCL_CONFIG_FSR_SHIFT));
+	}
+}
+
 static int inv_check_compass_self_test(struct inv_mpu_iio_s *st)
 {
 	int result;
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 21c80cdeb3f6..82a874d895c0 100644
--- a/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c
+++ b/drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c
@@ -476,8 +476,8 @@ static int inv_mpu_resume(struct device *dev)
 
 	pr_debug("%s inv_mpu_resume\n", st->hw->name);
 
+	mutex_lock(&indio_dev->mlock);
 	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) {
@@ -501,12 +501,10 @@ static int inv_mpu_resume(struct device *dev)
 			pr_err("%s, set user_ctrl failed\n", __func__);
 			goto rw_err;
 		}
-		inv_reg_recover(st);
-		mutex_unlock(&indio_dev->mlock);
+		inv_resume_recover_setting(st);
 	} else {
 		result = st->set_power_state(st, true);
 	}
-	return result;
 
 rw_err:
 	mutex_unlock(&indio_dev->mlock);
@@ -522,8 +520,6 @@ static int inv_mpu_suspend(struct device *dev)
 	pr_debug("%s inv_mpu_suspend\n", st->hw->name);
 
 	mutex_lock(&indio_dev->mlock);
-	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))