Skip to content

Commit bad7b99

Browse files
lgirdwoodlgirdwood
authored
Merge pull request #454 from singalsu/eq_fir_add_16_24_data_proposal
EQ FIR: Add support for 16 and 24 bit data
2 parents 8aff3de + 0c1756c commit bad7b99

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)