adding in suspend_again

Change-Id: I7981062e9bb5dc91f878acb1f6d099800c0a03fc

4 files changed, 84 insertions, 4 deletions

diff --git a/arch/arm/mach-omap2/pm.c b/arch/arm/mach-omap2/pm.c
index a6c441842cc..6bd5f3b9c7a 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>
 
@@ -239,14 +244,80 @@ static void omap_pm_finish(void)
 		omap_prcm_irq_complete();
 }
 
+bool olio_suspend_again(void);
 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  = olio_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;
+
+
+void susp_again_post_decision(bool should_suspend)
+{
+	int res = mutex_trylock(&susp_again_decision_mutex);
+	if(res) {
+		accel_should_suspend = should_suspend;
+		wake_up(&wq_susp_again);
+		mutex_unlock(&susp_again_decision_mutex);
+	}
+	else
+		pr_err("Unable to post decision, didn't get lock");
+}
+
+//Return True if going back to sleep
+bool olio_suspend_again(void) {
+	int res;
+	bool should_suspend = false;
+	int irqno_6d = 143; // FIXME query 143 from somewhere
+	int wake_was_6d = last_wakeup_reason_test(irqno_6d); 
+	if (!wake_was_6d) {
+		// Someone else woke us, so don't go back to suspend
+		return false;
+	}
+
+	/// gpio_set_value (163, 1); // FIXME: Raise UART3 ttyS2 CTS gpio163
+
+	/*Right now we don't need to thaw any threads, but this might be required
+	 * if extending functionality to other drivers */
+#ifdef SUSPEND_AGAIN_THAW_KTHREADS
+	thaw_kernel_threads();
+#endif
+	
+	// Wait for a decision of 6D driver
+	// We expect the driver to finish in 10ms, so 15 is safe.
+#define MAX_WAIT_TIME_SUSP_AGAIN msecs_to_jiffies(15)
+	res = wait_event_timeout(wq_susp_again, accel_should_suspend != NO_DECISION, MAX_WAIT_TIME_SUSP_AGAIN);
+	if(!res)
+		pr_warn("Suspend again was not acknowledged by driver\n");
+
+#ifdef SUSPEND_AGAIN_THAW_KTHREADS
+	freeze_kernel_threads();	
+#endif
+
+	if (accel_should_suspend != NO_DECISION) {
+		pr_info("Suspend again decision: %i", accel_should_suspend);
+		should_suspend =  accel_should_suspend;
+	}
+	//clear flags for next time
+	res = mutex_trylock(&susp_again_decision_mutex);
+	if(res) {
+		pr_info("susp again: clearing flag");
+		accel_should_suspend = NO_DECISION; // Not true or false
+		mutex_unlock(&susp_again_decision_mutex);
+	}
+	return should_suspend;
+}
+
 #endif /* CONFIG_SUSPEND */
 
 static void __init omap3_init_voltages(void)
@@ -290,6 +361,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_i2c.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
index 436f09e0060..a903a1ebebf 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_i2c.c
@@ -128,7 +128,8 @@ static int st_lsm6ds3_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops st_lsm6ds3_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(st_lsm6ds3_suspend, st_lsm6ds3_resume)
+	.suspend_noirq = st_lsm6ds3_suspend,
+	.resume_noirq = st_lsm6ds3_resume,
 };
 
 #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..45a1d213423 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"
 
@@ -193,15 +194,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;
 			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..07a0e8ad54c 100644
--- a/include/linux/suspend.h
+++ b/include/linux/suspend.h
@@ -364,6 +364,10 @@ 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);
 
+/* arch/arm/mach-omap2/pm.c */
+extern void susp_again_post_decision(bool should_suspend);
+
+
 static inline void lock_system_sleep(void)
 {
 	current->flags |= PF_FREEZER_SKIP;