1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2017 Intel Corporation. All rights reserved.
4 //
5 // Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
6
7 #include <sof/math/fir_config.h>
8
9 #if FIR_HIFIEP
10
11 #include <sof/audio/eq_fir/eq_fir.h>
12 #include <sof/audio/buffer.h>
13 #include <sof/audio/format.h>
14 #include <sof/math/fir_hifi2ep.h>
15 #include <xtensa/config/defs.h>
16 #include <xtensa/tie/xt_hifi2.h>
17 #include <errno.h>
18 #include <stddef.h>
19 #include <stdint.h>
20
21 #if CONFIG_FORMAT_S32LE
22 /* For even frame lengths use FIR filter that processes two sequential
23 * sample per call.
24 */
eq_fir_2x_s32(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)25 void eq_fir_2x_s32(struct fir_state_32x16 fir[], const struct audio_stream *source,
26 struct audio_stream *sink, int frames, int nch)
27 {
28 struct fir_state_32x16 *f;
29 int32_t *src = (int32_t *)source->r_ptr;
30 int32_t *snk = (int32_t *)sink->w_ptr;
31 int32_t *x0;
32 int32_t *y0;
33 int32_t *x1;
34 int32_t *y1;
35 int ch;
36 int i;
37 int rshift;
38 int lshift;
39 int inc = nch << 1;
40
41 for (ch = 0; ch < nch; ch++) {
42 /* Get FIR instance and get shifts to e.g. apply mute
43 * without overhead.
44 */
45 f = &fir[ch];
46 fir_get_lrshifts(f, &lshift, &rshift);
47
48 /* Setup circular buffer for FIR input data delay */
49 fir_hifiep_setup_circular(f);
50
51 x0 = src++;
52 y0 = snk++;
53 for (i = 0; i < (frames >> 1); i++) {
54 x1 = x0 + nch;
55 y1 = y0 + nch;
56 fir_32x16_2x_hifiep(f, *x0, *x1, y0, y1,
57 lshift, rshift);
58 x0 += inc;
59 y0 += inc;
60 }
61 }
62 }
63
64 /* FIR for any number of frames */
eq_fir_s32(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)65 void eq_fir_s32(struct fir_state_32x16 fir[], const struct audio_stream *source,
66 struct audio_stream *sink, int frames, int nch)
67 {
68 struct fir_state_32x16 *f;
69 int32_t *src = (int32_t *)source->r_ptr;
70 int32_t *snk = (int32_t *)sink->w_ptr;
71 int32_t *x;
72 int32_t *y;
73 int ch;
74 int i;
75 int rshift;
76 int lshift;
77
78 for (ch = 0; ch < nch; ch++) {
79 /* Get FIR instance and get shifts to e.g. apply mute
80 * without overhead.
81 */
82 f = &fir[ch];
83 fir_get_lrshifts(f, &lshift, &rshift);
84
85 /* Setup circular buffer for FIR input data delay */
86 fir_hifiep_setup_circular(f);
87
88 x = src++;
89 y = snk++;
90 for (i = 0; i < frames; i++) {
91 fir_32x16_hifiep(f, *x, y, lshift, rshift);
92 x += nch;
93 y += nch;
94 }
95 }
96 }
97 #endif /* CONFIG_FORMAT_S32LE */
98
99 #if CONFIG_FORMAT_S24LE
eq_fir_2x_s24(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)100 void eq_fir_2x_s24(struct fir_state_32x16 fir[], const struct audio_stream *source,
101 struct audio_stream *sink, int frames, int nch)
102 {
103 struct fir_state_32x16 *f;
104 int32_t *src = (int32_t *)source->r_ptr;
105 int32_t *snk = (int32_t *)sink->w_ptr;
106 int32_t *x0;
107 int32_t *y0;
108 int32_t *x1;
109 int32_t *y1;
110 int32_t z0;
111 int32_t z1;
112 int ch;
113 int i;
114 int rshift;
115 int lshift;
116 int inc = nch << 1;
117
118 for (ch = 0; ch < nch; ch++) {
119 /* Get FIR instance and get shifts to e.g. apply mute
120 * without overhead.
121 */
122 f = &fir[ch];
123 fir_get_lrshifts(f, &lshift, &rshift);
124
125 /* Setup circular buffer for FIR input data delay */
126 fir_hifiep_setup_circular(f);
127
128 x0 = src++;
129 y0 = snk++;
130 for (i = 0; i < (frames >> 1); i++) {
131 x1 = x0 + nch;
132 y1 = y0 + nch;
133 fir_32x16_2x_hifiep(f, *x0 << 8, *x1 << 8, &z0, &z1,
134 lshift, rshift);
135 *y0 = sat_int24(Q_SHIFT_RND(z0, 31, 23));
136 *y1 = sat_int24(Q_SHIFT_RND(z1, 31, 23));
137 x0 += inc;
138 y0 += inc;
139 }
140 }
141 }
142
143 /* FIR for any number of frames */
eq_fir_s24(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)144 void eq_fir_s24(struct fir_state_32x16 fir[], const struct audio_stream *source,
145 struct audio_stream *sink, int frames, int nch)
146 {
147 struct fir_state_32x16 *f;
148 int32_t *src = (int32_t *)source->r_ptr;
149 int32_t *snk = (int32_t *)sink->w_ptr;
150 int32_t *x;
151 int32_t *y;
152 int32_t z;
153 int ch;
154 int i;
155 int rshift;
156 int lshift;
157
158 for (ch = 0; ch < nch; ch++) {
159 /* Get FIR instance and get shifts to e.g. apply mute
160 * without overhead.
161 */
162 f = &fir[ch];
163 fir_get_lrshifts(f, &lshift, &rshift);
164
165 /* Setup circular buffer for FIR input data delay */
166 fir_hifiep_setup_circular(f);
167
168 x = src++;
169 y = snk++;
170 for (i = 0; i < frames; i++) {
171 fir_32x16_hifiep(f, *x << 8, &z, lshift, rshift);
172 *y = sat_int24(Q_SHIFT_RND(z, 31, 23));
173 x += nch;
174 y += nch;
175 }
176 }
177 }
178 #endif /* CONFIG_FORMAT_S24LE */
179
180 #if CONFIG_FORMAT_S16LE
eq_fir_2x_s16(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)181 void eq_fir_2x_s16(struct fir_state_32x16 fir[], const struct audio_stream *source,
182 struct audio_stream *sink, int frames, int nch)
183 {
184 struct fir_state_32x16 *f;
185 int16_t *src = (int16_t *)source->r_ptr;
186 int16_t *snk = (int16_t *)sink->w_ptr;
187 int16_t *x0;
188 int16_t *y0;
189 int16_t *x1;
190 int16_t *y1;
191 int32_t z0;
192 int32_t z1;
193 int ch;
194 int i;
195 int rshift;
196 int lshift;
197 int inc = nch << 1;
198
199 for (ch = 0; ch < nch; ch++) {
200 /* Get FIR instance and get shifts to e.g. apply mute
201 * without overhead.
202 */
203 f = &fir[ch];
204 fir_get_lrshifts(f, &lshift, &rshift);
205
206 /* Setup circular buffer for FIR input data delay */
207 fir_hifiep_setup_circular(f);
208
209 x0 = src++;
210 y0 = snk++;
211 for (i = 0; i < (frames >> 1); i++) {
212 x1 = x0 + nch;
213 y1 = y0 + nch;
214 fir_32x16_2x_hifiep(f, *x0 << 16, *x1 << 16, &z0, &z1,
215 lshift, rshift);
216 *y0 = sat_int16(Q_SHIFT_RND(z0, 31, 15));
217 *y1 = sat_int16(Q_SHIFT_RND(z1, 31, 15));
218 x0 += inc;
219 y0 += inc;
220 }
221 }
222 }
223
224 /* FIR for any number of frames */
eq_fir_s16(struct fir_state_32x16 fir[],const struct audio_stream * source,struct audio_stream * sink,int frames,int nch)225 void eq_fir_s16(struct fir_state_32x16 fir[], const struct audio_stream *source,
226 struct audio_stream *sink, int frames, int nch)
227 {
228 struct fir_state_32x16 *f;
229 int16_t *src = (int16_t *)source->r_ptr;
230 int16_t *snk = (int16_t *)sink->w_ptr;
231 int16_t *x;
232 int16_t *y;
233 int32_t z;
234 int ch;
235 int i;
236 int rshift;
237 int lshift;
238
239 for (ch = 0; ch < nch; ch++) {
240 /* Get FIR instance and get shifts to e.g. apply mute
241 * without overhead.
242 */
243 f = &fir[ch];
244 fir_get_lrshifts(f, &lshift, &rshift);
245
246 /* Setup circular buffer for FIR input data delay */
247 fir_hifiep_setup_circular(f);
248
249 x = src++;
250 y = snk++;
251 for (i = 0; i < frames; i++) {
252 fir_32x16_hifiep(f, *x << 16, &z, lshift, rshift);
253 *y = sat_int16(Q_SHIFT_RND(z, 31, 15));
254 x += nch;
255 y += nch;
256 }
257 }
258 }
259 #endif /* CONFIG_FORMAT_S16LE */
260
261 #endif /* FIR_HIFIEP */
262
