diff --git a/src/modules/module-filter-chain.c b/src/modules/module-filter-chain.c index e211043a2..1cb311e21 100644 --- a/src/modules/module-filter-chain.c +++ b/src/modules/module-filter-chain.c @@ -239,6 +239,10 @@ PW_LOG_TOPIC_STATIC(mod_topic, "mod." NAME); * specifying a number of chained biquads and it can also load configuration from a * file. * + * The parametric EQ supports multichannel processing and has 8 input and 8 output ports + * that don't all need to be connected. The ports are named `In 1` to `In 8` and + * `Out 1` to `Out 8`. + * *\code{.unparsed} * filter.graph = { * nodes = [ @@ -248,11 +252,13 @@ PW_LOG_TOPIC_STATIC(mod_topic, "mod." NAME); * label = param_eq * config = { * filename = "..." + * #filename1 = "...", filename2 = "...", ... * filters = [ * { type = ..., freq = ..., gain = ..., q = ... }, * { type = ..., freq = ..., gain = ..., q = ... }, * .... * ] + * #filters1 = [ ... ], filters2 = [ ... ], ... * } * ... * } @@ -261,7 +267,10 @@ PW_LOG_TOPIC_STATIC(mod_topic, "mod." NAME); * } *\endcode * - * Either a `filename` or a `filters` array can be specified. + * Either a `filename` or a `filters` array can be specified. The configuration + * will be used for all channels. Alternatively `filenameX` or `filtersX` where + * X is the channel number (between 1 and 8) can be used to load a channel + * specific configuration. * * The `filename` must point to a parametric equalizer configuration * generated from the AutoEQ project or Squiglink. Both the projects allow @@ -291,8 +300,8 @@ PW_LOG_TOPIC_STATIC(mod_topic, "mod." NAME); * shelf and high shelf filter respectively. More often than not only peaking * filters are involved. * - * The `filters` can contain an array of filter specification object with the following - * keys: + * The `filters` (or channel specific `filtersX` where X is the channel between 1 and + * 8) can contain an array of filter specification object with the following keys: * * `type` specifies the filter type, choose one from the available biquad labels. * `freq` is the frequency passed to the biquad. @@ -2524,6 +2533,7 @@ static int setup_graph(struct graph *graph, struct spa_json *inputs, struct spa_ struct descriptor *desc; const struct fc_descriptor *d; char v[256]; + bool allow_unused; first = spa_list_first(&graph->node_list, struct node, link); last = spa_list_last(&graph->node_list, struct node, link); @@ -2532,16 +2542,22 @@ static int setup_graph(struct graph *graph, struct spa_json *inputs, struct spa_ * If we have a list of inputs/outputs, just count them. Otherwise * we count all input ports of the first node and all output * ports of the last node */ - if (inputs != NULL) { + if (inputs != NULL) n_input = count_array(inputs); - } else { + else n_input = first->desc->n_input; - } - if (outputs != NULL) { + + if (outputs != NULL) n_output = count_array(outputs); - } else { + else n_output = last->desc->n_output; - } + + /* we allow unconnected ports when not explicitly given and the nodes support + * NULL data */ + allow_unused = inputs == NULL && outputs == NULL && + SPA_FLAG_IS_SET(first->desc->desc->flags, FC_DESCRIPTOR_SUPPORTS_NULL_DATA) && + SPA_FLAG_IS_SET(last->desc->desc->flags, FC_DESCRIPTOR_SUPPORTS_NULL_DATA); + if (n_input == 0) { pw_log_error("no inputs"); res = -EINVAL; @@ -2578,7 +2594,8 @@ static int setup_graph(struct graph *graph, struct spa_json *inputs, struct spa_ } if (n_hndl == 0) { n_hndl = 1; - pw_log_warn("The capture stream has %1$d channels and " + if (!allow_unused) + pw_log_warn("The capture stream has %1$d channels and " "the filter has %2$d inputs. The playback stream has %3$d channels " "and the filter has %4$d outputs. Some filter ports will be " "unconnected..", diff --git a/src/modules/module-filter-chain/builtin_plugin.c b/src/modules/module-filter-chain/builtin_plugin.c index ff2a94ba3..de53fce7b 100644 --- a/src/modules/module-filter-chain/builtin_plugin.c +++ b/src/modules/module-filter-chain/builtin_plugin.c @@ -1668,34 +1668,22 @@ static const struct fc_descriptor sine_desc = { .cleanup = builtin_cleanup, }; -#define PARAM_EQ_NUM_PORTS 2 -static struct fc_port param_eq_ports[] = { - { .index = 0, - .name = "Out", - .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, - }, - { .index = 1, - .name = "In", - .flags = FC_PORT_INPUT | FC_PORT_AUDIO, - }, -}; - -#define PARAM_EQ_MAX 128 +#define PARAM_EQ_MAX 64 struct param_eq_impl { unsigned long rate; - float *port[2]; + float *port[8*2]; uint32_t n_bq; - struct biquad bq[PARAM_EQ_MAX]; + struct biquad bq[PARAM_EQ_MAX * 8]; }; -static int load_eq_bands(struct param_eq_impl *impl, const char *filename) +static int load_eq_bands(const char *filename, int rate, struct biquad *bq, uint32_t max_bq, uint32_t *n_bq) { FILE *f = NULL; char *line = NULL; ssize_t nread; size_t linelen; - uint32_t freq; + uint32_t freq, n = 0; char filter_type[4]; char filter[4]; char q[7], gain[7]; @@ -1722,11 +1710,11 @@ static int load_eq_bands(struct param_eq_impl *impl, const char *filename) nread = getline(&line, &linelen, f); if (nread != -1 && sscanf(line, "%*s %6s %*s", gain) == 1) { if (spa_json_parse_float(gain, strlen(gain), &vg)) - biquad_set(&impl->bq[impl->n_bq++], BQ_HIGHSHELF, 0.0f, 1.0f, vg); + biquad_set(&bq[n++], BQ_HIGHSHELF, 0.0f, 1.0f, vg); } /* Read the filter bands */ while ((nread = getline(&line, &linelen, f)) != -1) { - if (impl->n_bq == PARAM_EQ_MAX) { + if (n == PARAM_EQ_MAX) { res = -ENOSPC; goto exit; } @@ -1757,37 +1745,95 @@ static int load_eq_bands(struct param_eq_impl *impl, const char *filename) if (spa_json_parse_float(gain, strlen(gain), &vg) && spa_json_parse_float(q, strlen(q), &vq)) - biquad_set(&impl->bq[impl->n_bq++], type, freq * 2.0f / impl->rate, vq, vg); + biquad_set(&bq[n++], type, freq * 2.0f / rate, vq, vg); } } } + *n_bq = n; exit: if (f) fclose(f); return res; } + + + +/* + * [ + * { type=bq_peaking freq=21 gain=6.7 q=1.100 } + * { type=bq_peaking freq=85 gain=6.9 q=3.000 } + * { type=bq_peaking freq=110 gain=-2.6 q=2.700 } + * { type=bq_peaking freq=210 gain=5.9 q=2.100 } + * { type=bq_peaking freq=710 gain=-1.0 q=0.600 } + * { type=bq_peaking freq=1600 gain=2.3 q=2.700 } + * ] + */ +static int parse_filters(struct spa_json *iter, int rate, struct biquad *bq, uint32_t max_bq, uint32_t *n_bq) +{ + struct spa_json it[1]; + const char *val; + char key[256]; + char type_str[17]; + int len; + uint32_t n = 0; + + while (spa_json_enter_object(iter, &it[0]) > 0) { + float freq = 0.0f, gain = 0.0f, q = 1.0f; + int type = BQ_NONE; + + while ((len = spa_json_object_next(&it[0], key, sizeof(key), &val)) > 0) { + if (spa_streq(key, "type")) { + if (spa_json_parse_stringn(val, len, type_str, sizeof(type_str)) <= 0) { + pw_log_error("param_eq:type requires a string"); + return -EINVAL; + } + type = bq_type_from_name(type_str); + } + else if (spa_streq(key, "freq")) { + if (spa_json_parse_float(val, len, &freq) <= 0) { + pw_log_error("param_eq:rate requires a number"); + return -EINVAL; + } + } + else if (spa_streq(key, "q")) { + if (spa_json_parse_float(val, len, &q) <= 0) { + pw_log_error("param_eq:q requires a float"); + return -EINVAL; + } + } + else if (spa_streq(key, "gain")) { + if (spa_json_parse_float(val, len, &gain) <= 0) { + pw_log_error("param_eq:gain requires a float"); + return -EINVAL; + } + } + else { + pw_log_warn("param_eq: ignoring filter key: '%s'", key); + } + } + if (n == max_bq) + return -ENOSPC; + biquad_set(&bq[n++], type, freq * 2 / rate, q, gain); + } + *n_bq = n; + return 0; +} + /* * { * filename = "...", - * filters = [ - * { type=bq_peaking freq=21 gain=6.7 q=1.100 } - * { type=bq_peaking freq=85 gain=6.9 q=3.000 } - * { type=bq_peaking freq=110 gain=-2.6 q=2.700 } - * { type=bq_peaking freq=210 gain=5.9 q=2.100 } - * { type=bq_peaking freq=710 gain=-1.0 q=0.600 } - * { type=bq_peaking freq=1600 gain=2.3 q=2.700 } - * } - * ] + * filenameX = "...", # to load channel X + * filters = [ ... ] + * filtersX = [ ... ] # to load channel X + * } */ - static void *param_eq_instantiate(const struct fc_descriptor * Descriptor, unsigned long SampleRate, int index, const char *config) { struct spa_json it[3]; const char *val; char key[256], filename[PATH_MAX]; - char type_str[17]; - int len, res; + int i, len, res; struct param_eq_impl *impl; if (config == NULL) { @@ -1808,61 +1854,45 @@ static void *param_eq_instantiate(const struct fc_descriptor * Descriptor, impl->rate = SampleRate; while ((len = spa_json_object_next(&it[0], key, sizeof(key), &val)) > 0) { - if (spa_streq(key, "filename")) { + int32_t idx = 0; + struct biquad *bq = impl->bq; + + if (spa_strstartswith(key, "filename")) { if (spa_json_parse_stringn(val, len, filename, sizeof(filename)) <= 0) { pw_log_error("param_eq: filename requires a string"); goto error; } - res = load_eq_bands(impl, filename); + if (spa_atoi32(key+8, &idx, 0)) + bq = &impl->bq[(SPA_CLAMP(idx, 1, 8) - 1) * PARAM_EQ_MAX]; + + res = load_eq_bands(filename, impl->rate, bq, PARAM_EQ_MAX, &impl->n_bq); if (res < 0) { - pw_log_error("failed to parse param_eq configuration from %s", filename); + pw_log_error("param_eq: failed to parse configuration from '%s'", filename); goto error; } } - else if (spa_streq(key, "filters")) { + else if (spa_strstartswith(key, "filters")) { if (!spa_json_is_array(val, len)) { pw_log_error("param_eq:filters require an array"); goto error; } spa_json_enter(&it[0], &it[1]); - while (spa_json_enter_object(&it[1], &it[2]) > 0) { - float freq = 0.0f, gain = 0.0f, q = 1.0f; - int type = BQ_NONE; - while ((len = spa_json_object_next(&it[2], key, sizeof(key), &val)) > 0) { - if (spa_streq(key, "type")) { - if (spa_json_parse_stringn(val, len, type_str, sizeof(type_str)) <= 0) { - pw_log_error("param_eq:type requires a string"); - goto error; - } - type = bq_type_from_name(type_str); - } - else if (spa_streq(key, "freq")) { - if (spa_json_parse_float(val, len, &freq) <= 0) { - pw_log_error("param_eq:rate requires a number"); - goto error; - } - } - else if (spa_streq(key, "q")) { - if (spa_json_parse_float(val, len, &q) <= 0) { - pw_log_error("param_eq:q requires a float"); - goto error; - } - } - else if (spa_streq(key, "gain")) { - if (spa_json_parse_float(val, len, &gain) <= 0) { - pw_log_error("param_eq:gain requires a float"); - goto error; - } - } - else { - pw_log_warn("param_eq: ignoring filter key: '%s'", key); - } - } - biquad_set(&impl->bq[impl->n_bq++], type, freq * 2 / impl->rate, q, gain); + if (spa_atoi32(key+7, &idx, 0)) + bq = &impl->bq[(SPA_CLAMP(idx, 1, 8) - 1) * PARAM_EQ_MAX]; + + res = parse_filters(&it[1], impl->rate, bq, PARAM_EQ_MAX, &impl->n_bq); + if (res < 0) { + pw_log_error("param_eq: failed to parse configuration"); + goto error; } } else { - pw_log_warn("delay: ignoring config key: '%s'", key); + pw_log_warn("param_eq: ignoring config key: '%s'", key); + } + if (idx == 0) { + for (i = 1; i < 8; i++) + memcpy(&impl->bq[i*PARAM_EQ_MAX], impl->bq, + sizeof(struct biquad) * PARAM_EQ_MAX); } } pw_log_info("loaded %d biquads", impl->n_bq); @@ -1882,16 +1912,83 @@ static void param_eq_connect_port(void * Instance, unsigned long Port, static void param_eq_run(void * Instance, unsigned long SampleCount) { struct param_eq_impl *impl = Instance; - const float *in[] = { impl->port[1] }; - float *out[] = { impl->port[0] }; - - dsp_ops_biquadn_run(dsp_ops, impl->bq, impl->n_bq, out, in, 1, SampleCount); + dsp_ops_biquadn_run(dsp_ops, impl->bq, impl->n_bq, PARAM_EQ_MAX, + &impl->port[8], (const float**)impl->port, 8, SampleCount); } +static struct fc_port param_eq_ports[] = { + { .index = 0, + .name = "In 1", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 1, + .name = "In 2", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 2, + .name = "In 3", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 3, + .name = "In 4", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 4, + .name = "In 5", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 5, + .name = "In 6", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 6, + .name = "In 7", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + { .index = 7, + .name = "In 8", + .flags = FC_PORT_INPUT | FC_PORT_AUDIO, + }, + + { .index = 8, + .name = "Out 1", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 9, + .name = "Out 2", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 10, + .name = "Out 3", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 11, + .name = "Out 4", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 12, + .name = "Out 5", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 13, + .name = "Out 6", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 14, + .name = "Out 7", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, + { .index = 15, + .name = "Out 8", + .flags = FC_PORT_OUTPUT | FC_PORT_AUDIO, + }, +}; + static const struct fc_descriptor param_eq_desc = { .name = "param_eq", + .flags = FC_DESCRIPTOR_SUPPORTS_NULL_DATA, - .n_ports = PARAM_EQ_NUM_PORTS, + .n_ports = SPA_N_ELEMENTS(param_eq_ports), .ports = param_eq_ports, .instantiate = param_eq_instantiate, diff --git a/src/modules/module-filter-chain/dsp-ops-c.c b/src/modules/module-filter-chain/dsp-ops-c.c index 16fb46bb0..0a576ce9a 100644 --- a/src/modules/module-filter-chain/dsp-ops-c.c +++ b/src/modules/module-filter-chain/dsp-ops-c.c @@ -135,19 +135,20 @@ void dsp_biquad_run_c(struct dsp_ops *ops, struct biquad *bq, #undef F } -void dsp_biquadn_run_c(struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, +void dsp_biquadn_run_c(struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, uint32_t bq_stride, float * SPA_RESTRICT out[], const float * SPA_RESTRICT in[], uint32_t n_src, uint32_t n_samples) { uint32_t i, j; const float *s; float *d; - struct biquad *b = bq; - for (i = 0; i < n_src; i++, bq+=n_src) { + for (i = 0; i < n_src; i++, bq+=bq_stride) { s = in[i]; d = out[i]; + if (s == NULL || d == NULL) + continue; for (j = 0; j < n_bq; j++) { - dsp_biquad_run_c(ops, &b[j], d, s, n_samples); + dsp_biquad_run_c(ops, &bq[j], d, s, n_samples); s = d; } } diff --git a/src/modules/module-filter-chain/dsp-ops-sse.c b/src/modules/module-filter-chain/dsp-ops-sse.c index 7ab18aaf2..a707e959a 100644 --- a/src/modules/module-filter-chain/dsp-ops-sse.c +++ b/src/modules/module-filter-chain/dsp-ops-sse.c @@ -151,7 +151,7 @@ void dsp_biquad_run_sse(struct dsp_ops *ops, struct biquad *bq, #undef F } -static void dsp_biquad_run2_sse(struct dsp_ops *ops, struct biquad *bq0, struct biquad *bq1, +static void dsp_biquad2_run_sse(struct dsp_ops *ops, struct biquad *bq0, struct biquad *bq1, float *out, const float *in, uint32_t n_samples) { __m128 x, y, z; @@ -195,25 +195,152 @@ static void dsp_biquad_run2_sse(struct dsp_ops *ops, struct biquad *bq0, struct #undef F } -void dsp_biquadn_run_sse(struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, +static void dsp_biquad_run2_sse(struct dsp_ops *ops, struct biquad *bqL, struct biquad *bqR, + float *outL, float *outR, const float *inL, const float *inR, uint32_t n_samples) +{ + __m128 x, y, z; + __m128 b0, b1, b2; + __m128 a1, a2; + __m128 x1, x2; + uint32_t i; + + b0 = _mm_setr_ps(bqL->b0, bqR->b0, 0.0f, 0.0f); /* b00 b10 0 0 */ + b1 = _mm_setr_ps(bqL->b1, bqR->b1, 0.0f, 0.0f); /* b01 b11 0 0 */ + b2 = _mm_setr_ps(bqL->b2, bqR->b2, 0.0f, 0.0f); /* b02 b12 0 0 */ + a1 = _mm_setr_ps(bqL->a1, bqR->a1, 0.0f, 0.0f); /* b00 b10 0 0 */ + a2 = _mm_setr_ps(bqL->a2, bqR->a2, 0.0f, 0.0f); /* b01 b11 0 0 */ + x1 = _mm_setr_ps(bqL->x1, bqR->x1, 0.0f, 0.0f); /* b00 b10 0 0 */ + x2 = _mm_setr_ps(bqL->x2, bqR->x2, 0.0f, 0.0f); /* b01 b11 0 0 */ + + for (i = 0; i < n_samples; i++) { + x = _mm_setr_ps(inL[i], inR[i], 0.0f, 0.0f); + + y = _mm_mul_ps(x, b0); /* y = x * b0 */ + y = _mm_add_ps(y, x1); /* y = x * b0 + x1*/ + z = _mm_mul_ps(y, a1); /* z = a1 * y */ + x1 = _mm_mul_ps(x, b1); /* x1 = x * b1 */ + x1 = _mm_add_ps(x1, x2); /* x1 = x * b1 + x2*/ + x1 = _mm_sub_ps(x1, z); /* x1 = x * b1 + x2 - a1 * y*/ + z = _mm_mul_ps(y, a2); /* z = a2 * y */ + x2 = _mm_mul_ps(x, b2); /* x2 = x * b2 */ + x2 = _mm_sub_ps(x2, z); /* x2 = x * b2 - a2 * y*/ + + outL[i] = y[0]; + outR[i] = y[1]; + } +#define F(x) (-FLT_MIN < (x) && (x) < FLT_MIN ? 0.0f : (x)) + bqL->x1 = F(x1[0]); + bqL->x2 = F(x2[0]); + bqR->x1 = F(x1[1]); + bqR->x2 = F(x2[1]); +#undef F +} + + +static void dsp_biquad2_run2_sse(struct dsp_ops *ops, struct biquad *bqL0, struct biquad *bqL1, + struct biquad *bqR0, struct biquad *bqR1, + float *outL, float *outR, const float *inL, const float *inR, uint32_t n_samples) +{ + __m128 x, y, z; + __m128 b00, b01, b02, b10, b11, b12; + __m128 a01, a02, a11, a12; + __m128 x01, x02, x11, x12; + uint32_t i; + + b00 = _mm_setr_ps(bqL0->b0, bqR0->b0, 0.0f, 0.0f); /* b00 b10 0 0 */ + b01 = _mm_setr_ps(bqL0->b1, bqR0->b1, 0.0f, 0.0f); /* b01 b11 0 0 */ + b02 = _mm_setr_ps(bqL0->b2, bqR0->b2, 0.0f, 0.0f); /* b02 b12 0 0 */ + a01 = _mm_setr_ps(bqL0->a1, bqR0->a1, 0.0f, 0.0f); /* b00 b10 0 0 */ + a02 = _mm_setr_ps(bqL0->a2, bqR0->a2, 0.0f, 0.0f); /* b01 b11 0 0 */ + x01 = _mm_setr_ps(bqL0->x1, bqR0->x1, 0.0f, 0.0f); /* b00 b10 0 0 */ + x02 = _mm_setr_ps(bqL0->x2, bqR0->x2, 0.0f, 0.0f); /* b01 b11 0 0 */ + + b10 = _mm_setr_ps(bqL1->b0, bqR1->b0, 0.0f, 0.0f); /* b00 b10 0 0 */ + b11 = _mm_setr_ps(bqL1->b1, bqR1->b1, 0.0f, 0.0f); /* b01 b11 0 0 */ + b12 = _mm_setr_ps(bqL1->b2, bqR1->b2, 0.0f, 0.0f); /* b02 b12 0 0 */ + a11 = _mm_setr_ps(bqL1->a1, bqR1->a1, 0.0f, 0.0f); /* b00 b10 0 0 */ + a12 = _mm_setr_ps(bqL1->a2, bqR1->a2, 0.0f, 0.0f); /* b01 b11 0 0 */ + x11 = _mm_setr_ps(bqL1->x1, bqR1->x1, 0.0f, 0.0f); /* b00 b10 0 0 */ + x12 = _mm_setr_ps(bqL1->x2, bqR1->x2, 0.0f, 0.0f); /* b01 b11 0 0 */ + + for (i = 0; i < n_samples; i++) { + x = _mm_setr_ps(inL[i], inR[i], 0.0f, 0.0f); + + y = _mm_mul_ps(x, b00); /* y = x * b0 */ + y = _mm_add_ps(y, x01); /* y = x * b0 + x1*/ + z = _mm_mul_ps(y, a01); /* z = a1 * y */ + x01 = _mm_mul_ps(x, b01); /* x1 = x * b1 */ + x01 = _mm_add_ps(x01, x02); /* x1 = x * b1 + x2*/ + x01 = _mm_sub_ps(x01, z); /* x1 = x * b1 + x2 - a1 * y*/ + z = _mm_mul_ps(y, a02); /* z = a2 * y */ + x02 = _mm_mul_ps(x, b02); /* x2 = x * b2 */ + x02 = _mm_sub_ps(x02, z); /* x2 = x * b2 - a2 * y*/ + + x = y; + + y = _mm_mul_ps(x, b10); /* y = x * b0 */ + y = _mm_add_ps(y, x11); /* y = x * b0 + x1*/ + z = _mm_mul_ps(y, a11); /* z = a1 * y */ + x11 = _mm_mul_ps(x, b11); /* x1 = x * b1 */ + x11 = _mm_add_ps(x11, x12); /* x1 = x * b1 + x2*/ + x11 = _mm_sub_ps(x11, z); /* x1 = x * b1 + x2 - a1 * y*/ + z = _mm_mul_ps(y, a12); /* z = a2 * y*/ + x12 = _mm_mul_ps(x, b12); /* x2 = x * b2 */ + x12 = _mm_sub_ps(x12, z); /* x2 = x * b2 - a2 * y*/ + + outL[i] = y[0]; + outR[i] = y[1]; + } +#define F(x) (-FLT_MIN < (x) && (x) < FLT_MIN ? 0.0f : (x)) + bqL0->x1 = F(x01[0]); + bqL0->x2 = F(x02[0]); + bqR0->x1 = F(x01[1]); + bqR0->x2 = F(x02[1]); + bqL1->x1 = F(x11[0]); + bqL1->x2 = F(x12[0]); + bqR1->x1 = F(x11[1]); + bqR1->x2 = F(x12[1]); +#undef F +} + +void dsp_biquadn_run_sse(struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, uint32_t bq_stride, float * SPA_RESTRICT out[], const float * SPA_RESTRICT in[], uint32_t n_src, uint32_t n_samples) { uint32_t i, j; - const float *s; - float *d; - uint32_t unrolled = n_bq & ~1; - struct biquad *b = bq; + uint32_t iunrolled = n_src & ~1; + uint32_t junrolled = n_bq & ~1; - for (i = 0; i < n_src; i++, b+=n_src) { - s = in[i]; - d = out[i]; - for (j = 0; j < unrolled; j+=2) { - dsp_biquad_run2_sse(ops, &b[j], &b[j+1], d, s, n_samples); + for (i = 0; i < iunrolled; i+=2, bq+=bq_stride) { + const float *s0 = in[i], *s1 = in[i+1]; + float *d0 = out[i], *d1 = out[i+1]; + if (s0 == NULL || s1 == NULL || d0 == NULL || d1 == NULL) + break; + + for (j = 0; j < junrolled; j+=2) { + dsp_biquad2_run2_sse(ops, &bq[j], &bq[j+1], &bq[j+bq_stride], &bq[j+bq_stride+1], + d0, d1, s0, s1, n_samples); + s0 = d0; + s1 = d1; + } + for (; j < n_bq; j++) { + dsp_biquad_run2_sse(ops, &bq[j], &bq[j+bq_stride], d0, d1, s0, s1, n_samples); + s0 = d0; + s1 = d1; + } + } + for (; i < n_src; i++, bq+=bq_stride) { + const float *s = in[i]; + float *d = out[i]; + if (s == NULL || d == NULL) + continue; + + for (j = 0; j < junrolled; j+=2) { + dsp_biquad2_run_sse(ops, &bq[j], &bq[j+1], d, s, n_samples); s = d; } for (; j < n_bq; j++) { - dsp_biquad_run_sse(ops, &b[j], d, s, n_samples); + dsp_biquad_run_sse(ops, &bq[j], d, s, n_samples); s = d; } } diff --git a/src/modules/module-filter-chain/dsp-ops.h b/src/modules/module-filter-chain/dsp-ops.h index f09869ee4..796455014 100644 --- a/src/modules/module-filter-chain/dsp-ops.h +++ b/src/modules/module-filter-chain/dsp-ops.h @@ -43,7 +43,7 @@ struct dsp_ops_funcs { void (*mult) (struct dsp_ops *ops, void * SPA_RESTRICT dst, const void * SPA_RESTRICT src[], uint32_t n_src, uint32_t n_samples); - void (*biquadn_run) (struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, + void (*biquadn_run) (struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, uint32_t bq_stride, float * SPA_RESTRICT out[], const float * SPA_RESTRICT in[], uint32_t n_src, uint32_t n_samples); }; @@ -99,7 +99,7 @@ void dsp_linear_##arch (struct dsp_ops *ops, float * SPA_RESTRICT dst, \ void dsp_mult_##arch(struct dsp_ops *ops, void * SPA_RESTRICT dst, \ const void * SPA_RESTRICT src[], uint32_t n_src, uint32_t n_samples) #define MAKE_BIQUADN_RUN_FUNC(arch) \ -void dsp_biquadn_run_##arch (struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, \ +void dsp_biquadn_run_##arch (struct dsp_ops *ops, struct biquad *bq, uint32_t n_bq, uint32_t bq_stride, \ float * SPA_RESTRICT out[], const float * SPA_RESTRICT in[], uint32_t n_src, uint32_t n_samples) #define MAKE_FFT_NEW_FUNC(arch) \