pipeline: register KCPS consumption and enable dynamic clock control

app/boards/intel_adsp_ace15_mtpm.conf

@@ -95,3 +95,5 @@ CONFIG_LOG_TIMESTAMP_64BIT=y
 
 CONFIG_COMP_GOOGLE_RTC_AUDIO_PROCESSING=y
 CONFIG_COMP_STUBS=y
+
+CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=y

app/perf_overlay.conf

@@ -2,3 +2,4 @@ CONFIG_PERFORMANCE_COUNTERS=y
 CONFIG_SYS_HEAP_RUNTIME_STATS=y
 CONFIG_TIMING_FUNCTIONS=y
 CONFIG_ADSP_IDLE_CLOCK_GATING=n
+CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL=n

src/audio/pipeline/pipeline-stream.c

@@ -11,12 +11,15 @@
 #include <sof/lib/dai.h>
 #include <rtos/wait.h>
 #include <sof/list.h>
+#include <rtos/interrupt.h>
 #include <rtos/spinlock.h>
 #include <rtos/string.h>
 #include <ipc/stream.h>
 #include <ipc/topology.h>
 #include <ipc4/module.h>
 #include <rtos/kernel.h>
+#include <sof/audio/module_adapter/module/generic.h>
+#include <sof/lib/cpu-clk-manager.h>
 
 #include <errno.h>
 #include <stdbool.h>
@@ -311,17 +314,80 @@ static void pipeline_trigger_xrun(struct pipeline *p, struct comp_dev **host)
 	} while (true);
 }
 
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+static int pipeline_calc_cps_consumption(struct comp_dev *current,
+					struct comp_buffer *calling_buf,
+					struct pipeline_walk_context *ctx, int dir)
+{
+	struct pipeline_data *ppl_data = ctx->comp_data;
+	struct ipc4_base_module_cfg *cd;
+	int comp_core, kcps;
+
+	pipe_dbg(ppl_data->p, "pipeline_calc_cps_consumption(), current->comp.id = %u, dir = %u",
+		 dev_comp_id(current), dir);
+
+	if (!comp_is_single_pipeline(current, ppl_data->start)) {
+		pipe_dbg(ppl_data->p, "pipeline_calc_cps_consumption(), current is from another pipeline");
+		return 0;
+	}
+	comp_core = current->ipc_config.core;
+
+	/* modules created through module adapter have different priv_data */
+	if (current->drv->type != SOF_COMP_MODULE_ADAPTER) {
+		cd = comp_get_drvdata(current);
+	} else {
+		struct processing_module *mod = comp_get_drvdata(current);
+		struct module_data *md = &mod->priv;
+
+		cd = &md->cfg.base_cfg;
+	}
+
+	if (cd->cpc == 0) {
+		/* Use maximum clock budget, assume 1ms chunk size */
+		ppl_data->kcps[comp_core] = CLK_MAX_CPU_HZ / 1000;
+		tr_warn(pipe,
+			"0 CPS requested for module: %#x, core: %d using safe max KCPS: %u",
+			current->ipc_config.id, comp_core, ppl_data->kcps[comp_core]);
+
+		return PPL_STATUS_PATH_STOP;
+	} else {
+		kcps = cd->cpc * 1000 / current->period;
+		tr_dbg(pipe, "Module: %#x KCPS consumption: %d, core: %d",
+		       current->ipc_config.id, kcps, comp_core);
+
+		ppl_data->kcps[comp_core] += kcps;
+	}
+
+	return pipeline_for_each_comp(current, ctx, dir);
+}
+#endif
+
 /* trigger pipeline in IPC context */
 int pipeline_trigger(struct pipeline *p, struct comp_dev *host, int cmd)
 {
 	int ret;
-
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+	struct pipeline_data data = {
+		.start = p->source_comp,
+		.p = p,
+	};
+	struct pipeline_walk_context walk_ctx = {
+		.comp_func = pipeline_calc_cps_consumption,
+		.comp_data = &data,
+	};
+	bool trigger_first = false;
+	uint32_t flags = 0;
+#endif
 	pipe_info(p, "pipe trigger cmd %d", cmd);
 
 	p->trigger.aborted = false;
 
 	switch (cmd) {
 	case COMP_TRIGGER_PAUSE:
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+		trigger_first = true;
+		COMPILER_FALLTHROUGH;
+#endif
 	case COMP_TRIGGER_STOP:
 		if (p->status == COMP_STATE_PAUSED || p->xrun_bytes) {
 			/* The task isn't running, trigger inline */
@@ -338,9 +404,40 @@ int pipeline_trigger(struct pipeline *p, struct comp_dev *host, int cmd)
 	case COMP_TRIGGER_PRE_RELEASE:
 	case COMP_TRIGGER_PRE_START:
 		/* Add all connected pipelines to the list and trigger them all */
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+		flags = irq_lock();
+		/* setup walking ctx for removing consumption */
+		if (!trigger_first) {
+			ret = walk_ctx.comp_func(p->source_comp, NULL, &walk_ctx, PPL_DIR_DOWNSTREAM);
+
+			for (int i = 0; i < arch_num_cpus(); i++) {
+				if (data.kcps[i] > 0) {
+					core_kcps_adjust(i, data.kcps[i]);
+					tr_info(pipe, "Sum of KCPS consumption: %d, core: %d", core_kcps_get(i), i);
+				}
+			}
+		}
+#endif
 		ret = pipeline_trigger_list(p, host, cmd);
-		if (ret < 0)
+		if (ret < 0) {
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+			irq_unlock(flags);
+#endif
 			return ret;
+		}
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+		if (trigger_first) {
+			ret = walk_ctx.comp_func(p->source_comp, NULL, &walk_ctx, PPL_DIR_DOWNSTREAM);
+
+			for (int i = 0; i < arch_num_cpus(); i++) {
+				if (data.kcps[i] > 0) {
+					core_kcps_adjust(i, -data.kcps[i]);
+					tr_info(pipe, "Sum of KCPS consumption: %d, core: %d", core_kcps_get(i), i);
+				}
+			}
+		}
+		irq_unlock(flags);
+#endif
 		/* IPC response will be sent from the task, unless it was paused */
 		return PPL_STATUS_SCHEDULED;
 	}

src/include/sof/audio/pipeline.h

@@ -122,6 +122,9 @@ struct pipeline_data {
 	struct pipeline *p;
 	int cmd;
 	uint32_t delay_ms;		/* between PRE_{START,RELEASE} and {START,RELEASE} */
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+	uint32_t kcps[CONFIG_CORE_COUNT]; /**< the max count of KCPS */
+#endif
 };
 
 /** \brief Task type registered by pipelines. */

src/init/init.c

@@ -14,6 +14,7 @@
 #include <rtos/interrupt.h>
 #include <sof/init.h>
 #include <sof/lib/cpu.h>
+#include <sof/lib/cpu-clk-manager.h>
 #include <sof/lib/memory.h>
 #include <sof/lib/mm_heap.h>
 #include <sof/lib/notifier.h>
@@ -203,6 +204,11 @@ int secondary_core_init(struct sof *sof)
 	if (err < 0)
 		return err;
 #endif
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+	err = core_kcps_adjust(cpu_get_id(), SECONDARY_CORE_BASE_CPS_USAGE);
+	if (err < 0)
+		return err;
+#endif
 
 	trace_point(TRACE_BOOT_PLATFORM);
 

src/lib/clk.c

@@ -28,8 +28,6 @@ DECLARE_TR_CTX(clock_tr, SOF_UUID(clock_uuid), LOG_LEVEL_INFO);
 
 SHARED_DATA struct k_spinlock clk_lock;
 
-struct clock_notify_data clk_notify_data;
-
 static inline uint32_t clock_get_nearest_freq_idx(const struct freq_table *tab,
 						  uint32_t size, uint32_t hz)
 {
@@ -59,39 +57,21 @@ void clock_set_freq(int clock, uint32_t hz)
 	uint32_t idx;
 	k_spinlock_key_t key;
 
-	clk_notify_data.old_freq =
-		clk_info->freqs[clk_info->current_freq_idx].freq;
-	clk_notify_data.old_ticks_per_msec =
-		clk_info->freqs[clk_info->current_freq_idx].ticks_per_msec;
-
 	/* atomic context for changing clocks */
 	key = clock_lock();
 
 	/* get nearest frequency that is >= requested Hz */
 	idx = clock_get_nearest_freq_idx(clk_info->freqs, clk_info->freqs_num,
 					 hz);
-	clk_notify_data.freq = clk_info->freqs[idx].freq;
 
 	tr_info(&clock_tr, "clock %d set freq %dHz freq_idx %d",
 		clock, hz, idx);
 
-	/* tell anyone interested we are about to change freq */
-	clk_notify_data.message = CLOCK_NOTIFY_PRE;
-	notifier_event(clk_info, clk_info->notification_id,
-		       clk_info->notification_mask, &clk_notify_data,
-		       sizeof(clk_notify_data));
-
 	if (!clk_info->set_freq ||
 	    clk_info->set_freq(clock, idx) == 0)
 		/* update clock frequency */
 		clk_info->current_freq_idx = idx;
 
-	/* tell anyone interested we have now changed freq */
-	clk_notify_data.message = CLOCK_NOTIFY_POST;
-	notifier_event(clk_info, clk_info->notification_id,
-		       clk_info->notification_mask, &clk_notify_data,
-		       sizeof(clk_notify_data));
-
 	clock_unlock(key);
 }
 

src/lib/cpu-clk-manager.c

@@ -64,7 +64,7 @@ int core_kcps_adjust(int adjusted_core_id, int kcps_delta)
 
 	for (core_id = 0; core_id < CONFIG_CORE_COUNT; core_id++) {
 		/* Convert kcps to cps */
-		ret = request_freq_change(core_id, freq * 1000);
+		ret = request_freq_change(core_id, MIN(freq * 1000, CLK_MAX_CPU_HZ));
 		if (ret < 0)
 			goto out;
 	}

src/platform/Kconfig

@@ -377,6 +377,14 @@ config CAVS_USE_LPRO_IN_WAITI
 	  After waiti exit clock source will be restored.
 	  Choose n if unclear.
 
+config KCPS_DYNAMIC_CLOCK_CONTROL
+	bool "Use KCPS budget to determine DSP clock"
+	default n
+	depends on IPC_MAJOR_4
+	help
+	  Select if we want to use compute budget
+	  expressed in Kilo Cycles Per Second (KCPS) to determine DSP clock.
+
 config L3_HEAP
 	bool "Use L3 memory heap"
 	default n

src/platform/imx93_a55/include/platform/lib/clk.h

@@ -12,6 +12,7 @@
 #define CPU_DEFAULT_IDX 0
 #define NUM_CLOCKS 1
 #define NUM_CPU_FREQ 1
+#define CLK_MAX_CPU_HZ CONFIG_SYS_CLOCK_HW_CYCLES_PER_SEC
 
 struct sof;
 

src/platform/intel/ace/platform.c

@@ -15,6 +15,7 @@
 #include <sof/lib/cpu-clk-manager.h>
 #include <sof/lib/mm_heap.h>
 #include <sof/lib/watchdog.h>
+#include <sof/lib/cpu-clk-manager.h>
 #include <sof/schedule/edf_schedule.h>
 #include <sof/schedule/dp_schedule.h>
 #include <sof/schedule/ll_schedule.h>
@@ -87,11 +88,13 @@ int platform_init(struct sof *sof)
 
 	trace_point(TRACE_BOOT_PLATFORM_CLOCK);
 	platform_clock_init(sof);
+	kcps_budget_init();
 
-	/* Set DSP clock to MAX using KCPS API. Value should be lowered when KCPS API
-	 * for modules is implemented
-	 */
+#if CONFIG_KCPS_DYNAMIC_CLOCK_CONTROL
+	ret = core_kcps_adjust(cpu_get_id(), PRIMARY_CORE_BASE_CPS_USAGE);
+#else
 	ret = core_kcps_adjust(cpu_get_id(), CLK_MAX_CPU_HZ / 1000);
+#endif
 	if (ret < 0)
 		return ret;
 

src/platform/meteorlake/include/platform/lib/clk.h

@@ -30,6 +30,9 @@
 
 #define NUM_SSP_FREQ		3
 
+#define PRIMARY_CORE_BASE_CPS_USAGE 20000
+#define SECONDARY_CORE_BASE_CPS_USAGE 10000
+
 #endif /* __PLATFORM_LIB_CLK_H__ */
 
 #else