From f2eda34716fca575d066ac762b5034d660865ad6 Mon Sep 17 00:00:00 2001
From: Zorro Liu <lyx@rock-chips.com>
Date: Thu, 8 Sep 2016 17:30:33 +0800
Subject: [PATCH] driver, mpu: Increase the speed of wake up after hardware
power off
Change-Id: I191324c9d4c2e5c632c6de4a408f99174f019203
Signed-off-by: Zorro Liu <lyx@rock-chips.com>
---
drivers/staging/iio/imu/inv_mpu/inv_mpu_i2c.c | 8 ++-----
drivers/staging/iio/imu/inv_mpu/inv_mpu_iio.h | 1 +
.../staging/iio/imu/inv_mpu/inv_mpu_misc.c | 21 +++++++++++++++++++
drivers/staging/iio/imu/inv_mpu/inv_mpu_spi.c | 8 ++-----
4 files changed, 26 insertions(+), 12 deletions(-)
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))
--
2.34.1