power: suspend: Add sleep timer and timeout handler

diff --git a/MAINTAINERS b/MAINTAINERS
index 867157311dc8..8ae91f5ff3ff 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -7787,6 +7787,7 @@  F:	drivers/base/power/
 F:	include/linux/freezer.h
 F:	include/linux/pm.h
 F:	include/linux/suspend.h
+F:	include/linux/suspend_timer.h
 F:	kernel/power/
 
 HID CORE LAYER
@@ -16629,6 +16630,7 @@  F:	drivers/base/power/
 F:	include/linux/freezer.h
 F:	include/linux/pm.h
 F:	include/linux/suspend.h
+F:	include/linux/suspend_timer.h
 F:	kernel/power/
 
 SVGA HANDLING
diff --git a/include/linux/console.h b/include/linux/console.h
index 0670d3491e0e..5436d8dc600f 100644
--- a/include/linux/console.h
+++ b/include/linux/console.h
@@ -192,6 +192,7 @@  static inline void console_sysfs_notify(void)
 { }
 #endif
 extern bool console_suspend_enabled;
+extern int console_is_suspended(void);
 
 /* Suspend and resume console messages over PM events */
 extern void suspend_console(void);
diff --git a/include/linux/suspend_timer.h b/include/linux/suspend_timer.h
new file mode 100644
index 000000000000..7e4c9e31bf09
--- /dev/null
+++ b/include/linux/suspend_timer.h
@@ -0,0 +1,90 @@ 
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef _LINUX_SLEEP_TIMER_H
+#define _LINUX_SLEEP_TIMER_H
+
+#include <linux/sched/debug.h>
+
+#ifdef CONFIG_PM_SLEEP_MONITOR
+struct sleep_timer {
+	struct task_struct	*tsk;
+	struct timer_list	timer;
+};
+
+#define DECLARE_SLEEP_TIMER(st) \
+	struct sleep_timer st
+
+/**
+ * init_sleep_timer - Initialize sleep timer.
+ * @st: Sleep timer to initialize.
+ * @func: Sleep timer timeout handler.
+ */
+static void init_sleep_timer(struct sleep_timer *st, void (*func))
+{
+	struct timer_list *timer = &st->timer;
+
+	timer_setup(timer, func, 0);
+}
+
+/**
+ * start_sleep_timer - Enable sleep timer to monitor suspend thread.
+ * @st: Sleep timer to enable.
+ */
+static void start_sleep_timer(struct sleep_timer *st)
+{
+	struct timer_list *timer = &st->timer;
+
+	st->tsk = current;
+
+	/* use same timeout value for both suspend and resume */
+	timer->expires = jiffies + HZ * CONFIG_PM_SLEEP_TIMER_TIMEOUT;
+	add_timer(timer);
+}
+
+/**
+ * stop_sleep_timer - Disable sleep timer.
+ * @st: sleep timer to disable.
+ */
+static void stop_sleep_timer(struct sleep_timer *st)
+{
+	struct timer_list *timer = &st->timer;
+
+	del_timer_sync(timer);
+}
+
+/**
+ * sleep_timeout_handler - sleep timer timeout handler.
+ * @t: The timer list that sleep timer depends on.
+ *
+ * Called when suspend thread has timeout suspending or resuming.
+ * Dump all uninterruptible tasks' call stack and call panic() to
+ * reboot system in second round timeout.
+ */
+static void sleep_timeout_handler(struct timer_list *t)
+{
+	struct sleep_timer *st = from_timer(st, t, timer);
+	static int timeout_count;
+
+	pr_info("Sleep timeout (timer is %d seconds)\n",
+		(CONFIG_PM_SLEEP_TIMER_TIMEOUT));
+	show_stack(st->tsk, NULL, KERN_EMERG);
+	show_state_filter(TASK_UNINTERRUPTIBLE);
+
+	if (timeout_count < 1) {
+		timeout_count++;
+		start_sleep_timer(st);
+		return;
+	}
+
+	if (console_is_suspended())
+		resume_console();
+
+	panic("Sleep timeout and panic\n");
+}
+#else
+#define DECLARE_SLEEP_TIMER(st)
+#define init_sleep_timer(x, y)
+#define start_sleep_timer(x)
+#define stop_sleep_timer(x)
+#endif
+
+#endif /* _LINUX_SLEEP_TIMER_H */
diff --git a/kernel/power/Kconfig b/kernel/power/Kconfig
index a7320f07689d..9e2b274db0c1 100644
--- a/kernel/power/Kconfig
+++ b/kernel/power/Kconfig
@@ -207,6 +207,21 @@  config PM_SLEEP_DEBUG
 	def_bool y
 	depends on PM_DEBUG && PM_SLEEP
 
+config PM_SLEEP_MONITOR
+	bool "Linux kernel suspend/resume process monitor"
+	depends on PM_SLEEP
+	help
+	This option will enable sleep timer to prevent device stuck
+	during suspend/resume process. Sleep timeout handler will dump
+	disk sleep task at first round timeout and trigger kernel panic
+	at second round timeout. The timer for each round is defined in
+	CONFIG_PM_SLEEP_TIMER_TIMEOUT.
+
+config PM_SLEEP_TIMER_TIMEOUT
+	int "Sleep timer timeout in seconds"
+	range 30 120
+	depends on PM_SLEEP_MONITOR
+
 config DPM_WATCHDOG
 	bool "Device suspend/resume watchdog"
 	depends on PM_DEBUG && PSTORE && EXPERT
diff --git a/kernel/power/suspend.c b/kernel/power/suspend.c
index 8b1bb5ee7e5d..366ebd49a353 100644
--- a/kernel/power/suspend.c
+++ b/kernel/power/suspend.c
@@ -24,6 +24,7 @@ 
 #include <linux/slab.h>
 #include <linux/export.h>
 #include <linux/suspend.h>
+#include <linux/suspend_timer.h>
 #include <linux/syscore_ops.h>
 #include <linux/swait.h>
 #include <linux/ftrace.h>
@@ -61,6 +62,8 @@  static DECLARE_SWAIT_QUEUE_HEAD(s2idle_wait_head);
 enum s2idle_states __read_mostly s2idle_state;
 static DEFINE_RAW_SPINLOCK(s2idle_lock);
 
+DECLARE_SLEEP_TIMER(st);
+
 /**
  * pm_suspend_default_s2idle - Check if suspend-to-idle is the default suspend.
  *
@@ -89,6 +92,8 @@  static void s2idle_enter(void)
 {
 	trace_suspend_resume(TPS("machine_suspend"), PM_SUSPEND_TO_IDLE, true);
 
+	stop_sleep_timer(&st);
+
 	raw_spin_lock_irq(&s2idle_lock);
 	if (pm_wakeup_pending())
 		goto out;
@@ -114,6 +119,8 @@  static void s2idle_enter(void)
 	s2idle_state = S2IDLE_STATE_NONE;
 	raw_spin_unlock_irq(&s2idle_lock);
 
+	start_sleep_timer(&st);
+
 	trace_suspend_resume(TPS("machine_suspend"), PM_SUSPEND_TO_IDLE, false);
 }
 
@@ -179,6 +186,8 @@  void __init pm_states_init(void)
 	 * initialize mem_sleep_states[] accordingly here.
 	 */
 	mem_sleep_states[PM_SUSPEND_TO_IDLE] = mem_sleep_labels[PM_SUSPEND_TO_IDLE];
+
+	init_sleep_timer(&st, sleep_timeout_handler);
 }
 
 static int __init mem_sleep_default_setup(char *str)
@@ -426,6 +435,8 @@  static int suspend_enter(suspend_state_t state, bool *wakeup)
 	if (error || suspend_test(TEST_CPUS))
 		goto Enable_cpus;
 
+	stop_sleep_timer(&st);
+
 	arch_suspend_disable_irqs();
 	BUG_ON(!irqs_disabled());
 
@@ -451,6 +462,8 @@  static int suspend_enter(suspend_state_t state, bool *wakeup)
 	arch_suspend_enable_irqs();
 	BUG_ON(irqs_disabled());
 
+	start_sleep_timer(&st);
+
  Enable_cpus:
 	suspend_enable_secondary_cpus();
 
@@ -610,6 +623,9 @@  int pm_suspend(suspend_state_t state)
 		return -EINVAL;
 
 	pr_info("suspend entry (%s)\n", mem_sleep_labels[state]);
+
+	start_sleep_timer(&st);
+
 	error = enter_state(state);
 	if (error) {
 		suspend_stats.fail++;
@@ -617,6 +633,9 @@  int pm_suspend(suspend_state_t state)
 	} else {
 		suspend_stats.success++;
 	}
+
+	stop_sleep_timer(&st);
+
 	pr_info("suspend exit\n");
 	return error;
 }
diff --git a/kernel/printk/printk.c b/kernel/printk/printk.c
index 9b75f6bfc333..488cbc9abaaf 100644
--- a/kernel/printk/printk.c
+++ b/kernel/printk/printk.c
@@ -2259,6 +2259,11 @@  module_param_named(console_suspend, console_suspend_enabled,
 MODULE_PARM_DESC(console_suspend, "suspend console during suspend"
 	" and hibernate operations");
 
+int console_is_suspended(void)
+{
+	return console_suspended;
+}
+
 /**
  * suspend_console - suspend the console subsystem
  *