adding in suspend_again

7 files changed, 231 insertions, 11 deletions

diff --git a/arch/arm/mach-omap2/pm.c b/arch/arm/mach-omap2/pm.c
index a6c441842cc..a119cbe9e7f 100644
--- a/arch/arm/mach-omap2/pm.c
+++ b/arch/arm/mach-omap2/pm.c
@@ -18,6 +18,11 @@
 #include <linux/suspend.h>
 #include <linux/cpu.h>
 #include <linux/of_platform.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/wakeup_reason.h>
+#include <linux/printk.h>
 
 #include <asm/system_misc.h>
 
@@ -38,6 +43,9 @@
  * suspend work
  */
 int (*omap_pm_suspend)(void);
+#ifdef CONFIG_PM_SUSPEND_AGAIN_OPS
+const static struct suspend_again_ops *suspend_again_ops;
+#endif
 
 #ifdef CONFIG_PM
 /**
@@ -239,14 +247,38 @@ static void omap_pm_finish(void)
 		omap_prcm_irq_complete();
 }
 
+#ifdef CONFIG_PM_SUSPEND_AGAIN_OPS
+void pm_register_suspend_again_ops(struct suspend_again_ops const *ops) {
+	suspend_again_ops = ops;
+}
+
+//Return True if going back to sleep
+bool omap_suspend_again(void) {
+	pr_info("Suspend again decision.");
+	if(suspend_again_ops && suspend_again_ops->suspend_again_check) {
+		return suspend_again_ops->suspend_again_check(suspend_again_ops->data_ptr);
+	}
+
+	return false;
+}
+#endif
+
 static const struct platform_suspend_ops omap_pm_ops = {
 	.begin		= omap_pm_begin,
 	.end		= omap_pm_end,
 	.enter		= omap_pm_enter,
 	.finish		= omap_pm_finish,
 	.valid		= suspend_valid_only_mem,
+	.suspend_again  = omap_suspend_again
 };
 
+
+
+DECLARE_WAIT_QUEUE_HEAD(wq_susp_again);
+DEFINE_MUTEX(susp_again_decision_mutex);
+#define NO_DECISION 3
+static int accel_should_suspend = NO_DECISION;
+
 #endif /* CONFIG_SUSPEND */
 
 static void __init omap3_init_voltages(void)
@@ -290,6 +322,7 @@ static int __init omap2_common_pm_init(void)
 	/* if (!of_have_populated_dt()) OLIO TEST */
 	omap2_init_processor_devices();
 
+
 	omap_pm_if_init();
 
 	return 0;
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index 25443f64c75..ce2f9198d73 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -267,6 +267,8 @@ int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
 				const struct iio_trigger_ops *trigger_ops);
 
 void st_lsm6ds3_deallocate_triggers(struct lsm6ds3_data *cdata);
+void st_lsm6ds3_set_wake_triggers(struct lsm6ds3_data *cdata);
+void st_lsm6ds3_clear_wake_triggers(struct lsm6ds3_data *cdata);
 
 #else /* CONFIG_IIO_TRIGGER */
 static inline int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
@@ -283,11 +285,21 @@ static inline void st_lsm6ds3_flush_works()
 {
 	return;
 }
+static inline void st_lsm6ds3_set_wake_triggers(struct lsm6ds3_data *cdata)
+{
+	return;
+}
+static inline void st_lsm6ds3_clear_wake_triggers(struct lsm6ds3_data *cdata)
+{
+	return;
+}
 #endif /* CONFIG_IIO_TRIGGER */
 
 #ifdef CONFIG_PM
 int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata);
 int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_common_suspend_noirq(struct lsm6ds3_data *cdata);
+int st_lsm6ds3_common_resume_noirq(struct lsm6ds3_data *cdata);
 #endif /* CONFIG_PM */
 
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index 766ebb42da0..cf161976705 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -25,6 +25,7 @@
 #include <linux/iio/buffer.h>
 #include <linux/iio/events.h>
 #include <linux/reboot.h>
+#include <linux/suspend.h>
 #include <asm/unaligned.h>
 #include <linux/wakelock.h>
 #include <linux/iio/common/st_sensors.h>
@@ -2388,6 +2389,22 @@ int st_lsm6ds3_reset(struct lsm6ds3_data *cdata, bool hard_reset) {
 	return 0;
 }
 
+static bool st_lsm6ds3_suspend_again_check(void *data) {
+	struct lsm6ds3_data *cdata = data;
+	if(cdata->last_wakeup_source == LSM6DS3_WAKEUP_OTHER) {
+		dev_info(cdata->dev, "LSM6DS3 suspend again TRUE");
+		return true;
+	}
+	else {
+		dev_info(cdata->dev, "LSM6DS3 suspend again check FALSE");
+		return false;
+	}
+}
+
+static struct suspend_again_ops st_lsm6ds3_suspend_again_ops = {
+	.suspend_again_check = st_lsm6ds3_suspend_again_check
+};
+
 int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 {
 	int i, n, err;
@@ -2507,6 +2524,9 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	if (err < 0)
 		goto iio_device_unregister_and_trigger_deallocate;
 
+	st_lsm6ds3_suspend_again_ops.data_ptr = cdata;
+	pm_register_suspend_again_ops(&st_lsm6ds3_suspend_again_ops);
+
 	device_init_wakeup(cdata->dev, true);
 
 	/* 
@@ -2613,6 +2633,20 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 	dev_info(cdata->dev, "before suspend int1: %x err:%i", reg_value, err);
 
 	err = cdata->tf->read(cdata,
+				0xe, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend int2: %x err:%i", reg_value, err);
+
+	err = cdata->tf->read(cdata,
+				0x5e, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend md1: %x err:%i", reg_value, err);
+
+	err = cdata->tf->read(cdata,
+				0x5f, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend md2: %x err:%i", reg_value, err);
+
+
+
+	err = cdata->tf->read(cdata,
 			0x58, 1, &reg_value, true);
 	dev_info(cdata->dev, "before suspend 0x58 tap enable: %x err:%i", reg_value, err);
 	err = cdata->tf->read(cdata,
@@ -2662,6 +2696,7 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 	struct lsm6ds3_sensor_data *sdata;
 	u8 reg_value;
 
+	dev_info(cdata->dev, "Resume.");
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
 		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
 			continue;
@@ -2699,6 +2734,23 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 	return 0;
 }
 EXPORT_SYMBOL(st_lsm6ds3_common_resume);
+
+int st_lsm6ds3_common_suspend_noirq(struct lsm6ds3_data *cdata)
+{
+	dev_info(cdata->dev, "Suspend noirq.");
+	st_lsm6ds3_clear_wake_triggers(cdata);
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_suspend_noirq);
+
+int st_lsm6ds3_common_resume_noirq(struct lsm6ds3_data *cdata)
+{
+	dev_info(cdata->dev, "Resume noirq.");
+	st_lsm6ds3_set_wake_triggers(cdata);
+	return 0;
+}
+EXPORT_SYMBOL(st_lsm6ds3_common_resume_noirq);
+
 #endif /* CONFIG_PM */
 
 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
index 436f09e0060..11e9fc96e81 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
@@ -127,8 +127,25 @@ static int st_lsm6ds3_resume(struct device *dev)
 	return st_lsm6ds3_common_resume(cdata);
 }
 
+static int st_lsm6ds3_suspend_noirq(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
+
+	return st_lsm6ds3_common_suspend_noirq(cdata);
+}
+
+static int st_lsm6ds3_resume_noirq(struct device *dev)
+{
+	struct lsm6ds3_data *cdata = i2c_get_clientdata(to_i2c_client(dev));
+
+	return st_lsm6ds3_common_resume_noirq(cdata);
+}
+
 static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
+	.suspend = st_lsm6ds3_suspend,
+	.resume = st_lsm6ds3_resume,
+	.suspend_noirq = st_lsm6ds3_suspend_noirq,
+	.resume_noirq = st_lsm6ds3_resume_noirq
 };
 
 #define ST_LSM6DS3_PM_OPS		(&st_lsm6ds3_pm_ops)
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index a97a5da517c..6fe42ec20fb 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -19,6 +19,7 @@
 #include <linux/iio/events.h>
 #include <linux/wakelock.h>
 #include <linux/wakeup_reason.h>
+#include <linux/suspend.h>
 
 #include "st_lsm6ds3.h"
 
@@ -56,13 +57,82 @@ irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
 	cdata->timestamp = timespec_to_ns(&ts);
 	cdata->accel_timestamp  = cdata->timestamp;
 	cdata->irq_timestamp = ts.tv_sec;
-	queue_work(st_lsm6ds3_wq, &cdata->data_work);
 
-	disable_irq_nosync(irq);
+	if(cdata->first_irq_from_resume && cdata->last_wakeup_source == LSM6DS3_WAKEUP_OTHER) {
+		dev_info(cdata->dev, "No valid wakeup. Ignoring IRQ.");
+		cdata->first_irq_from_resume = 0;
+	} else {
+		dev_info(cdata->dev, "IRQ handled. Scheduling work.");
+		queue_work(st_lsm6ds3_wq, &cdata->data_work);
+		disable_irq_nosync(irq);
+	}
 
 	return IRQ_HANDLED;
 }
 
+void st_lsm6ds3_set_wake_triggers(struct lsm6ds3_data* cdata)
+{
+	u8 d6d_src_reg = 0x00, tap_src_reg = 0x00, func_src_reg = 0x00, wake_up_src_reg = 0x00;
+	u8 d6d_event = 0;
+	u8 tap_event = 0;
+	int wake_irq;
+	int ignore_event = 0;
+
+	wake_irq = last_wakeup_reason_test(cdata->irq);
+	// if not the proper wakeup irq, don't set wake trigger
+	if(!wake_irq) {
+		return;
+	}
+
+	cdata->tf->read(cdata, ST_LSM6DS3_6D_SRC_ADDR, 1, &d6d_src_reg, true);
+	cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 1, &tap_src_reg, true);
+	cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &func_src_reg, true);
+	cdata->tf->read(cdata, 0x1b, 1, &wake_up_src_reg, true);
+	dev_info(cdata->dev, "ST irq start: src_value: 0x%x, 6d: 0x%x, tap: 0x%x, wakeup: 0x%x",
+			func_src_reg, d6d_src_reg, tap_src_reg, wake_up_src_reg);
+
+	if(d6d_src_reg & ST_LSM6DS3_6D_SRC_DETECTED_MASK){
+		dev_info(cdata->dev, "D6D IRQ val:0x%x mask:0x%x",
+			 SIXD_MASK_VALID_BITS & d6d_src_reg, cdata->sixd_mask);
+
+		if(cdata->sixd_mask & d6d_src_reg){
+			d6d_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_6D;
+		} else {
+			ignore_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			dev_info(cdata->dev, "ignoring 6d interrupt, wrong axis. mask: 0x%x",
+				 cdata->sixd_mask);
+		}
+	}
+
+	if(tap_src_reg & ST_LSM6DS3_TAP_SRC_DETECTED_MASK){
+		dev_info(cdata->dev, "TAP IRQ");
+		if(tap_src_reg & (ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK |
+				  ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK)){
+			tap_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			dev_info(cdata->dev, "Valid Tap");
+		} else {
+			dev_info(cdata->dev, "Ignoring tap");
+		}
+	}
+
+	if(cdata->first_irq_from_resume && wake_irq){
+		if(!d6d_event && !tap_event && !ignore_event){
+			dev_info(cdata->dev, "No event from first resume, assuming lost TAP");
+			tap_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			dev_info(cdata->dev, "Valid Tap from sleep");
+		}
+	}
+}
+
+void st_lsm6ds3_clear_wake_triggers(struct lsm6ds3_data* cdata)
+{
+	cdata->first_irq_from_resume = 1;
+	cdata->last_wakeup_source = 0;
+}
 #ifdef WAKE_STATS_DEBUG_INFO
 static int wakeup_irq_count = 0;
 static int wakeup_irq_stayawake_count = 0;
@@ -143,7 +213,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 {
 	struct lsm6ds3_data *cdata;
 	u8 d6d_src_reg, tap_src_reg;
-	u8 src_value = 0x00, src_fifo = 0x00;
+	u8 src_value = 0x00, src_fifo = 0x00, wake_up_src_reg;
 	u8 d6d_event = 0;
 	u8 tap_event = 0;
 	int wake_irq;
@@ -157,11 +227,13 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	cdata->tf->read(cdata, ST_LSM6DS3_6D_SRC_ADDR, 1, &d6d_src_reg, true);
 	cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 1, &tap_src_reg, true);
 	cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &src_value, true);
+	cdata->tf->read(cdata, 0x1b, 1, &wake_up_src_reg, true);
 	cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1,
 							&src_fifo, true);
 
-	dev_dbg(cdata->dev, "ST irq start :src_value, 6d, tap:%x %x %x",
-		src_value, d6d_src_reg, tap_src_reg);
+	dev_info(cdata->dev, "IRQ scheduled work running.");
+	dev_info(cdata->dev, "ST irq start: src_value: 0x%x, 6d: 0x%x, tap: 0x%x, wakeup: 0x%x",
+		src_value, d6d_src_reg, tap_src_reg, wake_up_src_reg);
 
 	if(d6d_src_reg & ST_LSM6DS3_6D_SRC_DETECTED_MASK){
 #ifdef WAKE_STATS_DEBUG_INFO
@@ -185,7 +257,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 		if(tap_src_reg & (ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK |
 				  ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK)){
 			tap_event = 1;
-			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
 			dev_info(cdata->dev, "Valid Tap");
 		} else {
 			dev_info(cdata->dev, "Ignoring tap");
@@ -193,15 +265,17 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	}
 	
 	if(cdata->first_irq_from_resume && wake_irq){
-#ifdef WAKE_STATS_DEBUG_INFO
-		wakeup_irq_count++;
-#endif 
 		if(!d6d_event && !tap_event && !ignore_event){
 			dev_info(cdata->dev, "No event from first resume, assuming lost TAP");
 			tap_event = 1;
-			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
 			dev_info(cdata->dev, "Valid Tap from sleep");
 		}	
+		pr_info("posting susp again decision: %i", ignore_event);
+		//susp_again_post_decision(ignore_event);
+#ifdef WAKE_STATS_DEBUG_INFO
+		wakeup_irq_count++;
+#endif 
 	}
 
 	if(!ignore_event && (tap_event || d6d_event) && cdata->first_irq_from_resume){
diff --git a/include/linux/suspend.h b/include/linux/suspend.h
index d4e3f16d5e8..9c26477f9d0 100644
--- a/include/linux/suspend.h
+++ b/include/linux/suspend.h
@@ -364,6 +364,31 @@ extern bool pm_get_wakeup_count(unsigned int *count, bool block);
 extern bool pm_save_wakeup_count(unsigned int count);
 extern void pm_wakep_autosleep_enabled(bool set);
 
+
+/**
+ * struct suspend_again_ops - suspend again support
+ *
+ * Any driver can register suspend_again_ops,
+ * so that they can respond to a suspend_again() check
+ *
+ * suspend_again() is registered at the platform level, so every platform must implement the register function
+ *
+ * @data_ptr: a pointer to data needed by the suspend again check
+ *
+ * @suspend_again_check: Function called by the platform suspend_again() function to check for suspend_again
+ */
+struct suspend_again_ops {
+	void *data_ptr;
+	bool (*suspend_again_check)(void *data);
+};
+
+
+#ifdef CONFIG_PM_SUSPEND_AGAIN_OPS
+extern void pm_register_suspend_again_ops(struct suspend_again_ops const *ops);
+#else
+static inline void pm_register_suspend_again_ops(struct suspend_again_ops const *ops) {}
+#endif
+
 static inline void lock_system_sleep(void)
 {
 	current->flags |= PF_FREEZER_SKIP;
diff --git a/kernel/power/Kconfig b/kernel/power/Kconfig
index 1c7f33b6e0f..b8f3ced65ce 100644
--- a/kernel/power/Kconfig
+++ b/kernel/power/Kconfig
@@ -296,3 +296,10 @@ config HAS_AMBIENTMODE
 	---help---
 	  Ambient mode should not turn off/on display/back-light/etc.
 	  at suspend/resume time
+	  
+config PM_SUSPEND_AGAIN_OPS
+	bool "Suspend again ops"
+	def_bool n
+	depends on PM || SUSPEND
+	---help---
+		Select if the platform supports suspend again ops