audio: modules & math: add single-precision floating-point acceleration for processing modules

src/audio/crossover/crossover.c

@@ -52,8 +52,12 @@ static void crossover_reset_state(struct processing_module *mod)
 	struct comp_data *cd = module_get_private_data(mod);
 	int i;
 
-	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
+	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
 		crossover_reset_state_ch(mod, &cd->state[i]);
+#if CONFIG_FORMAT_FLOAT
+		crossover_reset_state_ch_float(mod, &cd->state_f[i]);
+#endif
+	}
 }
 
 /**
@@ -227,6 +231,88 @@ int crossover_init_coef_ch(struct processing_module *mod,
 	return 0;
 }
 
+#if CONFIG_FORMAT_FLOAT
+static int crossover_init_coef_lr4_float(struct processing_module *mod,
+					 struct sof_eq_iir_biquad *coef,
+					 struct iir_state_df1_float *lr4)
+{
+	const float inv_q30 = 1.0f / 1073741824.0f;
+	const float inv_q14 = 1.0f / 16384.0f;
+	float gain_factor;
+	float b_scale;
+	float b0, b1, b2, a1, a2;
+
+	lr4->coef = mod_zalloc(mod, 2 * 5 * sizeof(float));
+	if (!lr4->coef)
+		return -ENOMEM;
+
+	lr4->delay = mod_zalloc(mod, 2 * 4 * sizeof(float));
+	if (!lr4->delay) {
+		mod_free(mod, lr4->coef);
+		lr4->coef = NULL;
+		return -ENOMEM;
+	}
+
+	lr4->biquads = CROSSOVER_LR4_NUM_BIQUADS;
+	lr4->biquads_in_series = CROSSOVER_LR4_NUM_BIQUADS;
+
+	gain_factor = (float)coef->output_gain * inv_q14;
+	if (coef->output_shift > 0)
+		gain_factor /= (float)(1 << coef->output_shift);
+	else if (coef->output_shift < 0)
+		gain_factor *= (float)(1 << (-coef->output_shift));
+
+	b_scale = gain_factor * inv_q30;
+
+	b0 = (float)coef->b0 * b_scale;
+	b1 = (float)coef->b1 * b_scale;
+	b2 = (float)coef->b2 * b_scale;
+	a1 = (float)coef->a1 * inv_q30;
+	a2 = (float)coef->a2 * inv_q30;
+
+	/* Biquad 1 */
+	lr4->coef[0] = b0;
+	lr4->coef[1] = b1;
+	lr4->coef[2] = b2;
+	lr4->coef[3] = a1;
+	lr4->coef[4] = a2;
+
+	/* Biquad 2 (identical coefficients in series) */
+	lr4->coef[5] = b0;
+	lr4->coef[6] = b1;
+	lr4->coef[7] = b2;
+	lr4->coef[8] = a1;
+	lr4->coef[9] = a2;
+
+	return 0;
+}
+
+int crossover_init_coef_ch_float(struct processing_module *mod,
+				 struct sof_eq_iir_biquad *coef,
+				 struct crossover_state_float *ch_state,
+				 int32_t num_sinks)
+{
+	int32_t i;
+	int32_t j = 0;
+	int32_t num_lr4s = num_sinks == CROSSOVER_2WAY_NUM_SINKS ? 1 : 3;
+	int err;
+
+	for (i = 0; i < num_lr4s; i++) {
+		err = crossover_init_coef_lr4_float(mod, &coef[j],
+						    &ch_state->lowpass[i]);
+		if (err < 0)
+			return -EINVAL;
+		err = crossover_init_coef_lr4_float(mod, &coef[j + 1],
+						    &ch_state->highpass[i]);
+		if (err < 0)
+			return -EINVAL;
+		j += 2;
+	}
+
+	return 0;
+}
+#endif
+
 /**
  * \brief Initializes the coefficients of the crossover filter
  *	  and assign them to the first nch channels.
@@ -257,8 +343,14 @@ static int crossover_init_coef(struct processing_module *mod, int nch)
 	/* Collect the coef array and assign it to every channel */
 	crossover = config->coef;
 	for (ch = 0; ch < nch; ch++) {
-		err = crossover_init_coef_ch(mod, crossover, &cd->state[ch],
-					     config->num_sinks);
+#if CONFIG_FORMAT_FLOAT
+		if (cd->source_format == SOF_IPC_FRAME_FLOAT)
+			err = crossover_init_coef_ch_float(mod, crossover, &cd->state_f[ch],
+							   config->num_sinks);
+		else
+#endif
+			err = crossover_init_coef_ch(mod, crossover, &cd->state[ch],
+						     config->num_sinks);
 		/* Free all previously allocated blocks in case of an error */
 		if (err < 0) {
 			comp_err(mod->dev, "could not assign coefficients to ch %d",
@@ -616,12 +708,25 @@ static int crossover_prepare(struct processing_module *mod,
 			return -EINVAL;
 		}
 
-		cd->crossover_split = crossover_find_split_func(cd->config->num_sinks);
-		if (!cd->crossover_split) {
-			comp_err(dev, "No split function matching num_sinks %i",
-				 cd->config->num_sinks);
-			return -EINVAL;
+#if CONFIG_FORMAT_FLOAT
+		if (cd->source_format == SOF_IPC_FRAME_FLOAT) {
+			cd->crossover_split_f = crossover_find_split_func_float(cd->config->num_sinks);
+			if (!cd->crossover_split_f) {
+				comp_err(dev, "No float split function matching num_sinks %i",
+					 cd->config->num_sinks);
+				return -EINVAL;
+			}
+		} else {
+#endif
+			cd->crossover_split = crossover_find_split_func(cd->config->num_sinks);
+			if (!cd->crossover_split) {
+				comp_err(dev, "No split function matching num_sinks %i",
+					 cd->config->num_sinks);
+				return -EINVAL;
+			}
+#if CONFIG_FORMAT_FLOAT
 		}
+#endif
 	} else {
 		comp_info(dev, "setting crossover to passthrough mode");
 

src/audio/crossover/crossover.h

@@ -64,6 +64,9 @@ typedef int (*crossover_process_func)(struct comp_data *cd,
 struct comp_data {
 	/**< filter state */
 	struct crossover_state state[PLATFORM_MAX_CHANNELS];
+#if CONFIG_FORMAT_FLOAT
+	struct crossover_state_float state_f[PLATFORM_MAX_CHANNELS];
+#endif
 #if CONFIG_IPC_MAJOR_4
 	uint32_t output_pin_index[SOF_CROSSOVER_MAX_STREAMS];
 	uint32_t num_output_pins;
@@ -73,6 +76,9 @@ struct comp_data {
 	enum sof_ipc_frame source_format;         /**< source frame format */
 	crossover_process_func crossover_process;      /**< processing function */
 	crossover_split crossover_split;          /**< split function */
+#if CONFIG_FORMAT_FLOAT
+	crossover_split_float crossover_split_f;  /**< float split function */
+#endif
 };
 
 struct crossover_proc_fnmap {
@@ -129,6 +135,14 @@ static inline int32_t crossover_generic_process_lr4(int32_t in,
 	return iir_df1_4th(lr4, in);
 }
 
+#if CONFIG_FORMAT_FLOAT
+static inline float crossover_generic_process_lr4_float(float in,
+							struct iir_state_df1_float *lr4)
+{
+	return iir_df1_float(lr4, in);
+}
+#endif
+
 static inline void crossover_free_config(struct sof_crossover_config **config)
 {
 	rfree(*config);

src/audio/crossover/crossover_generic.c

@@ -378,6 +378,147 @@ static int crossover_s32_default(struct comp_data *cd,
 }
 #endif /* CONFIG_FORMAT_S32LE */
 
+#if CONFIG_FORMAT_FLOAT
+static inline void crossover_generic_lr4_split_float(struct iir_state_df1_float *lp,
+						     struct iir_state_df1_float *hp,
+						     float x, float *y1, float *y2)
+{
+	*y1 = crossover_generic_process_lr4_float(x, lp);
+	*y2 = crossover_generic_process_lr4_float(x, hp);
+}
+
+static inline void crossover_generic_lr4_merge_float(struct iir_state_df1_float *lp,
+						     struct iir_state_df1_float *hp,
+						     float x, float *y)
+{
+	float z1 = crossover_generic_process_lr4_float(x, lp);
+	float z2 = crossover_generic_process_lr4_float(x, hp);
+
+	*y = z1 + z2;
+}
+
+static void crossover_generic_split_2way_float(float in,
+					       float out[],
+					       struct crossover_state_float *state)
+{
+	crossover_generic_lr4_split_float(&state->lowpass[0], &state->highpass[0],
+					  in, &out[0], &out[1]);
+}
+
+static void crossover_generic_split_3way_float(float in,
+					       float out[],
+					       struct crossover_state_float *state)
+{
+	float z1, z2;
+
+	crossover_generic_lr4_split_float(&state->lowpass[0], &state->highpass[0],
+					  in, &z1, &z2);
+	crossover_generic_lr4_merge_float(&state->lowpass[1], &state->highpass[1],
+					  z1, &out[0]);
+	crossover_generic_lr4_split_float(&state->lowpass[2], &state->highpass[2],
+					  z2, &out[1], &out[2]);
+}
+
+static void crossover_generic_split_4way_float(float in,
+					       float out[],
+					       struct crossover_state_float *state)
+{
+	float z1, z2;
+
+	crossover_generic_lr4_split_float(&state->lowpass[1], &state->highpass[1],
+					  in, &z1, &z2);
+	crossover_generic_lr4_split_float(&state->lowpass[0], &state->highpass[0],
+					  z1, &out[0], &out[1]);
+	crossover_generic_lr4_split_float(&state->lowpass[2], &state->highpass[2],
+					  z2, &out[2], &out[3]);
+}
+
+const crossover_split_float crossover_split_float_fnmap[] = {
+	crossover_generic_split_2way_float,
+	crossover_generic_split_3way_float,
+	crossover_generic_split_4way_float,
+};
+
+static int crossover_float_default(struct comp_data *cd,
+				   struct sof_source *source,
+				   struct sof_sink **sinks,
+				   unsigned int num_sinks,
+				   size_t frames)
+{
+	float *y[SOF_CROSSOVER_MAX_STREAMS];
+	float *y_start[SOF_CROSSOVER_MAX_STREAMS];
+	float *y_end[SOF_CROSSOVER_MAX_STREAMS];
+	int out_idx[SOF_CROSSOVER_MAX_STREAMS];
+	float out[SOF_CROSSOVER_MAX_STREAMS];
+	struct crossover_state_float *state;
+	float const *x, *x_start, *x_end;
+	size_t x_samples, y_samples;
+	unsigned int nch = source_get_channels(source);
+	size_t bytes = frames * source_get_frame_bytes(source);
+	unsigned int active_sinks = 0;
+	size_t remaining_samples;
+	size_t i, n;
+	unsigned int ch, j;
+	int ret;
+
+	ret = source_get_data_s32(source, bytes, (const int32_t **)&x,
+				  (const int32_t **)&x_start, &x_samples);
+	if (ret)
+		return ret;
+	x_end = x_start + x_samples;
+
+	/* Identify active sinks, keeping the band index for correct routing */
+	for (j = 0; j < num_sinks; j++) {
+		if (!sinks[j])
+			continue;
+		ret = sink_get_buffer_s32(sinks[j], bytes, (int32_t **)&y[active_sinks],
+					  (int32_t **)&y_start[active_sinks], &y_samples);
+		if (ret) {
+			while (active_sinks--) {
+				sink_commit_buffer(sinks[out_idx[active_sinks]], 0);
+			}
+			source_release_data(source, 0);
+			return ret;
+		}
+		y_end[active_sinks] = y_start[active_sinks] + y_samples;
+		out_idx[active_sinks] = j;
+		active_sinks++;
+	}
+
+	remaining_samples = frames * nch;
+	while (remaining_samples) {
+		n = MIN(remaining_samples, cir_buf_samples_without_wrap_s32((const int32_t *)x, (const int32_t *)x_end));
+		for (j = 0; j < active_sinks; j++)
+			n = MIN(n, cir_buf_samples_without_wrap_s32((const int32_t *)y[j], (const int32_t *)y_end[j]));
+
+		for (ch = 0; ch < nch; ch++) {
+			state = &cd->state_f[ch];
+			for (i = ch; i < n; i += nch) {
+				cd->crossover_split_f(x[i], out, state);
+				for (j = 0; j < active_sinks; j++)
+					y[j][i] = out[out_idx[j]];
+			}
+		}
+
+		remaining_samples -= n;
+		x = (const float *)cir_buf_wrap((const int32_t *)x + n, (const int32_t *)x_start, (const int32_t *)x_end);
+		for (j = 0; j < active_sinks; j++)
+			y[j] = (float *)cir_buf_wrap((int32_t *)y[j] + n, (int32_t *)y_start[j], (int32_t *)y_end[j]);
+	}
+
+	ret = source_release_data(source, bytes);
+	if (ret)
+		return ret;
+	for (j = 0; j < active_sinks; j++) {
+		ret = sink_commit_buffer(sinks[out_idx[j]], bytes);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+#endif /* CONFIG_FORMAT_FLOAT */
+
 const struct crossover_proc_fnmap crossover_proc_fnmap[] = {
 /* { SOURCE_FORMAT , PROCESSING FUNCTION } */
 #if CONFIG_FORMAT_S16LE
@@ -391,6 +532,10 @@ const struct crossover_proc_fnmap crossover_proc_fnmap[] = {
 #if CONFIG_FORMAT_S32LE
 	{ SOF_IPC_FRAME_S32_LE, crossover_s32_default },
 #endif /* CONFIG_FORMAT_S32LE */
+
+#if CONFIG_FORMAT_FLOAT
+	{ SOF_IPC_FRAME_FLOAT, crossover_float_default },
+#endif /* CONFIG_FORMAT_FLOAT */
 };
 
 const struct crossover_proc_fnmap crossover_proc_fnmap_pass[] = {
@@ -406,6 +551,10 @@ const struct crossover_proc_fnmap crossover_proc_fnmap_pass[] = {
 #if CONFIG_FORMAT_S32LE
 	{ SOF_IPC_FRAME_S32_LE, crossover_default_pass },
 #endif /* CONFIG_FORMAT_S32LE */
+
+#if CONFIG_FORMAT_FLOAT
+	{ SOF_IPC_FRAME_FLOAT, crossover_default_pass },
+#endif /* CONFIG_FORMAT_FLOAT */
 };
 
 const size_t crossover_proc_fncount = ARRAY_SIZE(crossover_proc_fnmap);

src/audio/dcblock/dcblock.c

@@ -51,8 +51,10 @@ static void dcblock_set_passthrough(struct processing_module *mod)
 	comp_info(mod->dev, "entry");
 	int i;
 
-	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
+	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) {
 		cd->R_coeffs[i] = ONE_Q2_30;
+		cd->R_coeffs_f[i] = 1.0f;
+	}
 }
 
 /**
@@ -62,8 +64,11 @@ static void dcblock_set_passthrough(struct processing_module *mod)
 static void dcblock_copy_coefficients(struct processing_module *mod)
 {
 	struct comp_data *cd = module_get_private_data(mod);
+	int i;
 
 	memcpy_s(cd->R_coeffs, sizeof(cd->R_coeffs), cd->config, sizeof(cd->R_coeffs));
+	for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
+		cd->R_coeffs_f[i] = (float)cd->R_coeffs[i] / (float)ONE_Q2_30;
 }
 
 /**