lib: MCPS budget manager

src/audio/base_fw.c

@@ -8,6 +8,8 @@
 #include <ipc4/pipeline.h>
 #include <ipc4/logging.h>
 #include <sof_versions.h>
+#include <sof/lib/cpu-clk-manager.h>
+#include <sof/lib/cpu.h>
 
 #if CONFIG_ACE_V1X_ART_COUNTER || CONFIG_ACE_V1X_RTC_COUNTER
 #include <zephyr/device.h>
@@ -323,6 +325,71 @@ static uint32_t basefw_get_ext_system_time(uint32_t *data_offset, char *data)
 	return IPC4_UNAVAILABLE;
 }
 
+static int basefw_register_kcps(bool first_block,
+				bool last_block,
+				uint32_t data_offset_or_size,
+				const char *data)
+{
+	if (!(first_block && last_block))
+		return -EINVAL;
+
+	/* value of kcps to request on core 0. Can be negative */
+	if (core_kcps_adjust(0, *(int32_t *)data))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int basefw_kcps_allocation_request(struct ipc4_resource_kcps *request)
+{
+	if (core_kcps_adjust(request->core_id, request->kcps))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int basefw_resource_allocation_request(bool first_block,
+					      bool last_block,
+					      uint32_t data_offset_or_size,
+					      const char *data)
+{
+	struct ipc4_resource_request *request;
+
+	if (!(first_block && last_block))
+		return -EINVAL;
+
+	request = (struct ipc4_resource_request *)data;
+
+	switch (request->ra_type) {
+	case IPC4_RAT_DSP_KCPS:
+		return basefw_kcps_allocation_request(&request->ra_data.kcps);
+	case IPC4_RAT_MEMORY:
+		return -EINVAL;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int basefw_power_state_info_get(uint32_t *data_offset, char *data)
+{
+	struct ipc4_tuple *tuple = (struct ipc4_tuple *)data;
+	uint32_t core_kcps[CONFIG_CORE_COUNT] = {0};
+	int core_id;
+
+	set_tuple_uint32(tuple, IPC4_ACTIVE_CORES_MASK, cpu_enabled_cores());
+	tuple = next_tuple(tuple);
+
+	for (core_id = 0; core_id < CONFIG_CORE_COUNT; core_id++) {
+		if (cpu_is_core_enabled(core_id))
+			core_kcps[core_id] = core_kcps_get(core_id);
+	}
+
+	set_tuple(tuple, IPC4_CORE_KCPS, sizeof(core_kcps), core_kcps);
+	tuple = next_tuple(tuple);
+	*data_offset = (int)((char *)tuple - data);
+	return 0;
+}
+
 static int basefw_get_large_config(struct comp_dev *dev,
 				   uint32_t param_id,
 				   bool first_block,
@@ -358,6 +425,9 @@ static int basefw_get_large_config(struct comp_dev *dev,
 		} else {
 			return ret;
 		}
+	break;
+	case IPC4_POWER_STATE_INFO_GET:
+		return basefw_power_state_info_get(data_offset, data);
 	/* TODO: add more support */
 	case IPC4_DSP_RESOURCE_STATE:
 	case IPC4_NOTIFICATION_MASK:
@@ -366,7 +436,6 @@ static int basefw_get_large_config(struct comp_dev *dev,
 	case IPC4_PIPELINE_PROPS_GET:
 	case IPC4_SCHEDULERS_INFO_GET:
 	case IPC4_GATEWAYS_INFO_GET:
-	case IPC4_POWER_STATE_INFO_GET:
 	case IPC4_LIBRARIES_INFO_GET:
 	case IPC4_PERF_MEASUREMENTS_STATE:
 	case IPC4_GLOBAL_PERF_DATA:
@@ -394,6 +463,11 @@ static int basefw_set_large_config(struct comp_dev *dev,
 						last_block, data_offset, data);
 	case IPC4_ENABLE_LOGS:
 		return ipc4_logging_enable_logs(first_block, last_block, data_offset, data);
+	case IPC4_REGISTER_KCPS:
+		return basefw_register_kcps(first_block, last_block, data_offset, data);
+	case IPC4_RESOURCE_ALLOCATION_REQUEST:
+		return basefw_resource_allocation_request(first_block, last_block, data_offset,
+							  data);
 	default:
 		break;
 	}

src/include/ipc4/base_fw.h

@@ -667,3 +667,8 @@ struct ipc4_log_state_info {
 	 */
 	uint32_t logs_mask[IPC4_MAX_LIBS_COUNT];
 } __attribute__((packed, aligned(4)));
+
+enum ipc4_power_state_type {
+	IPC4_ACTIVE_CORES_MASK = 0,
+	IPC4_CORE_KCPS = 1,
+};

src/include/sof/lib/cpu-clk-manager.h

@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Copyright(c) 2022 Intel Corporation. All rights reserved.
+ *
+ * Author: Krzysztof Frydryk <frydryk.krzysztof@intel.com>
+ */
+
+/**
+ * \file include/sof/lib/cpu-clk-manager.h
+ * \brief CPU clk manager definition
+ * \author Krzysztof Frydryk <krzysztofx.frydryk@intel.com>
+ */
+
+#ifndef __SOF_LIB_CPU_CLK_MANAGER_H__
+#define __SOF_LIB_CPU_CLK_MANAGER_H__
+
+#include <rtos/spinlock.h>
+#include <sof/sof.h>
+#include <stdint.h>
+
+/**
+ * \brief CPS budget data.
+ */
+struct kcps_budget_data {
+	/* uncache only */
+	int kcps_consumption[CONFIG_CORE_COUNT];	/* Sum of declared consumptions on core */
+	struct k_spinlock lock;
+};
+
+/**
+ * \brief Declare KCPS usage on core
+ *
+ * Declare KCPS (kilo cycles per second) CPU usage on core and adjust
+ * CPU clock according to sum of declared consumptions.
+ *
+ * Declared consumption can be negative in case of freeing CPS.
+ *
+ * @param core The core to which consumption should be pinned
+ * @param kcps_delta declared usage. Can be negative.
+ */
+int core_kcps_adjust(int core, int kcps_delta);
+
+/**
+ * \brief Get KCPS usage on core
+ *
+ * Get declared KCPS usage on core
+ *
+ * @param core The core which consumption should be returned
+ */
+int core_kcps_get(int core);
+
+/**
+ * \brief Init KCPS budget mechanism
+ */
+int kcps_budget_init(void);
+
+#endif /*__SOF_LIB_CPU_CLK_MANAGER_H__ */

src/lib/CMakeLists.txt

@@ -23,4 +23,5 @@ add_local_sources(sof
 	dma.c
 	dai.c
 	wait.c
+	cpu-clk-manager.c
 )

src/lib/cpu-clk-manager.c

@@ -0,0 +1,95 @@
+// SPDX-License-Identifier: BSD-3-Clause
+//
+// Copyright(c) 2022 Intel Corporation. All rights reserved.
+//
+// Author: Krzysztof Frydryk <frydryk.krzysztof@intel.com>
+
+#include <rtos/spinlock.h>
+#include <sof/sof.h>
+#include <stdint.h>
+#include <sof/lib/cpu-clk-manager.h>
+#include <rtos/clk.h>
+#include <sof/sof.h>
+#include <sof/math/numbers.h>
+#include <errno.h>
+#ifdef __ZEPHYR__
+#include <zephyr/sys/util.h>
+#endif /* __ZEPHYR__ */
+
+static struct kcps_budget_data kcps_data;
+
+static int request_freq_change(unsigned int core, int freq)
+{
+	int current_freq;
+	int selected_freq_id;
+	struct clock_info *clk;
+
+	clk = clocks_get() + core;
+
+	for (selected_freq_id = 0; selected_freq_id < clk->freqs_num; selected_freq_id++) {
+		if (freq < clk->freqs[selected_freq_id].freq)
+			break;
+	}
+
+	/* don't change clock frequency if already using proper clock */
+	current_freq = clock_get_freq(core);
+	if (clk->freqs[selected_freq_id].freq != current_freq)
+		clock_set_freq(core, freq);
+
+	return 0;
+}
+
+static int max_core_consumption(void)
+{
+	int result = 0;
+	unsigned int core;
+
+	for (core = 0; core < CONFIG_CORE_COUNT; core++)
+		result = MAX(result, kcps_data.kcps_consumption[core]);
+
+	return result;
+}
+
+int core_kcps_adjust(int adjusted_core_id, int kcps_delta)
+{
+	k_spinlock_key_t key;
+	int freq;
+	unsigned int core_id;
+	int ret = 0;
+
+	key = k_spin_lock(&kcps_data.lock);
+	kcps_data.kcps_consumption[adjusted_core_id] += kcps_delta;
+
+	/* set clock according to maximum requested mcps budget */
+	freq = max_core_consumption();
+
+	for (core_id = 0; core_id < CONFIG_CORE_COUNT; core_id++) {
+		/* Convert kcps to cps */
+		ret = request_freq_change(core_id, freq * 1000);
+		if (ret < 0)
+			goto out;
+	}
+
+out:
+	k_spin_unlock(&kcps_data.lock, key);
+
+	return ret;
+}
+
+int core_kcps_get(int core)
+{
+	k_spinlock_key_t key;
+	int ret;
+
+	key = k_spin_lock(&kcps_data.lock);
+	ret = kcps_data.kcps_consumption[core];
+	k_spin_unlock(&kcps_data.lock, key);
+	return ret;
+}
+
+int kcps_budget_init(void)
+{
+	k_spinlock_init(&kcps_data.lock);
+
+	return 0;
+}

zephyr/CMakeLists.txt

@@ -549,6 +549,7 @@ zephyr_library_sources(
 	${SOF_LIB_PATH}/clk.c
 	${SOF_LIB_PATH}/notifier.c
 	${SOF_LIB_PATH}/pm_runtime.c
+	${SOF_LIB_PATH}/cpu-clk-manager.c
 	${SOF_LIB_PATH}/dma.c
 	${SOF_LIB_PATH}/dai.c