1 /******************************************************************************
2 *
3 * Copyright (C) 2018 The Android Open Source Project
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 *****************************************************************************
18 * Originally developed and contributed by Ittiam Systems Pvt. Ltd, Bangalore
19 */
20 #include <stdio.h>
21 #include <stdlib.h>
22 #include <math.h>
23 #include <string.h>
24 #include "impd_type_def.h"
25 #include "impd_drc_extr_delta_coded_info.h"
26 #include "impd_drc_common.h"
27 #include "impd_drc_struct.h"
28 #include "impd_drc_interface.h"
29 #include "impd_drc_gain_dec.h"
30 #include "impd_drc_eq.h"
31
32 #define CONFIG_REAL_POLE 0
33 #define CONFIG_COMPLEX_POLE 1
34 #define CONFIG_REAL_ZERO_RADIUS_ONE 2
35 #define CONFIG_REAL_ZERO 3
36 #define CONFIG_GENERIC_ZERO 4
37
38 #define STEP_RATIO_F_LO 20.0f
39 #define STEP_RATIO_F_HI 24000.0f
40 #define STEP_RATIO_EQ_NODE_COUNT_MAX 33
41
42 #define FILTER_ELEMENT_FORMAT_POLE_ZERO 0
43 #define FILTER_ELEMENT_FORMAT_FIR 1
44
45 #ifndef M_PI
46 #define M_PI (3.14159265358979323846)
47 #endif
48
impd_derive_subband_center_freq(WORD32 eq_subband_gain_count,WORD32 eq_subband_gain_format,FLOAT32 sample_rate,FLOAT32 * subband_center_freq)49 WORD32 impd_derive_subband_center_freq(WORD32 eq_subband_gain_count,
50 WORD32 eq_subband_gain_format,
51 FLOAT32 sample_rate,
52 FLOAT32* subband_center_freq) {
53 WORD32 i;
54 FLOAT32 width, offset;
55 switch (eq_subband_gain_format) {
56 case GAINFORMAT_QMF32:
57 case GAINFORMAT_QMF64:
58 case GAINFORMAT_QMF128:
59 case GAINFORMAT_UNIFORM:
60 width = 0.5f * sample_rate / (FLOAT32)eq_subband_gain_count;
61 offset = 0.5f * width;
62 for (i = 0; i < eq_subband_gain_count; i++) {
63 subband_center_freq[i] = offset;
64 offset = offset + width;
65 }
66 break;
67 case GAINFORMAT_QMFHYBRID39:
68 case GAINFORMAT_QMFHYBRID71:
69 case GAINFORMAT_QMFHYBRID135:
70 return (UNEXPECTED_ERROR);
71 break;
72 default:
73 break;
74 }
75 return (0);
76 }
77
impd_calc_fir_filt_response(WORD32 fir_order,WORD32 fir_symmetry,FLOAT32 * coeff,FLOAT32 frequency_radian,FLOAT32 * response)78 VOID impd_calc_fir_filt_response(WORD32 fir_order, WORD32 fir_symmetry,
79 FLOAT32* coeff, FLOAT32 frequency_radian,
80 FLOAT32* response) {
81 WORD32 m;
82 FLOAT32 sum = 0.0f;
83 WORD32 o2;
84
85 if ((fir_order & 0x1) == 0) {
86 o2 = fir_order / 2;
87 if (fir_symmetry == 0) {
88 /*ITTIAM: sum is been over written after the loop
89 None of the conformance streams with us entering this function*/
90 for (m = 1; m <= o2; m++) {
91 sum += coeff[o2 - m] * (FLOAT32)cos(m * frequency_radian);
92 }
93 sum += sum;
94 sum = coeff[o2];
95 } else {
96 for (m = 1; m <= o2; m++) {
97 sum += coeff[o2 - m] * (FLOAT32)sin(m * frequency_radian);
98 }
99 sum += sum;
100 }
101 } else {
102 o2 = (fir_order + 1) / 2;
103 if (fir_symmetry == 0) {
104 for (m = 1; m <= o2; m++) {
105 sum += coeff[o2 - m] * (FLOAT32)cos((m - 0.5f) * frequency_radian);
106 }
107 } else {
108 for (m = 1; m <= o2; m++) {
109 sum += coeff[o2 - m] * (FLOAT32)sin((m - 0.5f) * frequency_radian);
110 }
111 }
112 sum += sum;
113 }
114 *response = sum;
115 return;
116 }
117
impd_calc_filt_ele_response(ia_unique_td_filt_element * element,FLOAT32 frequency_radian,FLOAT32 * response)118 VOID impd_calc_filt_ele_response(ia_unique_td_filt_element* element,
119 FLOAT32 frequency_radian, FLOAT32* response) {
120 WORD32 i;
121 FLOAT32 part_response, radius, angle_radian;
122 FLOAT64 total_response = 1.0;
123
124 if (element->eq_filter_format == FILTER_ELEMENT_FORMAT_POLE_ZERO) {
125 for (i = 0; i < element->bs_real_zero_radius_one_count; i++) {
126 part_response =
127 1.0f + 1.0f -
128 2.0f * 1.0f *
129 (FLOAT32)cos(frequency_radian - (FLOAT32)element->zero_sign[i]);
130 total_response *= part_response;
131 }
132 for (i = 0; i < element->real_zero_count; i++) {
133 if (element->real_zero_radius[i] < 0.0f) {
134 radius = -element->real_zero_radius[i];
135 angle_radian = (FLOAT32)M_PI;
136 } else {
137 radius = element->real_zero_radius[i];
138 angle_radian = 0.0f;
139 }
140 part_response =
141 1.0f + radius * radius -
142 2.0f * radius * (FLOAT32)cos(frequency_radian - angle_radian);
143 total_response *= part_response;
144 part_response =
145 1.0f + radius * radius -
146 2.0f * radius * (FLOAT32)cos(frequency_radian - angle_radian);
147 total_response *= part_response;
148 }
149
150 total_response = sqrt(total_response);
151
152 for (i = 0; i < element->generic_zero_count; i++) {
153 radius = element->generic_zero_radius[i];
154 part_response =
155 1.0f + radius * radius -
156 2.0f * radius *
157 (FLOAT32)cos(frequency_radian - element->generic_zero_angle[i]);
158 total_response *= part_response;
159 part_response =
160 1.0f + radius * radius -
161 2.0f * radius *
162 (FLOAT32)cos(frequency_radian - element->generic_zero_angle[i]);
163 total_response *= part_response;
164 }
165 for (i = 0; i < element->real_pole_count; i++) {
166 if (element->real_pole_radius[i] < 0.0f) {
167 radius = -element->real_pole_radius[i];
168 angle_radian = (FLOAT32)(-M_PI);
169 } else {
170 radius = element->real_pole_radius[i];
171 angle_radian = 0.0f;
172 }
173 part_response =
174 1 / (1.0f + radius * radius -
175 2.0f * radius * (FLOAT32)cos(frequency_radian - angle_radian));
176 total_response *= part_response;
177 }
178 for (i = 0; i < element->cmplx_pole_count; i++) {
179 part_response =
180 1 /
181 (1.0f + element->real_pole_radius[i] * element->real_pole_radius[i] -
182 2.0f * element->real_pole_radius[i] *
183 (FLOAT32)cos(frequency_radian - element->complex_pole_angle[i]));
184 total_response *= part_response * part_response;
185 }
186 } else {
187 impd_calc_fir_filt_response(element->fir_filt_order, element->fir_symmetry,
188 element->fir_coeff, frequency_radian,
189 &part_response);
190
191 total_response *= part_response;
192 }
193 *response = (FLOAT32)total_response;
194 return;
195 }
196
impd_calc_filt_block_response(ia_unique_td_filt_element * unique_td_filt_ele,ia_filt_block_struct * str_filter_block,FLOAT32 frequency_radian,FLOAT32 * response)197 VOID impd_calc_filt_block_response(
198 ia_unique_td_filt_element* unique_td_filt_ele,
199 ia_filt_block_struct* str_filter_block, FLOAT32 frequency_radian,
200 FLOAT32* response) {
201 WORD32 i;
202 FLOAT32 part_response;
203 FLOAT64 total_response = 1.0;
204 for (i = 0; i < str_filter_block->filter_element_count; i++) {
205 ia_filt_ele_struct* str_filter_element =
206 &str_filter_block->str_filter_element[i];
207
208 impd_calc_filt_ele_response(
209 &(unique_td_filt_ele[str_filter_element->filt_ele_idx]),
210 frequency_radian, &part_response);
211 total_response *= part_response;
212
213 if (str_filter_element->filt_ele_gain_flag == 1) {
214 total_response *= pow(10.0f, 0.05f * str_filter_element->filt_ele_gain);
215 }
216 }
217 *response = (FLOAT32)total_response;
218 return;
219 }
220
impd_calc_subband_gains_td_cascade(ia_unique_td_filt_element * unique_td_filt_ele,ia_filt_block_struct * str_filter_block,ia_td_filter_cascade_struct * str_td_filter_cascade,WORD32 eq_subband_gain_format,WORD32 eq_ch_group_count,FLOAT32 sample_rate,WORD32 eq_frame_size_subband,ia_subband_filt_struct * subband_filt)221 WORD32 impd_calc_subband_gains_td_cascade(
222 ia_unique_td_filt_element* unique_td_filt_ele,
223 ia_filt_block_struct* str_filter_block,
224 ia_td_filter_cascade_struct* str_td_filter_cascade,
225 WORD32 eq_subband_gain_format, WORD32 eq_ch_group_count,
226 FLOAT32 sample_rate, WORD32 eq_frame_size_subband,
227 ia_subband_filt_struct* subband_filt) {
228 WORD32 i, err = 0, g, b;
229 FLOAT32 response, frequency_radian;
230 FLOAT32 subband_center_freq[256];
231 FLOAT64 total_response;
232
233 WORD32 eq_subband_gain_count = subband_filt->coeff_count;
234
235 err = impd_derive_subband_center_freq(eq_subband_gain_count,
236 eq_subband_gain_format, sample_rate,
237 subband_center_freq);
238 if (err) return (err);
239
240 for (g = 0; g < eq_ch_group_count; g++) {
241 for (b = 0; b < eq_subband_gain_count; b++) {
242 total_response =
243 pow(10.0f, 0.05f * str_td_filter_cascade->eq_cascade_gain[g]);
244 frequency_radian =
245 (FLOAT32)(2.0f * M_PI * subband_center_freq[b] / sample_rate);
246 for (i = 0;
247 i <
248 str_td_filter_cascade->str_filter_block_refs[g].filter_block_count;
249 i++) {
250 impd_calc_filt_block_response(
251 unique_td_filt_ele,
252 &(str_filter_block[str_td_filter_cascade->str_filter_block_refs[g]
253 .filter_block_index[i]]),
254 frequency_radian, &response);
255 total_response *= response;
256 }
257 subband_filt[g].subband_coeff[b] = (FLOAT32)total_response;
258 }
259 subband_filt[g].eq_frame_size_subband = eq_frame_size_subband;
260 }
261 return (0);
262 }
263
impd_add_cascade(ia_cascade_align_group_struct * pstr_cascade_align_grp,WORD32 c1,WORD32 c2,WORD32 * done)264 VOID impd_add_cascade(ia_cascade_align_group_struct* pstr_cascade_align_grp,
265 WORD32 c1, WORD32 c2, WORD32* done) {
266 WORD32 m, n;
267
268 *done = 0;
269 for (m = 0; m < pstr_cascade_align_grp->member_count; m++) {
270 if (pstr_cascade_align_grp->member_idx[m] == c1) {
271 for (n = 0; n < pstr_cascade_align_grp->member_count; n++) {
272 if (pstr_cascade_align_grp->member_idx[n] == c2) {
273 *done = 1;
274 }
275 }
276 if (*done == 0) {
277 pstr_cascade_align_grp
278 ->member_idx[pstr_cascade_align_grp->member_count] = c2;
279 pstr_cascade_align_grp->member_count++;
280 *done = 1;
281 }
282 }
283 }
284 return;
285 }
286
impd_calc_cascade_align_groups(WORD32 eq_ch_group_count,WORD32 eq_phase_alignment_present,WORD32 eq_phase_alignment[][EQ_CHANNEL_GROUP_COUNT_MAX],WORD32 * cascade_align_grp_cnt,ia_cascade_align_group_struct * pstr_cascade_align_grp)287 VOID impd_calc_cascade_align_groups(
288 WORD32 eq_ch_group_count, WORD32 eq_phase_alignment_present,
289 WORD32 eq_phase_alignment[][EQ_CHANNEL_GROUP_COUNT_MAX],
290 WORD32* cascade_align_grp_cnt,
291 ia_cascade_align_group_struct* pstr_cascade_align_grp) {
292 WORD32 i, k, g, group_count, done;
293
294 group_count = 0;
295
296 if (eq_phase_alignment_present == 0) {
297 if (eq_ch_group_count > 1) {
298 for (i = 0; i < eq_ch_group_count; i++) {
299 pstr_cascade_align_grp[group_count].member_idx[i] = i;
300 }
301 pstr_cascade_align_grp[group_count].member_count = eq_ch_group_count;
302 group_count = 1;
303 }
304 } else {
305 for (i = 0; i < eq_ch_group_count; i++) {
306 for (k = i + 1; k < eq_ch_group_count; k++) {
307 if (eq_phase_alignment[i][k] == 1) {
308 done = 0;
309 for (g = 0; g < group_count; g++) {
310 impd_add_cascade(&pstr_cascade_align_grp[g], i, k, &done);
311
312 if (done == 0) {
313 impd_add_cascade(&pstr_cascade_align_grp[g], k, i, &done);
314 }
315 }
316 if (done == 0) {
317 pstr_cascade_align_grp[group_count].member_idx[0] = i;
318 pstr_cascade_align_grp[group_count].member_idx[1] = k;
319 pstr_cascade_align_grp[group_count].member_count = 2;
320 group_count++;
321 }
322 }
323 }
324 }
325 }
326 *cascade_align_grp_cnt = group_count;
327 return;
328 }
329
impd_calc_phase_filt_params(WORD32 config,FLOAT32 radius,FLOAT32 angle,ia_ph_alignment_filt_struct * ph_alignment_filt)330 VOID impd_calc_phase_filt_params(
331 WORD32 config, FLOAT32 radius, FLOAT32 angle,
332 ia_ph_alignment_filt_struct* ph_alignment_filt) {
333 WORD32 channel;
334 FLOAT32 zReal, zImag;
335 FLOAT32 prod;
336 WORD32 section = ph_alignment_filt->section_count;
337 ia_filt_sect_struct* filt_section = &ph_alignment_filt->filt_section[section];
338 switch (config) {
339 case CONFIG_REAL_POLE:
340 ph_alignment_filt->gain *= (-radius);
341 filt_section->a1 = -radius;
342 filt_section->a2 = 0.0f;
343 filt_section->b1 = -1.0f / radius;
344 filt_section->b2 = 0.0f;
345 ph_alignment_filt->section_count++;
346 break;
347 case CONFIG_COMPLEX_POLE:
348 zReal = radius * (FLOAT32)cos(M_PI * angle);
349 zImag = radius * (FLOAT32)sin(M_PI * angle);
350 prod = zReal * zReal + zImag * zImag;
351 ph_alignment_filt->gain *= prod;
352 filt_section->a1 = -2.0f * zReal;
353 filt_section->a2 = prod;
354 filt_section->b1 = -2.0f * zReal / prod;
355 filt_section->b2 = 1.0f / prod;
356 ph_alignment_filt->section_count++;
357 break;
358 default:
359 break;
360 }
361 for (channel = 0; channel < EQ_CHANNEL_COUNT_MAX; channel++) {
362 filt_section->filt_sect_state[channel].in_state_1 = 0.0f;
363 filt_section->filt_sect_state[channel].in_state_2 = 0.0f;
364 filt_section->filt_sect_state[channel].out_state_1 = 0.0f;
365 filt_section->filt_sect_state[channel].out_state_2 = 0.0f;
366 }
367
368 return;
369 }
370
impd_calc_phase_filt_delay(ia_unique_td_filt_element * element,ia_ph_alignment_filt_struct * ph_alignment_filt)371 VOID impd_calc_phase_filt_delay(
372 ia_unique_td_filt_element* element,
373 ia_ph_alignment_filt_struct* ph_alignment_filt) {
374 WORD32 i, delay = 0, channel;
375 if (element->eq_filter_format == FILTER_ELEMENT_FORMAT_POLE_ZERO) {
376 if (element->bs_real_zero_radius_one_count == 0) {
377 delay = element->real_zero_count + 2 * element->generic_zero_count -
378 element->real_pole_count - 2 * element->cmplx_pole_count;
379 delay = max(0, delay);
380 ph_alignment_filt->validity_flag = 1;
381 }
382 }
383 ph_alignment_filt->audio_delay.delay = delay;
384 for (channel = 0; channel < EQ_CHANNEL_COUNT_MAX; channel++) {
385 for (i = 0; i < delay; i++) {
386 ph_alignment_filt->audio_delay.state[channel][i] = 0.0f;
387 }
388 }
389
390 return;
391 }
392
impd_calc_phase_filt(ia_unique_td_filt_element * element,WORD32 filt_ele_idx,ia_matching_ph_filt_struct * matching_ph_filt)393 VOID impd_calc_phase_filt(ia_unique_td_filt_element* element,
394 WORD32 filt_ele_idx,
395 ia_matching_ph_filt_struct* matching_ph_filt) {
396 WORD32 i;
397
398 memset(matching_ph_filt, 0, sizeof(ia_matching_ph_filt_struct));
399 matching_ph_filt->gain = 1.0f;
400
401 if (element->eq_filter_format == FILTER_ELEMENT_FORMAT_POLE_ZERO) {
402 for (i = 0; i < element->real_pole_count; i++) {
403 impd_calc_phase_filt_params(CONFIG_REAL_POLE,
404 element->real_pole_radius[i], 0.0f,
405 matching_ph_filt);
406 }
407 for (i = 0; i < element->cmplx_pole_count; i++) {
408 impd_calc_phase_filt_params(
409 CONFIG_COMPLEX_POLE, element->complex_pole_radius[i],
410 element->complex_pole_angle[i], matching_ph_filt);
411 }
412 }
413 impd_calc_phase_filt_delay(element, matching_ph_filt);
414
415 matching_ph_filt->num_matches_filter = 1;
416 matching_ph_filt->matches_filter[0] = filt_ele_idx;
417
418 return;
419 }
420
impd_calc_filt_params(ia_unique_td_filt_element * element,ia_interm_filt_params_struct * interm_filt_params)421 WORD32 impd_calc_filt_params(ia_unique_td_filt_element* element,
422 ia_interm_filt_params_struct* interm_filt_params) {
423 FLOAT32 zReal;
424 FLOAT32* coeff;
425 // WORD32 offset_idx = 0;
426 WORD32 i;
427 WORD32 param_idx = 0;
428
429 ia_2nd_order_filt_params_struct* pstr_2nd_oder_filt_params =
430 &interm_filt_params->ord_2_filt_params_of_zeros[0];
431
432 for (i = 0; i < element->bs_real_zero_radius_one_count; i += 2) {
433 FLOAT32 radius = (FLOAT32)element->zero_sign[i + 0];
434 FLOAT32 angle = (FLOAT32)element->zero_sign[i + 1];
435 FLOAT32 angle1 = radius;
436 FLOAT32 angle2 = angle;
437 pstr_2nd_oder_filt_params->radius = 1.0f;
438 coeff = pstr_2nd_oder_filt_params->coeff;
439
440 if (angle1 != angle2) {
441 coeff[0] = 0.0f;
442 coeff[1] = -1.0f;
443 } else if (angle1 == 1.0f) {
444 coeff[0] = -2.0f;
445 coeff[1] = 1.0f;
446 } else {
447 coeff[0] = 2.0f;
448 coeff[1] = 1.0f;
449 }
450 pstr_2nd_oder_filt_params += 1;
451 param_idx += 1;
452 }
453 for (i = 0; i < element->real_zero_count; i++) {
454 FLOAT32 radius = element->real_zero_radius[i];
455 // FLOAT32 angle = 0.0f;
456
457 pstr_2nd_oder_filt_params->radius = radius;
458 if (fabs(radius) == 1.0f) {
459 return (-1);
460 } else {
461 coeff = pstr_2nd_oder_filt_params->coeff;
462 coeff[0] = -(radius + 1.0f / radius);
463 coeff[1] = 1.0f;
464 }
465 pstr_2nd_oder_filt_params += 1;
466 param_idx += 1;
467 }
468
469 for (i = 0; i < element->generic_zero_count; i++) {
470 FLOAT32 radius = element->generic_zero_radius[i];
471 FLOAT32 angle = element->generic_zero_angle[i];
472 zReal = radius * (FLOAT32)cos(M_PI * angle);
473 pstr_2nd_oder_filt_params->radius = radius;
474 coeff = pstr_2nd_oder_filt_params->coeff;
475 coeff[0] = -2.0f * zReal;
476 coeff[1] = radius * radius;
477
478 pstr_2nd_oder_filt_params += 1;
479
480 zReal = (FLOAT32)cos(M_PI * angle) / radius;
481 pstr_2nd_oder_filt_params->radius = radius;
482 coeff = pstr_2nd_oder_filt_params->coeff;
483 coeff[0] = -2.0f * zReal;
484 coeff[1] = 1.0f / (radius * radius);
485
486 pstr_2nd_oder_filt_params += 1;
487
488 param_idx += 2;
489 }
490
491 interm_filt_params->filter_param_count_of_zeros = param_idx;
492 param_idx = 0;
493
494 pstr_2nd_oder_filt_params =
495 &interm_filt_params->ord_2_filt_params_of_poles[0];
496
497 for (i = 0; i < element->real_pole_count; i++) {
498 FLOAT32 radius = element->real_pole_radius[i];
499 pstr_2nd_oder_filt_params->radius = radius;
500 coeff = pstr_2nd_oder_filt_params->coeff;
501 coeff[0] = -2.0f * radius;
502 coeff[1] = radius * radius;
503 param_idx += 1;
504 pstr_2nd_oder_filt_params += 1;
505 }
506
507 for (i = 0; i < element->cmplx_pole_count; i++) {
508 FLOAT32 radius = element->complex_pole_radius[i];
509 FLOAT32 angle = element->complex_pole_angle[i];
510
511 zReal = radius * (FLOAT32)cos(M_PI * angle);
512 pstr_2nd_oder_filt_params->radius = radius;
513 coeff = pstr_2nd_oder_filt_params->coeff;
514 coeff[0] = -2.0f * zReal;
515 coeff[1] = radius * radius;
516
517 pstr_2nd_oder_filt_params += 1;
518
519 pstr_2nd_oder_filt_params->radius = radius;
520 pstr_2nd_oder_filt_params->coeff[0] = coeff[0];
521 pstr_2nd_oder_filt_params->coeff[1] = coeff[1];
522
523 pstr_2nd_oder_filt_params += 1;
524 param_idx += 2;
525 }
526 interm_filt_params->filter_param_count_of_poles = param_idx;
527 return 0;
528 }
529
impd_convert_fir_filt_params(WORD32 fir_filt_order,WORD32 fir_symmetry,FLOAT32 * fir_coeff,ia_fir_filter_struct * fir_filter)530 VOID impd_convert_fir_filt_params(WORD32 fir_filt_order, WORD32 fir_symmetry,
531 FLOAT32* fir_coeff,
532 ia_fir_filter_struct* fir_filter) {
533 WORD32 i, channel;
534 FLOAT32* coeff = fir_filter->coeff;
535
536 fir_filter->coeff_count = fir_filt_order + 1;
537 for (i = 0; i < fir_filt_order / 2 + 1; i++) {
538 coeff[i] = fir_coeff[i];
539 }
540
541 if (fir_symmetry == 1) {
542 for (i = 0; i < (fir_filt_order + 1) / 2; i++) {
543 coeff[fir_filt_order - i] = -coeff[i];
544 }
545
546 if ((fir_filt_order & 1) == 0) {
547 coeff[fir_filt_order / 2] = 0.0f;
548 }
549 } else {
550 for (i = 0; i < (fir_filt_order + 1) / 2; i++) {
551 coeff[fir_filt_order - i] = coeff[i];
552 }
553 }
554
555 for (channel = 0; channel < EQ_CHANNEL_COUNT_MAX; channel++) {
556 for (i = 0; i < fir_filt_order + 1; i++) {
557 fir_filter->state[channel][i] = 0.0f;
558 }
559 }
560 return;
561 }
562
impd_calc_filt_params_all(ia_unique_td_filt_element * element,ia_interm_filt_params_struct * interm_filt_params)563 WORD32 impd_calc_filt_params_all(
564 ia_unique_td_filt_element* element,
565 ia_interm_filt_params_struct* interm_filt_params) {
566 WORD32 err = 0;
567
568 interm_filt_params->filter_format = element->eq_filter_format;
569 if (element->eq_filter_format == FILTER_ELEMENT_FORMAT_POLE_ZERO) {
570 err = impd_calc_filt_params(element, interm_filt_params);
571 if (err) return err;
572 } else {
573 interm_filt_params->filter_param_count_of_zeros = 0;
574 interm_filt_params->filter_param_count_of_poles = 0;
575
576 impd_convert_fir_filt_params(element->fir_filt_order, element->fir_symmetry,
577 element->fir_coeff,
578 &interm_filt_params->fir_filter);
579 }
580 return (0);
581 }
582
impd_calc_eq_filt_elements(ia_interm_filt_params_struct * interm_filt_params,ia_eq_filt_ele_struct * eq_filt_element)583 VOID impd_calc_eq_filt_elements(
584 ia_interm_filt_params_struct* interm_filt_params,
585 ia_eq_filt_ele_struct* eq_filt_element) {
586 WORD32 i, poles_idx, zeros_idx, pole_order = 0, section, channel;
587 WORD32 poles_over[REAL_POLE_COUNT_MAX + COMPLEX_POLE_COUNT_MAX];
588 WORD32 zeros_over[REAL_ZERO_COUNT_MAX + COMPLEX_ZERO_COUNT_MAX];
589 FLOAT32 max_radius, diff_radius;
590 WORD32 coeff_count;
591 FLOAT32* coeff;
592
593 for (i = 0; i < REAL_POLE_COUNT_MAX + COMPLEX_POLE_COUNT_MAX; i++) {
594 poles_over[i] = 0;
595 }
596 for (i = 0; i < REAL_ZERO_COUNT_MAX + COMPLEX_ZERO_COUNT_MAX; i++) {
597 zeros_over[i] = 0;
598 }
599 section = 0;
600 do {
601 max_radius = -1.0;
602 poles_idx = -1;
603 for (i = 0; i < interm_filt_params->filter_param_count_of_poles; i++) {
604 if (poles_over[i] == 0) {
605 if (interm_filt_params->filter_format == 0) {
606 if (max_radius <
607 fabs(interm_filt_params->ord_2_filt_params_of_poles[i].radius)) {
608 max_radius = (FLOAT32)fabs(
609 interm_filt_params->ord_2_filt_params_of_poles[i].radius);
610 poles_idx = i;
611 if (interm_filt_params->ord_2_filt_params_of_poles[i].coeff[1] !=
612 0.0f) {
613 pole_order = 2;
614 } else {
615 pole_order = 1;
616 }
617 }
618 }
619 }
620 }
621 if (poles_idx >= 0) {
622 diff_radius = 10.0f;
623 zeros_idx = -1;
624 for (i = 0; i < interm_filt_params->filter_param_count_of_zeros; i++) {
625 if (zeros_over[i] == 0) {
626 if (interm_filt_params->filter_format == 0) {
627 if (pole_order == 2) {
628 if (interm_filt_params->ord_2_filt_params_of_zeros[i].coeff[1] !=
629 0.0f) {
630 if (diff_radius >
631 fabs(fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
632 .radius) -
633 max_radius)) {
634 diff_radius = (FLOAT32)fabs(
635 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
636 .radius) -
637 max_radius);
638 zeros_idx = i;
639 }
640 }
641 } else {
642 if (interm_filt_params->ord_2_filt_params_of_zeros[i].coeff[1] ==
643 0.0f) {
644 if (diff_radius >
645 (FLOAT32)(fabs(
646 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
647 .radius) -
648 max_radius))) {
649 diff_radius = (FLOAT32)(fabs(
650 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
651 .radius) -
652 max_radius));
653 zeros_idx = i;
654 }
655 }
656 }
657 }
658 }
659 }
660 if (zeros_idx == -1) {
661 for (i = 0; i < interm_filt_params->filter_param_count_of_zeros; i++) {
662 if (zeros_over[i] == 0) {
663 if (interm_filt_params->filter_format == 0) {
664 if (pole_order == 2) {
665 if (interm_filt_params->ord_2_filt_params_of_zeros[i]
666 .coeff[1] == 0.0f) {
667 if (diff_radius >
668 (FLOAT32)(fabs(
669 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
670 .radius) -
671 max_radius))) {
672 diff_radius = (FLOAT32)(fabs(
673 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
674 .radius) -
675 max_radius));
676 zeros_idx = i;
677 }
678 }
679 } else {
680 if (interm_filt_params->ord_2_filt_params_of_zeros[i]
681 .coeff[1] != 0.0f) {
682 if (diff_radius >
683 (FLOAT32)(fabs(
684 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
685 .radius) -
686 max_radius))) {
687 diff_radius = (FLOAT32)(fabs(
688 fabs(interm_filt_params->ord_2_filt_params_of_zeros[i]
689 .radius) -
690 max_radius));
691 zeros_idx = i;
692 }
693 }
694 }
695 }
696 }
697 }
698 }
699 eq_filt_element->pstr_pole_zero_filt.filt_section[section].a1 =
700 interm_filt_params->ord_2_filt_params_of_poles[poles_idx].coeff[0];
701 eq_filt_element->pstr_pole_zero_filt.filt_section[section].a2 =
702 interm_filt_params->ord_2_filt_params_of_poles[poles_idx].coeff[1];
703 if (zeros_idx >= 0) {
704 eq_filt_element->pstr_pole_zero_filt.filt_section[section].b1 =
705 interm_filt_params->ord_2_filt_params_of_zeros[zeros_idx].coeff[0];
706 eq_filt_element->pstr_pole_zero_filt.filt_section[section].b2 =
707 interm_filt_params->ord_2_filt_params_of_zeros[zeros_idx].coeff[1];
708 } else {
709 eq_filt_element->pstr_pole_zero_filt.filt_section[section].b1 = 0.0f;
710 eq_filt_element->pstr_pole_zero_filt.filt_section[section].b2 = 0.0f;
711 eq_filt_element->pstr_pole_zero_filt.audio_delay.delay++;
712 }
713 for (channel = 0; channel < EQ_CHANNEL_COUNT_MAX; channel++) {
714 eq_filt_element->pstr_pole_zero_filt.filt_section[section]
715 .filt_sect_state[channel]
716 .in_state_1 = 0.0f;
717 eq_filt_element->pstr_pole_zero_filt.filt_section[section]
718 .filt_sect_state[channel]
719 .in_state_2 = 0.0f;
720 eq_filt_element->pstr_pole_zero_filt.filt_section[section]
721 .filt_sect_state[channel]
722 .out_state_1 = 0.0f;
723 eq_filt_element->pstr_pole_zero_filt.filt_section[section]
724 .filt_sect_state[channel]
725 .out_state_2 = 0.0f;
726 }
727 if (zeros_idx >= 0) zeros_over[zeros_idx] = 1;
728 if (poles_idx >= 0) poles_over[poles_idx] = 1;
729 section++;
730 }
731 } while (poles_idx >= 0);
732
733 eq_filt_element->pstr_pole_zero_filt.section_count = section;
734
735 coeff_count = 1;
736 coeff = eq_filt_element->pstr_pole_zero_filt.fir_filter.coeff;
737 coeff[0] = 1.0f;
738 for (i = 0; i < interm_filt_params->filter_param_count_of_zeros; i++) {
739 if (zeros_over[i] == 0) {
740 if (interm_filt_params->filter_format == 0) {
741 WORD32 k;
742 FLOAT32 b1, b2;
743 b1 = interm_filt_params->ord_2_filt_params_of_zeros[i].coeff[0];
744 b2 = interm_filt_params->ord_2_filt_params_of_zeros[i].coeff[1];
745
746 coeff_count += 2;
747 k = coeff_count - 1;
748 coeff[k] = b2 * coeff[k - 2];
749 k--;
750 if (k > 1) {
751 coeff[k] = b1 * coeff[k - 1] + b2 * coeff[k - 2];
752 k--;
753 for (; k > 1; k--) {
754 coeff[k] += b1 * coeff[k - 1] + b2 * coeff[k - 2];
755 }
756 coeff[1] += b1 * coeff[0];
757 } else {
758 coeff[1] = b1 * coeff[0];
759 }
760 }
761 }
762 zeros_over[i] = 1;
763 }
764 if (coeff_count > 1) {
765 eq_filt_element->pstr_pole_zero_filt.filt_coeffs_flag = 1;
766 eq_filt_element->pstr_pole_zero_filt.fir_filter.coeff_count = coeff_count;
767 } else {
768 eq_filt_element->pstr_pole_zero_filt.filt_coeffs_flag = 0;
769 eq_filt_element->pstr_pole_zero_filt.fir_filter.coeff_count = 0;
770 }
771
772 return;
773 }
774
impd_calc_filt_block(ia_unique_td_filt_element * unique_td_filt_ele,ia_filt_block_struct * str_filter_block,ia_eq_filt_block_struct * pstr_eq_filt_block)775 WORD32 impd_calc_filt_block(ia_unique_td_filt_element* unique_td_filt_ele,
776 ia_filt_block_struct* str_filter_block,
777 ia_eq_filt_block_struct* pstr_eq_filt_block) {
778 WORD32 i, k, err;
779 ia_interm_filt_params_struct interm_filt_params;
780 ia_matching_ph_filt_struct matching_ph_filt[FILTER_ELEMENT_COUNT_MAX];
781
782 for (i = 0; i < str_filter_block->filter_element_count; i++) {
783 if ((unique_td_filt_ele[str_filter_block->str_filter_element[i]
784 .filt_ele_idx]
785 .eq_filter_format == FILTER_ELEMENT_FORMAT_FIR) &&
786 (str_filter_block->filter_element_count > 1)) {
787 return (-1);
788 }
789 }
790 for (i = 0; i < str_filter_block->filter_element_count; i++) {
791 ia_eq_filt_ele_struct* eq_filt_element =
792 &pstr_eq_filt_block->eq_filt_element[i];
793 ia_filt_ele_struct* str_filter_element =
794 &str_filter_block->str_filter_element[i];
795 WORD32 filterIndex = str_filter_element->filt_ele_idx;
796
797 if (unique_td_filt_ele[filterIndex].eq_filter_format ==
798 FILTER_ELEMENT_FORMAT_POLE_ZERO) {
799 err = impd_calc_filt_params_all(&(unique_td_filt_ele[filterIndex]),
800 &interm_filt_params);
801 if (err) return (err);
802
803 impd_calc_eq_filt_elements(&interm_filt_params, eq_filt_element);
804
805 eq_filt_element->format = FILTER_ELEMENT_FORMAT_POLE_ZERO;
806 } else {
807 impd_convert_fir_filt_params(
808 unique_td_filt_ele[filterIndex].fir_filt_order,
809 unique_td_filt_ele[filterIndex].fir_symmetry,
810 unique_td_filt_ele[filterIndex].fir_coeff,
811 &eq_filt_element->fir_filter);
812
813 eq_filt_element->format = FILTER_ELEMENT_FORMAT_FIR;
814 }
815 if (str_filter_element->filt_ele_gain_flag == 1) {
816 eq_filt_element->elementGainLinear =
817 (FLOAT32)(pow(10.0f, 0.05f * str_filter_element->filt_ele_gain));
818 } else {
819 eq_filt_element->elementGainLinear = 1.0f;
820 }
821 for (k = 0; k < unique_td_filt_ele[filterIndex].real_zero_count; k++) {
822 if (unique_td_filt_ele[filterIndex].real_zero_radius[k] > 0.0f) {
823 eq_filt_element->elementGainLinear =
824 -eq_filt_element->elementGainLinear;
825 }
826 }
827 impd_calc_phase_filt(&(unique_td_filt_ele[filterIndex]), i,
828 &matching_ph_filt[i]);
829 }
830 pstr_eq_filt_block->element_count = str_filter_block->filter_element_count;
831
832 pstr_eq_filt_block->matching_ph_filt_ele_0 = matching_ph_filt[0];
833
834 return (0);
835 }
836
impd_calc_cascade_phase_align_filt(ia_td_filter_cascade_struct * str_td_filter_cascade,WORD32 ch_group_cnt)837 VOID impd_calc_cascade_phase_align_filt(
838 ia_td_filter_cascade_struct* str_td_filter_cascade, WORD32 ch_group_cnt) {
839 // WORD32 err = 0;
840 WORD32 cascade_align_grp_cnt = 0;
841 ia_cascade_align_group_struct
842 pstr_cascade_align_grp[EQ_CHANNEL_GROUP_COUNT_MAX / 2];
843
844 impd_calc_cascade_align_groups(
845 ch_group_cnt, str_td_filter_cascade->eq_phase_alignment_present,
846 str_td_filter_cascade->eq_phase_alignment, &cascade_align_grp_cnt,
847 pstr_cascade_align_grp);
848 return;
849 }
850
impd_calc_filt_cascade(ia_unique_td_filt_element * unique_td_filt_ele,ia_filt_block_struct * str_filter_block,ia_td_filter_cascade_struct * str_td_filter_cascade,WORD32 ch_group_cnt,ia_filt_cascade_td_struct filt_cascade_td[])851 WORD32 impd_calc_filt_cascade(
852 ia_unique_td_filt_element* unique_td_filt_ele,
853 ia_filt_block_struct* str_filter_block,
854 ia_td_filter_cascade_struct* str_td_filter_cascade, WORD32 ch_group_cnt,
855 ia_filt_cascade_td_struct filt_cascade_td[]) {
856 WORD32 i, err, g;
857
858 for (g = 0; g < ch_group_cnt; g++) {
859 for (i = 0;
860 i < str_td_filter_cascade->str_filter_block_refs[g].filter_block_count;
861 i++) {
862 err = impd_calc_filt_block(
863 unique_td_filt_ele,
864 &(str_filter_block[str_td_filter_cascade->str_filter_block_refs[g]
865 .filter_block_index[i]]),
866 &(filt_cascade_td[g].pstr_eq_filt_block[i]));
867 if (err) return (err);
868 }
869 filt_cascade_td[g].block_count = i;
870 filt_cascade_td[g].cascade_gain_linear = (FLOAT32)(
871 pow(10.0f, 0.05f * str_td_filter_cascade->eq_cascade_gain[g]));
872 }
873
874 impd_calc_cascade_phase_align_filt(str_td_filter_cascade, ch_group_cnt);
875 return (0);
876 }
877
impd_calc_subband_eq(ia_eq_subband_gain_vector * str_eq_subband_gain_vector,WORD32 eq_subband_gain_count,ia_subband_filt_struct * subband_filt)878 VOID impd_calc_subband_eq(ia_eq_subband_gain_vector* str_eq_subband_gain_vector,
879 WORD32 eq_subband_gain_count,
880 ia_subband_filt_struct* subband_filt) {
881 WORD32 i;
882
883 for (i = 0; i < eq_subband_gain_count; i++) {
884 subband_filt->subband_coeff[i] =
885 str_eq_subband_gain_vector->eq_subband_gain[i];
886 }
887 subband_filt->coeff_count = eq_subband_gain_count;
888 return;
889 }
890
impd_decode_eq_node_freq(WORD32 eq_node_freq_idx)891 FLOAT32 impd_decode_eq_node_freq(WORD32 eq_node_freq_idx) {
892 /*((FLOAT32)((log10(STEP_RATIO_F_HI) / log10(STEP_RATIO_F_LO) - 1.0f) /
893 * (STEP_RATIO_EQ_NODE_COUNT_MAX - 1.0f)))*/
894 FLOAT32 step_ratio = 0.0739601809794f;
895 return (
896 (FLOAT32)(pow(STEP_RATIO_F_LO, 1.0f + eq_node_freq_idx * step_ratio)));
897 }
898
impd_calc_warp_freq_delta(FLOAT32 fsubband,FLOAT32 node_freq,WORD32 eq_node_freq_idx)899 FLOAT32 impd_calc_warp_freq_delta(FLOAT32 fsubband, FLOAT32 node_freq,
900 WORD32 eq_node_freq_idx) {
901 /*((FLOAT32)((log10(STEP_RATIO_F_HI) / log10(STEP_RATIO_F_LO) - 1.0f) /
902 * (STEP_RATIO_EQ_NODE_COUNT_MAX - 1.0f)))*/
903 FLOAT32 step_ratio = 0.0739601809794f;
904 return ((FLOAT32)((log10(fsubband) / log10(node_freq) - 1.0f) / step_ratio -
905 (FLOAT32)eq_node_freq_idx));
906 }
907
impd_interpolate_eq_gain(WORD32 band_step,FLOAT32 left_gain,FLOAT32 right_gain,FLOAT32 left_slope,FLOAT32 right_slope,FLOAT32 f,FLOAT32 * interpolated_gain)908 VOID impd_interpolate_eq_gain(WORD32 band_step, FLOAT32 left_gain,
909 FLOAT32 right_gain, FLOAT32 left_slope,
910 FLOAT32 right_slope, FLOAT32 f,
911 FLOAT32* interpolated_gain) {
912 FLOAT32 k1, k2, a, b, c, d;
913 FLOAT32 inv_band_step = (FLOAT32)(1.0 / (FLOAT32)band_step);
914 FLOAT32 inv_band_step_sqr = inv_band_step * inv_band_step;
915 k1 = (right_gain - left_gain) * inv_band_step_sqr;
916 left_slope = (FLOAT32)(left_slope / 3.128f);
917 right_slope = (FLOAT32)(right_slope / 3.128f);
918
919 k2 = right_slope + left_slope;
920 a = inv_band_step * (inv_band_step * k2 - 2.0f * k1);
921 b = 3.0f * k1 - inv_band_step * (k2 + left_slope);
922 c = left_slope;
923 d = left_gain;
924 *interpolated_gain = (((a * f + b) * f + c) * f) + d;
925 return;
926 }
927
impd_interpolate_subband_spline(ia_eq_subband_gain_spline_struct * str_eq_subband_gain_spline,WORD32 eq_subband_gain_count,WORD32 eq_subband_gain_format,FLOAT32 sample_rate,ia_subband_filt_struct * subband_filt)928 WORD32 impd_interpolate_subband_spline(
929 ia_eq_subband_gain_spline_struct* str_eq_subband_gain_spline,
930 WORD32 eq_subband_gain_count, WORD32 eq_subband_gain_format,
931 FLOAT32 sample_rate, ia_subband_filt_struct* subband_filt) {
932 WORD32 b, n, err;
933
934 FLOAT32 eq_gain[32];
935 WORD32 eq_node_freq_idx[32];
936 FLOAT32 eq_node_freq[32];
937 FLOAT32 subband_center_freq[256];
938 WORD32 num_eq_nodes = str_eq_subband_gain_spline->num_eq_nodes;
939
940 FLOAT32* eq_slope = str_eq_subband_gain_spline->eq_slope;
941 WORD32* eq_freq_delta = str_eq_subband_gain_spline->eq_freq_delta;
942 FLOAT32 eq_gain_initial = str_eq_subband_gain_spline->eq_gain_initial;
943 FLOAT32* eq_gain_delta = str_eq_subband_gain_spline->eq_gain_delta;
944
945 FLOAT32* subband_coeff = subband_filt->subband_coeff;
946 WORD32 max_eq_node_idx = 32;
947
948 eq_gain[0] = eq_gain_initial;
949 eq_node_freq_idx[0] = 0;
950 eq_node_freq[0] = impd_decode_eq_node_freq(eq_node_freq_idx[0]);
951 for (n = 1; n < num_eq_nodes; n++) {
952 eq_gain[n] = eq_gain[n - 1] + eq_gain_delta[n];
953 eq_node_freq_idx[n] = eq_node_freq_idx[n - 1] + eq_freq_delta[n];
954 eq_node_freq[n] = impd_decode_eq_node_freq(eq_node_freq_idx[n]);
955 }
956 if ((eq_node_freq[num_eq_nodes - 1] < sample_rate * 0.5f) &&
957 (eq_node_freq_idx[num_eq_nodes - 1] < max_eq_node_idx)) {
958 eq_slope[num_eq_nodes] = 0;
959 eq_gain[num_eq_nodes] = eq_gain[num_eq_nodes - 1];
960 eq_freq_delta[num_eq_nodes] =
961 max_eq_node_idx - eq_node_freq_idx[num_eq_nodes - 1];
962 eq_node_freq_idx[num_eq_nodes] = max_eq_node_idx;
963 eq_node_freq[num_eq_nodes] =
964 impd_decode_eq_node_freq(eq_node_freq_idx[num_eq_nodes]);
965 num_eq_nodes += 1;
966 }
967
968 err = impd_derive_subband_center_freq(eq_subband_gain_count,
969 eq_subband_gain_format, sample_rate,
970 subband_center_freq);
971 if (err) return (err);
972
973 for (n = 0; n < num_eq_nodes - 1; n++) {
974 for (b = 0; b < eq_subband_gain_count; b++) {
975 FLOAT32 fSub;
976 fSub = max(subband_center_freq[b], eq_node_freq[0]);
977 fSub = min(fSub, eq_node_freq[num_eq_nodes - 1]);
978 if ((fSub >= eq_node_freq[n]) && (fSub <= eq_node_freq[n + 1])) {
979 FLOAT32 warpedDeltaFreq = impd_calc_warp_freq_delta(
980 fSub, eq_node_freq[0], eq_node_freq_idx[n]);
981 FLOAT32 gEqSubbandDb;
982 impd_interpolate_eq_gain(eq_freq_delta[n + 1], eq_gain[n],
983 eq_gain[n + 1], eq_slope[n], eq_slope[n + 1],
984 warpedDeltaFreq, &gEqSubbandDb);
985
986 subband_coeff[b] = (FLOAT32)pow(2.0, gEqSubbandDb / 6.0f);
987 }
988 }
989 }
990 subband_filt->coeff_count = eq_subband_gain_count;
991 return (0);
992 }
993
impd_calc_subband_gains(ia_eq_coeff_struct * str_eq_coeff,WORD32 eq_ch_group_count,WORD32 * subband_gains_index,FLOAT32 sample_rate,WORD32 eq_frame_size_subband,ia_subband_filt_struct * subband_filt)994 WORD32 impd_calc_subband_gains(ia_eq_coeff_struct* str_eq_coeff,
995 WORD32 eq_ch_group_count,
996 WORD32* subband_gains_index, FLOAT32 sample_rate,
997 WORD32 eq_frame_size_subband,
998 ia_subband_filt_struct* subband_filt) {
999 WORD32 g, err;
1000 WORD32 eq_subband_gain_representation =
1001 str_eq_coeff->eq_subband_gain_representation;
1002 WORD32 eq_subband_gain_count = str_eq_coeff->eq_subband_gain_count;
1003 WORD32 eq_subband_gain_format = str_eq_coeff->eq_subband_gain_format;
1004
1005 for (g = 0; g < eq_ch_group_count; g++) {
1006 if (eq_subband_gain_representation == 1) {
1007 err = impd_interpolate_subband_spline(
1008 &(str_eq_coeff->str_eq_subband_gain_spline[subband_gains_index[g]]),
1009 eq_subband_gain_count, eq_subband_gain_format, sample_rate,
1010 &(subband_filt[g]));
1011 if (err) return (err);
1012 } else {
1013 impd_calc_subband_eq(
1014 &(str_eq_coeff->str_eq_subband_gain_vector[subband_gains_index[g]]),
1015 eq_subband_gain_count, &(subband_filt[g]));
1016 }
1017 subband_filt[g].eq_frame_size_subband = eq_frame_size_subband;
1018 }
1019 return (0);
1020 }
1021
impd_calc_filt_sect_delay(WORD32 section_count,ia_filt_sect_struct * filt_section,FLOAT32 * delay)1022 VOID impd_calc_filt_sect_delay(WORD32 section_count,
1023 ia_filt_sect_struct* filt_section,
1024 FLOAT32* delay) {
1025 WORD32 i;
1026 FLOAT32 d = 0.0f;
1027 for (i = 0; i < section_count; i++) {
1028 if (filt_section[i].b2 != 0.0f) {
1029 d += 1.0f;
1030 } else if (filt_section[i].b1 != 0.0f) {
1031 d += 0.5f;
1032 }
1033 }
1034 *delay = d;
1035 return;
1036 }
1037
impd_get_eq_set_delay(ia_eq_set_struct * eq_set,WORD32 * cascade_delay)1038 VOID impd_get_eq_set_delay(ia_eq_set_struct* eq_set, WORD32* cascade_delay) {
1039 FLOAT32 delay, sect_delay;
1040 WORD32 k, g, c, b;
1041
1042 delay = 0;
1043 for (c = 0; c < eq_set->audio_num_chan; c++) {
1044 g = eq_set->eq_ch_group_of_channel[c];
1045 if (g >= 0) {
1046 switch (eq_set->domain) {
1047 case EQ_FILTER_DOMAIN_TIME: {
1048 ia_filt_cascade_td_struct* filt_cascade_td =
1049 &eq_set->filt_cascade_td[g];
1050 for (b = 0; b < filt_cascade_td->block_count; b++) {
1051 ia_eq_filt_ele_struct* eq_filt_element =
1052 &filt_cascade_td->pstr_eq_filt_block[b].eq_filt_element[0];
1053 switch (eq_filt_element->format) {
1054 case FILTER_ELEMENT_FORMAT_POLE_ZERO:
1055 impd_calc_filt_sect_delay(
1056 eq_filt_element->pstr_pole_zero_filt.section_count,
1057 eq_filt_element->pstr_pole_zero_filt.filt_section,
1058 §_delay);
1059 delay += sect_delay;
1060 if (eq_filt_element->pstr_pole_zero_filt.filt_coeffs_flag) {
1061 delay += 0.5f * (eq_filt_element->pstr_pole_zero_filt
1062 .fir_filter.coeff_count -
1063 1);
1064 }
1065 break;
1066 case FILTER_ELEMENT_FORMAT_FIR:
1067 delay += 0.5f * (eq_filt_element->fir_filter.coeff_count - 1);
1068 break;
1069 default:
1070 break;
1071 }
1072 for (k = 0; k < eq_filt_element->num_ph_align_filt; k++) {
1073 ia_ph_alignment_filt_struct* ph_alignment_filt =
1074 &eq_filt_element->ph_alignment_filt[k];
1075 impd_calc_filt_sect_delay(ph_alignment_filt->section_count,
1076 ph_alignment_filt->filt_section,
1077 §_delay);
1078 delay += sect_delay;
1079 }
1080 }
1081 for (b = 0; b < filt_cascade_td->num_ph_align_filt; b++) {
1082 ia_ph_alignment_filt_struct* ph_alignment_filt =
1083 &filt_cascade_td->ph_alignment_filt[b];
1084 impd_calc_filt_sect_delay(ph_alignment_filt->section_count,
1085 ph_alignment_filt->filt_section,
1086 §_delay);
1087 delay += sect_delay;
1088 }
1089 } break;
1090 case EQ_FILTER_DOMAIN_SUBBAND:
1091 case EQ_FILTER_DOMAIN_NONE:
1092 default:
1093 break;
1094 }
1095 }
1096 break;
1097 }
1098 *cascade_delay = (WORD32)delay;
1099 return;
1100 }
1101
impd_derive_eq_set(ia_eq_coeff_struct * str_eq_coeff,ia_eq_instructions_struct * str_eq_instructions,FLOAT32 sample_rate,WORD32 drc_frame_size,WORD32 sub_band_domain_mode,ia_eq_set_struct * eq_set)1102 WORD32 impd_derive_eq_set(ia_eq_coeff_struct* str_eq_coeff,
1103 ia_eq_instructions_struct* str_eq_instructions,
1104 FLOAT32 sample_rate, WORD32 drc_frame_size,
1105 WORD32 sub_band_domain_mode,
1106 ia_eq_set_struct* eq_set) {
1107 WORD32 err, i, eq_frame_size_subband;
1108
1109 eq_set->domain = EQ_FILTER_DOMAIN_NONE;
1110
1111 if (sub_band_domain_mode == SUBBAND_DOMAIN_MODE_OFF) {
1112 if (str_eq_instructions->td_filter_cascade_present == 1) {
1113 err = impd_calc_filt_cascade(
1114 str_eq_coeff->unique_td_filt_ele, str_eq_coeff->str_filter_block,
1115 &str_eq_instructions->str_td_filter_cascade,
1116 str_eq_instructions->eq_ch_group_count, eq_set->filt_cascade_td);
1117 if (err) return (err);
1118 }
1119
1120 eq_set->domain |= EQ_FILTER_DOMAIN_TIME;
1121 }
1122 if (sub_band_domain_mode != SUBBAND_DOMAIN_MODE_OFF) {
1123 switch (sub_band_domain_mode) {
1124 case SUBBAND_DOMAIN_MODE_QMF64:
1125 if (str_eq_coeff->eq_subband_gain_count !=
1126 AUDIO_CODEC_SUBBAND_COUNT_QMF64) {
1127 return (-1);
1128 }
1129 eq_frame_size_subband =
1130 drc_frame_size / AUDIO_CODEC_SUBBAND_DOWNSAMPLING_FACTOR_QMF64;
1131 break;
1132 case SUBBAND_DOMAIN_MODE_QMF71:
1133 if (str_eq_coeff->eq_subband_gain_count !=
1134 AUDIO_CODEC_SUBBAND_COUNT_QMF71) {
1135 return (-1);
1136 }
1137 eq_frame_size_subband =
1138 drc_frame_size / AUDIO_CODEC_SUBBAND_DOWNSAMPLING_FACTOR_QMF71;
1139 break;
1140 case SUBBAND_DOMAIN_MODE_STFT256:
1141 if (str_eq_coeff->eq_subband_gain_count !=
1142 AUDIO_CODEC_SUBBAND_COUNT_STFT256) {
1143 return (-1);
1144 }
1145 eq_frame_size_subband =
1146 drc_frame_size / AUDIO_CODEC_SUBBAND_DOWNSAMPLING_FACTOR_STFT256;
1147 break;
1148 default:
1149 return (-1);
1150 break;
1151 }
1152 if (str_eq_instructions->subband_gains_present == 1) {
1153 err = impd_calc_subband_gains(
1154 str_eq_coeff, str_eq_instructions->eq_ch_group_count,
1155 str_eq_instructions->subband_gains_index, sample_rate,
1156 eq_frame_size_subband, eq_set->subband_filt);
1157 if (err) return (err);
1158 } else {
1159 if (str_eq_instructions->td_filter_cascade_present == 1) {
1160 err = impd_calc_subband_gains_td_cascade(
1161 str_eq_coeff->unique_td_filt_ele, str_eq_coeff->str_filter_block,
1162 &str_eq_instructions->str_td_filter_cascade,
1163 str_eq_coeff->eq_subband_gain_format,
1164 str_eq_instructions->eq_ch_group_count, sample_rate,
1165 eq_frame_size_subband, eq_set->subband_filt);
1166 if (err) return (err);
1167 }
1168 }
1169 eq_set->domain |= EQ_FILTER_DOMAIN_SUBBAND;
1170 }
1171 eq_set->audio_num_chan = str_eq_instructions->eq_channel_count;
1172 eq_set->eq_ch_group_count = str_eq_instructions->eq_ch_group_count;
1173
1174 for (i = 0; i < str_eq_instructions->eq_channel_count; i++) {
1175 eq_set->eq_ch_group_of_channel[i] =
1176 str_eq_instructions->eq_ch_group_of_channel[i];
1177 }
1178
1179 return (0);
1180 }
1181
impd_process_filt_sect(ia_filt_sect_struct filt_section[EQ_FILTER_SECTION_COUNT_MAX],WORD32 channel,FLOAT32 * audio_out,WORD32 section_count)1182 VOID impd_process_filt_sect(
1183 ia_filt_sect_struct filt_section[EQ_FILTER_SECTION_COUNT_MAX],
1184 WORD32 channel, FLOAT32* audio_out, WORD32 section_count) {
1185 WORD32 i;
1186
1187 for (i = 0; i < section_count; i++) {
1188 ia_filt_sect_state_struct* filt_sect_state =
1189 &filt_section[i].filt_sect_state[channel];
1190 FLOAT32 audio_in = *audio_out;
1191 *audio_out = audio_in + filt_section[i].b1 * filt_sect_state->in_state_1 +
1192 filt_section[i].b2 * filt_sect_state->in_state_2 -
1193 filt_section[i].a1 * filt_sect_state->out_state_1 -
1194 filt_section[i].a2 * filt_sect_state->out_state_2;
1195
1196 filt_sect_state->in_state_2 = filt_sect_state->in_state_1;
1197 filt_sect_state->in_state_1 = audio_in;
1198 filt_sect_state->out_state_2 = filt_sect_state->out_state_1;
1199 filt_sect_state->out_state_1 = *audio_out;
1200 }
1201 return;
1202 }
1203
impd_fir_filt_process(ia_fir_filter_struct * fir_filter,WORD32 channel,FLOAT32 audio_in,FLOAT32 * audio_out)1204 VOID impd_fir_filt_process(ia_fir_filter_struct* fir_filter, WORD32 channel,
1205 FLOAT32 audio_in, FLOAT32* audio_out) {
1206 WORD32 i;
1207 FLOAT32* coeff = fir_filter->coeff;
1208 FLOAT32* state = fir_filter->state[channel];
1209 FLOAT32 sum;
1210 sum = coeff[0] * audio_in;
1211 for (i = 1; i < fir_filter->coeff_count; i++) {
1212 sum += coeff[i] * state[i - 1];
1213 }
1214 *audio_out = sum;
1215 for (i = fir_filter->coeff_count - 2; i > 0; i--) {
1216 state[i] = state[i - 1];
1217 }
1218 state[0] = audio_in;
1219 return;
1220 }
1221
impd_audio_delay_process(ia_audio_delay_struct * audio_delay,WORD32 channel,FLOAT32 audio_in,FLOAT32 * ptr_audio_out)1222 VOID impd_audio_delay_process(ia_audio_delay_struct* audio_delay,
1223 WORD32 channel, FLOAT32 audio_in,
1224 FLOAT32* ptr_audio_out) {
1225 WORD32 i;
1226 FLOAT32* state = audio_delay->state[channel];
1227 if (audio_delay->delay > 0) {
1228 *ptr_audio_out = state[audio_delay->delay - 1];
1229 for (i = audio_delay->delay - 1; i > 0; i--) {
1230 state[i] = state[i - 1];
1231 }
1232 state[0] = audio_in;
1233 } else {
1234 *ptr_audio_out = audio_in;
1235 }
1236 return;
1237 }
1238
impd_pole_zero_filt_process(ia_pole_zero_filt_struct * pstr_pole_zero_filt,WORD32 channel,FLOAT32 audio_in,FLOAT32 * ptr_audio_out)1239 VOID impd_pole_zero_filt_process(ia_pole_zero_filt_struct* pstr_pole_zero_filt,
1240 WORD32 channel, FLOAT32 audio_in,
1241 FLOAT32* ptr_audio_out) {
1242 FLOAT32 inp = audio_in;
1243 FLOAT32 out = inp;
1244
1245 impd_process_filt_sect(pstr_pole_zero_filt->filt_section, channel, &out,
1246 pstr_pole_zero_filt->section_count);
1247 inp = out;
1248
1249 if (pstr_pole_zero_filt->filt_coeffs_flag == 1) {
1250 impd_fir_filt_process(&pstr_pole_zero_filt->fir_filter, channel, inp, &out);
1251 inp = out;
1252 }
1253 impd_audio_delay_process(&pstr_pole_zero_filt->audio_delay, channel, inp,
1254 &out);
1255
1256 *ptr_audio_out = out;
1257 return;
1258 }
1259
impd_subband_filter_process(ia_subband_filt_struct * pstr_subband_filt,FLOAT32 * ptr_audio_real_buff,FLOAT32 * ptr_audio_imag_buff)1260 VOID impd_subband_filter_process(ia_subband_filt_struct* pstr_subband_filt,
1261 FLOAT32* ptr_audio_real_buff,
1262 FLOAT32* ptr_audio_imag_buff) {
1263 WORD32 i, j;
1264 WORD32 eq_frame_size_subband = pstr_subband_filt->eq_frame_size_subband;
1265 WORD32 coeff_count = pstr_subband_filt->coeff_count;
1266
1267 FLOAT32* ptr_subband_coeff = pstr_subband_filt->subband_coeff;
1268
1269 for (i = 0; i < eq_frame_size_subband; i++) {
1270 for (j = 0; j < coeff_count; j++) {
1271 ptr_audio_real_buff[j] *= ptr_subband_coeff[j];
1272 ptr_audio_imag_buff[j] *= ptr_subband_coeff[j];
1273 }
1274 ptr_audio_real_buff += coeff_count;
1275 ptr_audio_imag_buff += coeff_count;
1276 }
1277 return;
1278 }
1279
impd_phase_align_filt_process(ia_ph_alignment_filt_struct * ph_alignment_filt,WORD32 channel,FLOAT32 * ptr_audio_out)1280 VOID impd_phase_align_filt_process(
1281 ia_ph_alignment_filt_struct* ph_alignment_filt, WORD32 channel,
1282 FLOAT32* ptr_audio_out) {
1283 FLOAT32 audio_in = *ptr_audio_out;
1284 FLOAT32 inp = audio_in;
1285 FLOAT32 out = inp;
1286
1287 impd_process_filt_sect(ph_alignment_filt->filt_section, channel, &out,
1288 ph_alignment_filt->section_count);
1289 inp = out;
1290
1291 impd_audio_delay_process(&ph_alignment_filt->audio_delay, channel, inp, &out);
1292
1293 *ptr_audio_out = out * ph_alignment_filt->gain;
1294 return;
1295 }
1296
impd_eq_filt_element_process(ia_eq_filt_block_struct str_eq_filt_block[EQ_FILTER_BLOCK_COUNT_MAX],WORD32 channel,FLOAT32 audio_in,FLOAT32 * ptr_audio_out,WORD32 block_count)1297 VOID impd_eq_filt_element_process(
1298 ia_eq_filt_block_struct str_eq_filt_block[EQ_FILTER_BLOCK_COUNT_MAX],
1299 WORD32 channel, FLOAT32 audio_in, FLOAT32* ptr_audio_out,
1300 WORD32 block_count) {
1301 WORD32 i;
1302 FLOAT32 inp = audio_in;
1303 FLOAT32 out = inp;
1304 WORD32 k, j;
1305 WORD32 element_count;
1306 for (j = 0; j < block_count; j++) {
1307 FLOAT32 sum = 0.0f;
1308 element_count = str_eq_filt_block[j].element_count;
1309 for (k = 0; k < element_count; k++) {
1310 switch (str_eq_filt_block[j].eq_filt_element[k].format) {
1311 case FILTER_ELEMENT_FORMAT_POLE_ZERO:
1312 impd_pole_zero_filt_process(
1313 &str_eq_filt_block[j].eq_filt_element[k].pstr_pole_zero_filt,
1314 channel, inp, &out);
1315 break;
1316 case FILTER_ELEMENT_FORMAT_FIR:
1317 impd_fir_filt_process(
1318 &str_eq_filt_block[j].eq_filt_element[k].fir_filter, channel, inp,
1319 &out);
1320 break;
1321 default:
1322 break;
1323 }
1324 out *= str_eq_filt_block[j].eq_filt_element[k].elementGainLinear;
1325
1326 for (i = 0; i < str_eq_filt_block[j].eq_filt_element[k].num_ph_align_filt;
1327 i++) {
1328 inp = out;
1329 impd_phase_align_filt_process(
1330 &str_eq_filt_block[j].eq_filt_element[k].ph_alignment_filt[i],
1331 channel, &out);
1332 }
1333 sum += out;
1334 }
1335 inp = sum;
1336 }
1337 *ptr_audio_out = inp;
1338 return;
1339 }
1340
impd_process_eq_set_time_domain(ia_eq_set_struct * pstr_eq_set,WORD32 channel,FLOAT32 * ptr_audio_in,FLOAT32 * ptr_audio_out,WORD32 frame_size)1341 WORD32 impd_process_eq_set_time_domain(ia_eq_set_struct* pstr_eq_set,
1342 WORD32 channel, FLOAT32* ptr_audio_in,
1343 FLOAT32* ptr_audio_out,
1344 WORD32 frame_size) {
1345 WORD32 i, j, g = 0;
1346
1347 if (pstr_eq_set == NULL) return 0;
1348
1349 g = pstr_eq_set->eq_ch_group_of_channel[channel];
1350
1351 if (g < 0) return 0;
1352
1353 for (i = 0; i < frame_size; i++) {
1354 impd_eq_filt_element_process(
1355 (pstr_eq_set->filt_cascade_td[g].pstr_eq_filt_block), channel,
1356 ptr_audio_in[i], &ptr_audio_out[i],
1357 pstr_eq_set->filt_cascade_td[g].block_count);
1358
1359 for (j = 0; j < pstr_eq_set->filt_cascade_td[g].num_ph_align_filt; j++) {
1360 impd_phase_align_filt_process(
1361 &pstr_eq_set->filt_cascade_td[g].ph_alignment_filt[j], channel,
1362 &ptr_audio_out[i]);
1363 }
1364
1365 ptr_audio_out[i] =
1366 ptr_audio_out[i] * pstr_eq_set->filt_cascade_td[g].cascade_gain_linear;
1367 }
1368 return 0;
1369 }
1370
impd_process_eq_set_subband_domain(ia_eq_set_struct * pstr_eq_set,WORD32 channel,FLOAT32 * ptr_audio_real_buff,FLOAT32 * ptr_audio_imag_buff)1371 WORD32 impd_process_eq_set_subband_domain(ia_eq_set_struct* pstr_eq_set,
1372 WORD32 channel,
1373 FLOAT32* ptr_audio_real_buff,
1374 FLOAT32* ptr_audio_imag_buff) {
1375 WORD32 g;
1376
1377 if (pstr_eq_set != NULL) {
1378 g = pstr_eq_set->eq_ch_group_of_channel[channel];
1379 if (g >= 0) {
1380 if (pstr_eq_set->domain == 0) {
1381 return (-1);
1382 } else {
1383 impd_subband_filter_process(&pstr_eq_set->subband_filt[g],
1384 &ptr_audio_real_buff[0],
1385 &ptr_audio_imag_buff[0]);
1386 }
1387 }
1388 }
1389 return (0);
1390 }
1391