diff --git a/src/audio/crossover/crossover.c b/src/audio/crossover/crossover.c index 2f696800db66..3a5c9cdd57b4 100644 --- a/src/audio/crossover/crossover.c +++ b/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"); diff --git a/src/audio/crossover/crossover.h b/src/audio/crossover/crossover.h index 068364b2b037..be49986dc377 100644 --- a/src/audio/crossover/crossover.h +++ b/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); diff --git a/src/audio/crossover/crossover_generic.c b/src/audio/crossover/crossover_generic.c index 41a55d9ec0d6..c4051bd93ea7 100644 --- a/src/audio/crossover/crossover_generic.c +++ b/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); diff --git a/src/audio/dcblock/dcblock.c b/src/audio/dcblock/dcblock.c index dbebc9ab5e32..45ccbacd63fe 100644 --- a/src/audio/dcblock/dcblock.c +++ b/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; } /** diff --git a/src/audio/dcblock/dcblock.h b/src/audio/dcblock/dcblock.h index dca1308b80ba..a88bb7892acc 100644 --- a/src/audio/dcblock/dcblock.h +++ b/src/audio/dcblock/dcblock.h @@ -25,8 +25,16 @@ struct cir_buf_source; struct cir_buf_sink; struct dcblock_state { - int32_t x_prev; /**< state variable referring to x[n-1] */ - int32_t y_prev; /**< state variable referring to y[n-1] */ + union { + struct { + int32_t x_prev; /**< state variable referring to x[n-1] */ + int32_t y_prev; /**< state variable referring to y[n-1] */ + }; + struct { + float x_prev_f; /**< float state variable x[n-1] */ + float y_prev_f; /**< float state variable y[n-1] */ + }; + }; }; /** @@ -48,6 +56,7 @@ struct comp_data { /** coefficients for the processing function */ int32_t R_coeffs[PLATFORM_MAX_CHANNELS]; + float R_coeffs_f[PLATFORM_MAX_CHANNELS]; struct comp_data_blob_handler *model_handler; int32_t *config; /**< pointer to setup blob */ diff --git a/src/audio/dcblock/dcblock_generic.c b/src/audio/dcblock/dcblock_generic.c index da22de8b39ae..cedc2af11c5d 100644 --- a/src/audio/dcblock/dcblock_generic.c +++ b/src/audio/dcblock/dcblock_generic.c @@ -175,6 +175,65 @@ static int dcblock_s32_default(struct comp_data *cd, } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +/** + * Generic float DC blocking filter: + * y[n] = x[n] - x[n-1] + R * y[n-1] + */ +static inline float dcblock_generic_float(struct dcblock_state *state, + float R, float x) +{ + float out = (x - state->x_prev_f) + (R * state->y_prev_f); + + state->y_prev_f = out; + state->x_prev_f = x; + + return out; +} + +/** + * dcblock_float_default() - Process FLOAT format. + * @cd: DC blocking filter component private data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * Return: Value zero for success, otherwise an error code. + */ +static int dcblock_float_default(struct comp_data *cd, + struct cir_buf_source *source, + struct cir_buf_sink *sink, + uint32_t frames) +{ + const float *x = source->ptr; + float *y = sink->ptr; + int samples_without_wrap; + int nch = cd->channels; + int remaining_samples = frames * nch; + int ch = 0; + int i; + + while (remaining_samples) { + samples_without_wrap = cir_buf_samples_without_wrap_s32(x, source->buf_end); + samples_without_wrap = MIN(samples_without_wrap, + cir_buf_samples_without_wrap_s32(y, sink->buf_end)); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + for (i = 0; i < samples_without_wrap; i++) { + *y = dcblock_generic_float(&cd->state[ch], cd->R_coeffs_f[ch], *x); + x++; + y++; + if (++ch == nch) + ch = 0; + } + x = cir_buf_wrap((void *)x, source->buf_start, source->buf_end); + y = cir_buf_wrap(y, sink->buf_start, sink->buf_end); + remaining_samples -= samples_without_wrap; + } + + return 0; +} +#endif /* CONFIG_FORMAT_FLOAT */ + const struct dcblock_func_map dcblock_fnmap[] = { /* { SOURCE_FORMAT , PROCESSING FUNCTION } */ #if CONFIG_FORMAT_S16LE @@ -186,6 +245,9 @@ const struct dcblock_func_map dcblock_fnmap[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, dcblock_s32_default }, #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, dcblock_float_default }, +#endif /* CONFIG_FORMAT_FLOAT */ }; const size_t dcblock_fncount = ARRAY_SIZE(dcblock_fnmap); diff --git a/src/audio/drc/drc.c b/src/audio/drc/drc.c index ceb1b1b0bd6e..37a0129962b6 100644 --- a/src/audio/drc/drc.c +++ b/src/audio/drc/drc.c @@ -402,6 +402,9 @@ static int drc_prepare(struct processing_module *mod, return -ENOTCONN; } + audio_stream_set_align(SOF_FRAME_BYTE_ALIGN, 2, &sourceb->stream); + audio_stream_set_align(SOF_FRAME_BYTE_ALIGN, 2, &sinkb->stream); + #if CONFIG_IPC_MAJOR_4 drc_params(mod); #endif @@ -435,11 +438,16 @@ static int drc_prepare(struct processing_module *mod, * non-enabled blob can be used when same pipeline is used for both * headphone and speaker where DRC should be off for headphone mode. */ - if (!cd->config->params.enabled) + if (!cd->config->params.enabled) cd->drc_func = drc_default_pass; + + comp_info(dev, "DRC mode: %s (master enabled=%d)", + cd->drc_func != drc_default_pass ? "ACTIVE COMPRESSOR" : "PASSTHROUGH", + cd->config->params.enabled); } else { /* Generic function for all formats */ cd->drc_func = drc_default_pass; + comp_info(dev, "DRC mode: PASSTHROUGH (no config blob)"); } comp_info(dev, "DRC is configured."); diff --git a/src/audio/drc/drc_generic.c b/src/audio/drc/drc_generic.c index 8f61972d5358..3fd1a1807504 100644 --- a/src/audio/drc/drc_generic.c +++ b/src/audio/drc/drc_generic.c @@ -114,6 +114,21 @@ void drc_update_detector_average(struct drc_state *state, abs_input_array[i] = MAX(abs_input_array[i], ABS(sample)); } } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + abs_input_array[i] = 0; + for (ch = 0; ch < nch; ch++) { + const float *sample_f = (const float *)state->pre_delay_buffers[ch] + div_start + i; + float val = *sample_f < 0.0f ? -*sample_f : *sample_f; + if (val > 1.0f) + val = 1.0f; + float scaled = val * 2147483647.0f; + sample = scaled >= 2147483647.0f ? INT32_MAX : (int32_t)scaled; + abs_input_array[i] = MAX(abs_input_array[i], sample); + } + } +#endif } else { /* 4 bytes per sample */ for (i = 0; i < DRC_DIVISION_FRAMES; i++) { abs_input_array[i] = 0; @@ -329,6 +344,34 @@ void drc_compress_output(struct drc_state *state, for (j = 0; j < 4; j++) x[j] = Q_MULTSR_32X32((int64_t)x[j], r4, 30, 30, 30); } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = + drc_sin_fixed(x[j] + base); + total_gain = Q_MULTSR_32X32((int64_t)p->master_linear_gain, + post_warp_compressor_gain, + 24, 31, 24); + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + for (j = 0; j < 4; j++) + x[j] = Q_MULTSR_32X32((int64_t)x[j], r4, 30, 30, 30); + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { @@ -412,6 +455,34 @@ void drc_compress_output(struct drc_state *state, x[j] = MIN(ONE_Q30, Q_MULTSR_32X32((int64_t)x[j], r4, 30, 30, 30)); } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); + total_gain = Q_MULTSR_32X32((int64_t)p->master_linear_gain, + post_warp_compressor_gain, + 24, 31, 24); + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + for (j = 0; j < 4; j++) + x[j] = MIN(ONE_Q30, + Q_MULTSR_32X32((int64_t)x[j], r4, 30, 30, 30)); + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { @@ -598,7 +669,7 @@ static void drc_s16_default(struct processing_module *mod, } #endif /* CONFIG_FORMAT_S16LE */ -#if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE +#if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE || CONFIG_FORMAT_FLOAT static void drc_delay_input_sample_s32(struct drc_state *state, const struct cir_buf_source *source, struct cir_buf_sink *sink, @@ -790,6 +861,47 @@ static void drc_s32_default(struct processing_module *mod, } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +static void drc_float_default(struct processing_module *mod, + const struct cir_buf_source *source, + struct cir_buf_sink *sink, + uint32_t frames) +{ + struct drc_comp_data *cd = module_get_private_data(mod); + int nch = cd->channels; + const int32_t *x = (int32_t *)source->ptr; + int32_t *y = (int32_t *)sink->ptr; + size_t samples = frames * nch; + struct drc_state *state = &cd->state; + const struct sof_drc_params *p = &cd->config->params; + size_t fragment_samples; + size_t fragment; + + if (!cd->enabled) { + drc_delay_input_sample_s32(state, source, sink, &x, &y, samples, nch); + return; + } + + if (!state->processed) { + drc_update_envelope(state, p); + drc_compress_output(state, p, 0, nch); + state->processed = 1; + } + + while (samples) { + fragment = DRC_DIVISION_FRAMES - + (state->pre_delay_write_index & DRC_DIVISION_FRAMES_MASK); + fragment_samples = fragment * nch; + fragment_samples = MIN(samples, fragment_samples); + drc_delay_input_sample_s32(state, source, sink, &x, &y, fragment_samples, nch); + samples -= fragment_samples; + + if ((state->pre_delay_write_index & DRC_DIVISION_FRAMES_MASK) == 0) + drc_process_one_division(state, p, 0, nch); + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + const struct drc_proc_fnmap drc_proc_fnmap[] = { /* { SOURCE_FORMAT , PROCESSING FUNCTION } */ #if CONFIG_FORMAT_S16LE @@ -803,6 +915,10 @@ const struct drc_proc_fnmap drc_proc_fnmap[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, drc_s32_default }, #endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, drc_float_default }, +#endif /* CONFIG_FORMAT_FLOAT */ }; const size_t drc_proc_fncount = ARRAY_SIZE(drc_proc_fnmap); diff --git a/src/audio/drc/drc_hifi3.c b/src/audio/drc/drc_hifi3.c index 3a5d23c7d230..9a5abdc04dc9 100644 --- a/src/audio/drc/drc_hifi3.c +++ b/src/audio/drc/drc_hifi3.c @@ -125,6 +125,23 @@ void drc_update_detector_average(struct drc_state *state, sample16_p++; } } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + for (ch = 0; ch < nch; ch++) { + const float *sample_f = (const float *)state->pre_delay_buffers[ch] + div_start; + abs_input_array_p = abs_input_array; + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + float val = *sample_f < 0.0f ? -*sample_f : *sample_f; + if (val > 1.0f) + val = 1.0f; + float scaled = val * 2147483647.0f; + sample = scaled >= 2147483647.0f ? INT32_MAX : (int32_t)scaled; + *abs_input_array_p = MAX(*abs_input_array_p, sample); + abs_input_array_p++; + sample_f++; + } + } +#endif } else { /* 4 bytes per sample */ for (ch = 0; ch < nch; ch++) { abs_input_array_p = abs_input_array; @@ -362,6 +379,36 @@ void drc_compress_output(struct drc_state *state, for (j = 0; j < 4; j++) x[j] = drc_mult_lshift(x[j], r4, lshift); } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + tmp = AE_ADD32(x[j], base); + post_warp_compressor_gain = drc_sin_fixed(tmp); /* Q1.31 */ + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); /* Q8.24 */ + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) + x[j] = drc_mult_lshift(x[j], r4, lshift); + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { @@ -452,6 +499,37 @@ void drc_compress_output(struct drc_state *state, x[j] = AE_MIN32(ONE_Q30, tmp); } } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); /* Q1.31 */ + lshift = drc_get_lshift(24, 31, 24); + total_gain = drc_mult_lshift(p->master_linear_gain, + post_warp_compressor_gain, + lshift); /* Q8.24 */ + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + div_start + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + lshift = drc_get_lshift(30, 30, 30); + for (j = 0; j < 4; j++) { + tmp = drc_mult_lshift(x[j], r4, lshift); + x[j] = AE_MIN32(ONE_Q30, tmp); + } + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { diff --git a/src/audio/drc/drc_hifi4.c b/src/audio/drc/drc_hifi4.c index 707aa2d89d98..74b5ff773758 100644 --- a/src/audio/drc/drc_hifi4.c +++ b/src/audio/drc/drc_hifi4.c @@ -158,6 +158,25 @@ void drc_update_detector_average(struct drc_state *state, AE_S32X2_IP(sample32, abs_input_array_p, sizeof(ae_int32x2)); } } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + for (ch = 0; ch < nch; ch++) { + const float *sample_f = (const float *)state->pre_delay_buffers[ch] + div_start; + abs_input_array_p = abs_input_array; + for (i = 0; i < DRC_DIVISION_FRAMES; i++) { + float val = *sample_f < 0.0f ? -*sample_f : *sample_f; + if (val > 1.0f) + val = 1.0f; + float scaled = val * 2147483647.0f; + int32_t sample = scaled >= 2147483647.0f ? INT32_MAX : (int32_t)scaled; + ae_int32 temp = AE_L32_I(abs_input_array_p, 0); + ae_int32 s32 = AE_MOVINT32_FROMINT32(sample); + s32 = AE_MAX32(s32, temp); + AE_S32_L_IP(s32, abs_input_array_p, sizeof(ae_int32)); + sample_f++; + } + } +#endif } else { /* 4 bytes per sample */ for (ch = 0; ch < nch; ch++) { abs_input_array_p = abs_input_array; @@ -387,6 +406,34 @@ void drc_compress_output(struct drc_state *state, for (j = 0; j < 4; j++) x[j] = drc_mult_lshift(x[j], r4, LSHIFT_QX30_QY30_QZ30); } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + tmp = AE_ADD32(x[j], base); + post_warp_compressor_gain = drc_sin_fixed(tmp); /* Q1.31 */ + tmp64 = AE_MULF32R_LL(master_linear_gain, + post_warp_compressor_gain); + total_gain = AE_ROUND32F48SSYM(tmp64); /* Q8.24 */ + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + for (j = 0; j < 4; j++) + x[j] = drc_mult_lshift(x[j], r4, LSHIFT_QX30_QY30_QZ30); + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { @@ -469,6 +516,35 @@ void drc_compress_output(struct drc_state *state, x[j] = AE_MIN32(ONE_Q30, tmp); } } +#if CONFIG_FORMAT_FLOAT + } else if (nbyte == 0) { /* float */ + const float gain_scale = 1.0f / 16777216.0f; + while (1) { + for (j = 0; j < 4; j++) { + post_warp_compressor_gain = drc_sin_fixed(x[j]); /* Q1.31 */ + tmp64 = AE_MULF32R_LL(master_linear_gain, + post_warp_compressor_gain); + total_gain = AE_ROUND32F48SSYM(tmp64); /* Q8.24 */ + float total_gain_f = (float)total_gain * gain_scale; + + for (ch = 0; ch < nch; ch++) { + float *sample_f = + (float *)state->pre_delay_buffers[ch] + + inc; + *sample_f = *sample_f * total_gain_f; + } + inc++; + } + + if (++i == count) + break; + + for (j = 0; j < 4; j++) { + tmp = drc_mult_lshift(x[j], r4, LSHIFT_QX30_QY30_QZ30); + x[j] = AE_MIN32(ONE_Q30, tmp); + } + } +#endif } else { /* 4 bytes per sample */ while (1) { for (j = 0; j < 4; j++) { diff --git a/src/audio/eq_fir/eq_fir.c b/src/audio/eq_fir/eq_fir.c index 066dea0f788d..ea5cb4c737dc 100644 --- a/src/audio/eq_fir/eq_fir.c +++ b/src/audio/eq_fir/eq_fir.c @@ -53,8 +53,15 @@ static void eq_fir_free_delaylines(struct processing_module *mod) mod_free(mod, cd->fir_delay); cd->fir_delay = NULL; cd->fir_delay_size = 0; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + + mod_free(mod, cd->fir_coef_f); + cd->fir_coef_f = NULL; + cd->fir_coef_f_size = 0; + + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { fir[i].delay = NULL; + fir[i].coef = NULL; + } } static int eq_fir_init_coef(struct comp_dev *dev, struct sof_eq_fir_config *config, @@ -226,12 +233,170 @@ static void eq_fir_init_delay(struct fir_state_32x16 *fir, } } +#if CONFIG_FORMAT_FLOAT +static int eq_fir_setup_float(struct processing_module *mod, int nch) +{ + struct comp_data *cd = module_get_private_data(mod); + struct comp_dev *dev = mod->dev; + struct sof_fir_coef_data *lookup[SOF_EQ_FIR_MAX_RESPONSES]; + float *lookup_f[SOF_EQ_FIR_MAX_RESPONSES] = {0}; + struct sof_fir_coef_data *eq; + int16_t *assign_response; + int16_t *coef_data; + size_t coef_words_max; + size_t delay_size_sum = 0; + size_t coef_size_sum = 0; + int resp = 0; + int i, j, s; + float *coef_storage; + float *delay_ptr; + + /* Free existing FIR channels data if it was allocated */ + eq_fir_free_delaylines(mod); + + if (!cd->config || cd->config_size < sizeof(*cd->config)) + return -EINVAL; + + if (cd->config->size > cd->config_size) + return -EINVAL; + + if (nch > PLATFORM_MAX_CHANNELS || + cd->config->channels_in_config > PLATFORM_MAX_CHANNELS || + !cd->config->channels_in_config) { + comp_err(dev, "invalid channels count"); + return -EINVAL; + } + + if (cd->config->channels_in_config & 0x1) { + comp_err(dev, "channels_in_config %u must be even", + cd->config->channels_in_config); + return -EINVAL; + } + + if (cd->config->number_of_responses > SOF_EQ_FIR_MAX_RESPONSES) { + comp_err(dev, "# of resp exceeds max"); + return -EINVAL; + } + + if (cd->config->size < sizeof(*cd->config) || + cd->config->size - sizeof(*cd->config) < + (size_t)cd->config->channels_in_config * sizeof(int16_t)) { + comp_err(dev, "config size %u too small", cd->config->size); + return -EINVAL; + } + + /* Update number of channels */ + cd->nch = nch; + + assign_response = ASSUME_ALIGNED(&cd->config->data[0], 4); + coef_data = ASSUME_ALIGNED(&cd->config->data[cd->config->channels_in_config], 4); + coef_words_max = (cd->config->size - sizeof(*cd->config)) / sizeof(int16_t) - + cd->config->channels_in_config; + + j = 0; + for (i = 0; i < SOF_EQ_FIR_MAX_RESPONSES; i++) { + if (i < cd->config->number_of_responses) { + if (j + SOF_FIR_COEF_NHEADER > coef_words_max) + return -EINVAL; + eq = (struct sof_fir_coef_data *)&coef_data[j]; + if (eq->length <= 0 || eq->length > SOF_FIR_MAX_LENGTH || + (eq->length & 0x3) || + j + SOF_FIR_COEF_NHEADER + eq->length > coef_words_max) + return -EINVAL; + lookup[i] = eq; + coef_size_sum += fir_coef_size_float(eq); + j += SOF_FIR_COEF_NHEADER + eq->length; + } else { + lookup[i] = NULL; + } + } + + /* Calculate delay line requirements */ + for (i = 0; i < nch; i++) { + if (i < cd->config->channels_in_config) + resp = assign_response[i]; + if (resp < 0) { + fir_reset_float(&cd->fir_f[i]); + continue; + } + if (resp >= cd->config->number_of_responses) + return -EINVAL; + + eq = lookup[resp]; + s = fir_delay_size_float(eq); + if (s <= 0) + return -EINVAL; + delay_size_sum += s; + } + + if (!delay_size_sum) + return 0; + + /* Allocate float coefficients */ + if (coef_size_sum) { + cd->fir_coef_f = mod_alloc(mod, coef_size_sum); + if (!cd->fir_coef_f) { + comp_err(dev, "coef allocation failed for size %zu", coef_size_sum); + return -ENOMEM; + } + cd->fir_coef_f_size = coef_size_sum; + } + + /* Allocate delay lines */ + cd->fir_delay = mod_alloc(mod, delay_size_sum); + if (!cd->fir_delay) { + comp_err(dev, "delay allocation failed for size %zu", delay_size_sum); + mod_free(mod, cd->fir_coef_f); + cd->fir_coef_f = NULL; + cd->fir_coef_f_size = 0; + return -ENOMEM; + } + memset(cd->fir_delay, 0, delay_size_sum); + cd->fir_delay_size = delay_size_sum; + + /* Initialize float coefficients and delay pointers */ + coef_storage = cd->fir_coef_f; + delay_ptr = (float *)cd->fir_delay; + + for (i = 0; i < nch; i++) { + if (i < cd->config->channels_in_config) + resp = assign_response[i]; + if (resp < 0) { + fir_reset_float(&cd->fir_f[i]); + continue; + } + eq = lookup[resp]; + if (!lookup_f[resp]) { + fir_init_coef_float(&cd->fir_f[i], eq, &coef_storage); + lookup_f[resp] = cd->fir_f[i].coef; + } else { + cd->fir_f[i].rwi = 0; + cd->fir_f[i].taps = (int)eq->length; + cd->fir_f[i].length = cd->fir_f[i].taps + 2; + cd->fir_f[i].out_shift = 0; + cd->fir_f[i].coef = lookup_f[resp]; + } + fir_init_delay_float(&cd->fir_f[i], &delay_ptr); + } + + return 0; +} +#endif + static int eq_fir_setup(struct processing_module *mod, int nch) { struct comp_data *cd = module_get_private_data(mod); struct comp_dev *dev = mod->dev; + struct comp_buffer *sourceb = comp_dev_get_first_data_producer(dev); + enum sof_ipc_frame fmt = sourceb ? audio_stream_get_frm_fmt(&sourceb->stream) : + SOF_IPC_FRAME_S32_LE; int delay_size; +#if CONFIG_FORMAT_FLOAT + if (fmt == SOF_IPC_FRAME_FLOAT) + return eq_fir_setup_float(mod, nch); +#endif + /* Free existing FIR channels data if it was allocated */ eq_fir_free_delaylines(mod); @@ -305,6 +470,8 @@ static int eq_fir_init(struct processing_module *mod) cd->eq_fir_func = NULL; cd->fir_delay = NULL; cd->fir_delay_size = 0; + cd->fir_coef_f = NULL; + cd->fir_coef_f_size = 0; cd->nch = -1; /* component model data handler */ @@ -317,8 +484,12 @@ static int eq_fir_init(struct processing_module *mod) md->private = cd; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { fir_reset(&cd->fir[i]); +#if CONFIG_FORMAT_FLOAT + fir_reset_float(&cd->fir_f[i]); +#endif + } return 0; } @@ -542,8 +713,12 @@ static int eq_fir_reset(struct processing_module *mod) eq_fir_free_delaylines(mod); cd->eq_fir_func = NULL; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { fir_reset(&cd->fir[i]); +#if CONFIG_FORMAT_FLOAT + fir_reset_float(&cd->fir_f[i]); +#endif + } return 0; } diff --git a/src/audio/eq_fir/eq_fir.h b/src/audio/eq_fir/eq_fir.h index ad3cebb75d30..a2f353c94ee5 100644 --- a/src/audio/eq_fir/eq_fir.h +++ b/src/audio/eq_fir/eq_fir.h @@ -23,6 +23,7 @@ #if SOF_USE_MIN_HIFI(3, FILTER) #include #endif +#include #include #include @@ -32,12 +33,17 @@ * FIR implementations receive already acquired circular source and sink views. */ struct comp_data { - struct fir_state_32x16 fir[PLATFORM_MAX_CHANNELS]; /**< filters state */ + union { + struct fir_state_32x16 fir[PLATFORM_MAX_CHANNELS]; /**< filters state */ + struct fir_state_float fir_f[PLATFORM_MAX_CHANNELS]; + }; struct comp_data_blob_handler *model_handler; struct sof_eq_fir_config *config; - int32_t *fir_delay; /**< pointer to allocated RAM */ + void *fir_delay; /**< pointer to allocated RAM */ + float *fir_coef_f; /**< pointer to float converted coefficients */ size_t config_size; /**< configuration size */ size_t fir_delay_size; /**< allocated size */ + size_t fir_coef_f_size; /**< allocated float coef size */ void (*eq_fir_func)(struct fir_state_32x16 fir[], const struct cir_buf_source *source, struct cir_buf_sink *sink, @@ -69,6 +75,19 @@ void eq_fir_2x_s32(struct fir_state_32x16 *fir, const struct cir_buf_source *sou struct cir_buf_sink *sink, size_t frames, unsigned int channels); #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +void eq_fir_float(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); + +void eq_fir_2x_float(struct fir_state_32x16 *fir, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels); + +static inline void set_float_fir(struct comp_data *cd) +{ + cd->eq_fir_func = eq_fir_2x_float; +} +#endif /* CONFIG_FORMAT_FLOAT */ + int set_fir_func(struct processing_module *mod, enum sof_ipc_frame fmt); int eq_fir_params(struct processing_module *mod); diff --git a/src/audio/eq_fir/eq_fir_generic.c b/src/audio/eq_fir/eq_fir_generic.c index d90cd20b4d6c..116b229809d5 100644 --- a/src/audio/eq_fir/eq_fir_generic.c +++ b/src/audio/eq_fir/eq_fir_generic.c @@ -138,5 +138,98 @@ void eq_fir_s32(struct fir_state_32x16 fir[], const struct cir_buf_source *sourc } } #endif /* CONFIG_FORMAT_S32LE */ - #endif /* SOF_USE_HIFI(NONE, FILTER) || SOF_USE_ARM_SIMD(FILTER) || SOF_USE_RISCV_SIMD(FILTER) */ + +#if CONFIG_FORMAT_FLOAT +void eq_fir_float(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) +{ + struct fir_state_float *fir_f = (struct fir_state_float *)fir; + struct fir_state_float *filter; + const float *src_channel; + float *dst_channel; + const float *src = source->ptr; + float *dst = sink->ptr; + size_t max_samples; + size_t chunk_samples; + size_t sample_index; + size_t remaining_samples = frames * channels; + unsigned int channel; + + while (remaining_samples) { + max_samples = cir_buf_samples_without_wrap_s32(src, source->buf_end); + chunk_samples = MIN(remaining_samples, max_samples); + max_samples = cir_buf_samples_without_wrap_s32(dst, sink->buf_end); + chunk_samples = MIN(chunk_samples, max_samples); + for (channel = 0; channel < channels; channel++) { + src_channel = src + channel; + dst_channel = dst + channel; + filter = &fir_f[channel]; + for (sample_index = 0; sample_index < chunk_samples; + sample_index += channels) { + *dst_channel = fir_float(filter, *src_channel); + src_channel += channels; + dst_channel += channels; + } + } + remaining_samples -= chunk_samples; + src = source_cir_buf_wrap(src + chunk_samples, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + chunk_samples, sink->buf_start, sink->buf_end); + } +} + +void eq_fir_2x_float(struct fir_state_32x16 fir[], const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames, unsigned int channels) +{ + struct fir_state_float *fir_f = (struct fir_state_float *)fir; + struct fir_state_float *filter; + const float *src = source->ptr; + float *dst = sink->ptr; + const float *x0, *x1; + float *y0, *y1; + size_t max_samples, chunk_samples, f; + size_t remaining_frames = frames; + unsigned int channel; + size_t inc_2channels = 2 * channels; + + while (remaining_frames) { + max_samples = cir_buf_samples_without_wrap_s32(src, source->buf_end); + chunk_samples = MIN(remaining_frames * channels, max_samples); + max_samples = cir_buf_samples_without_wrap_s32(dst, sink->buf_end); + chunk_samples = MIN(chunk_samples, max_samples); + f = chunk_samples / channels; + + if (f >= 2) { + f &= ~0x1; /* Process even number of frames */ + for (channel = 0; channel < channels; channel++) { + x0 = src + channel; + x1 = x0 + channels; + y0 = dst + channel; + y1 = y0 + channels; + filter = &fir_f[channel]; + for (size_t i = 0; i < f; i += 2) { + fir_float_2x(filter, *x0, *x1, y0, y1); + x0 += inc_2channels; + x1 += inc_2channels; + y0 += inc_2channels; + y1 += inc_2channels; + } + } + remaining_frames -= f; + src = source_cir_buf_wrap(src + f * channels, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + f * channels, sink->buf_start, sink->buf_end); + } else if (f == 1) { + /* Single frame fallback */ + for (channel = 0; channel < channels; channel++) { + filter = &fir_f[channel]; + *(dst + channel) = fir_float(filter, *(src + channel)); + } + remaining_frames -= 1; + src = source_cir_buf_wrap(src + channels, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst + channels, sink->buf_start, sink->buf_end); + } else { + break; + } + } +} +#endif /* CONFIG_FORMAT_FLOAT */ diff --git a/src/audio/eq_fir/eq_fir_ipc3.c b/src/audio/eq_fir/eq_fir_ipc3.c index ecfd873ffd7f..75c18a6be179 100644 --- a/src/audio/eq_fir/eq_fir_ipc3.c +++ b/src/audio/eq_fir/eq_fir_ipc3.c @@ -39,6 +39,12 @@ int set_fir_func(struct processing_module *mod, enum sof_ipc_frame fmt) set_s32_fir(cd); break; #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + comp_dbg(mod->dev, "SOF_IPC_FRAME_FLOAT"); + set_float_fir(cd); + break; +#endif /* CONFIG_FORMAT_FLOAT */ default: comp_err(mod->dev, "invalid frame_fmt"); return -EINVAL; diff --git a/src/audio/eq_fir/eq_fir_ipc4.c b/src/audio/eq_fir/eq_fir_ipc4.c index 64ec2895e0dd..769a3cc9f977 100644 --- a/src/audio/eq_fir/eq_fir_ipc4.c +++ b/src/audio/eq_fir/eq_fir_ipc4.c @@ -24,6 +24,13 @@ int set_fir_func(struct processing_module *mod, enum sof_ipc_frame fmt) struct comp_data *cd = module_get_private_data(mod); unsigned int valid_bit_depth = mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth; +#if CONFIG_FORMAT_FLOAT + if (fmt == SOF_IPC_FRAME_FLOAT) { + set_float_fir(cd); + return 0; + } +#endif + comp_dbg(mod->dev, "valid_bit_depth %d", valid_bit_depth); switch (valid_bit_depth) { #if CONFIG_FORMAT_S16LE diff --git a/src/audio/eq_iir/eq_iir.c b/src/audio/eq_iir/eq_iir.c index 2df6bed85ed0..edf4d3bf55e2 100644 --- a/src/audio/eq_iir/eq_iir.c +++ b/src/audio/eq_iir/eq_iir.c @@ -307,6 +307,17 @@ void eq_iir_free_delaylines(struct processing_module *mod) cd->iir_delay_size = 0; for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) iir[i].delay = NULL; + +#if CONFIG_FORMAT_FLOAT + mod_free(mod, cd->iir_delay_float); + mod_free(mod, cd->iir_coef_float); + cd->iir_delay_float = NULL; + cd->iir_coef_float = NULL; + cd->iir_delay_float_size = 0; + cd->iir_coef_float_size = 0; + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + iir_reset_df1_float(&cd->iir_float[i]); +#endif } void eq_iir_pass(struct processing_module *mod, const struct cir_buf_source *source, @@ -374,6 +385,136 @@ int eq_iir_setup(struct processing_module *mod, unsigned int nch) return 0; } +#if CONFIG_FORMAT_FLOAT +int eq_iir_setup_float(struct processing_module *mod, unsigned int nch) +{ + struct comp_data *cd = module_get_private_data(mod); + struct sof_eq_iir_config *config = cd->config; + struct sof_eq_iir_header *lookup[SOF_EQ_IIR_MAX_RESPONSES]; + int32_t *assign_response; + size_t delay_size = 0; + size_t coef_size = 0; + int resp = 0; + unsigned int i; + int ret; + + cd->channels = nch; + + eq_iir_free_delaylines(mod); + + if (nch > PLATFORM_MAX_CHANNELS) + return -EINVAL; + + ret = eq_iir_walk_config(mod->dev, config, cd->config_size, lookup); + if (ret < 0) + return ret; + + assign_response = ASSUME_ALIGNED(&config->data[0], 4); + for (i = 0; i < nch; i++) { + if (i < config->channels_in_config) + resp = assign_response[i]; + + if (resp < 0 || resp >= config->number_of_responses) { + iir_reset_df1_float(&cd->iir_float[i]); + continue; + } + + struct sof_eq_iir_header *eq = lookup[resp]; + int ds = iir_delay_size_df1_float(eq); + int cs = iir_coef_size_df1_float(eq); + if (ds <= 0 || cs <= 0) + return -EINVAL; + delay_size += (size_t)ds; + coef_size += (size_t)cs; + } + + if (!delay_size) + return 0; + + cd->iir_delay_float = mod_zalloc(mod, delay_size); + cd->iir_coef_float = mod_zalloc(mod, coef_size); + if (!cd->iir_delay_float || !cd->iir_coef_float) { + eq_iir_free_delaylines(mod); + return -ENOMEM; + } + + cd->iir_delay_float_size = delay_size; + cd->iir_coef_float_size = coef_size; + cd->iir_delay_size = delay_size; /* non-zero marks active */ + + float *delay_ptr = cd->iir_delay_float; + float *coef_ptr = cd->iir_coef_float; + + for (i = 0; i < nch; i++) { + if (i < config->channels_in_config) + resp = assign_response[i]; + + if (resp < 0 || resp >= config->number_of_responses) { + iir_reset_df1_float(&cd->iir_float[i]); + continue; + } + + struct sof_eq_iir_header *eq = lookup[resp]; + iir_init_coef_df1_float(&cd->iir_float[i], eq, &coef_ptr); + iir_init_delay_df1_float(&cd->iir_float[i], &delay_ptr); + } + + return 0; +} + +void eq_iir_float_default(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const float *x0; + float *y0; + const float *x; + float *y; + size_t nmax; + size_t n1; + size_t n2; + unsigned int i; + size_t j; + size_t n; + const unsigned int nch = cd->channels; + const size_t samples = frames * nch; + size_t processed = 0; + + x = source->ptr; + y = sink->ptr; + while (processed < samples) { + nmax = samples - processed; + n1 = cir_buf_samples_without_wrap_s32(x, source->buf_end); + n2 = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + n = MIN(n1, n2); + n = MIN(n, nmax); + for (i = 0; i < nch; i++) { + x0 = x + i; + y0 = y + i; + struct iir_state_df1_float *filter = &cd->iir_float[i]; + + if (!filter->biquads) { + for (j = 0; j < n; j += nch) { + *y0 = *x0; + x0 += nch; + y0 += nch; + } + continue; + } + + for (j = 0; j < n; j += nch) { + *y0 = iir_df1_float(filter, *x0); + x0 += nch; + y0 += nch; + } + } + processed += n; + x = source_cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif + /* * End of EQ setup code. Next the standard component methods. */ @@ -405,8 +546,12 @@ static int eq_iir_init(struct processing_module *mod) */ comp_data_blob_set_validator(cd->model_handler, eq_iir_validate_config); - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { iir_reset_df1(&cd->iir[i]); +#if CONFIG_FORMAT_FLOAT + iir_reset_df1_float(&cd->iir_float[i]); +#endif + } return 0; } @@ -624,8 +769,12 @@ static int eq_iir_reset(struct processing_module *mod) cd->eq_iir_func = NULL; cd->channels = 0; cd->frame_bytes = 0; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) { iir_reset_df1(&cd->iir[i]); +#if CONFIG_FORMAT_FLOAT + iir_reset_df1_float(&cd->iir_float[i]); +#endif + } return 0; } @@ -682,26 +831,33 @@ static int eq_iir_static_apply_switch(struct comp_dev *dev, uint32_t channels, i /* Disable EQ by engaging pass-through routine */ cd->eq_iir_func = eq_iir_pass; } else if (cd->iir_delay_size) { - /* Re-enable EQ processing matching configured valid bit depth */ - unsigned int valid_bit_depth = mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth; +#if CONFIG_IPC_MAJOR_4 +#if CONFIG_FORMAT_FLOAT + if (mod->priv.cfg.base_cfg.audio_fmt.s_type == IPC4_TYPE_FLOAT) { + cd->eq_iir_func = eq_iir_float_default; + } else +#endif + { + /* Re-enable EQ processing matching configured valid bit depth */ + unsigned int valid_bit_depth = mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth; - switch (valid_bit_depth) { + switch (valid_bit_depth) { #if CONFIG_FORMAT_S16LE - case IPC4_DEPTH_16BIT: - cd->eq_iir_func = eq_iir_s16_default; - break; + case IPC4_DEPTH_16BIT: + cd->eq_iir_func = eq_iir_s16_default; + break; #endif #if CONFIG_FORMAT_S24LE - case IPC4_DEPTH_24BIT: - cd->eq_iir_func = eq_iir_s24_default; - break; + case IPC4_DEPTH_24BIT: + cd->eq_iir_func = eq_iir_s24_default; + break; #endif #if CONFIG_FORMAT_S32LE - case IPC4_DEPTH_32BIT: - cd->eq_iir_func = eq_iir_s32_default; - break; + case IPC4_DEPTH_32BIT: + cd->eq_iir_func = eq_iir_s32_default; + break; #endif - default: + default: /* Fallback inspecting producer stream frame format */ { struct comp_buffer *sourceb = comp_dev_get_first_data_producer(dev); @@ -720,6 +876,16 @@ static int eq_iir_static_apply_switch(struct comp_dev *dev, uint32_t channels, i case SOF_IPC_FRAME_S24_3LE: cd->eq_iir_func = eq_iir_s24_default; break; +#endif +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + cd->eq_iir_func = eq_iir_float_default; + break; +#endif +#if CONFIG_FORMAT_S32LE + case SOF_IPC_FRAME_S32_LE: + cd->eq_iir_func = eq_iir_s32_default; + break; #endif default: #if CONFIG_FORMAT_S32LE @@ -734,7 +900,49 @@ static int eq_iir_static_apply_switch(struct comp_dev *dev, uint32_t channels, i } break; } + } +#else + /* Under IPC3: inspect producer stream frame format directly */ + struct comp_buffer *sourceb = comp_dev_get_first_data_producer(dev); + enum sof_ipc_frame fmt = sourceb ? + audio_stream_get_frm_fmt(&sourceb->stream) : + SOF_IPC_FRAME_S32_LE; + + switch (fmt) { +#if CONFIG_FORMAT_S16LE + case SOF_IPC_FRAME_S16_LE: + cd->eq_iir_func = eq_iir_s16_default; + break; +#endif +#if CONFIG_FORMAT_S24LE + case SOF_IPC_FRAME_S24_4LE: + case SOF_IPC_FRAME_S24_3LE: + cd->eq_iir_func = eq_iir_s24_default; + break; +#endif +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + cd->eq_iir_func = eq_iir_float_default; + break; +#endif +#if CONFIG_FORMAT_S32LE + case SOF_IPC_FRAME_S32_LE: + cd->eq_iir_func = eq_iir_s32_default; + break; +#endif + default: +#if CONFIG_FORMAT_S32LE + cd->eq_iir_func = eq_iir_s32_default; +#elif CONFIG_FORMAT_S16LE + cd->eq_iir_func = eq_iir_s16_default; +#else + cd->eq_iir_func = eq_iir_pass; +#endif + break; + } +#endif } + return 0; } diff --git a/src/audio/eq_iir/eq_iir.h b/src/audio/eq_iir/eq_iir.h index 8ca41fbc74b4..ded5f029fc24 100644 --- a/src/audio/eq_iir/eq_iir.h +++ b/src/audio/eq_iir/eq_iir.h @@ -31,9 +31,18 @@ struct eq_iir_func_map { eq_iir_func func; /**< processing function */ }; +#include + /* IIR component private data */ struct comp_data { struct iir_state_df1 iir[PLATFORM_MAX_CHANNELS]; /**< filters state */ +#if CONFIG_FORMAT_FLOAT + struct iir_state_df1_float iir_float[PLATFORM_MAX_CHANNELS]; /**< float filters state */ + float *iir_coef_float; /**< pointer to float coef memory */ + float *iir_delay_float; /**< pointer to float delay memory */ + size_t iir_coef_float_size; /**< allocated float coef size */ + size_t iir_delay_float_size; /**< allocated float delay size */ +#endif struct comp_data_blob_handler *model_handler; struct sof_eq_iir_config *config; int32_t *iir_delay; /**< pointer to allocated RAM */ @@ -57,6 +66,12 @@ void eq_iir_s24_default(struct processing_module *mod, const struct cir_buf_sour void eq_iir_s32_default(struct processing_module *mod, const struct cir_buf_source *source, struct cir_buf_sink *sink, size_t frames); +#if CONFIG_FORMAT_FLOAT +void eq_iir_float_default(struct processing_module *mod, const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames); +int eq_iir_setup_float(struct processing_module *mod, unsigned int nch); +#endif + int eq_iir_new_blob(struct processing_module *mod, enum sof_ipc_frame source_format, enum sof_ipc_frame sink_format, unsigned int channels); diff --git a/src/audio/eq_iir/eq_iir_ipc3.c b/src/audio/eq_iir/eq_iir_ipc3.c index fda3822dc23c..c0f183af0d86 100644 --- a/src/audio/eq_iir/eq_iir_ipc3.c +++ b/src/audio/eq_iir/eq_iir_ipc3.c @@ -211,6 +211,9 @@ const struct eq_iir_func_map fm_configured[] = { #if CONFIG_FORMAT_S32LE {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, eq_iir_s32_default}, #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + {SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_FLOAT, eq_iir_float_default}, +#endif /* CONFIG_FORMAT_FLOAT */ }; const struct eq_iir_func_map fm_passthrough[] = { @@ -236,6 +239,9 @@ const struct eq_iir_func_map fm_passthrough[] = { #if CONFIG_FORMAT_S32LE {SOF_IPC_FRAME_S32_LE, SOF_IPC_FRAME_S32_LE, eq_iir_pass}, #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + {SOF_IPC_FRAME_FLOAT, SOF_IPC_FRAME_FLOAT, eq_iir_pass}, +#endif /* CONFIG_FORMAT_FLOAT */ }; static eq_iir_func eq_iir_find_func(enum sof_ipc_frame source_format, @@ -299,16 +305,33 @@ int eq_iir_new_blob(struct processing_module *mod, enum sof_ipc_frame source_for struct comp_data *cd = module_get_private_data(mod); int ret; +#if CONFIG_FORMAT_FLOAT + if (source_format == SOF_IPC_FRAME_FLOAT) { + ret = eq_iir_setup_float(mod, channels); + if (ret < 0) { + comp_err(mod->dev, "failed float IIR setup"); + return ret; + } else if (cd->iir_delay_size) { + comp_info(mod->dev, "IIR EQ mode: ACTIVE FLOAT (delay_size=%zu)", cd->iir_delay_size); + cd->eq_iir_func = eq_iir_float_default; + } else { + comp_info(mod->dev, "IIR EQ mode: PASSTHROUGH FLOAT"); + cd->eq_iir_func = eq_iir_pass; + } + return 0; + } +#endif + ret = eq_iir_setup(mod, channels); if (ret < 0) { comp_err(mod->dev, "failed IIR setup"); return ret; } else if (cd->iir_delay_size) { - comp_dbg(mod->dev, "active"); + comp_info(mod->dev, "IIR EQ mode: ACTIVE (delay_size=%zu)", cd->iir_delay_size); cd->eq_iir_func = eq_iir_find_func(source_format, sink_format, fm_configured, ARRAY_SIZE(fm_configured)); } else { - comp_dbg(mod->dev, "pass-through"); + comp_info(mod->dev, "IIR EQ mode: PASSTHROUGH"); cd->eq_iir_func = eq_iir_find_func(source_format, sink_format, fm_passthrough, ARRAY_SIZE(fm_passthrough)); } diff --git a/src/audio/eq_iir/eq_iir_ipc4.c b/src/audio/eq_iir/eq_iir_ipc4.c index 3290e0e31c6c..02f07d5af7d0 100644 --- a/src/audio/eq_iir/eq_iir_ipc4.c +++ b/src/audio/eq_iir/eq_iir_ipc4.c @@ -82,13 +82,24 @@ int eq_iir_new_blob(struct processing_module *mod, enum sof_ipc_frame source_for struct comp_data *cd = module_get_private_data(mod); int ret; - ret = eq_iir_setup(mod, channels); +#if CONFIG_FORMAT_FLOAT + if (source_format == SOF_IPC_FRAME_FLOAT) + ret = eq_iir_setup_float(mod, channels); + else +#endif + ret = eq_iir_setup(mod, channels); + if (ret < 0) { comp_err(mod->dev, "failed IIR setup"); return ret; } else if (cd->iir_delay_size) { comp_dbg(mod->dev, "active"); - cd->eq_iir_func = eq_iir_find_func(mod); +#if CONFIG_FORMAT_FLOAT + if (source_format == SOF_IPC_FRAME_FLOAT) + cd->eq_iir_func = eq_iir_float_default; + else +#endif + cd->eq_iir_func = eq_iir_find_func(mod); } else { comp_dbg(mod->dev, "pass-through"); cd->eq_iir_func = eq_iir_pass; diff --git a/src/audio/level_multiplier/level_multiplier-generic.c b/src/audio/level_multiplier/level_multiplier-generic.c index 50649ffe300b..7daa2e92a165 100644 --- a/src/audio/level_multiplier/level_multiplier-generic.c +++ b/src/audio/level_multiplier/level_multiplier-generic.c @@ -252,6 +252,81 @@ static int level_multiplier_s32(const struct processing_module *mod, } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +/** + * level_multiplier_float() - Process FLOAT format. + * @mod: Pointer to module data. + * @source: Source for PCM samples data. + * @sink: Sink for PCM samples data. + * @frames: Number of audio data frames to process. + * + * Return: Value zero for success, otherwise an error code. + */ +static int level_multiplier_float(const struct processing_module *mod, + struct sof_source *source, + struct sof_sink *sink, + uint32_t frames) +{ + struct level_multiplier_comp_data *cd = module_get_private_data(mod); + const float gain = (float)cd->gain * (1.0f / (float)LEVEL_MULTIPLIER_GAIN_ONE); + int32_t const *x, *x_start; + int32_t *y, *y_start; + size_t x_size, y_size; + int source_samples_without_wrap; + int samples_without_wrap; + int remaining_samples = frames * cd->channels; + int bytes = frames * cd->frame_bytes; + int ret; + int i; + + ret = source_get_data_s32(source, bytes, &x, &x_start, &x_size); + if (ret) + return ret; + + ret = sink_get_buffer_s32(sink, bytes, &y, &y_start, &y_size); + if (ret) { + source_release_data(source, 0); + return ret; + } + + float const *xf = (float const *)x; + float const *xf_start = (float const *)x_start; + float const *xf_end = xf_start + x_size; + float *yf = (float *)y; + float *yf_start = (float *)y_start; + float *yf_end = yf_start + y_size; + + while (remaining_samples) { + source_samples_without_wrap = xf_end - xf; + samples_without_wrap = yf_end - yf; + samples_without_wrap = MIN(samples_without_wrap, source_samples_without_wrap); + samples_without_wrap = MIN(samples_without_wrap, remaining_samples); + + int samples4 = samples_without_wrap & ~3; + for (i = 0; i < samples4; i += 4) { + yf[0] = xf[0] * gain; + yf[1] = xf[1] * gain; + yf[2] = xf[2] * gain; + yf[3] = xf[3] * gain; + xf += 4; + yf += 4; + } + for (; i < samples_without_wrap; i++) { + *yf++ = *xf++ * gain; + } + + xf = (xf >= xf_end) ? xf - x_size : xf; + yf = (yf >= yf_end) ? yf - y_size : yf; + + remaining_samples -= samples_without_wrap; + } + + source_release_data(source, bytes); + sink_commit_buffer(sink, bytes); + return 0; +} +#endif /* CONFIG_FORMAT_FLOAT */ + /* This struct array defines the used processing functions for * the PCM formats */ @@ -265,6 +340,9 @@ const struct level_multiplier_proc_fnmap level_multiplier_proc_fnmap[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, level_multiplier_s32 }, #endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, level_multiplier_float }, +#endif }; /** diff --git a/src/audio/level_multiplier/level_multiplier-ipc4.c b/src/audio/level_multiplier/level_multiplier-ipc4.c index 02289d844f63..26e918793d97 100644 --- a/src/audio/level_multiplier/level_multiplier-ipc4.c +++ b/src/audio/level_multiplier/level_multiplier-ipc4.c @@ -36,6 +36,9 @@ __cold int level_multiplier_set_config(struct processing_module *mod, } memcpy_s(&cd->gain, sizeof(int32_t), fragment, sizeof(int32_t)); +#if CONFIG_FORMAT_FLOAT + cd->gain_f = (float)cd->gain * (1.0f / (float)LEVEL_MULTIPLIER_GAIN_ONE); +#endif comp_dbg(mod->dev, "Gain set to %d", cd->gain); return 0; } diff --git a/src/audio/level_multiplier/level_multiplier.c b/src/audio/level_multiplier/level_multiplier.c index 3adfea0ad24c..4bb4ce441d26 100644 --- a/src/audio/level_multiplier/level_multiplier.c +++ b/src/audio/level_multiplier/level_multiplier.c @@ -41,6 +41,9 @@ __cold static int level_multiplier_init(struct processing_module *mod) md->private = cd; cd->gain = LEVEL_MULTIPLIER_GAIN_ONE; +#if CONFIG_FORMAT_FLOAT + cd->gain_f = 1.0f; +#endif return 0; } @@ -146,6 +149,9 @@ static int level_multiplier_reset(struct processing_module *mod) memset(cd, 0, sizeof(*cd)); cd->gain = LEVEL_MULTIPLIER_GAIN_ONE; +#if CONFIG_FORMAT_FLOAT + cd->gain_f = 1.0f; +#endif return 0; } diff --git a/src/audio/level_multiplier/level_multiplier.h b/src/audio/level_multiplier/level_multiplier.h index e6df98c68c61..7fa300f13acb 100644 --- a/src/audio/level_multiplier/level_multiplier.h +++ b/src/audio/level_multiplier/level_multiplier.h @@ -47,6 +47,9 @@ typedef int (*level_multiplier_func)(const struct processing_module *mod, struct level_multiplier_comp_data { level_multiplier_func level_multiplier_func; int32_t gain; +#if CONFIG_FORMAT_FLOAT + float gain_f; +#endif int source_format; int frame_bytes; int channels; diff --git a/src/audio/mixer/mixer_generic.c b/src/audio/mixer/mixer_generic.c index b4fbd98160bd..aec764072aa8 100644 --- a/src/audio/mixer/mixer_generic.c +++ b/src/audio/mixer/mixer_generic.c @@ -33,16 +33,36 @@ static void mix_n_s16(struct cir_buf_sink *sink, struct cir_buf_source *sources, ns = cir_buf_samples_without_wrap_s16(src[j], sources[j].buf_end); n = MIN(n, ns); } - for (i = 0; i < n; i++) { - val = 0; - for (j = 0; j < num_sources; j++) { - val += *src[j]; - src[j]++; + if (num_sources == 2) { + const int16_t *s0 = src[0]; + const int16_t *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = sat_int16((int32_t)s0[0] + (int32_t)s1[0]); + dest[1] = sat_int16((int32_t)s0[1] + (int32_t)s1[1]); + dest[2] = sat_int16((int32_t)s0[2] + (int32_t)s1[2]); + dest[3] = sat_int16((int32_t)s0[3] + (int32_t)s1[3]); + s0 += 4; + s1 += 4; + dest += 4; } + for (; i < n; i++) { + *dest++ = sat_int16((int32_t)*s0++ + (int32_t)*s1++); + } + src[0] = (int16_t *)s0; + src[1] = (int16_t *)s1; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + val += *src[j]; + src[j]++; + } - /* Saturate to 16 bits */ - *dest = sat_int16(val); - dest++; + /* Saturate to 16 bits */ + *dest = sat_int16(val); + dest++; + } } processed += n; dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); @@ -77,17 +97,37 @@ static void mix_n_s24(struct cir_buf_sink *sink, struct cir_buf_source *sources, ns = cir_buf_samples_without_wrap_s32(src[j], sources[j].buf_end); n = MIN(n, ns); } - for (i = 0; i < n; i++) { - val = 0; - for (j = 0; j < num_sources; j++) { - x = *src[j] << 8; - val += x >> 8; /* Sign extend */ - src[j]++; + if (num_sources == 2) { + const int32_t *s0 = src[0]; + const int32_t *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = sat_int24(sign_extend_s24(s0[0]) + sign_extend_s24(s1[0])); + dest[1] = sat_int24(sign_extend_s24(s0[1]) + sign_extend_s24(s1[1])); + dest[2] = sat_int24(sign_extend_s24(s0[2]) + sign_extend_s24(s1[2])); + dest[3] = sat_int24(sign_extend_s24(s0[3]) + sign_extend_s24(s1[3])); + s0 += 4; + s1 += 4; + dest += 4; + } + for (; i < n; i++) { + *dest++ = sat_int24(sign_extend_s24(*s0++) + sign_extend_s24(*s1++)); } + src[0] = (int32_t *)s0; + src[1] = (int32_t *)s1; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + x = *src[j] << 8; + val += x >> 8; /* Sign extend */ + src[j]++; + } - /* Saturate to 24 bits */ - *dest = sat_int24(val); - dest++; + /* Saturate to 24 bits */ + *dest = sat_int24(val); + dest++; + } } processed += n; dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); @@ -121,16 +161,36 @@ static void mix_n_s32(struct cir_buf_sink *sink, struct cir_buf_source *sources, ns = cir_buf_samples_without_wrap_s32(src[j], sources[j].buf_end); n = MIN(n, ns); } - for (i = 0; i < n; i++) { - val = 0; - for (j = 0; j < num_sources; j++) { - val += *src[j]; - src[j]++; + if (num_sources == 2) { + const int32_t *s0 = src[0]; + const int32_t *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = sat_int32((int64_t)s0[0] + (int64_t)s1[0]); + dest[1] = sat_int32((int64_t)s0[1] + (int64_t)s1[1]); + dest[2] = sat_int32((int64_t)s0[2] + (int64_t)s1[2]); + dest[3] = sat_int32((int64_t)s0[3] + (int64_t)s1[3]); + s0 += 4; + s1 += 4; + dest += 4; + } + for (; i < n; i++) { + *dest++ = sat_int32((int64_t)*s0++ + (int64_t)*s1++); } + src[0] = (int32_t *)s0; + src[1] = (int32_t *)s1; + } else { + for (i = 0; i < n; i++) { + val = 0; + for (j = 0; j < num_sources; j++) { + val += *src[j]; + src[j]++; + } - /* Saturate to 32 bits */ - *dest = sat_int32(val); - dest++; + /* Saturate to 32 bits */ + *dest = sat_int32(val); + dest++; + } } processed += n; dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); @@ -141,6 +201,68 @@ static void mix_n_s32(struct cir_buf_sink *sink, struct cir_buf_source *sources, } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +/* Mix n float source streams to one sink stream */ +static void mix_n_float(struct cir_buf_sink *sink, struct cir_buf_source *sources, + int num_sources, size_t samples) +{ + const float *src[PLATFORM_MAX_CHANNELS]; + float *dest = sink->ptr; + float val; + size_t nmax, ns, n, i; + size_t processed = 0; + int j; + + for (j = 0; j < num_sources; j++) + src[j] = sources[j].ptr; + + while (processed < samples) { + nmax = samples - processed; + n = cir_buf_samples_without_wrap_s32(dest, sink->buf_end); + n = MIN(n, nmax); + for (j = 0; j < num_sources; j++) { + ns = cir_buf_samples_without_wrap_s32(src[j], sources[j].buf_end); + n = MIN(n, ns); + } + + if (num_sources == 2) { + const float *s0 = src[0]; + const float *s1 = src[1]; + i = 0; + for (; i + 3 < n; i += 4) { + dest[0] = s0[0] + s1[0]; + dest[1] = s0[1] + s1[1]; + dest[2] = s0[2] + s1[2]; + dest[3] = s0[3] + s1[3]; + s0 += 4; + s1 += 4; + dest += 4; + } + for (; i < n; i++) { + *dest++ = *s0++ + *s1++; + } + src[0] = s0; + src[1] = s1; + } else { + for (i = 0; i < n; i++) { + val = 0.0f; + for (j = 0; j < num_sources; j++) { + val += *src[j]; + src[j]++; + } + *dest++ = val; + } + } + + processed += n; + dest = cir_buf_wrap(dest, sink->buf_start, sink->buf_end); + for (j = 0; j < num_sources; j++) + src[j] = source_cir_buf_wrap(src[j], sources[j].buf_start, + sources[j].buf_end); + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + const struct mixer_func_map mixer_func_map[] = { #if CONFIG_FORMAT_S16LE { SOF_IPC_FRAME_S16_LE, mix_n_s16 }, @@ -151,6 +273,9 @@ const struct mixer_func_map mixer_func_map[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, mix_n_s32 }, #endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, mix_n_float }, +#endif }; const size_t mixer_func_count = ARRAY_SIZE(mixer_func_map); diff --git a/src/audio/mixin_mixout/mixin_mixout.c b/src/audio/mixin_mixout/mixin_mixout.c index 7b7ba12513c5..6921f22a8894 100644 --- a/src/audio/mixin_mixout/mixin_mixout.c +++ b/src/audio/mixin_mixout/mixin_mixout.c @@ -738,6 +738,9 @@ static int mixin_prepare(struct processing_module *mod, case SOF_IPC_FRAME_S16_LE: case SOF_IPC_FRAME_S24_4LE: case SOF_IPC_FRAME_S32_LE: +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: +#endif mixin_get_processing_functions(fmt, &md->mix, &md->gain_mix); break; default: diff --git a/src/audio/mixin_mixout/mixin_mixout_generic.c b/src/audio/mixin_mixout/mixin_mixout_generic.c index 55abbe1761ec..5d3852217dae 100644 --- a/src/audio/mixin_mixout/mixin_mixout_generic.c +++ b/src/audio/mixin_mixout/mixin_mixout_generic.c @@ -36,10 +36,18 @@ static void mix_s16(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixe n = MIN(left_samples, nmax); nmax = (int16_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int16(*dst + *src++); - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int16((int32_t)dst[i + 0] + (int32_t)src[i + 0]); + dst[i + 1] = sat_int16((int32_t)dst[i + 1] + (int32_t)src[i + 1]); + dst[i + 2] = sat_int16((int32_t)dst[i + 2] + (int32_t)src[i + 2]); + dst[i + 3] = sat_int16((int32_t)dst[i + 3] + (int32_t)src[i + 3]); } + for (; i < n; i++) { + dst[i] = sat_int16((int32_t)dst[i] + (int32_t)src[i]); + } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -79,12 +87,23 @@ static void mix_s16_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t n = MIN(left_samples, nmax); nmax = (int16_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int16((int32_t)*dst + - q_mults_16x16(*src, gain, IPC4_MIXIN_GAIN_SHIFT)); - src++; - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int16((int32_t)dst[i + 0] + + q_mults_16x16(src[i + 0], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 1] = sat_int16((int32_t)dst[i + 1] + + q_mults_16x16(src[i + 1], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 2] = sat_int16((int32_t)dst[i + 2] + + q_mults_16x16(src[i + 2], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 3] = sat_int16((int32_t)dst[i + 3] + + q_mults_16x16(src[i + 3], gain, IPC4_MIXIN_GAIN_SHIFT)); } + for (; i < n; i++) { + dst[i] = sat_int16((int32_t)dst[i] + + q_mults_16x16(src[i], gain, IPC4_MIXIN_GAIN_SHIFT)); + } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -95,11 +114,18 @@ static void mix_s16_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t nmax = (int16_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = q_mults_16x16(*src, gain, IPC4_MIXIN_GAIN_SHIFT); - src++; - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = q_mults_16x16(src[i + 0], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = q_mults_16x16(src[i + 1], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = q_mults_16x16(src[i + 2], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = q_mults_16x16(src[i + 3], gain, IPC4_MIXIN_GAIN_SHIFT); } + for (; i < n; i++) { + dst[i] = q_mults_16x16(src[i], gain, IPC4_MIXIN_GAIN_SHIFT); + } + dst += n; + src += n; } } #endif /* CONFIG_FORMAT_S16LE */ @@ -128,10 +154,18 @@ static void mix_s24(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixe n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int24(sign_extend_s24(*dst) + sign_extend_s24(*src++)); - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int24(sign_extend_s24(dst[i + 0]) + sign_extend_s24(src[i + 0])); + dst[i + 1] = sat_int24(sign_extend_s24(dst[i + 1]) + sign_extend_s24(src[i + 1])); + dst[i + 2] = sat_int24(sign_extend_s24(dst[i + 2]) + sign_extend_s24(src[i + 2])); + dst[i + 3] = sat_int24(sign_extend_s24(dst[i + 3]) + sign_extend_s24(src[i + 3])); } + for (; i < n; i++) { + dst[i] = sat_int24(sign_extend_s24(dst[i]) + sign_extend_s24(src[i])); + } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -170,13 +204,28 @@ static void mix_s24_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int24(sign_extend_s24(*dst) + - (int32_t)q_mults_32x32(sign_extend_s24(*src), + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int24(sign_extend_s24(dst[i + 0]) + + (int32_t)q_mults_32x32(sign_extend_s24(src[i + 0]), + gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 1] = sat_int24(sign_extend_s24(dst[i + 1]) + + (int32_t)q_mults_32x32(sign_extend_s24(src[i + 1]), + gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 2] = sat_int24(sign_extend_s24(dst[i + 2]) + + (int32_t)q_mults_32x32(sign_extend_s24(src[i + 2]), + gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 3] = sat_int24(sign_extend_s24(dst[i + 3]) + + (int32_t)q_mults_32x32(sign_extend_s24(src[i + 3]), + gain, IPC4_MIXIN_GAIN_SHIFT)); + } + for (; i < n; i++) { + dst[i] = sat_int24(sign_extend_s24(dst[i]) + + (int32_t)q_mults_32x32(sign_extend_s24(src[i]), gain, IPC4_MIXIN_GAIN_SHIFT)); - src++; - dst++; } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -186,11 +235,18 @@ static void mix_s24_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = q_mults_32x32(sign_extend_s24(*src), gain, IPC4_MIXIN_GAIN_SHIFT); - src++; - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = q_mults_32x32(sign_extend_s24(src[i + 0]), gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = q_mults_32x32(sign_extend_s24(src[i + 1]), gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = q_mults_32x32(sign_extend_s24(src[i + 2]), gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = q_mults_32x32(sign_extend_s24(src[i + 3]), gain, IPC4_MIXIN_GAIN_SHIFT); + } + for (; i < n; i++) { + dst[i] = q_mults_32x32(sign_extend_s24(src[i]), gain, IPC4_MIXIN_GAIN_SHIFT); } + dst += n; + src += n; } } #endif /* CONFIG_FORMAT_S24LE */ @@ -218,10 +274,18 @@ static void mix_s32(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixe n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int32((int64_t)*dst + (int64_t)*src++); - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int32((int64_t)dst[i + 0] + (int64_t)src[i + 0]); + dst[i + 1] = sat_int32((int64_t)dst[i + 1] + (int64_t)src[i + 1]); + dst[i + 2] = sat_int32((int64_t)dst[i + 2] + (int64_t)src[i + 2]); + dst[i + 3] = sat_int32((int64_t)dst[i + 3] + (int64_t)src[i + 3]); } + for (; i < n; i++) { + dst[i] = sat_int32((int64_t)dst[i] + (int64_t)src[i]); + } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -259,12 +323,23 @@ static void mix_s32_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = sat_int32((int64_t)*dst + - q_mults_32x32(*src, gain, IPC4_MIXIN_GAIN_SHIFT)); - src++; - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = sat_int32((int64_t)dst[i + 0] + + q_mults_32x32(src[i + 0], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 1] = sat_int32((int64_t)dst[i + 1] + + q_mults_32x32(src[i + 1], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 2] = sat_int32((int64_t)dst[i + 2] + + q_mults_32x32(src[i + 2], gain, IPC4_MIXIN_GAIN_SHIFT)); + dst[i + 3] = sat_int32((int64_t)dst[i + 3] + + q_mults_32x32(src[i + 3], gain, IPC4_MIXIN_GAIN_SHIFT)); } + for (; i < n; i++) { + dst[i] = sat_int32((int64_t)dst[i] + + q_mults_32x32(src[i], gain, IPC4_MIXIN_GAIN_SHIFT)); + } + dst += n; + src += n; } for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { @@ -274,15 +349,130 @@ static void mix_s32_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t n = MIN(left_samples, nmax); nmax = (int32_t *)sink->buf_end - dst; n = MIN(n, nmax); - for (i = 0; i < n; i++) { - *dst = q_mults_32x32(*src, gain, IPC4_MIXIN_GAIN_SHIFT); - src++; - dst++; + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = q_mults_32x32(src[i + 0], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 1] = q_mults_32x32(src[i + 1], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 2] = q_mults_32x32(src[i + 2], gain, IPC4_MIXIN_GAIN_SHIFT); + dst[i + 3] = q_mults_32x32(src[i + 3], gain, IPC4_MIXIN_GAIN_SHIFT); } + for (; i < n; i++) { + dst[i] = q_mults_32x32(src[i], gain, IPC4_MIXIN_GAIN_SHIFT); + } + dst += n; + src += n; } } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +static void mix_float(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + float *dst = (float *)sink->ptr + start_sample; + const float *src = (const float *)source->ptr; + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] += src[i + 0]; + dst[i + 1] += src[i + 1]; + dst[i + 2] += src[i + 2]; + dst[i + 3] += src[i + 3]; + } + for (; i < n; i++) { + dst[i] += src[i]; + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + memcpy_s(dst, n * sizeof(float), src, n * sizeof(float)); + dst += n; + src += n; + } +} + +static void mix_float_gain(struct cir_buf_ptr *sink, int32_t start_sample, int32_t mixed_samples, + const struct cir_buf_ptr *source, + int32_t sample_count, uint16_t gain) +{ + int32_t samples_to_mix, samples_to_copy, left_samples; + int32_t n, nmax, i; + float *dst = (float *)sink->ptr + start_sample; + const float *src = (const float *)source->ptr; + const float gain_f = (float)gain * (1.0f / (float)IPC4_MIXIN_UNITY_GAIN); + + assert(mixed_samples >= start_sample); + samples_to_mix = mixed_samples - start_sample; + samples_to_mix = MIN(samples_to_mix, sample_count); + samples_to_copy = sample_count - samples_to_mix; + + for (left_samples = samples_to_mix; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] += src[i + 0] * gain_f; + dst[i + 1] += src[i + 1] * gain_f; + dst[i + 2] += src[i + 2] * gain_f; + dst[i + 3] += src[i + 3] * gain_f; + } + for (; i < n; i++) { + dst[i] += src[i] * gain_f; + } + dst += n; + src += n; + } + + for (left_samples = samples_to_copy; left_samples > 0; left_samples -= n) { + src = cir_buf_wrap((void *)src, source->buf_start, source->buf_end); + dst = cir_buf_wrap((void *)dst, sink->buf_start, sink->buf_end); + nmax = (const float *)source->buf_end - src; + n = MIN(left_samples, nmax); + nmax = (float *)sink->buf_end - dst; + n = MIN(n, nmax); + int32_t n4 = n & ~3; + for (i = 0; i < n4; i += 4) { + dst[i + 0] = src[i + 0] * gain_f; + dst[i + 1] = src[i + 1] * gain_f; + dst[i + 2] = src[i + 2] * gain_f; + dst[i + 3] = src[i + 3] * gain_f; + } + for (; i < n; i++) { + dst[i] = src[i] * gain_f; + } + dst += n; + src += n; + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + __cold_rodata const struct mix_func_map mix_func_map[] = { #if CONFIG_FORMAT_S16LE { SOF_IPC_FRAME_S16_LE, mix_s16, mix_s16_gain }, @@ -291,7 +481,10 @@ __cold_rodata const struct mix_func_map mix_func_map[] = { { SOF_IPC_FRAME_S24_4LE, mix_s24, mix_s24_gain }, #endif #if CONFIG_FORMAT_S32LE - { SOF_IPC_FRAME_S32_LE, mix_s32, mix_s32_gain } + { SOF_IPC_FRAME_S32_LE, mix_s32, mix_s32_gain }, +#endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, mix_float, mix_float_gain }, #endif }; diff --git a/src/audio/multiband_drc/multiband_drc.c b/src/audio/multiband_drc/multiband_drc.c index 5316c1d41972..209e1fc264ba 100644 --- a/src/audio/multiband_drc/multiband_drc.c +++ b/src/audio/multiband_drc/multiband_drc.c @@ -93,6 +93,72 @@ static int multiband_drc_eq_init_coef_ch(struct processing_module *mod, return 0; } +#if CONFIG_FORMAT_FLOAT +static void multiband_drc_reset_state_float(struct processing_module *mod, + struct multiband_drc_state_float *state) +{ + int i; + + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + multiband_drc_iir_reset_state_ch_float(mod, &state->emphasis[i]); + + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + crossover_reset_state_ch_float(mod, &state->crossover[i]); + + for (i = 0; i < SOF_MULTIBAND_DRC_MAX_BANDS; i++) + drc_reset_state(mod, &state->drc[i]); + + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + multiband_drc_iir_reset_state_ch_float(mod, &state->deemphasis[i]); +} + +static int multiband_drc_eq_init_coef_ch_float(struct processing_module *mod, + struct sof_eq_iir_biquad *coef, + struct iir_state_df1_float *eq) +{ + const float inv_q30 = 1.0f / 1073741824.0f; + const float inv_q14 = 1.0f / 16384.0f; + float gain_factor; + float b_scale; + int b; + + eq->coef = mod_zalloc(mod, sizeof(float) * 5 * SOF_EMP_DEEMP_BIQUADS); + if (!eq->coef) + return -ENOMEM; + + eq->delay = mod_zalloc(mod, sizeof(float) * 4 * SOF_EMP_DEEMP_BIQUADS); + if (!eq->delay) { + mod_free(mod, eq->coef); + eq->coef = NULL; + return -ENOMEM; + } + + eq->biquads = SOF_EMP_DEEMP_BIQUADS; + eq->biquads_in_series = SOF_EMP_DEEMP_BIQUADS; + + for (b = 0; b < SOF_EMP_DEEMP_BIQUADS; b++) { + struct sof_eq_iir_biquad *c = &coef[b]; + float *c_out = &eq->coef[b * 5]; + + gain_factor = (float)c->output_gain * inv_q14; + if (c->output_shift > 0) + gain_factor /= (float)(1 << c->output_shift); + else if (c->output_shift < 0) + gain_factor *= (float)(1 << (-c->output_shift)); + + b_scale = gain_factor * inv_q30; + + c_out[0] = (float)c->b0 * b_scale; + c_out[1] = (float)c->b1 * b_scale; + c_out[2] = (float)c->b2 * b_scale; + c_out[3] = (float)c->a1 * inv_q30; + c_out[4] = (float)c->a2 * inv_q30; + } + + return 0; +} +#endif + static int multiband_drc_init_coef(struct processing_module *mod, int16_t nch, uint32_t rate) { struct comp_dev *dev = mod->dev; @@ -124,6 +190,90 @@ static int multiband_drc_init_coef(struct processing_module *mod, int16_t nch, u return -EINVAL; } +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) { + struct multiband_drc_state_float *state_f = &cd->state_f; + + comp_info(dev, "initializing %i-way crossover float", + config->num_bands); + + /* Crossover: determine the split function */ + cd->crossover_split_f = crossover_find_split_func_float(config->num_bands); + if (!cd->crossover_split_f) { + comp_err(dev, "No crossover_split_f for band count(%i)", + config->num_bands); + return -EINVAL; + } + + /* Crossover: collect the coef array and assign it to every channel */ + crossover = config->crossover_coef; + for (ch = 0; ch < nch; ch++) { + ret = crossover_init_coef_ch_float(mod, crossover, &state_f->crossover[ch], + config->num_bands); + /* Free all previously allocated blocks in case of an error */ + if (ret < 0) { + comp_err(dev, + "could not assign coeffs to ch %d", ch); + goto err_f; + } + } + + comp_info(dev, "initializing emphasis_eq float"); + + /* Emphasis: collect the coef array and assign it to every channel */ + emphasis = config->emp_coef; + for (ch = 0; ch < nch; ch++) { + ret = multiband_drc_eq_init_coef_ch_float(mod, emphasis, &state_f->emphasis[ch]); + /* Free all previously allocated blocks in case of an error */ + if (ret < 0) { + comp_err(dev, "could not assign coeffs to ch %d", + ch); + goto err_f; + } + } + + comp_info(dev, "initializing deemphasis_eq float"); + + /* Deemphasis: collect the coef array and assign it to every channel */ + deemphasis = config->deemp_coef; + for (ch = 0; ch < nch; ch++) { + ret = multiband_drc_eq_init_coef_ch_float(mod, deemphasis, &state_f->deemphasis[ch]); + /* Free all previously allocated blocks in case of an error */ + if (ret < 0) { + comp_err(dev, "could not assign coeffs to ch %d", + ch); + goto err_f; + } + } + + /* Allocate all DRC pre-delay buffers and set delay time with band number */ + for (i = 0; i < num_bands; i++) { + comp_info(dev, "initializing drc band %d float", i); + + ret = drc_init_pre_delay_buffers(mod, &state_f->drc[i], + sizeof(float), (int)nch); + if (ret < 0) { + comp_err(dev, + "could not init pre delay buffers"); + goto err_f; + } + + ret = drc_set_pre_delay_time(&state_f->drc[i], + cd->config->drc_coef[i].pre_delay_time, rate); + if (ret < 0) { + comp_err(dev, "could not set pre delay time"); + goto err_f; + } + } + + return 0; + +err_f: + multiband_drc_reset_state_float(mod, state_f); + return ret; + } +#endif + comp_info(dev, "initializing %i-way crossover", config->num_bands); @@ -210,7 +360,12 @@ static int multiband_drc_setup(struct processing_module *mod, int16_t channels, struct multiband_drc_comp_data *cd = module_get_private_data(mod); /* Reset any previous state */ - multiband_drc_reset_state(mod, &cd->state); +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) + multiband_drc_reset_state_float(mod, &cd->state_f); + else +#endif + multiband_drc_reset_state(mod, &cd->state); /* Setup Crossover, Emphasis EQ, Deemphasis EQ, and DRC */ return multiband_drc_init_coef(mod, channels, rate); @@ -268,6 +423,9 @@ static int multiband_drc_init(struct processing_module *mod) md->private = cd; cd->multiband_drc_func = NULL; cd->crossover_split = NULL; +#if CONFIG_FORMAT_FLOAT + cd->crossover_split_f = NULL; +#endif /* Initialize to enabled is a workaround for IPC4 kernel version 6.6 and * before where the processing is never enabled via switch control. New * kernel sends the IPC4 switch control and sets this to desired state @@ -426,11 +584,19 @@ static int multiband_drc_reset(struct processing_module *mod) comp_info(mod->dev, "entry"); - multiband_drc_reset_state(mod, &cd->state); +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) + multiband_drc_reset_state_float(mod, &cd->state_f); + else +#endif + multiband_drc_reset_state(mod, &cd->state); cd->source_format = 0; cd->multiband_drc_func = NULL; cd->crossover_split = NULL; +#if CONFIG_FORMAT_FLOAT + cd->crossover_split_f = NULL; +#endif return 0; } diff --git a/src/audio/multiband_drc/multiband_drc.h b/src/audio/multiband_drc/multiband_drc.h index ba939663de5d..20d8a11bddba 100644 --- a/src/audio/multiband_drc/multiband_drc.h +++ b/src/audio/multiband_drc/multiband_drc.h @@ -28,6 +28,17 @@ struct multiband_drc_state { struct iir_state_df1 deemphasis[PLATFORM_MAX_CHANNELS]; }; +#if CONFIG_FORMAT_FLOAT +#include + +struct multiband_drc_state_float { + struct iir_state_df1_float emphasis[PLATFORM_MAX_CHANNELS]; + struct crossover_state_float crossover[PLATFORM_MAX_CHANNELS]; + struct drc_state drc[SOF_MULTIBAND_DRC_MAX_BANDS]; + struct iir_state_df1_float deemphasis[PLATFORM_MAX_CHANNELS]; +}; +#endif + typedef int (*multiband_drc_func)(const struct processing_module *mod, struct sof_source *source, struct sof_sink *sink, @@ -36,6 +47,9 @@ typedef int (*multiband_drc_func)(const struct processing_module *mod, /* Multiband DRC component private data */ struct multiband_drc_comp_data { struct multiband_drc_state state; /**< compressor state */ +#if CONFIG_FORMAT_FLOAT + struct multiband_drc_state_float state_f; /**< float compressor state */ +#endif struct comp_data_blob_handler *model_handler; struct sof_multiband_drc_config *config; /**< pointer to setup blob */ bool config_ready; /**< set when fully received */ @@ -43,6 +57,9 @@ struct multiband_drc_comp_data { bool process_enabled; /**< true if component is enabled */ multiband_drc_func multiband_drc_func; /**< processing function */ crossover_split crossover_split; /**< crossover n-way split func */ +#if CONFIG_FORMAT_FLOAT + crossover_split_float crossover_split_f; /**< crossover float split func */ +#endif }; struct multiband_drc_proc_fnmap { @@ -99,6 +116,18 @@ static inline void multiband_drc_iir_reset_state_ch(struct processing_module *mo iir->delay = NULL; } +#if CONFIG_FORMAT_FLOAT +static inline void multiband_drc_iir_reset_state_ch_float(struct processing_module *mod, + struct iir_state_df1_float *iir) +{ + mod_free(mod, iir->coef); + mod_free(mod, iir->delay); + + iir->coef = NULL; + iir->delay = NULL; +} +#endif + void multiband_drc_process_enable(bool *process_enabled); int multiband_drc_set_ipc_config(struct processing_module *mod, uint32_t param_id, const uint8_t *fragment, enum module_cfg_fragment_position pos, diff --git a/src/audio/multiband_drc/multiband_drc_generic.c b/src/audio/multiband_drc/multiband_drc_generic.c index 489e9d16f57a..1a1eea302198 100644 --- a/src/audio/multiband_drc/multiband_drc_generic.c +++ b/src/audio/multiband_drc/multiband_drc_generic.c @@ -458,6 +458,207 @@ static int multiband_drc_s32_default(const struct processing_module *mod, } #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +static void multiband_drc_process_emp_crossover_float(struct multiband_drc_state_float *state, + crossover_split_float split_func, + float *buf_src, + float *buf_sink, + int enable_emp, + int nch, + int nband) +{ + struct iir_state_df1_float *emp_s; + struct crossover_state_float *crossover_s; + float *buf_sink_band; + int ch, band; + float emp_out; + float crossover_out[nband]; + + for (ch = 0; ch < nch; ch++) { + emp_s = &state->emphasis[ch]; + crossover_s = &state->crossover[ch]; + + if (enable_emp) + emp_out = iir_df1_float(emp_s, *buf_src); + else + emp_out = *buf_src; + + split_func(emp_out, crossover_out, crossover_s); + buf_sink_band = buf_sink; + for (band = 0; band < nband; band++) { + *buf_sink_band = crossover_out[band]; + buf_sink_band += PLATFORM_MAX_CHANNELS; + } + + buf_src++; + buf_sink++; + } +} + +static void multiband_drc_float_process_drc(struct drc_state *state, + const struct sof_drc_params *p, + float *buf_src, + float *buf_sink, + int nch) +{ + float *pd_write; + float *pd_read; + int ch; + int pd_write_index; + int pd_read_index; + + if (p->enabled && !state->processed) { + drc_update_envelope(state, p); + drc_compress_output(state, p, 0, nch); + state->processed = 1; + } + + pd_write_index = state->pre_delay_write_index; + pd_read_index = state->pre_delay_read_index; + + for (ch = 0; ch < nch; ++ch) { + pd_write = (float *)state->pre_delay_buffers[ch] + pd_write_index; + pd_read = (float *)state->pre_delay_buffers[ch] + pd_read_index; + *pd_write = *buf_src; + *buf_sink = *pd_read; + + buf_src++; + buf_sink++; + } + + pd_write_index = (pd_write_index + 1) & DRC_MAX_PRE_DELAY_FRAMES_MASK; + pd_read_index = (pd_read_index + 1) & DRC_MAX_PRE_DELAY_FRAMES_MASK; + state->pre_delay_write_index = pd_write_index; + state->pre_delay_read_index = pd_read_index; + + if (!p->enabled) + return; + + /* Process the input division (32 frames). */ + if (!(pd_write_index & DRC_DIVISION_FRAMES_MASK)) { + drc_update_detector_average(state, p, 0, nch); + drc_update_envelope(state, p); + drc_compress_output(state, p, 0, nch); + } +} + +static void multiband_drc_process_deemp_float(struct multiband_drc_state_float *state, + float *buf_src, + float *buf_sink, + int enable_deemp, + int nch, + int nband) +{ + struct iir_state_df1_float *deemp_s; + float *buf_src_band; + int ch, band; + float mix_out; + + for (ch = 0; ch < nch; ch++) { + deemp_s = &state->deemphasis[ch]; + + buf_src_band = buf_src; + mix_out = 0.0f; + for (band = 0; band < nband; band++) { + mix_out += *buf_src_band; + buf_src_band += PLATFORM_MAX_CHANNELS; + } + + if (enable_deemp) + *buf_sink = iir_df1_float(deemp_s, mix_out); + else + *buf_sink = mix_out; + + buf_src++; + buf_sink++; + } +} + +static int multiband_drc_float_default(const struct processing_module *mod, + struct sof_source *source, + struct sof_sink *sink, + uint32_t frames) +{ + struct multiband_drc_comp_data *cd = module_get_private_data(mod); + struct multiband_drc_state_float *state = &cd->state_f; + float buf_src[PLATFORM_MAX_CHANNELS]; + float buf_sink[PLATFORM_MAX_CHANNELS]; + float buf_drc_src[PLATFORM_MAX_CHANNELS * SOF_MULTIBAND_DRC_MAX_BANDS]; + float buf_drc_sink[PLATFORM_MAX_CHANNELS * SOF_MULTIBAND_DRC_MAX_BANDS]; + float *band_buf_drc_src; + float *band_buf_drc_sink; + const int32_t *x, *buf_x_start; + int32_t *y, *buf_y_start; + size_t buf_x_samples, buf_y_samples; + size_t n_bytes = frames * source_get_frame_bytes(source); + int band; + int nbuf; + int npcm; + int ret; + int ch; + int i; + int nch = source_get_channels(source); + int nband = cd->config->num_bands; + int enable_emp_deemp = cd->config->enable_emp_deemp; + int samples = frames * nch; + + ret = source_get_data_s32(source, n_bytes, &x, &buf_x_start, &buf_x_samples); + if (ret) + return ret; + + ret = sink_get_buffer_s32(sink, n_bytes, &y, &buf_y_start, &buf_y_samples); + if (ret) { + source_release_data(source, 0); + return ret; + } + + const float *xf = (const float *)x; + float *yf = (float *)y; + + while (samples) { + nbuf = cir_buf_samples_to_wrap_s32((const int32_t *)xf, buf_x_start, buf_x_samples); + npcm = MIN(samples, nbuf); + nbuf = cir_buf_samples_to_wrap_s32((int32_t *)yf, buf_y_start, buf_y_samples); + npcm = MIN(npcm, nbuf); + for (i = 0; i < npcm; i += nch) { + for (ch = 0; ch < nch; ch++) { + buf_src[ch] = *xf++; + } + + multiband_drc_process_emp_crossover_float(state, cd->crossover_split_f, + buf_src, buf_drc_src, + enable_emp_deemp, nch, nband); + + band_buf_drc_src = buf_drc_src; + band_buf_drc_sink = buf_drc_sink; + for (band = 0; band < nband; ++band) { + multiband_drc_float_process_drc(&state->drc[band], + &cd->config->drc_coef[band], + band_buf_drc_src, band_buf_drc_sink, + nch); + band_buf_drc_src += PLATFORM_MAX_CHANNELS; + band_buf_drc_sink += PLATFORM_MAX_CHANNELS; + } + + multiband_drc_process_deemp_float(state, buf_drc_sink, buf_sink, + enable_emp_deemp, nch, nband); + + for (ch = 0; ch < nch; ch++) { + *yf++ = buf_sink[ch]; + } + } + samples -= npcm; + if ((const int32_t *)xf >= buf_x_start + buf_x_samples) + xf = (const float *)buf_x_start; + if ((int32_t *)yf >= buf_y_start + buf_y_samples) + yf = (float *)buf_y_start; + } + source_release_data(source, n_bytes); + sink_commit_buffer(sink, n_bytes); + return 0; +} +#endif /* CONFIG_FORMAT_FLOAT */ + const struct multiband_drc_proc_fnmap multiband_drc_proc_fnmap[] = { /* { SOURCE_FORMAT , PROCESSING FUNCTION } */ #if CONFIG_FORMAT_S16LE @@ -471,6 +672,10 @@ const struct multiband_drc_proc_fnmap multiband_drc_proc_fnmap[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, multiband_drc_s32_default }, #endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, multiband_drc_float_default }, +#endif /* CONFIG_FORMAT_FLOAT */ }; const struct multiband_drc_proc_fnmap multiband_drc_proc_fnmap_pass[] = { @@ -486,6 +691,10 @@ const struct multiband_drc_proc_fnmap multiband_drc_proc_fnmap_pass[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, multiband_drc_default_pass }, #endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, multiband_drc_default_pass }, +#endif /* CONFIG_FORMAT_FLOAT */ }; const size_t multiband_drc_proc_fncount = ARRAY_SIZE(multiband_drc_proc_fnmap); diff --git a/src/audio/mux/mux_generic.c b/src/audio/mux/mux_generic.c index 851c7c289fd4..9fecc50413b8 100644 --- a/src/audio/mux/mux_generic.c +++ b/src/audio/mux/mux_generic.c @@ -285,7 +285,7 @@ static void mux_s16le(struct comp_dev *dev, struct sof_sink *sink, } #endif /* CONFIG_FORMAT_S16LE */ -#if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE +#if CONFIG_FORMAT_S24LE || CONFIG_FORMAT_S32LE || CONFIG_FORMAT_FLOAT static size_t mux_calc_frames_without_wrap_s32(struct cir_buf_sink *sink, struct cir_buf_source *source_bufs, @@ -540,6 +540,9 @@ const struct comp_func_map mux_func_map[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, &mux_s32le, &demux_s32le }, #endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, &mux_s32le, &demux_s32le }, +#endif }; void mux_prepare_look_up_table(struct processing_module *mod) diff --git a/src/audio/pcm_converter/pcm_converter_generic.c b/src/audio/pcm_converter/pcm_converter_generic.c index 34bf086f9305..efe903ec6430 100644 --- a/src/audio/pcm_converter/pcm_converter_generic.c +++ b/src/audio/pcm_converter/pcm_converter_generic.c @@ -512,7 +512,7 @@ static inline int32_t _pcm_shift(int32_t d, int32_t a) * Use '0' for normal conversion to integers * \return (int32_t)src * 2**pow */ -static int32_t _pcm_convert_f_to_i(int32_t src, int32_t pow) +static __maybe_unused int32_t _pcm_convert_f_to_i(int32_t src, int32_t pow) { int32_t exponent, mantissa, dst; @@ -546,7 +546,7 @@ static int32_t _pcm_convert_f_to_i(int32_t src, int32_t pow) * operations library inclusion by compiler, when in whole topology * only external component needs float input */ -static int32_t _pcm_convert_i_to_f(int32_t src, int32_t pow) +static __maybe_unused int32_t _pcm_convert_i_to_f(int32_t src, int32_t pow) { int sign, mantissa, exponent, dst, abs_clz; @@ -566,6 +566,49 @@ static int32_t _pcm_convert_i_to_f(int32_t src, int32_t pow) #endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && (CONFIG_PCM_CONVERTER_FORMAT_S16LE || CONFIG_PCM_CONVERTER_FORMAT_S24LE || CONFIG_PCM_CONVERTER_FORMAT_S32LE) */ #if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S16LE +#if CONFIG_FPU +static inline int32_t float_to_int32_rnd(float val) +{ +#if defined(__riscv) && defined(__riscv_flen) + int32_t res; + __asm__ ("fcvt.w.s %0, %1, rne" : "=r"(res) : "f"(val)); + return res; +#else + return (int32_t)(val >= 0.0f ? (val + 0.5f) : (val - 0.5f)); +#endif +} + +static void pcm_convert_s16_to_f_lin(const void *psrc, void *pdst, + size_t samples) +{ + const int16_t *src = psrc; + float *dst = pdst; + const float scale = 1.0f / 32768.0f; + size_t i; + + for (i = 0; i < samples; i++) + dst[i] = (float)src[i] * scale; +} + +static void pcm_convert_f_to_s16_lin(const void *psrc, void *pdst, + size_t samples) +{ + const float *src = psrc; + int16_t *dst = pdst; + const float scale = 32768.0f; + size_t i; + + for (i = 0; i < samples; i++) { + float val = src[i] * scale; + if (val >= 32767.0f) + dst[i] = 32767; + else if (val <= -32768.0f) + dst[i] = -32768; + else + dst[i] = (int16_t)float_to_int32_rnd(val); + } +} +#else static void pcm_convert_s16_to_f_lin(const void *psrc, void *pdst, size_t samples) { @@ -591,6 +634,7 @@ static void pcm_convert_f_to_s16_lin(const void *psrc, void *pdst, for (i = 0; i < samples; i++) dst[i] = sat_int16(_pcm_convert_f_to_i(src[i], 15)); } +#endif static int pcm_convert_s16_to_f(const struct cir_buf_source *source, uint32_t src_channels, struct cir_buf_sink *sink, @@ -610,6 +654,38 @@ static int pcm_convert_f_to_s16(const struct cir_buf_source *source, #endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S16LE */ #if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE +#if CONFIG_FPU +static void pcm_convert_s24_to_f_lin(const void *psrc, void *pdst, + size_t samples) +{ + const int32_t *src = psrc; + float *dst = pdst; + const float scale = 1.0f / 8388608.0f; + size_t i; + + for (i = 0; i < samples; i++) + dst[i] = (float)sign_extend_s24(src[i]) * scale; +} + +static void pcm_convert_f_to_s24_lin(const void *psrc, void *pdst, + size_t samples) +{ + const float *src = psrc; + int32_t *dst = pdst; + const float scale = 8388608.0f; + size_t i; + + for (i = 0; i < samples; i++) { + float val = src[i] * scale; + if (val >= 8388607.0f) + dst[i] = 8388607; + else if (val <= -8388608.0f) + dst[i] = -8388608; + else + dst[i] = float_to_int32_rnd(val); + } +} +#else static void pcm_convert_s24_to_f_lin(const void *psrc, void *pdst, size_t samples) { @@ -635,6 +711,7 @@ static void pcm_convert_f_to_s24_lin(const void *psrc, void *pdst, for (i = 0; i < samples; i++) dst[i] = sat_int24(_pcm_convert_f_to_i(src[i], 23)); } +#endif static int pcm_convert_s24_to_f(const struct cir_buf_source *source, uint32_t src_channels, struct cir_buf_sink *sink, @@ -654,6 +731,38 @@ static int pcm_convert_f_to_s24(const struct cir_buf_source *source, #endif /* CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S24LE */ #if CONFIG_PCM_CONVERTER_FORMAT_FLOAT && CONFIG_PCM_CONVERTER_FORMAT_S32LE +#if CONFIG_FPU +static void pcm_convert_s32_to_f_lin(const void *psrc, void *pdst, + size_t samples) +{ + const int32_t *src = psrc; + float *dst = pdst; + const float scale = 1.0f / 2147483648.0f; + size_t i; + + for (i = 0; i < samples; i++) + dst[i] = (float)src[i] * scale; +} + +static void pcm_convert_f_to_s32_lin(const void *psrc, void *pdst, + size_t samples) +{ + const float *src = psrc; + int32_t *dst = pdst; + const float scale = 2147483648.0f; + size_t i; + + for (i = 0; i < samples; i++) { + float val = src[i] * scale; + if (val >= 2147483647.0f) + dst[i] = 2147483647; + else if (val <= -2147483648.0f) + dst[i] = -2147483648LL; + else + dst[i] = float_to_int32_rnd(val); + } +} +#else static void pcm_convert_s32_to_f_lin(const void *psrc, void *pdst, size_t samples) { @@ -679,6 +788,7 @@ static void pcm_convert_f_to_s32_lin(const void *psrc, void *pdst, for (i = 0; i < samples; i++) dst[i] = _pcm_convert_f_to_i(src[i], 31); } +#endif static int pcm_convert_s32_to_f(const struct cir_buf_source *source, uint32_t src_channels, struct cir_buf_sink *sink, diff --git a/src/audio/selector/selector.c b/src/audio/selector/selector.c index 285a48396e4f..42d45b252be3 100644 --- a/src/audio/selector/selector.c +++ b/src/audio/selector/selector.c @@ -173,11 +173,18 @@ static struct comp_dev *selector_new(const struct comp_driver *drv, comp_set_drvdata(dev, cd); - ret = memcpy_s(&cd->config, sizeof(cd->config), ipc_process->data, bs); - if (ret) { - rfree(cd); - comp_free_device(dev); - return NULL; + if (ipc_process && ipc_process->data && bs > 0) { + ret = memcpy_s(&cd->config, sizeof(cd->config), ipc_process->data, bs); + if (ret) { + rfree(cd); + comp_free_device(dev); + return NULL; + } + } else { + /* Defaults: 2 channels in/out, channel 0 selected */ + cd->config.in_channels_count = 2; + cd->config.out_channels_count = 2; + cd->config.sel_channel = 0; } dev->state = COMP_STATE_READY; @@ -388,12 +395,30 @@ static int selector_cmd(struct comp_dev *dev, int cmd, void *data, case COMP_CMD_GET_DATA: ret = selector_ctrl_get_data(dev, cdata, max_data_size); break; - case COMP_CMD_SET_VALUE: - comp_dbg(dev, "COMP_CMD_SET_VALUE"); + case COMP_CMD_SET_VALUE: { + struct comp_data *cd = comp_get_drvdata(dev); + if (cd && cdata->cmd == SOF_CTRL_CMD_ENUM) { + if (cdata->num_elems != 1) + return -EINVAL; + uint32_t val = cdata->chanv[0].value; + struct comp_buffer *sourceb = comp_dev_get_first_data_producer(dev); + uint32_t in_ch = sourceb ? audio_stream_get_channels(&sourceb->stream) : + PLATFORM_MAX_CHANNELS; + if (val >= in_ch || val >= PLATFORM_MAX_CHANNELS) + return -EINVAL; + cd->config.sel_channel = val; + } break; - case COMP_CMD_GET_VALUE: - comp_dbg(dev, "COMP_CMD_GET_VALUE"); + } + case COMP_CMD_GET_VALUE: { + struct comp_data *cd = comp_get_drvdata(dev); + if (cd && cdata->cmd == SOF_CTRL_CMD_ENUM) { + cdata->num_elems = 1; + cdata->chanv[0].channel = 0; + cdata->chanv[0].value = cd->config.sel_channel; + } break; + } default: comp_err(dev, "invalid command"); ret = -EINVAL; @@ -1349,5 +1374,12 @@ static struct sof_static_module_ops sel4_static_ops = { .apply_enum = sel_static_apply_enum, }; DECLARE_STATIC_MODULE_OPS(selector4, &sel4_static_ops); +#else +static struct sof_static_module_ops sel_static_ops = { + .uuid = &selector_uuid, + .create = sel_static_create, + .apply_enum = sel_static_apply_enum, +}; +DECLARE_STATIC_MODULE_OPS(selector, &sel_static_ops); #endif #endif /* CONFIG_STATIC_PIPELINE */ diff --git a/src/audio/selector/selector_generic.c b/src/audio/selector/selector_generic.c index cb6eea84c833..490a2dbe34d8 100644 --- a/src/audio/selector/selector_generic.c +++ b/src/audio/selector/selector_generic.c @@ -419,6 +419,55 @@ static void sel_s32le(struct processing_module *mod, } } #endif /* CONFIG_FORMAT_S32LE */ + +#if CONFIG_FORMAT_FLOAT +static void process_frame_float(float dst[], int dst_channels, + const float src[], int src_channels, + struct ipc4_selector_coeffs_config *coeffs_config) +{ + float accum; + int i, j; + + for (i = 0; i < dst_channels; i++) { + accum = 0.0f; + for (j = 0; j < src_channels; j++) { + int16_t c = coeffs_config->coeffs[i][j]; + if (c != 0) + accum += src[j] * ((float)c * (1.0f / 1024.0f)); + } + dst[i] = accum; + } +} + +static void sel_float(struct processing_module *mod, + const struct cir_buf_source *source, + struct cir_buf_sink *sink, size_t frames) +{ + struct comp_data *cd = module_get_private_data(mod); + const unsigned int src_channels = cd->config.in_channels_count; + const unsigned int dst_channels = cd->config.out_channels_count; + const unsigned int n_chan_source = MIN(SEL_SOURCE_CHANNELS_MAX, src_channels); + const unsigned int n_chan_sink = MIN(SEL_SINK_CHANNELS_MAX, dst_channels); + const float *src = source->ptr; + float *dst = sink->ptr; + size_t i, n, processed = 0; + + while (processed < frames) { + n = frames - processed; + n = MIN(n, cir_buf_samples_without_wrap_s32((const int32_t *)src, source->buf_end) / src_channels); + n = MIN(n, cir_buf_samples_without_wrap_s32((const int32_t *)dst, sink->buf_end) / dst_channels); + for (i = 0; i < n; i++) { + process_frame_float(dst, n_chan_sink, src, n_chan_source, + &cd->coeffs_config); + src += src_channels; + dst += dst_channels; + } + src = source_cir_buf_wrap(src, source->buf_start, source->buf_end); + dst = cir_buf_wrap(dst, sink->buf_start, sink->buf_end); + processed += n; + } +} +#endif /* CONFIG_FORMAT_FLOAT */ #endif const struct comp_func_map func_table[] = { @@ -438,6 +487,11 @@ const struct comp_func_map func_table[] = { {SOF_IPC_FRAME_S32_LE, 2, sel_s32le_nch}, {SOF_IPC_FRAME_S32_LE, 4, sel_s32le_nch}, #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + {SOF_IPC_FRAME_FLOAT, 1, sel_s32le_1ch}, + {SOF_IPC_FRAME_FLOAT, 2, sel_s32le_nch}, + {SOF_IPC_FRAME_FLOAT, 4, sel_s32le_nch}, +#endif /* CONFIG_FORMAT_FLOAT */ #else #if CONFIG_FORMAT_S16LE {SOF_IPC_FRAME_S16_LE, 0, sel_s16le}, @@ -448,6 +502,9 @@ const struct comp_func_map func_table[] = { #if CONFIG_FORMAT_S32LE {SOF_IPC_FRAME_S32_LE, 0, sel_s32le}, #endif +#if CONFIG_FORMAT_FLOAT + {SOF_IPC_FRAME_FLOAT, 0, sel_float}, +#endif #endif }; diff --git a/src/audio/src/src_common.c b/src/audio/src/src_common.c index 36847afd4491..bcede8c9b24b 100644 --- a/src/audio/src/src_common.c +++ b/src/audio/src/src_common.c @@ -375,6 +375,9 @@ int src_copy_sxx(struct comp_data *cd, struct sof_source *source, case SOF_IPC_FRAME_S16_LE: case SOF_IPC_FRAME_S24_4LE: case SOF_IPC_FRAME_S32_LE: +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: +#endif return source_to_sink_copy(source, sink, true, frames * source_get_frame_bytes(source)); default: diff --git a/src/audio/src/src_common.h b/src/audio/src/src_common.h index 98f29a263131..f02d27b96791 100644 --- a/src/audio/src/src_common.h +++ b/src/audio/src/src_common.h @@ -139,6 +139,10 @@ void src_polyphase_stage_cir(struct src_stage_prm *s); void src_polyphase_stage_cir_s16(struct src_stage_prm *s); #endif /* CONFIG_FORMAT_S16LE */ +#if CONFIG_FORMAT_FLOAT +void src_polyphase_stage_cir_float(struct src_stage_prm *s); +#endif /* CONFIG_FORMAT_FLOAT */ + int32_t src_input_rates(void); int32_t src_output_rates(void); diff --git a/src/audio/src/src_generic.c b/src/audio/src/src_generic.c index 3072fe1f741c..b7ae856eeac3 100644 --- a/src/audio/src/src_generic.c +++ b/src/audio/src/src_generic.c @@ -10,14 +10,14 @@ #include "src_config.h" -#if SRC_GENERIC - #include #include #include #include "src_common.h" +#if SRC_GENERIC + #if SRC_SHORT /* 16 bit coefficients version */ static inline void fir_filter_generic(int32_t *rp, const void *cp, int32_t *wp0, @@ -404,5 +404,183 @@ void src_polyphase_stage_cir_s16(struct src_stage_prm *s) s->y_wptr = y_wptr; } #endif /* CONFIG_FORMAT_S16LE */ +#endif /* SRC_GENERIC */ + +#if CONFIG_FORMAT_FLOAT +static inline void fir_filter_generic_float(float *rp, const void *cp, float *wp0, + float *fir_start, float *fir_end, + const int taps_x_nch, + const int nch) +{ + float y0, y1; + float *data; + int i, j; + int frames, n1, n2; + float *d = rp; + float *wp = wp0; + +#if SRC_SHORT + const int16_t *coef = (const int16_t *)cp; + const float inv_coef_scale = 1.0f / 32768.0f; +#else + const int32_t *coef = (const int32_t *)cp; + const float inv_coef_scale = 1.0f / 8388608.0f; +#endif + + if (nch == 2) { + data = d - 1; + y0 = 0.0f; + y1 = 0.0f; + frames = fir_end - data; + n1 = ((taps_x_nch < frames) ? taps_x_nch : frames) >> 1; + n2 = (taps_x_nch >> 1) - n1; + + for (i = 0; i < n1; i++, coef++, data += 2) { +#if SRC_SHORT + float c = (float)(*coef) * inv_coef_scale; +#else + float c = (float)(*coef >> 8) * inv_coef_scale; +#endif + y0 += c * data[0]; + y1 += c * data[1]; + } + + data = fir_start; + for (i = 0; i < n2; i++, coef++, data += 2) { +#if SRC_SHORT + float c = (float)(*coef) * inv_coef_scale; +#else + float c = (float)(*coef >> 8) * inv_coef_scale; +#endif + y0 += c * data[0]; + y1 += c * data[1]; + } + + *wp = y1; + *(wp + 1) = y0; + return; + } + + for (j = 0; j < nch; j++) { + data = d--; + y0 = 0.0f; +#if SRC_SHORT + coef = (const int16_t *)cp; +#else + coef = (const int32_t *)cp; +#endif + frames = fir_end - data + nch - j - 1; + n1 = (taps_x_nch < frames) ? taps_x_nch : frames; + n2 = taps_x_nch - n1; + + for (i = 0; i < n1; i += nch, coef++, data += nch) { +#if SRC_SHORT + float c = (float)(*coef) * inv_coef_scale; +#else + float c = (float)(*coef >> 8) * inv_coef_scale; +#endif + y0 += c * (*data); + } + + data = fir_start + nch - j - 1; + for (i = 0; i < n2; i += nch, coef++, data += nch) { +#if SRC_SHORT + float c = (float)(*coef) * inv_coef_scale; +#else + float c = (float)(*coef >> 8) * inv_coef_scale; +#endif + y0 += c * (*data); + } + *wp = y0; + wp++; + } +} + +void src_polyphase_stage_cir_float(struct src_stage_prm *s) +{ + int i, n, m; + int n_wrap_buf, n_wrap_fir, n_min; + float *rp, *wp; + + struct src_state *fir = s->state; + const struct src_stage *cfg = s->stage; + float *fir_delay = (float *)fir->fir_delay; + float *fir_end = (float *)&fir->fir_delay[fir->fir_delay_size]; + float *out_delay_end = (float *)&fir->out_delay[fir->out_delay_size]; + float *fir_wp = (float *)fir->fir_wp; + float *out_rp = (float *)fir->out_rp; + const void *cp; + const size_t out_size = fir->out_delay_size * sizeof(float); + const int nch = s->nch; + const int nch_x_odm = cfg->odm * nch; + const int blk_in_words = nch * cfg->blk_in; + const int blk_out_words = nch * cfg->num_of_subfilters; + const int rewind = nch * (cfg->blk_in + (cfg->num_of_subfilters - 1) * cfg->idm); + const int nch_x_idm = nch * cfg->idm; + const size_t fir_size = fir->fir_delay_size * sizeof(float); + const int taps_x_nch = cfg->subfilter_length * nch; + float *x_rptr = (float *)s->x_rptr; + float *y_wptr = (float *)s->y_wptr; + float *x_end_addr = (float *)s->x_end_addr; + float *y_end_addr = (float *)s->y_end_addr; + +#if SRC_SHORT + const size_t subfilter_size = cfg->subfilter_length * sizeof(int16_t); +#else + const size_t subfilter_size = cfg->subfilter_length * sizeof(int32_t); #endif + + for (n = 0; n < s->times; n++) { + m = blk_in_words; + while (m > 0) { + n_wrap_buf = x_end_addr - x_rptr; + n_wrap_fir = fir_wp - fir_delay + 1; + n_min = MIN(n_wrap_fir, n_wrap_buf); + n_min = MIN(m, n_min); + m -= n_min; + for (i = 0; i < n_min; i++) { + *fir_wp = *x_rptr; + fir_wp--; + x_rptr++; + } + src_dec_wrap((int32_t **)&fir_wp, (int32_t *)fir_delay, fir_size); + src_inc_wrap((int32_t **)&x_rptr, (int32_t *)x_end_addr, s->x_size); + } + + cp = cfg->coefs; + rp = fir_wp + rewind; + src_inc_wrap((int32_t **)&rp, (int32_t *)fir_end, fir_size); + wp = out_rp; + for (i = 0; i < cfg->num_of_subfilters; i++) { + fir_filter_generic_float(rp, cp, wp, fir_delay, fir_end, + taps_x_nch, nch); + wp += nch_x_odm; + cp = (char *)cp + subfilter_size; + src_inc_wrap((int32_t **)&wp, (int32_t *)out_delay_end, out_size); + rp -= nch_x_idm; + src_dec_wrap((int32_t **)&rp, (int32_t *)fir_delay, fir_size); + } + + m = blk_out_words; + while (m > 0) { + n_wrap_fir = out_delay_end - out_rp; + n_wrap_buf = y_end_addr - y_wptr; + n_min = MIN(n_wrap_fir, n_wrap_buf); + n_min = MIN(m, n_min); + m -= n_min; + for (i = 0; i < n_min; i++) { + *y_wptr = *out_rp; + y_wptr++; + out_rp++; + } + src_inc_wrap((int32_t **)&y_wptr, (int32_t *)y_end_addr, s->y_size); + src_inc_wrap((int32_t **)&out_rp, (int32_t *)out_delay_end, out_size); + } + } + fir->fir_wp = (int32_t *)fir_wp; + fir->out_rp = (int32_t *)out_rp; + s->x_rptr = x_rptr; + s->y_wptr = y_wptr; +} +#endif /* CONFIG_FORMAT_FLOAT */ diff --git a/src/audio/src/src_ipc3.c b/src/audio/src/src_ipc3.c index 028f747378d7..df68d0de2f52 100644 --- a/src/audio/src/src_ipc3.c +++ b/src/audio/src/src_ipc3.c @@ -139,6 +139,12 @@ int src_prepare_general(struct processing_module *mod, cd->polyphase_func = src_polyphase_stage_cir; break; #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + cd->data_shift = 0; + cd->polyphase_func = src_polyphase_stage_cir_float; + break; +#endif /* CONFIG_FORMAT_FLOAT */ default: comp_err(dev, "invalid format %d", source_format); ret = -EINVAL; diff --git a/src/audio/src/src_ipc4.c b/src/audio/src/src_ipc4.c index c2ab47d425c2..ac17590a022f 100644 --- a/src/audio/src/src_ipc4.c +++ b/src/audio/src/src_ipc4.c @@ -154,6 +154,14 @@ int src_prepare_general(struct processing_module *mod, /* set align requirements */ src_set_alignment(source); +#if CONFIG_FORMAT_FLOAT + if (source_get_frm_fmt(source) == SOF_IPC_FRAME_FLOAT) { + cd->data_shift = 0; + cd->polyphase_func = src_polyphase_stage_cir_float; + goto out; + } +#endif + switch (cd->ipc_config.base.audio_fmt.valid_bit_depth) { #if CONFIG_FORMAT_S16LE case IPC4_DEPTH_16BIT: diff --git a/src/audio/tdfb/tdfb.c b/src/audio/tdfb/tdfb.c index fb9cc9ba5afe..793342b995e3 100644 --- a/src/audio/tdfb/tdfb.c +++ b/src/audio/tdfb/tdfb.c @@ -43,6 +43,9 @@ LOG_MODULE_REGISTER(tdfb, CONFIG_SOF_LOG_LEVEL); SOF_DEFINE_REG_UUID(tdfb); +static void tdfb_pass_same_format(struct tdfb_comp_data *cd, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, int frames); + static inline int set_func(struct processing_module *mod, enum sof_ipc_frame fmt) { struct tdfb_comp_data *cd = module_get_private_data(mod); @@ -66,6 +69,12 @@ static inline int set_func(struct processing_module *mod, enum sof_ipc_frame fmt cd->tdfb_func = tdfb_fir_s32; break; #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + comp_dbg(mod->dev, "SOF_IPC_FRAME_FLOAT"); + cd->tdfb_func = tdfb_fir_float; + break; +#endif /* CONFIG_FORMAT_FLOAT */ default: comp_err(mod->dev, "invalid frame_fmt"); return -EINVAL; @@ -178,7 +187,7 @@ static void tdfb_pass_s24(struct tdfb_comp_data *cd, struct input_stream_buffer } #endif -#if CONFIG_FORMAT_S32LE +#if CONFIG_FORMAT_S32LE || CONFIG_FORMAT_FLOAT static void tdfb_pass_s32(struct tdfb_comp_data *cd, struct input_stream_buffer *bsource, struct output_stream_buffer *bsink, int frames) { @@ -247,6 +256,12 @@ static inline int set_pass_func(struct processing_module *mod, enum sof_ipc_fram cd->tdfb_func = tdfb_pass_s32; break; #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + comp_dbg(mod->dev, "SOF_IPC_FRAME_FLOAT"); + cd->tdfb_func = tdfb_pass_s32; + break; +#endif /* CONFIG_FORMAT_FLOAT */ default: comp_err(mod->dev, "invalid frame_fmt"); return -EINVAL; @@ -270,8 +285,17 @@ static void tdfb_free_delaylines(struct processing_module *mod) mod_free(mod, cd->fir_delay); cd->fir_delay = NULL; cd->fir_delay_size = 0; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - fir[i].delay = NULL; + +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) { + for (i = 0; i < SOF_TDFB_FIR_MAX_COUNT; i++) + cd->fir_f[i].delay = NULL; + } else +#endif + { + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir[i].delay = NULL; + } } static int16_t *tdfb_filter_seek(struct sof_tdfb_config *config, int num_filters) @@ -539,6 +563,47 @@ static int tdfb_init_coef(struct processing_module *mod) /* Seek to proper filter for requested angle or beam off configuration */ coefp = tdfb_filter_seek(config, idx); +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) { + int delay_size_sum = 0; + int coef_size_sum = 0; + int16_t *coefp_scan = coefp; + float *coef_ptr; + + for (i = 0; i < config->num_filters; i++) { + coef_data = (struct sof_fir_coef_data *)coefp_scan; + int dsz = fir_delay_size_float(coef_data); + int csz = fir_coef_size_float(coef_data); + + if (dsz < 0 || csz < 0) + return -EINVAL; + delay_size_sum += dsz; + coef_size_sum += csz; + coefp_scan = coef_data->coef + coef_data->length; + } + + if (coef_size_sum > cd->fir_coef_f_size) { + mod_free(mod, cd->fir_coef_f); + cd->fir_coef_f = mod_balloc(mod, coef_size_sum); + if (!cd->fir_coef_f) { + comp_err(dev, "allocation failed for coef size %d", + coef_size_sum); + return -ENOMEM; + } + cd->fir_coef_f_size = coef_size_sum; + } + + coef_ptr = cd->fir_coef_f; + for (i = 0; i < config->num_filters; i++) { + coef_data = (struct sof_fir_coef_data *)coefp; + fir_init_coef_float(&cd->fir_f[i], coef_data, &coef_ptr); + coefp = coef_data->coef + coef_data->length; + } + + return delay_size_sum; + } +#endif + /* Initialize filter bank. FIR header bounds and length validity were * already checked when the blob entered the component. */ @@ -554,6 +619,18 @@ static int tdfb_init_coef(struct processing_module *mod) static void tdfb_init_delay(struct tdfb_comp_data *cd) { +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) { + float *fir_delay = (float *)cd->fir_delay; + int i; + + for (i = 0; i < cd->config->num_filters; i++) { + if (cd->fir_f[i].length > 0) + fir_init_delay_float(&cd->fir_f[i], &fir_delay); + } + return; + } +#endif int32_t *fir_delay = cd->fir_delay; int i; @@ -570,6 +647,10 @@ static int tdfb_setup(struct processing_module *mod, int source_nch, int sink_nc struct tdfb_comp_data *cd = module_get_private_data(mod); int delay_size; +#if CONFIG_FORMAT_FLOAT + cd->source_format = fmt; +#endif + /* If beam on, restore processing function. If off, use for same source and * sink format the efficient 1:1 copy, otherwise faster pass-through processing * functions those copy selected source channels to selected sink channels. @@ -666,6 +747,10 @@ static int tdfb_init(struct processing_module *mod) for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) fir_reset(&cd->fir[i]); +#if CONFIG_FORMAT_FLOAT + for (i = 0; i < SOF_TDFB_FIR_MAX_COUNT; i++) + fir_reset_float(&cd->fir_f[i]); +#endif /* Allow different number of channels in source and sink, in other * aspects TDFB is simple component type. @@ -693,6 +778,9 @@ static int tdfb_free(struct processing_module *mod) mod_ipc_msg_free(mod, cd->msg); tdfb_free_delaylines(mod); +#if CONFIG_FORMAT_FLOAT + mod_free(mod, cd->fir_coef_f); +#endif mod_data_blob_handler_free(mod, cd->model_handler); tdfb_direction_free(mod); mod_free(mod, cd->ctrl_data); @@ -751,7 +839,7 @@ static int tdfb_process(struct processing_module *mod, } /* Handle enum controls */ - if (cd->update) { + if (cd->update && cd->config) { cd->update = false; ret = tdfb_setup(mod, audio_stream_get_channels(source), audio_stream_get_channels(sink), @@ -767,21 +855,28 @@ static int tdfb_process(struct processing_module *mod, * optimized filter function loads the successive input samples from * internal delay line with a 64 bit load operation. */ - frame_count = MIN(frame_count, cd->max_frames) & ~0x1; + int max_f = cd->max_frames > 0 ? cd->max_frames : frame_count; + frame_count = MIN(frame_count, max_f) & ~0x1; if (frame_count) { - cd->tdfb_func(cd, input_buffers, output_buffers, frame_count); + if (cd->tdfb_func) { + cd->tdfb_func(cd, input_buffers, output_buffers, frame_count); + } else { + tdfb_pass_same_format(cd, &input_buffers[0], &output_buffers[0], frame_count); + } module_update_buffer_position(input_buffers, output_buffers, frame_count); - /* Update sound direction estimate */ - tdfb_direction_estimate(cd, frame_count, audio_stream_get_channels(source)); - comp_dbg(dev, "tdfb_dint %u %d %d %d", cd->direction.trigger, cd->direction.level, - (int32_t)(cd->direction.level_ambient >> 32), cd->direction.az_slow); - - if (cd->direction_updates && cd->direction_change) { - tdfb_send_ipc_notification(mod); - cd->direction_change = false; - comp_dbg(dev, "tdfb_dupd %d %d", - cd->az_value_estimate, cd->direction.az_slow); + if (cd->config) { + /* Update sound direction estimate */ + tdfb_direction_estimate(cd, frame_count, audio_stream_get_channels(source)); + comp_dbg(dev, "tdfb_dint %u %d %d %d", cd->direction.trigger, cd->direction.level, + (int32_t)(cd->direction.level_ambient >> 32), cd->direction.az_slow); + + if (cd->direction_updates && cd->direction_change) { + tdfb_send_ipc_notification(mod); + cd->direction_change = false; + comp_dbg(dev, "tdfb_dupd %d %d", + cd->az_value_estimate, cd->direction.az_slow); + } } } @@ -841,8 +936,16 @@ static int tdfb_prepare(struct processing_module *mod, /* Initialize filter */ cd->config = comp_get_data_blob(cd->model_handler, &cd->config_size, NULL); if (!cd->config) { - ret = -EINVAL; - goto out; + if (source_channels == sink_channels) { + cd->tdfb_func = tdfb_pass_same_format; + } else { + comp_err(dev, "no blob and unequal channel counts: %d -> %d", + source_channels, sink_channels); + return -EINVAL; + } + cd->max_frames = INT32_MAX; + comp_info(dev, "no blob, set to pass-through"); + return 0; } /* Re-validate now that the stream channel counts are cached: the @@ -907,8 +1010,16 @@ static int tdfb_reset(struct processing_module *mod) tdfb_free_delaylines(mod); cd->tdfb_func = NULL; - for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) - fir_reset(&cd->fir[i]); +#if CONFIG_FORMAT_FLOAT + if (cd->source_format == SOF_IPC_FRAME_FLOAT) { + for (i = 0; i < SOF_TDFB_FIR_MAX_COUNT; i++) + fir_reset_float(&cd->fir_f[i]); + } else +#endif + { + for (i = 0; i < PLATFORM_MAX_CHANNELS; i++) + fir_reset(&cd->fir[i]); + } /* Clear in/out buffers */ memset(cd->in, 0, TDFB_IN_BUF_LENGTH * sizeof(int32_t)); diff --git a/src/audio/tdfb/tdfb_comp.h b/src/audio/tdfb/tdfb_comp.h index f59ff48bacaf..6fbb70cdd2f6 100644 --- a/src/audio/tdfb/tdfb_comp.h +++ b/src/audio/tdfb/tdfb_comp.h @@ -45,6 +45,10 @@ /* Process max 10% more frames than one period */ #define TDFB_MAX_FRAMES_MULT_Q14 Q_CONVERT_FLOAT(1.10, 14) +#if CONFIG_FORMAT_FLOAT +#include +#endif + /* TDFB component private data */ struct tdfb_direction_data { @@ -72,7 +76,12 @@ struct tdfb_direction_data { }; struct tdfb_comp_data { - struct fir_state_32x16 fir[SOF_TDFB_FIR_MAX_COUNT]; /**< FIR state */ + union { + struct fir_state_32x16 fir[SOF_TDFB_FIR_MAX_COUNT]; /**< FIR state */ +#if CONFIG_FORMAT_FLOAT + struct fir_state_float fir_f[SOF_TDFB_FIR_MAX_COUNT]; +#endif + }; struct comp_data_blob_handler *model_handler; struct sof_tdfb_config *config; /**< pointer to setup blob */ struct sof_tdfb_angle *filter_angles; @@ -80,9 +89,24 @@ struct tdfb_comp_data { struct sof_ipc_ctrl_data *ctrl_data; struct ipc_msg *msg; struct tdfb_direction_data direction; - int32_t in[TDFB_IN_BUF_LENGTH]; /**< input samples buffer */ - int32_t out[TDFB_IN_BUF_LENGTH]; /**< output samples mix buffer */ + union { + int32_t in[TDFB_IN_BUF_LENGTH]; /**< input samples buffer */ +#if CONFIG_FORMAT_FLOAT + float in_f[TDFB_IN_BUF_LENGTH]; +#endif + }; + union { + int32_t out[TDFB_IN_BUF_LENGTH]; /**< output samples mix buffer */ +#if CONFIG_FORMAT_FLOAT + float out_f[TDFB_IN_BUF_LENGTH]; +#endif + }; int32_t *fir_delay; /**< pointer to allocated RAM */ +#if CONFIG_FORMAT_FLOAT + float *fir_coef_f; + size_t fir_coef_f_size; + enum sof_ipc_frame source_format; +#endif int16_t *input_channel_select; /**< For each FIR define in ch */ int16_t *output_channel_mix; /**< For each FIR define out ch */ int16_t *output_stream_mix; /**< for each FIR define stream */ @@ -121,6 +145,12 @@ void tdfb_fir_s32(struct tdfb_comp_data *cd, struct output_stream_buffer *bsink, int frames); #endif +#if CONFIG_FORMAT_FLOAT +void tdfb_fir_float(struct tdfb_comp_data *cd, + struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, int frames); +#endif + int tdfb_direction_init(struct processing_module *mod, int32_t fs, int channels); void tdfb_direction_copy_emphasis(struct tdfb_comp_data *cd, int channels, int *channel, int32_t x); void tdfb_direction_estimate(struct tdfb_comp_data *cd, int frames, int channels); diff --git a/src/audio/tdfb/tdfb_generic.c b/src/audio/tdfb/tdfb_generic.c index ba4e44c8073f..2997fa83c87b 100644 --- a/src/audio/tdfb/tdfb_generic.c +++ b/src/audio/tdfb/tdfb_generic.c @@ -200,6 +200,94 @@ void tdfb_fir_s32(struct tdfb_comp_data *cd, struct input_stream_buffer *bsource } } #endif - #endif /* TDFB_GENERIC */ +#if CONFIG_FORMAT_FLOAT +static inline void tdfb_core_float(struct tdfb_comp_data *cd, int in_nch, int out_nch) +{ + struct fir_state_float *filter; + float y0; + float y1; + int is2; + int is; + int om; + int i; + int k; + const int num_filters = cd->config->num_filters; + + /* Clear output mix */ + memset(cd->out_f, 0, 2 * out_nch * sizeof(float)); + + /* Run and mix all filters to their output channel */ + for (i = 0; i < num_filters; i++) { + is = cd->input_channel_select[i]; + is2 = is + in_nch; + om = cd->output_channel_mix[i]; + filter = &cd->fir_f[i]; + + fir_float_2x(filter, cd->in_f[is], cd->in_f[is2], &y0, &y1); + + for (k = 0; k < out_nch; k++) { + if (om & 1) { + cd->out_f[k] += y0; + cd->out_f[k + out_nch] += y1; + } + om = om >> 1; + } + } +} + +void tdfb_fir_float(struct tdfb_comp_data *cd, struct input_stream_buffer *bsource, + struct output_stream_buffer *bsink, int frames) +{ + struct audio_stream *source = bsource->data; + struct audio_stream *sink = bsink->data; + float *x = audio_stream_get_rptr(source); + float *y = audio_stream_get_wptr(sink); + int fmax; + int i; + int j; + int f; + const int in_nch = audio_stream_get_channels(source); + const int out_nch = audio_stream_get_channels(sink); + int remaining_frames = frames; + int emp_ch = 0; + + while (remaining_frames) { + fmax = audio_stream_frames_without_wrap(source, x); + f = MIN(remaining_frames, fmax); + fmax = audio_stream_frames_without_wrap(sink, y); + f = MIN(f, fmax); + for (j = 0; j < f; j += 2) { + /* Read two frames from all input channels */ + for (i = 0; i < 2 * in_nch; i++) { + cd->in_f[i] = *x; + if (cd->direction_updates) { + float val = *x; + if (val > 1.0f) + val = 1.0f; + else if (val < -1.0f) + val = -1.0f; + float scaled = val * 2147483647.0f; + int32_t sample = scaled >= 2147483647.0f ? INT32_MAX : (int32_t)scaled; + tdfb_direction_copy_emphasis(cd, in_nch, &emp_ch, sample); + } + x++; + } + + /* Process */ + tdfb_core_float(cd, in_nch, out_nch); + + /* Write two frames of output */ + for (i = 0; i < 2 * out_nch; i++) { + *y = cd->out_f[i]; + y++; + } + } + remaining_frames -= f; + x = audio_stream_wrap(source, x); + y = audio_stream_wrap(sink, y); + } +} +#endif + diff --git a/src/audio/tone/tone-ipc3.c b/src/audio/tone/tone-ipc3.c index 88240593e511..5f6cff5280b4 100644 --- a/src/audio/tone/tone-ipc3.c +++ b/src/audio/tone/tone-ipc3.c @@ -101,9 +101,21 @@ static int tone_params(struct comp_dev *dev, comp_info(dev, "config->frame_fmt = %u", dev->ipc_config.frame_fmt); - /* Tone supports only S32_LE PCM format atm */ - if (dev->ipc_config.frame_fmt != SOF_IPC_FRAME_S32_LE) + /* Tone supports S32_LE and optionally FLOAT format */ + switch (dev->ipc_config.frame_fmt) { +#if CONFIG_FORMAT_S32LE + case SOF_IPC_FRAME_S32_LE: + cd->tone_func = tone_s32_default; + break; +#endif +#if CONFIG_FORMAT_FLOAT + case SOF_IPC_FRAME_FLOAT: + cd->tone_func = tone_float_default; + break; +#endif + default: return -EINVAL; + } audio_stream_set_frm_fmt(&sourceb->stream, dev->ipc_config.frame_fmt); audio_stream_set_frm_fmt(&sinkb->stream, dev->ipc_config.frame_fmt); diff --git a/src/audio/tone/tone-ipc4.c b/src/audio/tone/tone-ipc4.c index 0ca7e3ecf91c..bc0b09546cdb 100644 --- a/src/audio/tone/tone-ipc4.c +++ b/src/audio/tone/tone-ipc4.c @@ -43,7 +43,9 @@ LOG_MODULE_DECLARE(tone, CONFIG_SOF_LOG_LEVEL); static int tone_init(struct processing_module *mod) { struct module_data *mod_data = &mod->priv; +#if CONFIG_IPC_MAJOR_4 struct module_config *mod_config = &mod->priv.cfg; +#endif struct comp_dev *dev = mod->dev; struct comp_data *cd; int i; @@ -54,13 +56,14 @@ static int tone_init(struct processing_module *mod) mod_data->private = cd; - /* Tnoe only supports 32-bit format */ + /* Tone only supports 32-bit format */ cd->tone_func = tone_s32_default; /* * set direction for the comp. In the case of the tone generator being used for * echo reference, the number of input pins will be non-zero */ +#if CONFIG_IPC_MAJOR_4 if (mod_config->nb_input_pins > 0) { dev->direction = SOF_IPC_STREAM_CAPTURE; cd->mode = TONE_MODE_SILENCE; @@ -69,6 +72,10 @@ static int tone_init(struct processing_module *mod) dev->direction = SOF_IPC_STREAM_PLAYBACK; cd->mode = TONE_MODE_TONEGEN; } +#else + dev->direction = SOF_IPC_STREAM_PLAYBACK; + cd->mode = TONE_MODE_TONEGEN; +#endif dev->direction_set = true; @@ -95,7 +102,7 @@ static int tone_params(struct processing_module *mod) struct comp_data *cd = module_get_private_data(mod); struct comp_dev *dev = mod->dev; struct comp_buffer *sinkb; - enum sof_ipc_frame frame_fmt, valid_fmt; + enum sof_ipc_frame frame_fmt; sinkb = comp_dev_get_first_data_consumer(dev); if (!sinkb) { @@ -103,18 +110,30 @@ static int tone_params(struct processing_module *mod) return -ENOTCONN; } +#if CONFIG_IPC_MAJOR_4 + enum sof_ipc_frame valid_fmt; + audio_stream_fmt_conversion(mod->priv.cfg.base_cfg.audio_fmt.depth, mod->priv.cfg.base_cfg.audio_fmt.valid_bit_depth, &frame_fmt, &valid_fmt, mod->priv.cfg.base_cfg.audio_fmt.s_type); cd->rate = mod->priv.cfg.base_cfg.audio_fmt.sampling_frequency; +#else + frame_fmt = audio_stream_get_frm_fmt(&sinkb->stream); + cd->rate = audio_stream_get_rate(&sinkb->stream); +#endif - /* Tone supports only S32_LE PCM format atm */ - if (frame_fmt != SOF_IPC_FRAME_S32_LE) { + /* Tone supports S32_LE and FLOAT format */ + if (frame_fmt != SOF_IPC_FRAME_S32_LE && frame_fmt != SOF_IPC_FRAME_FLOAT) { comp_err(dev, "unsupported frame_fmt = %u", frame_fmt); return -EINVAL; } + if (frame_fmt == SOF_IPC_FRAME_FLOAT) + cd->tone_func = tone_float_default; + else + cd->tone_func = tone_s32_default; + return 0; } @@ -144,7 +163,14 @@ static int tone_prepare(struct processing_module *mod, struct sof_source **sourc if (ret < 0) return ret; +#if CONFIG_IPC_MAJOR_4 cd->channels = mod->priv.cfg.base_cfg.audio_fmt.channels_count; +#else + if (num_of_sinks > 0 && sinks[0]) + cd->channels = sink_get_channels(sinks[0]); + else + cd->channels = 2; +#endif for (i = 0; i < cd->channels; i++) { f = tonegen_get_f(&cd->sg[i]); diff --git a/src/audio/tone/tone.c b/src/audio/tone/tone.c index ee2368709346..8d139c673593 100644 --- a/src/audio/tone/tone.c +++ b/src/audio/tone/tone.c @@ -280,6 +280,76 @@ int tone_s32_default(struct processing_module *mod, struct sof_sink *sink, return sink_commit_buffer(sink, bytes); } +int tone_float_default(struct processing_module *mod, struct sof_sink *sink, + struct sof_source *source) +{ + struct comp_data *cd = module_get_private_data(mod); + size_t output_frame_bytes, output_frames; + float *output_pos, *output_start; + size_t output_cirbuf_size; + float *output_end; + uint32_t frames, bytes; + int nch = cd->channels; + int i; + int n; + int n_wrap_dest; + int n_min; + int ret; + + if (cd->mode == TONE_MODE_PASSTHROUGH) + return tone_s32_passthrough(mod, sink, source); + + output_frames = sink_get_free_frames(sink); + output_frame_bytes = sink_get_frame_bytes(sink); + output_frames = mod->period_bytes / output_frame_bytes; + + ret = sink_get_buffer_s32(sink, output_frames * output_frame_bytes, + (int32_t **)&output_pos, (int32_t **)&output_start, + &output_cirbuf_size); + if (ret) + return -ENODATA; + + frames = output_frames; + + if (frames * output_frame_bytes >= mod->period_bytes) + frames = mod->period_bytes / output_frame_bytes; + bytes = frames * output_frame_bytes; + + output_end = output_start + output_cirbuf_size; + + n = frames * nch; + if (!source) { + const float norm = 1.0f / 2147483648.0f; + while (n > 0) { + n_wrap_dest = output_end - output_pos; + + n_min = (n < n_wrap_dest) ? n : n_wrap_dest; + while (n_min > 0) { + n -= nch; + n_min -= nch; + for (i = 0; i < nch; i++) { + switch (cd->mode) { + case TONE_MODE_TONEGEN: + tonegen_control(&cd->sg[i]); + *output_pos = (float)tonegen(&cd->sg[i]) * norm; + break; + case TONE_MODE_SILENCE: + *output_pos = 0.0f; + break; + default: + break; + } + output_pos++; + } + } + + output_pos = output_start; + } + } + + return sink_commit_buffer(sink, bytes); +} + void tonegen_update_f(struct tone_state *sg, int32_t f) { int64_t w_tmp; diff --git a/src/audio/tone/tone.h b/src/audio/tone/tone.h index 1a3978b710b5..f65a677ca911 100644 --- a/src/audio/tone/tone.h +++ b/src/audio/tone/tone.h @@ -97,6 +97,8 @@ int tonegen_init(struct tone_state *sg, int32_t fs, int32_t f, int32_t a); void tonegen_update_f(struct tone_state *sg, int32_t f); int tone_s32_default(struct processing_module *mod, struct sof_sink *sink, struct sof_source *source); +int tone_float_default(struct processing_module *mod, struct sof_sink *sink, + struct sof_source *source); /* Set sine amplitude */ static inline void tonegen_set_a(struct tone_state *sg, int32_t a) diff --git a/src/audio/volume/volume.c b/src/audio/volume/volume.c index 0414e9246194..bfc527a57ea4 100644 --- a/src/audio/volume/volume.c +++ b/src/audio/volume/volume.c @@ -193,6 +193,47 @@ static size_t vol_zc_get_s32(struct cir_buf_source *source, const unsigned int c #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT +static size_t vol_zc_get_float(struct cir_buf_source *source, const unsigned int channels, + size_t frames, int64_t *prev_sum) +{ + size_t curr_frames = frames; + const float *x = source->ptr; + size_t bytes; + size_t nmax; + size_t i, n; + unsigned int j; + size_t remaining_samples = frames * channels; + + /* Go to last channel */ + x = source_cir_buf_wrap(x + remaining_samples - 1, + source->buf_start, source->buf_end); + while (remaining_samples) { + bytes = cir_buf_bytes_without_wrap_rewind(x, source->buf_start); + nmax = (bytes / sizeof(float)) + 1; + n = MIN(nmax, remaining_samples); + for (i = 0; i < n; i += channels) { + float sum = 0.0f; + for (j = 0; j < channels; j++) { + sum += *x; + x--; + } + + int64_t sign = (sum < 0.0f) ? -1 : 1; + if (*prev_sum && (sign ^ *prev_sum) < 0) + return curr_frames; + + *prev_sum = sign; + curr_frames--; + } + remaining_samples -= n; + x = source_cir_buf_rewind_wrap(x, source->buf_start, source->buf_end); + } + + return frames; +} +#endif /* CONFIG_FORMAT_FLOAT */ + /** * \brief Map of formats with dedicated zc functions. * @@ -208,6 +249,9 @@ __cold_rodata static const struct comp_zc_func_map zc_func_map[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, vol_zc_get_s32 }, #endif /* CONFIG_FORMAT_S32LE */ +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, vol_zc_get_float }, +#endif /* CONFIG_FORMAT_FLOAT */ }; #if CONFIG_COMP_VOLUME_LINEAR_RAMP @@ -902,6 +946,7 @@ static struct comp_dev *vol_static_create(const struct comp_driver *drv, const struct sof_static_comp *cdesc, uint32_t period_us) { +#if CONFIG_IPC_MAJOR_4 struct static_ipc4_vol_init_cfg vol_cfg; memset(&vol_cfg, 0, sizeof(vol_cfg)); @@ -916,6 +961,20 @@ static struct comp_dev *vol_static_create(const struct comp_driver *drv, .data = (const uint8_t *)&vol_cfg, }; return drv->ops.create(drv, cfg, &spec); +#else + static const struct ipc_config_volume vol_cfg = { + .channels = 2, + .min_value = 0, + .max_value = 0x7fffffff, + .ramp = 0, + .initial_ramp = 0, + }; + struct ipc_config_process spec = { + .size = sizeof(vol_cfg), + .data = (const uint8_t *)&vol_cfg, + }; + return drv->ops.create(drv, cfg, &spec); +#endif } static int vol_static_apply_volume(struct comp_dev *dev, uint32_t channels, int32_t val) diff --git a/src/audio/volume/volume_generic.c b/src/audio/volume/volume_generic.c index c16701bc8034..6ca250d52690 100644 --- a/src/audio/volume/volume_generic.c +++ b/src/audio/volume/volume_generic.c @@ -298,6 +298,78 @@ static void vol_passthrough_s16_to_s16(struct processing_module *mod, } #endif /* CONFIG_FORMAT_S16LE */ +#if CONFIG_FORMAT_FLOAT +static void vol_float_to_float(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x; + float *y; + int nmax, n, i, j; + const int nch = cd->channels; + int remaining_samples = frames * nch; + float vol_f[PLATFORM_MAX_CHANNELS]; + + for (j = 0; j < nch; j++) { +#if COMP_VOLUME_Q8_16 + vol_f[j] = (float)cd->volume[j] * (1.0f / 65536.0f); +#elif COMP_VOLUME_Q1_31 + vol_f[j] = (float)cd->volume[j] * (1.0f / 2147483648.0f); +#else + vol_f[j] = (float)cd->volume[j] * (1.0f / 8388608.0f); +#endif + } + + x = source->ptr; + y = sink->ptr; + while (remaining_samples) { + nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); + n = MIN(remaining_samples, nmax); + nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + n = MIN(n, nmax); + for (j = 0; j < nch; j++) { + const float *x0 = x + j; + float *y0 = y + j; + const float vol = vol_f[j]; + for (i = 0; i < n; i += nch) { + y0[i] = x0[i] * vol; + } + } + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} + +static void vol_passthrough_float_to_float(struct processing_module *mod, + struct cir_buf_source *source, + struct cir_buf_sink *sink, uint32_t frames, + uint32_t attenuation) +{ + struct vol_data *cd = module_get_private_data(mod); + const float *x; + float *y; + int nmax, n; + const int nch = cd->channels; + int remaining_samples = frames * nch; + + x = source->ptr; + y = sink->ptr; + while (remaining_samples) { + nmax = cir_buf_samples_without_wrap_s32(x, source->buf_end); + n = MIN(remaining_samples, nmax); + nmax = cir_buf_samples_without_wrap_s32(y, sink->buf_end); + n = MIN(n, nmax); + memcpy_s(y, n * sizeof(float), x, n * sizeof(float)); + remaining_samples -= n; + x = cir_buf_wrap(x + n, source->buf_start, source->buf_end); + y = cir_buf_wrap(y + n, sink->buf_start, sink->buf_end); + } +} +#endif /* CONFIG_FORMAT_FLOAT */ + const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S16LE { SOF_IPC_FRAME_S16_LE, vol_s16_to_s16, vol_passthrough_s16_to_s16}, @@ -308,6 +380,9 @@ const struct comp_func_map volume_func_map[] = { #if CONFIG_FORMAT_S32LE { SOF_IPC_FRAME_S32_LE, vol_s32_to_s32, vol_passthrough_s32_to_s32}, #endif +#if CONFIG_FORMAT_FLOAT + { SOF_IPC_FRAME_FLOAT, vol_float_to_float, vol_passthrough_float_to_float}, +#endif }; const size_t volume_func_count = ARRAY_SIZE(volume_func_map); diff --git a/src/include/module/crossover/crossover_common.h b/src/include/module/crossover/crossover_common.h index d238ff7c1008..2e5945520e58 100644 --- a/src/include/module/crossover/crossover_common.h +++ b/src/include/module/crossover/crossover_common.h @@ -85,4 +85,56 @@ static inline crossover_split crossover_find_split_func(int32_t num_sinks) return crossover_split_fnmap[num_sinks - CROSSOVER_2WAY_NUM_SINKS]; } +#if CONFIG_FORMAT_FLOAT +#include + +/** + * Stores the state of one channel of the Crossover filter (Float) + */ +struct crossover_state_float { + /* Store the state for each LR4 filter. */ + struct iir_state_df1_float lowpass[CROSSOVER_MAX_LR4]; + struct iir_state_df1_float highpass[CROSSOVER_MAX_LR4]; +}; + +typedef void (*crossover_split_float)(float in, float out[], + struct crossover_state_float *state); + +extern const crossover_split_float crossover_split_float_fnmap[]; + +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); + +static inline void crossover_reset_state_lr4_float(struct processing_module *mod, + struct iir_state_df1_float *lr4) +{ + mod_free(mod, lr4->coef); + mod_free(mod, lr4->delay); + + lr4->coef = NULL; + lr4->delay = NULL; +} + +static inline void crossover_reset_state_ch_float(struct processing_module *mod, + struct crossover_state_float *ch_state) +{ + int i; + + for (i = 0; i < CROSSOVER_MAX_LR4; i++) { + crossover_reset_state_lr4_float(mod, &ch_state->lowpass[i]); + crossover_reset_state_lr4_float(mod, &ch_state->highpass[i]); + } +} + +static inline crossover_split_float crossover_find_split_func_float(int32_t num_sinks) +{ + if (num_sinks < CROSSOVER_2WAY_NUM_SINKS || + num_sinks > CROSSOVER_4WAY_NUM_SINKS) + return NULL; + return crossover_split_float_fnmap[num_sinks - CROSSOVER_2WAY_NUM_SINKS]; +} +#endif /* CONFIG_FORMAT_FLOAT */ + #endif /* __SOF_CROSSOVER_COMMON_H__ */ diff --git a/src/include/sof/audio/selector.h b/src/include/sof/audio/selector.h index 17c07e4f86b1..6d1199892dc6 100644 --- a/src/include/sof/audio/selector.h +++ b/src/include/sof/audio/selector.h @@ -37,11 +37,13 @@ struct cir_buf_sink; #define SEL_SOURCE_1CH 1 #define SEL_SOURCE_2CH 2 #define SEL_SOURCE_4CH 4 +#define SEL_SOURCE_CHANNELS_MAX 8 /** \brief Supported channel count on output. */ #define SEL_SINK_1CH 1 #define SEL_SINK_2CH 2 #define SEL_SINK_4CH 4 +#define SEL_SINK_CHANNELS_MAX 8 #else /** \brief Maximum supported channel count on input. */ #define SEL_SOURCE_CHANNELS_MAX 8 diff --git a/src/include/sof/math/fir_float.h b/src/include/sof/math/fir_float.h new file mode 100644 index 000000000000..f87a3eb1a070 --- /dev/null +++ b/src/include/sof/math/fir_float.h @@ -0,0 +1,41 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + */ + +#ifndef __SOF_MATH_FIR_FLOAT_H__ +#define __SOF_MATH_FIR_FLOAT_H__ + +#include +#include +#include +#include + +struct sof_fir_coef_data; + +struct fir_state_float { + int rwi; /* Circular read and write index */ + int taps; /* Number of FIR taps */ + int length; /* Number of FIR taps plus input length (even) */ + int out_shift; /* Unused in float kernel (folded into coefficients) */ + float *coef; /* Pointer to FIR float coefficients */ + float *delay; /* Pointer to FIR float delay line */ +}; + +void fir_reset_float(struct fir_state_float *fir); + +int fir_delay_size_float(struct sof_fir_coef_data *config); + +int fir_coef_size_float(struct sof_fir_coef_data *config); + +int fir_init_coef_float(struct fir_state_float *fir, + struct sof_fir_coef_data *config, + float **coef_storage); + +void fir_init_delay_float(struct fir_state_float *fir, float **data); + +float fir_float(struct fir_state_float *fir, float x); + +void fir_float_2x(struct fir_state_float *fir, float x0, float x1, float *y0, float *y1); + +#endif /* __SOF_MATH_FIR_FLOAT_H__ */ diff --git a/src/include/sof/math/iir_df1_float.h b/src/include/sof/math/iir_df1_float.h new file mode 100644 index 000000000000..3723baf4776a --- /dev/null +++ b/src/include/sof/math/iir_df1_float.h @@ -0,0 +1,67 @@ +/* SPDX-License-Identifier: BSD-3-Clause + * + * Copyright(c) 2026 Sound Open Firmware. All rights reserved. + */ + +#ifndef __SOF_MATH_IIR_DF1_FLOAT_H__ +#define __SOF_MATH_IIR_DF1_FLOAT_H__ + +#include +#include +#include + +struct sof_eq_iir_header; + +struct iir_state_df1_float { + unsigned int biquads; /* Number of IIR 2nd order sections */ + unsigned int biquads_in_series; /* Number of sections in series */ + float *coef; /* {b0, b1, b2, a1, a2} per biquad (5 floats) */ + float *delay; /* {x1, x2, y1, y2} per biquad (4 floats) */ +}; + +int iir_delay_size_df1_float(struct sof_eq_iir_header *config); +int iir_coef_size_df1_float(struct sof_eq_iir_header *config); +int iir_init_coef_df1_float(struct iir_state_df1_float *iir, struct sof_eq_iir_header *config, float **coef_storage); +void iir_init_delay_df1_float(struct iir_state_df1_float *iir, float **delay_storage); +void iir_reset_df1_float(struct iir_state_df1_float *iir); + +static inline float iir_df1_float(struct iir_state_df1_float *iir, float x) +{ + if (!iir->biquads) + return x; + + const float *coefp = iir->coef; + float *delay = iir->delay; + float in = x; + int c = 0; + int d = 0; + + for (unsigned int j = 0; j < iir->biquads; j++) { + const float b0 = coefp[c + 0]; + const float b1 = coefp[c + 1]; + const float b2 = coefp[c + 2]; + const float a1 = coefp[c + 3]; + const float a2 = coefp[c + 4]; + + const float x1 = delay[d + 0]; + const float x2 = delay[d + 1]; + const float y1 = delay[d + 2]; + const float y2 = delay[d + 3]; + + /* 5 single-cycle FMA operations on RV32F */ + float out = b0 * in + b1 * x1 + b2 * x2 + a1 * y1 + a2 * y2; + + delay[d + 1] = x1; + delay[d + 0] = in; + delay[d + 3] = y1; + delay[d + 2] = out; + + in = out; + c += 5; + d += 4; + } + + return in; +} + +#endif /* __SOF_MATH_IIR_DF1_FLOAT_H__ */ diff --git a/src/math/CMakeLists.txt b/src/math/CMakeLists.txt index cb47e8c3f01b..efd4cb77a57c 100644 --- a/src/math/CMakeLists.txt +++ b/src/math/CMakeLists.txt @@ -51,7 +51,7 @@ if(CONFIG_MATH_FIR STREQUAL "m" AND DEFINED CONFIG_LLEXT) add_subdirectory(fir_llext ${PROJECT_BINARY_DIR}/aux1_fir_llext) add_dependencies(app aux1_fir) elseif(CONFIG_MATH_FIR) - list(APPEND base_files fir_generic.c fir_hifi2ep.c fir_hifi3.c fir_hifi5.c fir_arm.c) + list(APPEND base_files fir_generic.c fir_hifi2ep.c fir_hifi3.c fir_hifi5.c fir_arm.c fir_float.c) endif() if(CONFIG_MATH_FFT) @@ -67,7 +67,7 @@ elseif(CONFIG_MATH_IIR) endif() if(CONFIG_MATH_IIR_DF1) - list(APPEND base_files iir_df1_generic.c iir_df1_hifi3.c iir_df1_hifi4.c iir_df1_hifi5.c iir_df1_arm.c iir_df1_riscv.c iir_df1.c) + list(APPEND base_files iir_df1_generic.c iir_df1_hifi3.c iir_df1_hifi4.c iir_df1_hifi5.c iir_df1_arm.c iir_df1_riscv.c iir_df1_float.c iir_df1.c) endif() endif() diff --git a/src/math/fir_float.c b/src/math/fir_float.c new file mode 100644 index 000000000000..9d2ef02d4839 --- /dev/null +++ b/src/math/fir_float.c @@ -0,0 +1,179 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +void fir_reset_float(struct fir_state_float *fir) +{ + fir->rwi = 0; + fir->taps = 0; + fir->length = 0; + fir->out_shift = 0; + fir->coef = NULL; +} +EXPORT_SYMBOL(fir_reset_float); + +int fir_delay_size_float(struct sof_fir_coef_data *config) +{ + if (config->length > SOF_FIR_MAX_LENGTH || config->length < 4) + return -EINVAL; + + if (config->length & 0x3) + return -EINVAL; + + return (config->length + 4) * sizeof(float); +} +EXPORT_SYMBOL(fir_delay_size_float); + +int fir_coef_size_float(struct sof_fir_coef_data *config) +{ + if (config->length > SOF_FIR_MAX_LENGTH || config->length < 4) + return -EINVAL; + + return config->length * sizeof(float); +} +EXPORT_SYMBOL(fir_coef_size_float); + +int fir_init_coef_float(struct fir_state_float *fir, + struct sof_fir_coef_data *config, + float **coef_storage) +{ + fir->rwi = 0; + fir->taps = (int)config->length; + fir->length = (int)fir->taps + 2; + fir->out_shift = 0; + + if (coef_storage && *coef_storage) { + float scale; + int shift = config->out_shift; + int i; + + fir->coef = *coef_storage; + *coef_storage += fir->taps; + + if (shift >= 0 && shift <= 30) + scale = (1.0f / 32768.0f) / (float)(1 << shift); + else if (shift < 0 && shift >= -30) + scale = (1.0f / 32768.0f) * (float)(1 << (-shift)); + else + scale = 1.0f / 32768.0f; + + for (i = 0; i < fir->taps; i++) + fir->coef[i] = (float)config->coef[i] * scale; + } + + return 0; +} +EXPORT_SYMBOL(fir_init_coef_float); + +void fir_init_delay_float(struct fir_state_float *fir, float **data) +{ + fir->delay = *data; + *data += fir->length; +} +EXPORT_SYMBOL(fir_init_delay_float); + +float fir_float(struct fir_state_float *fir, float x) +{ + float *data = &fir->delay[fir->rwi]; + const float *coef = &fir->coef[0]; + float y = 0.0f; + int n1, n2, n; + const int length = fir->length; + const int taps = fir->taps; + + if (!fir->length) + return x; + + *data = x; + + n1 = ++fir->rwi; + if (fir->rwi == length) + fir->rwi = 0; + + n1 = MIN(n1, taps); + for (n = 0; n < n1; n++) { + y += (*coef) * (*data); + coef++; + data--; + } + + n2 = taps - n1; + data = &fir->delay[length - 1]; + for (n = 0; n < n2; n++) { + y += (*coef) * (*data); + coef++; + data--; + } + + return y; +} +EXPORT_SYMBOL(fir_float); + +void fir_float_2x(struct fir_state_float *fir, float x0, float x1, float *y0, float *y1) +{ + if (!fir->taps) { + *y0 = x0; + *y1 = x1; + return; + } + + /* When write index is at the last slot, wrap the second sample to slot 0 */ + if (fir->rwi == fir->length - 1) { + *y0 = fir_float(fir, x0); + *y1 = fir_float(fir, x1); + return; + } + + float a0 = 0.0f; + float a1 = 0.0f; + float sample0; + float sample1; + float tap; + float *data = &fir->delay[fir->rwi]; + const float *coef = &fir->coef[0]; + int n1, n2, i; + const int length = fir->length; + const int taps = fir->taps; + + *data = x0; + *(data + 1) = x1; + + n1 = fir->rwi + 1; + fir->rwi += 2; + if (fir->rwi >= length) + fir->rwi -= length; + + sample1 = x1; + n1 = MIN(n1, taps); + + for (i = 0; i < n1; i++) { + tap = *coef++; + sample0 = *data--; + a1 += tap * sample1; + a0 += tap * sample0; + sample1 = sample0; + } + + n2 = taps - n1; + data = &fir->delay[length - 1]; + for (i = 0; i < n2; i++) { + tap = *coef++; + sample0 = *data--; + a1 += tap * sample1; + a0 += tap * sample0; + sample1 = sample0; + } + + *y0 = a0; + *y1 = a1; +} +EXPORT_SYMBOL(fir_float_2x); diff --git a/src/math/iir_df1_float.c b/src/math/iir_df1_float.c new file mode 100644 index 000000000000..5cdcab1b9756 --- /dev/null +++ b/src/math/iir_df1_float.c @@ -0,0 +1,102 @@ +// SPDX-License-Identifier: BSD-3-Clause +// +// Copyright(c) 2026 Sound Open Firmware. All rights reserved. + +#include +#include +#include +#include +#include +#include +#include + +int iir_delay_size_df1_float(struct sof_eq_iir_header *config) +{ + uint32_t n = config->num_sections; + + if (!n || n > SOF_EQ_IIR_BIQUADS_MAX) + return -EINVAL; + + if (!config->num_sections_in_series || + config->num_sections_in_series > n) + return -EINVAL; + + return 4 * n * sizeof(float); +} +EXPORT_SYMBOL(iir_delay_size_df1_float); + +int iir_coef_size_df1_float(struct sof_eq_iir_header *config) +{ + uint32_t n = config->num_sections; + + if (!n || n > SOF_EQ_IIR_BIQUADS_MAX) + return -EINVAL; + + return 5 * n * sizeof(float); +} +EXPORT_SYMBOL(iir_coef_size_df1_float); + +int iir_init_coef_df1_float(struct iir_state_df1_float *iir, + struct sof_eq_iir_header *config, + float **coef_storage) +{ + uint32_t n = config->num_sections; + const int32_t *raw_coef = (const int32_t *)ASSUME_ALIGNED(&config->biquads[0], 4); + float *c_out = *coef_storage; + + iir->biquads = n; + iir->biquads_in_series = config->num_sections_in_series; + iir->coef = c_out; + + /* Convert each fixed-point biquad {a2, a1, b2, b1, b0, shift, gain} + * to normalized float {b0, b1, b2, a1, a2} */ + const float inv_q30 = 1.0f / 1073741824.0f; + const float inv_q14 = 1.0f / 16384.0f; + + for (uint32_t j = 0; j < n; j++) { + int32_t a2_q30 = raw_coef[0]; + int32_t a1_q30 = raw_coef[1]; + int32_t b2_q30 = raw_coef[2]; + int32_t b1_q30 = raw_coef[3]; + int32_t b0_q30 = raw_coef[4]; + int32_t shift = raw_coef[5]; + int32_t gain = raw_coef[6]; + + float gain_factor = (float)gain * inv_q14; + if (shift > 0) + gain_factor /= (float)(1 << shift); + else if (shift < 0) + gain_factor *= (float)(1 << (-shift)); + + float b_scale = gain_factor * inv_q30; + + c_out[0] = (float)b0_q30 * b_scale; + c_out[1] = (float)b1_q30 * b_scale; + c_out[2] = (float)b2_q30 * b_scale; + c_out[3] = (float)a1_q30 * inv_q30; + c_out[4] = (float)a2_q30 * inv_q30; + + raw_coef += 7; + c_out += 5; + } + + *coef_storage = c_out; + return 0; +} +EXPORT_SYMBOL(iir_init_coef_df1_float); + +void iir_init_delay_df1_float(struct iir_state_df1_float *iir, float **delay_storage) +{ + iir->delay = *delay_storage; + *delay_storage += 4 * iir->biquads; +} +EXPORT_SYMBOL(iir_init_delay_df1_float); + +void iir_reset_df1_float(struct iir_state_df1_float *iir) +{ + iir->biquads = 0; + iir->biquads_in_series = 0; + iir->coef = NULL; + iir->delay = NULL; +} +EXPORT_SYMBOL(iir_reset_df1_float); diff --git a/test/cmocka/src/audio/eq_fir/CMakeLists.txt b/test/cmocka/src/audio/eq_fir/CMakeLists.txt index 226128b220d2..8ab897c1356c 100644 --- a/test/cmocka/src/audio/eq_fir/CMakeLists.txt +++ b/test/cmocka/src/audio/eq_fir/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(audio_for_eq_fir STATIC ${PROJECT_SOURCE_DIR}/src/audio/eq_fir/eq_fir_hifi2ep.c ${PROJECT_SOURCE_DIR}/src/audio/eq_fir/eq_fir_hifi3.c ${PROJECT_SOURCE_DIR}/src/math/fir_generic.c + ${PROJECT_SOURCE_DIR}/src/math/fir_float.c ${PROJECT_SOURCE_DIR}/src/math/fir_hifi2ep.c ${PROJECT_SOURCE_DIR}/src/math/fir_hifi3.c ${PROJECT_SOURCE_DIR}/src/math/numbers.c diff --git a/test/cmocka/src/audio/eq_iir/CMakeLists.txt b/test/cmocka/src/audio/eq_iir/CMakeLists.txt index b5ff8770eec2..eb603d332c5b 100644 --- a/test/cmocka/src/audio/eq_iir/CMakeLists.txt +++ b/test/cmocka/src/audio/eq_iir/CMakeLists.txt @@ -16,6 +16,7 @@ add_library(audio_for_eq_iir STATIC ${PROJECT_SOURCE_DIR}/src/audio/eq_iir/eq_iir_generic.c ${PROJECT_SOURCE_DIR}/src/audio/eq_iir/eq_iir_ipc3.c ${PROJECT_SOURCE_DIR}/src/math/iir_df1.c + ${PROJECT_SOURCE_DIR}/src/math/iir_df1_float.c ${PROJECT_SOURCE_DIR}/src/math/iir_df1_generic.c ${PROJECT_SOURCE_DIR}/src/math/iir_df1_hifi3.c ${PROJECT_SOURCE_DIR}/src/math/iir_df1_hifi4.c diff --git a/test/cmocka/src/audio/mux/mux_get_processing_function.c b/test/cmocka/src/audio/mux/mux_get_processing_function.c index 0e4f22ff7021..6b3b06410936 100644 --- a/test/cmocka/src/audio/mux/mux_get_processing_function.c +++ b/test/cmocka/src/audio/mux/mux_get_processing_function.c @@ -102,20 +102,32 @@ static int teardown_test_case(void **state) } #if CONFIG_FORMAT_FLOAT -static void test_mux_get_processing_function_invalid_float(void **state) +static void test_mux_get_processing_function_valid_float(void **state) { struct test_data *td = *state; mux_func func = NULL; - /* set frame format value to unsupported value */ audio_stream_set_frm_fmt(&td->sink->stream, SOF_IPC_FRAME_FLOAT); func = mux_get_processing_function(td->mod); - assert_ptr_equal(func, NULL); + assert_ptr_not_equal(func, NULL); } #endif /* CONFIG_FORMAT_FLOAT */ +static void test_mux_get_processing_function_invalid_format(void **state) +{ + struct test_data *td = *state; + mux_func func = NULL; + + /* set frame format value to unsupported value */ + audio_stream_set_frm_fmt(&td->sink->stream, SOF_IPC_FRAME_INVALID); + + func = mux_get_processing_function(td->mod); + + assert_ptr_equal(func, NULL); +} + #if CONFIG_FORMAT_S16LE static void test_mux_get_processing_function_valid_s16le(void **state) { @@ -167,8 +179,9 @@ int main(void) { const struct CMUnitTest tests[] = { #if CONFIG_FORMAT_FLOAT - TEST_CASE(test_mux_get_processing_function_invalid_float), + TEST_CASE(test_mux_get_processing_function_valid_float), #endif /* CONFIG_FORMAT_FLOAT */ + TEST_CASE(test_mux_get_processing_function_invalid_format), #if CONFIG_FORMAT_S16LE TEST_CASE(test_mux_get_processing_function_valid_s16le), #endif /* CONFIG_FORMAT_S16LE */