Skip to content

Commit 0c1756c

Browse files
singalsusingalsu
committed
EQ FIR: Add support for 16 and 24 bit data
This patch adds to equalizer capability to process 16 and 24 bit pipelines in addition to 32 bit similarly as IIR. The generic C, HiFiEP, and HiFi3 versions are updated with the capability. Signed-off-by: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
1 parent 1c19a79 commit 0c1756c

7 files changed

Lines changed: 591 additions & 65 deletions

File tree

‎src/audio/eq_fir.c‎

Lines changed: 149 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -63,20 +63,124 @@ struct comp_data {
6363
uint32_t period_bytes;
6464
int32_t *fir_delay;
6565
size_t fir_delay_size;
66+
void (*eq_fir_func_even)(struct fir_state_32x16 fir[],
67+
struct comp_buffer *source,
68+
struct comp_buffer *sink,
69+
int frames, int nch);
6670
void (*eq_fir_func)(struct fir_state_32x16 fir[],
6771
struct comp_buffer *source,
6872
struct comp_buffer *sink,
6973
int frames, int nch);
70-
void (*eq_fir_func_odd)(struct fir_state_32x16 fir[],
71-
struct comp_buffer *source,
72-
struct comp_buffer *sink,
73-
int frames, int nch);
7474
};
7575

76-
static void eq_fir_passthrough(struct fir_state_32x16 fir[],
77-
struct comp_buffer *source,
78-
struct comp_buffer *sink,
79-
int frames, int nch)
76+
/* The optimized FIR functions variants need to be updated into function
77+
* set_fir_func. The cd->eq_fir_func is a function that can process any
78+
* number of samples. The cd->eq_fir_func_even is for optimized version
79+
* that is guaranteed to be called with even samples number.
80+
*/
81+
82+
#if FIR_HIFI3
83+
static inline void set_s16_fir(struct comp_data *cd)
84+
{
85+
cd->eq_fir_func_even = eq_fir_2x_s16_hifi3;
86+
cd->eq_fir_func = eq_fir_s16_hifi3;
87+
}
88+
89+
static inline void set_s24_fir(struct comp_data *cd)
90+
{
91+
cd->eq_fir_func_even = eq_fir_2x_s24_hifi3;
92+
cd->eq_fir_func = eq_fir_s24_hifi3;
93+
}
94+
95+
static inline void set_s32_fir(struct comp_data *cd)
96+
{
97+
cd->eq_fir_func_even = eq_fir_2x_s32_hifi3;
98+
cd->eq_fir_func = eq_fir_s32_hifi3;
99+
}
100+
#elif FIR_HIFIEP
101+
static inline void set_s16_fir(struct comp_data *cd)
102+
{
103+
cd->eq_fir_func_even = eq_fir_2x_s16_hifiep;
104+
cd->eq_fir_func = eq_fir_s16_hifiep;
105+
}
106+
107+
static inline void set_s24_fir(struct comp_data *cd)
108+
{
109+
cd->eq_fir_func_even = eq_fir_2x_s24_hifiep;
110+
cd->eq_fir_func = eq_fir_s24_hifiep;
111+
}
112+
113+
static inline void set_s32_fir(struct comp_data *cd)
114+
{
115+
cd->eq_fir_func_even = eq_fir_2x_s32_hifiep;
116+
cd->eq_fir_func = eq_fir_s32_hifiep;
117+
}
118+
#else
119+
/* FIR_GENERIC */
120+
static inline void set_s16_fir(struct comp_data *cd)
121+
{
122+
cd->eq_fir_func_even = eq_fir_s16;
123+
cd->eq_fir_func = eq_fir_s16;
124+
}
125+
126+
static inline void set_s24_fir(struct comp_data *cd)
127+
{
128+
cd->eq_fir_func_even = eq_fir_s24;
129+
cd->eq_fir_func = eq_fir_s24;
130+
}
131+
132+
static inline void set_s32_fir(struct comp_data *cd)
133+
{
134+
cd->eq_fir_func_even = eq_fir_s32;
135+
cd->eq_fir_func = eq_fir_s32;
136+
}
137+
#endif
138+
139+
static inline int set_fir_func(struct comp_dev *dev)
140+
{
141+
struct comp_data *cd = comp_get_drvdata(dev);
142+
struct sof_ipc_comp_config *config = COMP_GET_CONFIG(dev);
143+
144+
switch (config->frame_fmt) {
145+
case SOF_IPC_FRAME_S16_LE:
146+
trace_eq("f16");
147+
set_s16_fir(cd);
148+
break;
149+
case SOF_IPC_FRAME_S24_4LE:
150+
trace_eq("f24");
151+
set_s24_fir(cd);
152+
break;
153+
case SOF_IPC_FRAME_S32_LE:
154+
trace_eq("f32");
155+
set_s32_fir(cd);
156+
break;
157+
default:
158+
trace_eq_error("eef");
159+
return -EINVAL;
160+
}
161+
return 0;
162+
}
163+
164+
/* Pass-trough functions to replace FIR core while not configured for
165+
* response.
166+
*/
167+
168+
static void eq_fir_s16_passthrough(struct fir_state_32x16 fir[],
169+
struct comp_buffer *source,
170+
struct comp_buffer *sink,
171+
int frames, int nch)
172+
{
173+
int16_t *src = (int16_t *)source->r_ptr;
174+
int16_t *dest = (int16_t *)sink->w_ptr;
175+
int n = frames * nch;
176+
177+
memcpy(dest, src, n * sizeof(int16_t));
178+
}
179+
180+
static void eq_fir_s32_passthrough(struct fir_state_32x16 fir[],
181+
struct comp_buffer *source,
182+
struct comp_buffer *sink,
183+
int frames, int nch)
80184
{
81185
int32_t *src = (int32_t *)source->r_ptr;
82186
int32_t *dest = (int32_t *)sink->w_ptr;
@@ -85,6 +189,32 @@ static void eq_fir_passthrough(struct fir_state_32x16 fir[],
85189
memcpy(dest, src, n * sizeof(int32_t));
86190
}
87191

192+
/* Function to select pass-trough depending on PCM format */
193+
194+
static inline int set_pass_func(struct comp_dev *dev)
195+
{
196+
struct comp_data *cd = comp_get_drvdata(dev);
197+
struct sof_ipc_comp_config *config = COMP_GET_CONFIG(dev);
198+
199+
switch (config->frame_fmt) {
200+
case SOF_IPC_FRAME_S16_LE:
201+
trace_eq("p16");
202+
cd->eq_fir_func_even = eq_fir_s16_passthrough;
203+
cd->eq_fir_func = eq_fir_s16_passthrough;
204+
break;
205+
case SOF_IPC_FRAME_S24_4LE:
206+
case SOF_IPC_FRAME_S32_LE:
207+
trace_eq("p32");
208+
cd->eq_fir_func_even = eq_fir_s32_passthrough;
209+
cd->eq_fir_func = eq_fir_s32_passthrough;
210+
break;
211+
default:
212+
trace_eq_error("epf");
213+
return -EINVAL;
214+
}
215+
return 0;
216+
}
217+
88218
/*
89219
* EQ control code is next. The processing is in fir_ C modules.
90220
*/
@@ -276,8 +406,8 @@ static struct comp_dev *eq_fir_new(struct sof_ipc_comp *comp)
276406

277407
comp_set_drvdata(dev, cd);
278408

279-
cd->eq_fir_func = eq_fir_passthrough;
280-
cd->eq_fir_func_odd = eq_fir_passthrough;
409+
cd->eq_fir_func_even = eq_fir_s32_passthrough;
410+
cd->eq_fir_func = eq_fir_s32_passthrough;
281411
cd->config = NULL;
282412

283413
/* Allocate and make a copy of the coefficients blob and reset FIR. If
@@ -340,10 +470,6 @@ static int eq_fir_params(struct comp_dev *dev)
340470
return err;
341471
}
342472

343-
/* EQ supports only S32_LE PCM format */
344-
if (config->frame_fmt != SOF_IPC_FRAME_S32_LE)
345-
return -EINVAL;
346-
347473
return 0;
348474
}
349475

@@ -531,9 +657,9 @@ static int eq_fir_copy(struct comp_dev *dev)
531657
}
532658

533659
if (dev->frames & 1)
534-
sd->eq_fir_func_odd(fir, source, sink, dev->frames, nch);
535-
else
536660
sd->eq_fir_func(fir, source, sink, dev->frames, nch);
661+
else
662+
sd->eq_fir_func_even(fir, source, sink, dev->frames, nch);
537663

538664
/* calc new free and available */
539665
comp_update_buffer_consume(source, sd->period_bytes);
@@ -554,31 +680,19 @@ static int eq_fir_prepare(struct comp_dev *dev)
554680
return ret;
555681

556682
/* Initialize EQ */
557-
cd->eq_fir_func = eq_fir_passthrough;
558683
if (cd->config) {
559684
ret = eq_fir_setup(cd, dev->params.channels);
560685
if (ret < 0) {
561686
comp_set_state(dev, COMP_TRIGGER_RESET);
562687
return ret;
563688
}
564-
#if FIR_GENERIC
565-
cd->eq_fir_func = eq_fir_s32;
566-
cd->eq_fir_func_odd = eq_fir_s32;
567-
#endif
568-
#if FIR_HIFIEP
569-
cd->eq_fir_func = eq_fir_2x_s32_hifiep;
570-
cd->eq_fir_func_odd = eq_fir_s32_hifiep;
571-
#endif
572-
#if FIR_HIFI3
573-
cd->eq_fir_func = eq_fir_2x_s32_hifi3;
574-
cd->eq_fir_func_odd = eq_fir_s32_hifi3;
575-
#endif
689+
690+
ret = set_fir_func(dev);
691+
return ret;
576692
}
577-
trace_eq("len");
578-
trace_value(cd->fir[0].length);
579-
trace_value(cd->fir[1].length);
580693

581-
return 0;
694+
ret = set_pass_func(dev);
695+
return ret;
582696
}
583697

584698
static int eq_fir_reset(struct comp_dev *dev)
@@ -590,8 +704,8 @@ static int eq_fir_reset(struct comp_dev *dev)
590704

591705
eq_fir_free_delaylines(cd);
592706

593-
cd->eq_fir_func = eq_fir_passthrough;
594-
cd->eq_fir_func_odd = eq_fir_passthrough;
707+
cd->eq_fir_func_even = eq_fir_s32_passthrough;
708+
cd->eq_fir_func = eq_fir_s32_passthrough;
595709
for (i = 0; i < PLATFORM_MAX_CHANNELS; i++)
596710
fir_reset(&cd->fir[i]);
597711

‎src/audio/fir.c‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,56 @@ void fir_init_delay(struct fir_state_32x16 *fir, int32_t **data)
8181
*data += fir->length; /* Point to next delay line start */
8282
}
8383

84+
void eq_fir_s16(struct fir_state_32x16 fir[], struct comp_buffer *source,
85+
struct comp_buffer *sink, int frames, int nch)
86+
{
87+
struct fir_state_32x16 *filter;
88+
int16_t *src = (int16_t *)source->r_ptr;
89+
int16_t *snk = (int16_t *)sink->w_ptr;
90+
int16_t *x;
91+
int16_t *y;
92+
int32_t z;
93+
int ch;
94+
int i;
95+
96+
for (ch = 0; ch < nch; ch++) {
97+
filter = &fir[ch];
98+
x = src++;
99+
y = snk++;
100+
for (i = 0; i < frames; i++) {
101+
z = fir_32x16(filter, *x << 16);
102+
*y = sat_int16(Q_SHIFT_RND(z, 31, 15));
103+
x += nch;
104+
y += nch;
105+
}
106+
}
107+
}
108+
109+
void eq_fir_s24(struct fir_state_32x16 fir[], struct comp_buffer *source,
110+
struct comp_buffer *sink, int frames, int nch)
111+
{
112+
struct fir_state_32x16 *filter;
113+
int32_t *src = (int32_t *)source->r_ptr;
114+
int32_t *snk = (int32_t *)sink->w_ptr;
115+
int32_t *x;
116+
int32_t *y;
117+
int32_t z;
118+
int ch;
119+
int i;
120+
121+
for (ch = 0; ch < nch; ch++) {
122+
filter = &fir[ch];
123+
x = src++;
124+
y = snk++;
125+
for (i = 0; i < frames; i++) {
126+
z = fir_32x16(filter, *x << 8);
127+
*y = sat_int24(Q_SHIFT_RND(z, 31, 23));
128+
x += nch;
129+
y += nch;
130+
}
131+
}
132+
}
133+
84134
void eq_fir_s32(struct fir_state_32x16 fir[], struct comp_buffer *source,
85135
struct comp_buffer *sink, int frames, int nch)
86136
{

‎src/audio/fir.h‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,13 @@ size_t fir_init_coef(struct fir_state_32x16 *fir,
5454

5555
void fir_init_delay(struct fir_state_32x16 *fir, int32_t **data);
5656

57-
void eq_fir_s32(struct fir_state_32x16 fir[], struct comp_buffer *source,
57+
void eq_fir_s16(struct fir_state_32x16 *fir, struct comp_buffer *source,
58+
struct comp_buffer *sink, int frames, int nch);
59+
60+
void eq_fir_s24(struct fir_state_32x16 *fir, struct comp_buffer *source,
61+
struct comp_buffer *sink, int frames, int nch);
62+
63+
void eq_fir_s32(struct fir_state_32x16 *fir, struct comp_buffer *source,
5864
struct comp_buffer *sink, int frames, int nch);
5965

6066
/* The next functions are inlined to optmize execution speed */

0 commit comments

Comments
 (0)