Fix clock change in waiti for multicore access

src/arch/host/include/arch/lib/cpu.h

@@ -24,6 +24,11 @@ static inline int arch_cpu_is_core_enabled(int id)
 	return 0;
 }
 
+static inline int arch_cpu_enabled_cores(void)
+{
+	return 1;
+}
+
 static inline int arch_cpu_get_id(void)
 {
 	return 0;

src/arch/xtensa/include/arch/lib/cpu.h

@@ -25,6 +25,8 @@ void arch_cpu_disable_core(int id);
 
 int arch_cpu_is_core_enabled(int id);
 
+int arch_cpu_enabled_cores(void);
+
 #else
 
 static inline int arch_cpu_enable_core(int id) { return 0; }
@@ -33,6 +35,8 @@ static inline void arch_cpu_disable_core(int id) { }
 
 static inline int arch_cpu_is_core_enabled(int id) { return 1; }
 
+static inline int arch_cpu_enabled_cores(void) { return 1; }
+
 #endif
 
 static inline int arch_cpu_get_id(void)

src/arch/xtensa/lib/cpu.c

@@ -31,7 +31,7 @@
 extern struct core_context *core_ctx_ptr[PLATFORM_CORE_COUNT];
 extern struct xtos_core_data *core_data_ptr[PLATFORM_CORE_COUNT];
 
-static uint32_t active_cores_mask;
+static uint32_t active_cores_mask = BIT(PLATFORM_MASTER_CORE_ID);
 static int dsp_sref[PLATFORM_CORE_COUNT];	/**< simple ref counter */
 
 #if CONFIG_NO_SLAVE_CORE_ROM
@@ -131,8 +131,12 @@ void arch_cpu_disable_core(int id)
 
 int arch_cpu_is_core_enabled(int id)
 {
-	return id == PLATFORM_MASTER_CORE_ID ||
-		active_cores_mask & (1 << id);
+	return active_cores_mask & BIT(id);
+}
+
+int arch_cpu_enabled_cores(void)
+{
+	return active_cores_mask;
 }
 
 void cpu_alloc_core_context(int core)

src/include/sof/lib/clk.h

@@ -12,6 +12,7 @@
 #include <platform/lib/clk.h>
 #include <sof/sof.h>
 #include <sof/spinlock.h>
+#include <stdbool.h>
 #include <stdint.h>
 
 struct timer;
@@ -40,13 +41,20 @@ struct clock_info {
 	uint32_t notification_id;
 	uint32_t notification_mask;
 	spinlock_t lock;
+
+	/* persistent change clock value in active state */
 	int (*set_freq)(int clock, int freq_idx);
+
+	/* temporary change clock - don't modify default clock settings */
+	void (*low_power_mode)(int clock, bool enable);
 };
 
 uint32_t clock_get_freq(int clock);
 
 void clock_set_freq(int clock, uint32_t hz);
 
+void clock_low_power_mode(int clock, bool enable);
+
 uint64_t clock_ms_to_ticks(int clock, uint64_t ms);
 
 void platform_timer_set_delta(struct timer *timer, uint64_t ns);

src/include/sof/lib/cpu.h

@@ -55,6 +55,11 @@ static inline int cpu_is_core_enabled(int id)
 	return arch_cpu_is_core_enabled(id);
 }
 
+static inline int cpu_enabled_cores(void)
+{
+	return arch_cpu_enabled_cores();
+}
+
 #endif
 
 #endif /* __SOF_LIB_CPU_H__ */

src/include/sof/lib/pm_runtime.h

@@ -40,6 +40,7 @@ enum pm_runtime_context {
 	DMIC_POW,			/**< DMIC Power */
 	DW_DMAC_CLK,			/**< DW DMAC Clock */
 	CORE_MEMORY_POW,		/**< Core Memory power */
+	CORE_HP_CLK,			/**< High Performance Clock*/
 	PM_RUNTIME_DSP			/**< DSP */
 };
 

src/lib/clk.c

@@ -85,6 +85,14 @@ void clock_set_freq(int clock, uint32_t hz)
 	spin_unlock_irq(&clk_info->lock, flags);
 }
 
+void clock_low_power_mode(int clock, bool enable)
+{
+	struct clock_info *clk_info = clocks_get() + clock;
+
+	if (clk_info->low_power_mode)
+		clk_info->low_power_mode(clock, enable);
+}
+
 uint64_t clock_ms_to_ticks(int clock, uint64_t ms)
 {
 	struct clock_info *clk_info = clocks_get() + clock;

src/platform/intel/cavs/include/cavs/lib/pm_runtime.h

@@ -22,6 +22,7 @@ struct pm_runtime_data;
 struct cavs_pm_runtime_data {
 	int dsp_d0_sref; /**< simple ref counter, accessed by core 0 only */
 	int host_dma_l1_sref; /**< ref counter for Host DMA accesses */
+	uint32_t sleep_core_mask; /**< represents cores in waiti state */
 };
 
 #endif

src/platform/intel/cavs/lib/clk.c

@@ -9,13 +9,14 @@
 #include <sof/lib/clk.h>
 #include <sof/lib/memory.h>
 #include <sof/lib/notifier.h>
+#include <sof/lib/pm_runtime.h>
 #include <sof/sof.h>
 #include <sof/spinlock.h>
 
 static SHARED_DATA struct clock_info platform_clocks_info[NUM_CLOCKS];
 
 #if CAVS_VERSION == CAVS_VERSION_1_5
-static inline void select_cpu_clock(int freq_idx, bool release_unused)
+static inline void select_cpu_clock_hw(int freq_idx, bool release_unused)
 {
 	uint32_t enc = cpu_freq_enc[freq_idx];
 
@@ -25,7 +26,7 @@ static inline void select_cpu_clock(int freq_idx, bool release_unused)
 			   enc);
 }
 #else
-static inline void select_cpu_clock(int freq_idx, bool release_unused)
+static inline void select_cpu_clock_hw(int freq_idx, bool release_unused)
 {
 	uint32_t enc = cpu_freq_enc[freq_idx];
 	uint32_t status_mask = cpu_freq_status_mask[freq_idx];
@@ -61,10 +62,27 @@ static inline void select_cpu_clock(int freq_idx, bool release_unused)
 }
 #endif
 
-static int clock_platform_set_cpu_freq(int clock, int freq_idx)
+static inline void select_cpu_clock(int freq_idx, bool release_unused)
 {
-	select_cpu_clock(freq_idx, true);
-	return 0;
+	struct clock_info *clk_info = clocks_get();
+	int flags[PLATFORM_CORE_COUNT];
+	int i;
+
+	/* lock clock for all cores */
+	for (i = 0; i < PLATFORM_CORE_COUNT; i++)
+		spin_lock_irq(&clk_info[CLK_CPU(i)].lock, flags[i]);
+
+	/* change clock */
+	select_cpu_clock_hw(freq_idx, release_unused);
+	for (i = 0; i < PLATFORM_CORE_COUNT; i++)
+		clk_info[CLK_CPU(i)].current_freq_idx = freq_idx;
+
+	/* unlock clock for all cores */
+	for (i = PLATFORM_CORE_COUNT - 1; i >= 0; i--)
+		spin_unlock_irq(&clk_info[CLK_CPU(i)].lock, flags[i]);
+
+	platform_shared_commit(clk_info,
+			       sizeof(*clk_info) * PLATFORM_CORE_COUNT);
 }
 
 #if CONFIG_CAVS_USE_LPRO_IN_WAITI
@@ -73,53 +91,73 @@ static int clock_platform_set_cpu_freq(int clock, int freq_idx)
  */
 static SHARED_DATA int active_freq_idx = CPU_DEFAULT_IDX;
 
-static inline int get_cpu_current_freq_idx(void)
+static inline int get_current_freq_idx(int clock)
 {
-	struct clock_info *clk_info = clocks_get() + CLK_CPU(cpu_get_id());
+	struct clock_info *clk_info = clocks_get() + clock;
 
 	return clk_info->current_freq_idx;
 }
 
-static inline void set_cpu_current_freq_idx(int freq_idx)
+/*
+ * This is call from public API, so overwrite currently used frequency index
+ * in 'active' state with new value. This value will be used in each wakeup.
+ */
+static inline void set_cpu_current_freq_idx(int freq_idx, bool release_unused)
 {
-	int i;
-	struct clock_info *clk_info = clocks_get();
-
-	for (i = 0; i < PLATFORM_CORE_COUNT; i++)
-		clk_info[i].current_freq_idx = freq_idx;
+	int *uncached_freq_idx = cache_to_uncache(&active_freq_idx);
 
-	platform_shared_commit(clk_info,
-			       sizeof(*clk_info) * PLATFORM_CORE_COUNT);
+	select_cpu_clock(freq_idx, release_unused);
+	*uncached_freq_idx = freq_idx;
 }
 
-void platform_clock_on_wakeup(void)
+static void platform_clock_low_power_mode(int clock, bool enable)
 {
+	int current_freq_idx = get_current_freq_idx(clock);
 	int freq_idx = *cache_to_uncache(&active_freq_idx);
 
-	if (freq_idx != get_cpu_current_freq_idx()) {
-		select_cpu_clock(freq_idx, true);
-		set_cpu_current_freq_idx(freq_idx);
-	}
-}
-
-void platform_clock_on_waiti(void)
-{
-	int freq_idx = get_cpu_current_freq_idx();
-
-	*cache_to_uncache(&active_freq_idx) = freq_idx;
-
-	if (freq_idx != CPU_LPRO_FREQ_IDX) {
+	if (enable && current_freq_idx != CPU_LPRO_FREQ_IDX)
 		/* LPRO requests are fast, but requests for other ROs
 		 * can take a lot of time. That's why it's better to
 		 * not release active clock just for waiti,
 		 * so they can be switched without delay on wake up.
 		 */
 		select_cpu_clock(CPU_LPRO_FREQ_IDX, false);
-		set_cpu_current_freq_idx(CPU_LPRO_FREQ_IDX);
-	}
+	else if (!enable && current_freq_idx != freq_idx)
+		select_cpu_clock(freq_idx, false);
+}
+
+void platform_clock_on_wakeup(void)
+{
+	pm_runtime_get(CORE_HP_CLK, cpu_get_id());
+}
+
+void platform_clock_on_waiti(void)
+{
+	pm_runtime_put(CORE_HP_CLK, cpu_get_id());
+}
+
+#else
+/*
+ * This is call from public API, there is no clock switch for core waiti, so
+ * value of value frequency index in 'active' state doesn't need to be saved in
+ * any additional variable and restored in future.
+ */
+static inline void set_cpu_current_freq_idx(int freq_idx, bool release_unused)
+{
+	select_cpu_clock(freq_idx, true);
+}
+
+static void platform_clock_low_power_mode(int clock, bool enable)
+{
 }
 #endif
 
+static int clock_platform_set_cpu_freq(int clock, int freq_idx)
+{
+	set_cpu_current_freq_idx(freq_idx, true);
+	return 0;
+}
+
 void platform_clock_init(struct sof *sof)
 {
 	int i;
@@ -136,6 +174,7 @@ void platform_clock_init(struct sof *sof)
 			.notification_id = NOTIFIER_ID_CPU_FREQ,
 			.notification_mask = NOTIFIER_TARGET_CORE_MASK(i),
 			.set_freq = clock_platform_set_cpu_freq,
+			.low_power_mode = platform_clock_low_power_mode,
 		};
 
 		spinlock_init(&sof->clocks[i].lock);

src/platform/intel/cavs/lib/pm_runtime.c

@@ -337,6 +337,47 @@ static inline void cavs_pm_runtime_core_en_memory(uint32_t index)
 #endif
 }
 
+static inline void cavs_pm_runtime_core_dis_hp_clk(uint32_t index)
+{
+	int all_active_cores_sleep;
+	int enabled_cores = cpu_enabled_cores();
+	struct pm_runtime_data *prd = pm_runtime_data_get();
+	struct cavs_pm_runtime_data *pprd = prd->platform_data;
+	uint32_t flags;
+
+	spin_lock_irq(&prd->lock, flags);
+
+	pprd->sleep_core_mask |= BIT(index);
+
+	all_active_cores_sleep =
+		(enabled_cores & pprd->sleep_core_mask) == enabled_cores;
+
+	if (all_active_cores_sleep)
+		clock_low_power_mode(CLK_CPU(index), true);
+
+	platform_shared_commit(prd, sizeof(*prd));
+	platform_shared_commit(pprd, sizeof(*pprd));
+
+	spin_unlock_irq(&prd->lock, flags);
+}
+
+static inline void cavs_pm_runtime_core_en_hp_clk(uint32_t index)
+{
+	struct pm_runtime_data *prd = pm_runtime_data_get();
+	struct cavs_pm_runtime_data *pprd = prd->platform_data;
+	uint32_t flags;
+
+	spin_lock_irq(&prd->lock, flags);
+
+	pprd->sleep_core_mask &= ~BIT(index);
+	clock_low_power_mode(CLK_CPU(index), false);
+
+	platform_shared_commit(prd, sizeof(*prd));
+	platform_shared_commit(pprd, sizeof(*pprd));
+
+	spin_unlock_irq(&prd->lock, flags);
+}
+
 static inline void cavs_pm_runtime_dis_dsp_pg(uint32_t index)
 {
 #if CAVS_VERSION >= CAVS_VERSION_1_8
@@ -434,6 +475,9 @@ void platform_pm_runtime_get(enum pm_runtime_context context, uint32_t index,
 	case CORE_MEMORY_POW:
 		cavs_pm_runtime_core_en_memory(index);
 		break;
+	case CORE_HP_CLK:
+		cavs_pm_runtime_core_en_hp_clk(index);
+		break;
 	case PM_RUNTIME_DSP:
 		cavs_pm_runtime_dis_dsp_pg(index);
 		break;
@@ -471,6 +515,9 @@ void platform_pm_runtime_put(enum pm_runtime_context context, uint32_t index,
 	case CORE_MEMORY_POW:
 		cavs_pm_runtime_core_dis_memory(index);
 		break;
+	case CORE_HP_CLK:
+		cavs_pm_runtime_core_dis_hp_clk(index);
+		break;
 	case PM_RUNTIME_DSP:
 		cavs_pm_runtime_en_dsp_pg(index);
 		break;