Tools: Testbench: Add to IPC4 version UUID based components load, apply initial byte control, etc.

tools/testbench/include/testbench/topology_ipc4.h

@@ -10,7 +10,13 @@
 #include "testbench/utils.h"
 
 #define TB_IPC4_MAX_TPLG_OBJECT_SIZE	4096
-#define TB_IPC4_MAX_MSG_SIZE		384
+
+/* See module_set_large_config() where message fragment is
+ * MAILBOX_DSPBOX_SIZE. The add of header size (8) is because
+ * testbench and plugin have the set large config header in
+ * same memory as the payload.
+ */
+#define TB_IPC4_MAX_MSG_SIZE	(MAILBOX_DSPBOX_SIZE + sizeof(struct ipc4_module_large_config))
 
 #define TB_PROCESS_MODULE_ID		0x95
 #define TB_PGA_MODULE_ID		0x6
@@ -31,6 +37,8 @@ int tb_new_dai_in_out(struct testbench_prm *tp, int dir);
 int tb_new_pga(struct testbench_prm *tp);
 int tb_new_process(struct testbench_prm *tp);
 int tb_pipelines_set_state(struct testbench_prm *tp, int state, int dir);
+int tb_send_bytes_data(struct tb_mq_desc *ipc_tx, struct tb_mq_desc *ipc_rx,
+		       uint32_t module_id, uint32_t instance_id, struct sof_abi_hdr *abi);
 int tb_set_reset_state(struct testbench_prm *tp);
 int tb_set_running_state(struct testbench_prm *tp);
 int tb_set_up_pipeline(struct testbench_prm *tp, struct tplg_pipeline_info *pipe_info);

tools/testbench/include/testbench/utils.h

@@ -37,6 +37,9 @@ struct file_comp_lookup {
 #define TB_NAME_SIZE		256
 #define TB_MAX_CONFIG_COUNT	2
 #define TB_MAX_CONFIG_NAME_SIZE	64
+#define TB_MAX_VOLUME_SIZE	120
+#define TB_MAX_DATA_SIZE	512
+#define TB_MAX_CTLS		16
 
 struct tb_mq_desc {
 	char queue_name[TB_NAME_SIZE];
@@ -52,6 +55,27 @@ struct tb_config {
 	int channels;
 	unsigned long format;
 };
+
+struct tb_ctl {
+	unsigned int module_id;
+	unsigned int instance_id;
+	unsigned int type;
+	unsigned int volume_table[TB_MAX_VOLUME_SIZE];
+	unsigned int index;
+	char data[TB_MAX_DATA_SIZE];
+	union {
+		struct snd_soc_tplg_mixer_control mixer_ctl;
+		struct snd_soc_tplg_enum_control enum_ctl;
+		struct snd_soc_tplg_bytes_control bytes_ctl;
+	};
+};
+
+struct tb_glb_state {
+	char magic[8];			/* SOF_MAGIC */
+	uint32_t num_ctls;		/* number of ctls */
+	size_t size;			/* size of this structure in bytes */
+	struct tb_ctl *ctl;
+};
 #endif
 
 /*
@@ -114,6 +138,7 @@ struct testbench_prm {
 	struct tb_config config[TB_MAX_CONFIG_COUNT];
 	int num_configs;
 	size_t period_size;
+	struct tb_glb_state glb_ctx;
 #endif
 };
 

tools/testbench/topology_ipc4.c

@@ -9,6 +9,7 @@
 #if CONFIG_IPC_MAJOR_4
 
 #include <ipc4/header.h>
+#include <kernel/header.h>
 #include <tplg_parser/tokens.h>
 #include <tplg_parser/topology.h>
 #include <stdio.h>
@@ -797,9 +798,9 @@ int tb_new_process(struct testbench_prm *tp)
 	comp_info->instance_id = tp->instance_ids[SND_SOC_TPLG_DAPM_EFFECT]++;
 	comp_info->module_id = TB_PROCESS_MODULE_ID;
 
-	/* skip kcontrols for now, set object to NULL */
+	/* set up kcontrols */
 	ret = tplg_create_controls(ctx, ctx->widget->num_kcontrols, tplg_ctl,
-				   ctx->hdr->payload_size, NULL);
+				   ctx->hdr->payload_size, comp_info);
 	if (ret < 0) {
 		fprintf(stderr, "error: loading controls\n");
 		goto out;
@@ -1006,6 +1007,200 @@ static int tb_load_widget(struct testbench_prm *tp)
 	return 0;
 }
 
+#define SOF_IPC4_VOL_ZERO_DB	0x7fffffff
+#define VOLUME_FWL		16
+/*
+ * Constants used in the computation of linear volume gain
+ * from dB gain 20th root of 10 in Q1.16 fixed-point notation
+ */
+#define VOL_TWENTIETH_ROOT_OF_TEN	73533
+/* 40th root of 10 in Q1.16 fixed-point notation*/
+#define VOL_FORTIETH_ROOT_OF_TEN	69419
+
+/* 0.5 dB step value in topology TLV */
+#define VOL_HALF_DB_STEP	50
+
+/*
+ * Function to truncate an unsigned 64-bit number
+ * by x bits and return 32-bit unsigned number. This
+ * function also takes care of rounding while truncating
+ */
+static uint32_t vol_shift_64(uint64_t i, uint32_t x)
+{
+	if (x == 0)
+		return (uint32_t)i;
+
+	/* do not truncate more than 32 bits */
+	if (x > 32)
+		x = 32;
+
+	return (uint32_t)(((i >> (x - 1)) + 1) >> 1);
+}
+
+/*
+ * Function to compute a ** exp where,
+ * a is a fractional number represented by a fixed-point integer with a fractional word length
+ * of "fwl"
+ * exp is an integer
+ * fwl is the fractional word length
+ * Return value is a fractional number represented by a fixed-point integer with a fractional
+ * word length of "fwl"
+ */
+static uint32_t vol_pow32(uint32_t a, int exp, uint32_t fwl)
+{
+	int i, iter;
+	uint32_t power = 1 << fwl;
+	unsigned long long numerator;
+
+	/* if exponent is 0, return 1 */
+	if (exp == 0)
+		return power;
+
+	/* determine the number of iterations based on the exponent */
+	if (exp < 0)
+		iter = exp * -1;
+	else
+		iter = exp;
+
+	/* multiply a "iter" times to compute power */
+	for (i = 0; i < iter; i++) {
+		/*
+		 * Product of 2 Qx.fwl fixed-point numbers yields a Q2*x.2*fwl
+		 * Truncate product back to fwl fractional bits with rounding
+		 */
+		power = vol_shift_64((uint64_t)power * a, fwl);
+	}
+
+	if (exp > 0) {
+		/* if exp is positive, return the result */
+		return power;
+	}
+
+	/* if exp is negative, return the multiplicative inverse */
+	numerator = (uint64_t)1 << (fwl << 1);
+	numerator /= power;
+
+	return (uint32_t)numerator;
+}
+
+/*
+ * Function to calculate volume gain from TLV data.
+ * This function can only handle gain steps that are multiples of 0.5 dB
+ */
+static uint32_t vol_compute_gain(uint32_t value, struct snd_soc_tplg_tlv_dbscale *scale)
+{
+	int dB_gain;
+	uint32_t linear_gain;
+	int f_step;
+
+	/* mute volume */
+	if (value == 0 && scale->mute)
+		return 0;
+
+	/* compute dB gain from tlv. tlv_step in topology is multiplied by 100 */
+	dB_gain = (int)scale->min / 100 + (value * scale->step) / 100;
+
+	/* compute linear gain represented by fixed-point int with VOLUME_FWL fractional bits */
+	linear_gain = vol_pow32(VOL_TWENTIETH_ROOT_OF_TEN, dB_gain, VOLUME_FWL);
+
+	/* extract the fractional part of volume step */
+	f_step = scale->step - (scale->step / 100);
+
+	/* if volume step is an odd multiple of 0.5 dB */
+	if (f_step == VOL_HALF_DB_STEP && (value & 1))
+		linear_gain = vol_shift_64((uint64_t)linear_gain * VOL_FORTIETH_ROOT_OF_TEN,
+					   VOLUME_FWL);
+
+	return linear_gain;
+}
+
+/* helper function to add new kcontrols to the list of kcontrols */
+static int tb_kcontrol_cb_new(struct snd_soc_tplg_ctl_hdr *tplg_ctl,
+			      void *comp, void *arg, int index)
+{
+	struct tplg_comp_info *comp_info = comp;
+	struct testbench_prm *tp = arg;
+	struct tb_glb_state *glb = &tp->glb_ctx;
+	struct tb_ctl *ctl;
+
+	if (glb->num_ctls >= TB_MAX_CTLS) {
+		fprintf(stderr, "Error: Too many controls already.\n");
+		return -EINVAL;
+	}
+
+	fprintf(stderr, "Info: Found control type %d: %s\n", tplg_ctl->type, tplg_ctl->name);
+
+	switch (tplg_ctl->ops.info) {
+	case SND_SOC_TPLG_CTL_RANGE:
+	case SND_SOC_TPLG_CTL_STROBE:
+		fprintf(stderr, "Warning: Not supported ctl type %d\n", tplg_ctl->type);
+		return 0;
+	case SND_SOC_TPLG_CTL_VOLSW:
+	case SND_SOC_TPLG_CTL_VOLSW_SX:
+	case SND_SOC_TPLG_CTL_VOLSW_XR_SX:
+		struct snd_soc_tplg_mixer_control *tplg_mixer =
+			(struct snd_soc_tplg_mixer_control *)tplg_ctl;
+		struct snd_soc_tplg_ctl_tlv *tlv;
+		struct snd_soc_tplg_tlv_dbscale *scale;
+		int i;
+
+		glb->size += sizeof(struct tb_ctl);
+		ctl = &glb->ctl[glb->num_ctls++];
+		ctl->module_id = comp_info->module_id;
+		ctl->instance_id = comp_info->instance_id;
+		ctl->mixer_ctl = *tplg_mixer;
+		ctl->index = index;
+		ctl->type = tplg_ctl->type;
+		tlv = &tplg_ctl->tlv;
+		scale = &tlv->scale;
+
+		/* populate the volume table */
+		for (i = 0; i < tplg_mixer->max + 1 ; i++) {
+			uint32_t val = vol_compute_gain(i, scale);
+
+			/* Can be over Q1.31, need to saturate */
+			uint64_t q31val = ((uint64_t)val) << 15;
+
+			ctl->volume_table[i] = q31val > SOF_IPC4_VOL_ZERO_DB ?
+							SOF_IPC4_VOL_ZERO_DB : q31val;
+		}
+		break;
+	case SND_SOC_TPLG_CTL_ENUM:
+	case SND_SOC_TPLG_CTL_ENUM_VALUE:
+		struct snd_soc_tplg_enum_control *tplg_enum =
+			(struct snd_soc_tplg_enum_control *)tplg_ctl;
+
+		glb->size += sizeof(struct tb_ctl);
+		ctl = &glb->ctl[glb->num_ctls++];
+		ctl->module_id = comp_info->module_id;
+		ctl->instance_id = comp_info->instance_id;
+		ctl->enum_ctl = *tplg_enum;
+		ctl->index = index;
+		ctl->type = tplg_ctl->type;
+		break;
+	case SND_SOC_TPLG_CTL_BYTES:
+	{
+		struct snd_soc_tplg_bytes_control *tplg_bytes =
+			(struct snd_soc_tplg_bytes_control *)tplg_ctl;
+
+		glb->size += sizeof(struct tb_ctl);
+		ctl = &glb->ctl[glb->num_ctls++];
+		ctl->module_id = comp_info->module_id;
+		ctl->instance_id = comp_info->instance_id;
+		ctl->bytes_ctl = *tplg_bytes;
+		ctl->index = index;
+		ctl->type = tplg_ctl->type;
+		memcpy(ctl->data, tplg_bytes->priv.data, tplg_bytes->priv.size);
+		break;
+	}
+	default:
+		fprintf(stderr, "Error: Invalid ctl type %d\n", tplg_ctl->type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /* parse topology file and set up pipeline */
 int tb_parse_topology(struct testbench_prm *tp)
 
@@ -1018,6 +1213,13 @@ int tb_parse_topology(struct testbench_prm *tp)
 	FILE *file;
 
 	ctx->ipc_major = 4;
+	ctx->ctl_arg = tp;
+	ctx->ctl_cb = tb_kcontrol_cb_new;
+	tp->glb_ctx.ctl = calloc(TB_MAX_CTLS, sizeof(struct tb_ctl));
+	if (!tp->glb_ctx.ctl) {
+		fprintf(stderr, "error: failed to allocate for controls.\n");
+		return -ENOMEM;
+	}
 
 	/* open topology file */
 	file = fopen(ctx->tplg_file, "rb");
@@ -1293,4 +1495,85 @@ int tb_free_route(struct testbench_prm *tp, struct tplg_route_info *route_info)
 	return 0;
 }
 
+static void tb_ctl_ipc_message(struct ipc4_module_large_config *config, int param_id,
+			       size_t offset_or_size, uint32_t module_id, uint32_t instance_id,
+			       uint32_t type)
+{
+	config->primary.r.type = type;
+	config->primary.r.msg_tgt = SOF_IPC4_MESSAGE_TARGET_MODULE_MSG;
+	config->primary.r.rsp = SOF_IPC4_MESSAGE_DIR_MSG_REQUEST;
+	config->primary.r.module_id = module_id;
+	config->primary.r.instance_id = instance_id;
+
+	config->extension.r.data_off_size = offset_or_size;
+	config->extension.r.large_param_id = param_id;
+}
+
+int tb_send_bytes_data(struct tb_mq_desc *ipc_tx, struct tb_mq_desc *ipc_rx,
+		       uint32_t module_id, uint32_t instance_id, struct sof_abi_hdr *abi)
+{
+	struct ipc4_module_large_config config = {{ 0 }};
+	struct ipc4_message_reply reply;
+	void *msg;
+	int ret;
+	size_t chunk_size;
+	size_t msg_size;
+	size_t msg_size_full = sizeof(config) + abi->size;
+	size_t remaining = abi->size;
+	size_t payload_limit = TB_IPC4_MAX_MSG_SIZE - sizeof(config);
+	size_t offset = 0;
+
+	/* allocate memory for IPC message */
+	msg_size = MIN(msg_size_full, TB_IPC4_MAX_MSG_SIZE);
+	msg = calloc(msg_size, 1);
+	if (!msg)
+		return -ENOMEM;
+
+	config.extension.r.init_block = 1;
+
+	/* configure the IPC message */
+	tb_ctl_ipc_message(&config, abi->type, remaining, module_id, instance_id,
+			   SOF_IPC4_MOD_LARGE_CONFIG_SET);
+
+	do {
+		if (remaining > payload_limit) {
+			chunk_size = payload_limit;
+		} else {
+			chunk_size = remaining;
+			config.extension.r.final_block = 1;
+		}
+
+		if (offset) {
+			config.extension.r.init_block = 0;
+			tb_ctl_ipc_message(&config, abi->type, offset, module_id, instance_id,
+					   SOF_IPC4_MOD_LARGE_CONFIG_SET);
+		}
+
+		/* set the IPC message data */
+		memcpy(msg, &config, sizeof(config));
+		memcpy(msg + sizeof(config), (char *)abi->data + offset, chunk_size);
+
+		/* send the message and check status */
+		ret = tb_mq_cmd_tx_rx(ipc_tx, ipc_rx, msg, chunk_size + sizeof(config),
+				      &reply, sizeof(reply));
+		if (ret < 0) {
+			fprintf(stderr, "Error: Failed to send IPC to set bytes data.\n");
+			goto out;
+		}
+
+		if (reply.primary.r.status != IPC4_SUCCESS) {
+			fprintf(stderr, "Error: Failed with status %d.\n", reply.primary.r.status);
+			ret = -EINVAL;
+			goto out;
+		}
+
+		offset += chunk_size;
+		remaining -= chunk_size;
+	} while (remaining);
+
+out:
+	free(msg);
+	return ret;
+}
+
 #endif /* CONFIG_IPC_MAJOR_4 */