input: misc: Fix LPA mode on MPU65xx

bug 1479946

Change-Id: I07d0b44568ac9670453ff056de0fa0f43e0fbf7b
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/403776
(cherry picked from commit 8767fa3db0b362519462de1e4080b82459f2a9e4)
Reviewed-on: http://git-master/r/408122
Reviewed-by: Automatic_Commit_Validation_User
Reviewed-by: Jordan Nien <jnien@nvidia.com>
Tested-by: Jordan Nien <jnien@nvidia.com>
Reviewed-by: Robert Collins <rcollins@nvidia.com>
Reviewed-by: Xiaohui Tao <xtao@nvidia.com>
Reviewed-by: Laxman Dewangan <ldewangan@nvidia.com>

3 files changed, 31 insertions, 19 deletions

diff --git a/drivers/input/misc/mpu/inv_gyro.c b/drivers/input/misc/mpu/inv_gyro.c
index e86d1e4cfd7d..e3202ed52244 100644
--- a/drivers/input/misc/mpu/inv_gyro.c
+++ b/drivers/input/misc/mpu/inv_gyro.c
@@ -46,7 +46,7 @@
 #include "inv_gyro.h"
 
 
-#define NVI_VERSION			(40)
+#define NVI_VERSION			(45)
 
 /* regulator names in order of powering on */
 static char *nvi_vregs[] = {
@@ -294,7 +294,7 @@ static int nvi_accel_config2_wr(struct inv_gyro_state_s *inf, u8 val)
 	int err = 0;
 
 	if (val != inf->hw.accl_config2) {
-		err = inv_i2c_single_write(inf, 0x1D, val);
+		err = inv_i2c_single_write(inf, REG_6500_ACCEL_CONFIG2, val);
 		if (!err) {
 			inf->hw.accl_config2 = val;
 			err = 1; /* flag change made */
@@ -930,7 +930,7 @@ static int nvi_nb_vreg(struct inv_gyro_state_s *st,
 	return NOTIFY_OK;
 }
 
-int nvi_pm_wr(struct inv_gyro_state_s *st,
+static int nvi_pm_wr(struct inv_gyro_state_s *st,
 	      u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lpa)
 {
 	s64 por_ns;
@@ -999,9 +999,17 @@ int nvi_pm_wr(struct inv_gyro_state_s *st,
 		break;
 
 	default: /* INV_MPU65XX */
-		ret_t |= nvi_lp_accel_odr_wr(st, lpa);
+		if (pwr_mgmt_1 & st->reg->cycle) {
+			ret_t |= nvi_lp_accel_odr_wr(st, lpa);
+			ret_t |= nvi_accel_config_wr(st, 0,
+			st->chip_config.accl_fsr, (BIT_FIFO_SIZE_1K
+						| BIT_ACCEL_FCHOCIE_B));
+		}
 		ret_t |= nvi_pwr_mgmt_2_wr(st, pwr_mgmt_2);
 		ret_t |= nvi_pwr_mgmt_1_wr(st, pwr_mgmt_1);
+		if (!(pwr_mgmt_1 & st->reg->cycle))
+			ret_t |= nvi_accel_config_wr(st, 0,
+			st->chip_config.accl_fsr, BIT_FIFO_SIZE_1K);
 		break;
 	}
 
@@ -1024,7 +1032,7 @@ static int nvi_reset(struct inv_gyro_state_s *inf,
  *      are for internal use.
  * @return int: returns 0 for success or error code
  */
-static int nvi_pm(struct inv_gyro_state_s *st, int pm_req)
+int nvi_pm(struct inv_gyro_state_s *st, int pm_req)
 {
 	bool irq;
 	u8 pwr_mgmt_1;
@@ -3929,7 +3937,7 @@ static ssize_t nvi_data_show(struct device *dev,
 	return -EINVAL;
 }
 
-#if DEBUG_SYSFS_INTERFACE
+#ifdef DEBUG_SYSFS_INTERFACE
 static ssize_t nvi_dbg_i2c_addr_store(struct device *dev,
 				      struct device_attribute *attr,
 				      const char *buf, size_t count)
@@ -4035,6 +4043,13 @@ static ssize_t nvi_dbg_dat_show(struct device *dev,
 		       __func__, dbg_i2c_addr, inf->dbg_reg, dbg_dat, err);
 	return ret;
 }
+
+static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWGRP,
+		   nvi_dbg_reg_show, nvi_dbg_reg_store);
+static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWGRP,
+		   nvi_dbg_dat_show, nvi_dbg_dat_store);
+static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP,
+		   nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
 #endif /* DEBUG_SYSFS_INTERFACE */
 
 static DEVICE_ATTR(gyro_enable, S_IRUGO | S_IWUSR | S_IWGRP,
@@ -4101,12 +4116,6 @@ static DEVICE_ATTR(self_test, S_IRUGO,
 		   inv_self_test_show, NULL);
 static DEVICE_ATTR(data, S_IRUGO | S_IWUSR | S_IWGRP,
 		   nvi_data_show, nvi_data_store);
-static DEVICE_ATTR(dbg_reg, S_IRUGO | S_IWUSR | S_IWGRP,
-		   nvi_dbg_reg_show, nvi_dbg_reg_store);
-static DEVICE_ATTR(dbg_dat, S_IRUGO | S_IWUSR | S_IWGRP,
-		   nvi_dbg_dat_show, nvi_dbg_dat_store);
-static DEVICE_ATTR(dbg_i2c_addr, S_IRUGO | S_IWUSR | S_IWGRP,
-		   nvi_dbg_i2c_addr_show, nvi_dbg_i2c_addr_store);
 static DEVICE_ATTR(reg_dump, S_IRUGO,
 		   inv_reg_dump_show, NULL);
 static DEVICE_ATTR(power_state, S_IRUGO | S_IWUSR | S_IWGRP,
@@ -4135,7 +4144,7 @@ static struct device_attribute *inv_attributes[] = {
 	&dev_attr_enable,
 	&dev_attr_power_state,
 	&dev_attr_key,
-#if DEBUG_SYSFS_INTERFACE
+#ifdef DEBUG_SYSFS_INTERFACE
 	&dev_attr_dbg_reg,
 	&dev_attr_dbg_dat,
 	&dev_attr_dbg_i2c_addr,
@@ -4495,7 +4504,7 @@ static int nvi_suspend(struct device *dev)
 	spin_unlock_irqrestore(&inf->time_stamp_lock, flags);
 	synchronize_irq(inf->i2c->irq);
 
-	flush_work_sync(&inf->work_struct);
+	flush_work(&inf->work_struct);
 
 	mutex_lock(&inf->mutex);
 	inf->suspend = true;
@@ -4551,7 +4560,7 @@ static void nvi_shutdown(struct i2c_client *client)
 	spin_unlock_irqrestore(&inf->time_stamp_lock, flags);
 	synchronize_irq(inf->i2c->irq);
 
-	flush_work_sync(&inf->work_struct);
+	flush_work(&inf->work_struct);
 
 	mutex_lock(&inf->mutex);
 	for (i = 0; i < AUX_PORT_SPECIAL; i++) {
diff --git a/drivers/input/misc/mpu/inv_gyro.h b/drivers/input/misc/mpu/inv_gyro.h
index 0f3ce3228288..5ccabb52bd9f 100644
--- a/drivers/input/misc/mpu/inv_gyro.h
+++ b/drivers/input/misc/mpu/inv_gyro.h
@@ -1,6 +1,6 @@
 /*
 * Copyright (C) 2012 Invensense, Inc.
-* Copyright (c) 2014, NVIDIA CORPORATION.  All rights reserved.
+* Copyright (c) 2013-2014, NVIDIA CORPORATION.  All rights reserved.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
@@ -502,6 +502,9 @@ struct inv_mpu_slave {
 #define REG_3500_OTP            (0x00)
 #define REG_AUX_VDDIO           (0x01)
 #define REG_ST_GCT_X            (0x0D)
+#define REG_6500_ACCEL_CONFIG2  (0x1D)
+#define BIT_ACCEL_FCHOCIE_B     (0x08)
+#define BIT_FIFO_SIZE_1K        (0x40)
 #define REG_6500_LP_ACCEL_ODR   (0x1E)
 #define REG_MOT_THR             (0x1F)
 #define REG_MOT_DUR             (0x20)
@@ -763,8 +766,7 @@ int nvi_user_ctrl_reset_wr(struct inv_gyro_state_s *inf, u8 val);
 int nvi_user_ctrl_en_wr(struct inv_gyro_state_s *inf, u8 val);
 int nvi_user_ctrl_en(struct inv_gyro_state_s *inf,
 		     bool fifo_enable, bool i2c_enable);
-int nvi_pm_wr(struct inv_gyro_state_s *inf,
-	      u8 pwr_mgmt_1, u8 pwr_mgmt_2, u8 lpa);
+int nvi_pm(struct inv_gyro_state_s *inf, int pm_req);
 int nvi_gyro_enable(struct inv_gyro_state_s *inf,
 		    unsigned char enable, unsigned char fifo_enable);
 int nvi_accl_enable(struct inv_gyro_state_s *inf,
diff --git a/drivers/input/misc/mpu/inv_gyro_misc.c b/drivers/input/misc/mpu/inv_gyro_misc.c
index 77a82b18ee66..ef7e41a1fd01 100644
--- a/drivers/input/misc/mpu/inv_gyro_misc.c
+++ b/drivers/input/misc/mpu/inv_gyro_misc.c
@@ -723,7 +723,7 @@ static int inv_do_test(struct inv_gyro_state_s *st, int self_test_flag,
 	has_accl = (st->chip_type != INV_ITG3500);
 	packet_size = 6 + 6 * has_accl;
 
-	result = nvi_pm_wr(st, INV_CLK_PLL, 0, 0);
+	result = nvi_pm(st, NVI_PM_ON_FULL);
 	if (result)
 		return result;
 
@@ -872,6 +872,7 @@ static void inv_recover_setting(struct inv_gyro_state_s *st)
 	st->chip_config.gyro_enable ^= 7;
 	st->chip_config.gyro_fifo_enable ^= 7;
 	nvi_gyro_enable(st, enable, fifo_enable);
+	nvi_pm(st, NVI_PM_AUTO);
 }
 
 /**