From a06664158a3b1459675a7aabeadb96b969a9ab6e Mon Sep 17 00:00:00 2001
From: Tomasz Leman <tomasz.m.leman@intel.com>
Date: Thu, 26 Oct 2023 10:31:30 +0200
Subject: [PATCH] power: add pm_idle_exit function

The purpose of the newly added function is to perform operations that
reverse the effects of changes introduced by pm_idle_entry.

This particular implementation includes dynamic clock swiching done for
Intel ADSP. As explained in commit fd65c04afb, the DSP can dynamically
change the clock to save power.

Signed-off-by: Tomasz Leman <tomasz.m.leman@intel.com>
---
 kernel/idle.c                      |  5 ++-
 soc/xtensa/intel_adsp/ace/power.c  | 15 +++++++++
 soc/xtensa/intel_adsp/common/clk.c | 49 ++++++++++++++++++++++++++++++
 3 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/kernel/idle.c b/kernel/idle.c
index 8fbd7f778fa114d..0490ff9d2c45669 100644
--- a/kernel/idle.c
+++ b/kernel/idle.c
@@ -85,8 +85,11 @@ void idle(void *unused1, void *unused2, void *unused3)
 		if (k_is_pre_kernel() || !pm_system_suspend(_kernel.idle)) {
 #ifdef CONFIG_PM_IDLE_POWER_OPTIMIZATIONS
 			pm_idle_entry();
-#endif
 			k_cpu_idle();
+			pm_idle_exit();
+#else
+			k_cpu_idle();
+#endif
 		}
 #else
 		k_cpu_idle();
diff --git a/soc/xtensa/intel_adsp/ace/power.c b/soc/xtensa/intel_adsp/ace/power.c
index db149682d1d5c89..ea0925ab76a78f1 100644
--- a/soc/xtensa/intel_adsp/ace/power.c
+++ b/soc/xtensa/intel_adsp/ace/power.c
@@ -375,6 +375,11 @@ void pm_state_exit_post_ops(enum pm_state state, uint8_t substate_id)
 
 #ifdef CONFIG_PM_IDLE_POWER_OPTIMIZATIONS
 
+#include <zephyr/pm/policy.h>
+
+extern void adsp_clock_idle_entry(void);
+extern void adsp_clock_idle_exit(void);
+
 void pm_idle_entry(void)
 {
 	/* NOTE: Decreasing a power state lock counter before entering idle. Primary core is
@@ -389,6 +394,16 @@ void pm_idle_entry(void)
 	} else {
 		DSPCS.bootctl[arch_proc_id()].bctl &= ~DSPBR_BCTL_WAITIPCG;
 	}
+
+	if (pm_policy_state_lock_is_active(PM_STATE_ACTIVE, 2)) {
+		adsp_clock_idle_entry();
+	}
+}
+
+void pm_idle_exit(void)
+{
+	pm_policy_state_lock_get(PM_STATE_ACTIVE, 2);
+	adsp_clock_idle_exit();
 }
 
 #endif
diff --git a/soc/xtensa/intel_adsp/common/clk.c b/soc/xtensa/intel_adsp/common/clk.c
index 0f3f3d32b80ed5f..7949689149728f7 100644
--- a/soc/xtensa/intel_adsp/common/clk.c
+++ b/soc/xtensa/intel_adsp/common/clk.c
@@ -153,3 +153,52 @@ uint32_t adsp_clock_source_frequency(int source)
 
 	return adsp_clk_src_info[source].frequency;
 }
+
+#ifdef CONFIG_PM_IDLE_POWER_OPTIMIZATIONS
+/**
+ * @brief Clock change counter.
+ *
+ * A atomic variable that counts how many times the clock has been tried to be lowered.
+ */
+atomic_t clock_change_count;
+
+/**
+ * @brief Clock switch on idle entry.
+ *
+ * This function should be called in Idle thread. Intel ADSP can change clock according to
+ * its needs. But in idle, when core is waiting for interrupt, clock can be change to
+ * lower frequanze.
+ *
+ * @see adsp_clock_idle_exit()
+ */
+void adsp_clock_idle_entry(void)
+{
+	/* we are already at the lowest clock, there is no need to do anything */
+	if (platform_cpu_clocks[0].current_freq != platform_cpu_clocks[0].lowest_freq) {
+		select_cpu_clock_hw(platform_cpu_clocks[0].lowest_freq);
+		(void)atomic_inc(&clock_change_count);
+	}
+}
+
+/**
+ * @brief Clock restore on idle exit.
+ *
+ * This function must be called when exiting the idle state. During idle entry core could switch
+ * clock to the lowest frequency and we need to restore previous settings.
+ * 
+ * @see adsp_clock_idle_entry()
+ */
+void adsp_clock_idle_exit(void)
+{
+	/* clock has not been changed */
+	if (!atomic_get(&clock_change_count))
+		return;
+
+	/* If the DSP should run at a higher clock than the lowest, restore this setting */
+	if (platform_cpu_clocks[0].current_freq != platform_cpu_clocks[0].lowest_freq) {
+		select_cpu_clock_hw(platform_cpu_clocks[0].current_freq);
+		(void)atomic_clear(&clock_change_count);
+	}
+}
+
+#endif