• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2  * Copyright (c) 2025 Huawei Device Co., Ltd.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15 
16 #include <stdlib.h>
17 #include <math.h>
18 #include <limits.h>
19 #include <stdint.h>
20 #include "audio_proresampler_process.h"
21 #include "audio_engine_log.h"
22 #include "securec.h"
23 
24 /**
25  * @brief Ratio of circumference to diameter
26  *
27  */
28 #ifndef M_PI
29 #define M_PI 3.14159265358979323846
30 #endif
31 
32 /**
33  * @brief Maximum number of numChannels
34  *
35  */
36 #define MAX_NUM_CHANNEL 16
37 
38 
39 #ifndef NULL
40 #define NULL 0
41 #endif
42 
43 #ifndef MONO
44 #define MONO 1
45 #endif
46 
47 #ifndef STEREO
48 #define STEREO 2
49 #endif
50 
51 #define TWO_STEPS 2
52 #define THREE_STEPS 3
53 #define FOUR_STEPS 4
54 #define QUALITY_LEVEL_TEN 10
55 #define BUFFER_SIZE 160
56 
57 // WARNING: Code for support to sudden changes in sampling frequency is deprecated!
58 // It is disabled because it is complex and untested.
59 // It is desirable to re-initialize for such a change in the sampling frequency.
60 
61 /**
62  * @brief A function that compares two values and returns the smaller one
63  *
64  */
65 
CompareMin(uint32_t a,uint32_t b)66 static inline uint32_t CompareMin(uint32_t a, uint32_t b)
67 {
68     return a < b ? a : b;
69 }
70 
71 /**
72  * @brief A function that compares two values and returns the larger one
73  *
74  */
CompareMax(uint32_t a,uint32_t b)75 static inline uint32_t CompareMax(uint32_t a, uint32_t b)
76 {
77     return a < b ? b : a;
78 }
79 /**
80  * @brief A function used to free memory if some error ocurrs and it's no longer used, preventing memory leak.
81  *
82  */
FreeMemoryAndReturnError(void * memory)83 static inline int32_t FreeMemoryAndReturnError(void *memory)
84 {
85     CHECK_AND_RETURN_RET_LOG(memory, RESAMPLER_ERR_ALLOC_FAILED, "no need to fail memory");
86     free(memory);
87     return RESAMPLER_ERR_ALLOC_FAILED;
88 }
89 
90 /**
91  * @brief A function that helps update resampler state
92  *
93 */
94 static int32_t UpdateFilterMemory(SingleStagePolyphaseResamplerState* state, uint32_t oldFilterLength);
95 struct QualityTable {
96     int32_t filterLength;
97     float coshParameter;
98 };
99 
100 
101 /**
102  * @brief This table contains internal parameters corresponding to quality levels.
103  *
104  * The relationship between coshParameter and side lobe decay is as follows:
105  * coshParameter = -8.722e-5 * attenuation^TWO_STEPS + 0.1335 * attenuation - 1.929 (50 < attenuation)
106  */
107 static const struct QualityTable QUALITY_TABLE[11] = {
108     {  8, 5.767008}, /* Q0 */
109     { 16, 5.767008}, /* Q1 */
110     { 32, 5.767008}, /* Q2 */ /*  ( ~60 dB stop) 6  */
111     { 48, 8.192792}, /* Q3 */ /*  ( ~80 dB stop) 8  */
112     { 64, 8.192792}, /* Q4 */ /*  ( ~80 dB stop) 8  */
113     { 80, 10.5488},  /* Q5 */ /*  (~100 dB stop) 10 */
114     { 96, 10.5488},  /* Q6 */ /*  (~100 dB stop) 10 */
115     {128, 10.5488},  /* Q7 */ /*  (~100 dB stop) 10 */
116     {160, 10.5488},  /* Q8 */ /*  (~100 dB stop) 10 */
117     {192, 10.5488},  /* Q9 */ /*  (~100 dB stop) 10 */
118     {256, 10.5488}, /* Q10 */ /*  (~100 dB stop) 10 */
119 };
120 
121 
CompHyperbolicCosineWindow(double x,float alpha)122 static double CompHyperbolicCosineWindow(double x, float alpha)
123 {
124     double x2;
125     double w;
126 
127     x2 = x * x;
128     if (x2 >= 1.0) {
129         return 0.f;
130     }
131 
132     w = alpha * sqrt(1.0f - x2);
133     w = cosh(w) / cosh(alpha);
134     return w;
135 }
136 
137 
Sinc(float x)138 static float Sinc(float x)
139 {
140     if (fabs(x) < 1e-6) {
141         return 1.0f;
142     }
143     return sin(M_PI * x) / (M_PI * x);
144 }
145 
146 
CalculateFilter(SingleStagePolyphaseResamplerState * state)147 static int32_t CalculateFilter(SingleStagePolyphaseResamplerState* state)
148 {
149     uint32_t i;
150     uint32_t j;
151     float phi0 = 0;
152     float phi = 0;
153     double w;
154     uint32_t requiredFilterCoefficientsSize;
155     float cutoff = state->cutoff;
156 
157     if (INT_MAX / sizeof(float) / state->interpolateFactor < state->filterLength) {
158         return RESAMPLER_ERR_ALLOC_FAILED;
159     }
160 
161     requiredFilterCoefficientsSize = state->filterLength * state->interpolateFactor;
162 
163     if (state->filterCoefficientsSize < requiredFilterCoefficientsSize) {
164         if (state->filterCoefficients == NULL) {
165             state->filterCoefficients = (float*)malloc(requiredFilterCoefficientsSize * sizeof(float));
166             CHECK_AND_RETURN_RET_LOG(state->filterCoefficients, RESAMPLER_ERR_ALLOC_FAILED,
167                 "malloc state->filterCoefficients fail!");
168         } else {
169             float* filterCoefficients = (float*)malloc(requiredFilterCoefficientsSize * sizeof(float));
170             CHECK_AND_RETURN_RET_LOG(filterCoefficients, RESAMPLER_ERR_ALLOC_FAILED, "malloc filterCoefficients fail!");
171             int32_t ret = memcpy_s(filterCoefficients, requiredFilterCoefficientsSize * sizeof(float),
172                 state->filterCoefficients, state->filterCoefficientsSize * sizeof(float));
173             CHECK_AND_RETURN_RET_LOG(ret == RESAMPLER_ERR_SUCCESS, FreeMemoryAndReturnError(filterCoefficients),
174                 "memcpy_s filterCoefficients fail with error code %{public}d", ret);
175             free(state->filterCoefficients);
176             state->filterCoefficients = filterCoefficients;
177         }
178         state->filterCoefficientsSize = requiredFilterCoefficientsSize;
179     }
180 
181     for (i = 0; i < state->interpolateFactor; i++) {
182         for (j = 0; j < state->filterLength; j++) {
183             phi = ((int32_t)j - (int32_t)state->filterLength / TWO_STEPS + 1) - phi0;
184             w = CompHyperbolicCosineWindow(fabs((double)TWO_STEPS * phi / state->filterLength),
185                 state->coshParameter);
186             state->filterCoefficients[i * state->filterLength + j] = w * cutoff * Sinc(cutoff * phi);
187         }
188         phi0 += 1.0 / state->interpolateFactor;
189     }
190     return 0;
191 }
192 
193 /*====== Filter multiplication function for general cases =====*/
MultiplyFilterMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)194 static void MultiplyFilterMono(SingleStagePolyphaseResamplerState* state, const float* coeffs,
195     const float* inputs, float* outputs, int32_t subfilterNum)
196 {
197     float sum = 0;
198 
199     for (uint32_t j = 0; j < state->filterLength; j += FOUR_STEPS) {
200         sum += (*coeffs++) * (*inputs++);
201         sum += (*coeffs++) * (*inputs++);
202         sum += (*coeffs++) * (*inputs++);
203         sum += (*coeffs++) * (*inputs++);
204     }
205     *outputs = sum;
206 }
207 
MultiplyFilterStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)208 static void MultiplyFilterStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
209     const float* inputs, float* outputs, int32_t subfilterNum)
210 {
211     float sumL = 0;
212     float sumR = 0;
213     float h;
214 
215     for (uint32_t j = 0; j < state->filterLength; j += FOUR_STEPS) {
216         h = *coeffs++;
217         sumL += h * (*inputs++);
218         sumR += h * (*inputs++);
219 
220         h = *coeffs++;
221         sumL += h * (*inputs++);
222         sumR += h * (*inputs++);
223 
224         h = *coeffs++;
225         sumL += h * (*inputs++);
226         sumR += h * (*inputs++);
227 
228         h = *coeffs++;
229         sumL += h * (*inputs++);
230         sumR += h * (*inputs++);
231     }
232     *outputs++ = sumL;
233     *outputs = sumR;
234 }
235 
MultiplyFilterMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)236 static void MultiplyFilterMultichannel(SingleStagePolyphaseResamplerState* state, const float* coeffs,
237     const float* inputs, float* outputs, int32_t subfilterNum)
238 {
239     const uint32_t numChannels = state->numChannels;
240     uint32_t ch;
241     uint32_t j;
242     float h;
243     float sum[MAX_NUM_CHANNEL];
244 
245     for (ch = 0; ch < numChannels; ch++) {
246         sum[ch] = 0;
247     }
248     for (j = 0; j < state->filterLength; j += FOUR_STEPS) {
249         h = *coeffs++;
250         for (ch = 0; ch < numChannels; ch++) {
251             sum[ch] += h * (*inputs++);
252         }
253         h = *coeffs++;
254         for (ch = 0; ch < numChannels; ch++) {
255             sum[ch] += h * (*inputs++);
256         }
257         h = *coeffs++;
258         for (ch = 0; ch < numChannels; ch++) {
259             sum[ch] += h * (*inputs++);
260         }
261         h = *coeffs++;
262         for (ch = 0; ch < numChannels; ch++) {
263             sum[ch] += h * (*inputs++);
264         }
265     }
266     for (ch = 0; ch < numChannels; ch++) {
267         *outputs++ = sum[ch];
268     }
269 }
270 
271 /*===== Filter multiplication function for coarse (integral) upsampling =====*/
MultiplyFilterSymmetricOddUpMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)272 static void MultiplyFilterSymmetricOddUpMono(SingleStagePolyphaseResamplerState* state, const float* coeffs,
273     const float* inputs, float* outputs, int32_t subfilterNum)
274 {
275     *outputs = inputs[state->filterLength / TWO_STEPS - 1];
276 }
277 
MultiplyFilterSymmetricOddUpStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)278 static void MultiplyFilterSymmetricOddUpStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
279     const float* inputs, float* outputs, int32_t subfilterNum)
280 {
281     *outputs++ = inputs[state->filterLength - TWO_STEPS];
282     *outputs = inputs[state->filterLength - 1];
283 }
284 
MultiplyFilterSymmetricOddUpMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)285 static void MultiplyFilterSymmetricOddUpMultichannel(SingleStagePolyphaseResamplerState* state, const float* coeffs,
286     const float* inputs, float* outputs, int32_t subfilterNum)
287 {
288     const uint32_t indCenter = state->filterLength / TWO_STEPS - 1;
289     for (uint32_t ch = 0; ch < state->numChannels; ch++) {
290         *outputs++ = inputs[state->numChannels * indCenter + ch];
291     }
292 }
293 
MultiplyFilterSymmetricEvenUpMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)294 static void MultiplyFilterSymmetricEvenUpMono(SingleStagePolyphaseResamplerState* state, const float* coeffs,
295     const float* inputs, float* outputs, int32_t subfilterNum)
296 {
297     const uint32_t n = state->filterLength;
298     float sum = 0;
299 
300     for (uint32_t j = 0; j < n / TWO_STEPS; j += FOUR_STEPS) {
301         sum += (*coeffs++) * (inputs[j] + inputs[(n - j - 1)]);
302         sum += (*coeffs++) * (inputs[j + 1] + inputs[(n - (j + 1) - 1)]);
303         sum += (*coeffs++) * (inputs[j + TWO_STEPS] + inputs[(n - (j + TWO_STEPS) - 1)]);
304         sum += (*coeffs++) * (inputs[j + THREE_STEPS] + inputs[(n - (j + THREE_STEPS) - 1)]);
305     }
306     *outputs = sum;
307 }
308 
MultiplyFilterSymmetricEvenUpStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)309 static void MultiplyFilterSymmetricEvenUpStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
310     const float* inputs, float* outputs, int32_t subfilterNum)
311 {
312     const uint32_t n = state->filterLength;
313     float sumL = 0;
314     float sumR = 0;
315     float h;
316 
317     for (uint32_t j = 0; j < n / TWO_STEPS; j += FOUR_STEPS) {
318         h = *coeffs++;
319         sumL += h * (inputs[j * STEREO] + inputs[(n - j - 1) * STEREO]);
320         sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - 1) * STEREO + 1]);
321         h = *coeffs++;
322         sumL += h * (inputs[(j + 1) * STEREO] + inputs[(n - (j + 1) - 1) * STEREO]);
323         sumR += h * (inputs[(j + 1) * STEREO + 1] + inputs[(n - (j + 1) - 1) * STEREO + 1]);
324         h = *coeffs++;
325         sumL += h * (inputs[(j + TWO_STEPS) * STEREO] + inputs[(n - (j + TWO_STEPS) - 1) * STEREO]);
326         sumR += h * (inputs[(j + TWO_STEPS) * STEREO + 1] + inputs[(n - (j + TWO_STEPS) - 1) * STEREO + 1]);
327         h = *coeffs++;
328         sumL += h * (inputs[(j + THREE_STEPS) * STEREO] + inputs[(n - (j + THREE_STEPS) - 1) * STEREO]);
329         sumR += h * (inputs[(j + THREE_STEPS) * STEREO + 1] + inputs[(n - (j + THREE_STEPS) - 1) * STEREO + 1]);
330     }
331     *outputs++ = sumL;
332     *outputs++ = sumR;
333 }
334 
MultiplyFilterSymmetricEvenUpMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)335 static void MultiplyFilterSymmetricEvenUpMultichannel(SingleStagePolyphaseResamplerState* state, const float* coeffs,
336     const float* inputs, float* outputs, int32_t subfilterNum)
337 {
338     const uint32_t n = state->filterLength;
339     const uint32_t numChannels = state->numChannels;
340     uint32_t ch;
341     uint32_t j;
342     float sum[MAX_NUM_CHANNEL];
343     float h;
344 
345     for (ch = 0; ch < numChannels; ch++) {
346         sum[ch] = 0;
347     }
348     for (j = 0; j < n / TWO_STEPS; j++) {
349         h = *coeffs++;
350         for (ch = 0; ch < numChannels; ch++) {
351             sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - 1) * numChannels + ch]);
352         }
353     }
354     for (ch = 0; ch < numChannels; ch++) {
355         *outputs++ = sum[ch];
356     }
357 }
358 
MultiplyFilterSymmetricOddDownMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)359 static void MultiplyFilterSymmetricOddDownMono(SingleStagePolyphaseResamplerState* state, const float* coeffs,
360     const float* inputs, float* outputs, int32_t subfilterNum)
361 {
362     const uint32_t n = state->filterLength;
363     const uint32_t indCenter = n / TWO_STEPS - 1;
364     const uint32_t decimateFactor = state->decimateFactor;
365     float sum;
366     uint32_t rem = indCenter % decimateFactor;
367     uint32_t len = indCenter / decimateFactor;
368     uint32_t i;
369     uint32_t j;
370     uint32_t l;
371     // center index
372     sum = coeffs[indCenter] * inputs[indCenter];
373     // symmetric indices
374     for (j = 0; j < rem; j++) {
375         sum += (*coeffs++) * (inputs[j] + inputs[(n - j - TWO_STEPS)]);
376     }
377     for (l = 0; l < len; l++) {
378         coeffs++;
379         j++;
380         for (i = 1; i < decimateFactor; i++) {
381             sum += (*coeffs++) * (inputs[j] + inputs[(n - j - TWO_STEPS)]);
382             j++;
383         }
384     }
385 
386     *outputs = sum;
387 }
388 
MultiplyFilterSymmetricOddDownStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)389 static void MultiplyFilterSymmetricOddDownStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
390     const float* inputs, float* outputs, int32_t subfilterNum)
391 {
392     const uint32_t n = state->filterLength;
393     const uint32_t indCenter = n / TWO_STEPS - 1;
394     const uint32_t decimateFactor = state->decimateFactor;
395     float sumL;
396     float sumR;
397     float h;
398     uint32_t rem = indCenter % decimateFactor;
399     uint32_t len = indCenter / decimateFactor;
400     uint32_t i;
401     uint32_t j;
402     uint32_t l;
403 
404     // center
405     h = coeffs[indCenter];
406     sumL = h * inputs[n - TWO_STEPS];
407     sumR = h * inputs[n - 1];
408     // symmetric indices
409     for (j = 0; j < rem; j++) {
410         h = *coeffs++;
411         sumL += h * (inputs[j * STEREO] + inputs[(n - j - TWO_STEPS) * STEREO]);
412         sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - TWO_STEPS) * STEREO + 1]);
413     }
414     for (l = 0; l < len; l++) {
415         coeffs++;
416         j++;
417         for (i = 1; i < decimateFactor; i++) {
418             h = *coeffs++;
419             sumL += h * (inputs[j * STEREO] + inputs[(n - j - TWO_STEPS) * STEREO]);
420             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - TWO_STEPS) * STEREO + 1]);
421             j++;
422         }
423     }
424 
425     *outputs++ = sumL;
426     *outputs = sumR;
427 }
428 
MultiplyFilterSymmetricOddDownMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)429 static void MultiplyFilterSymmetricOddDownMultichannel(SingleStagePolyphaseResamplerState* state, const float* coeffs,
430     const float* inputs, float* outputs, int32_t subfilterNum)
431 {
432     const uint32_t n = state->filterLength;
433     const uint32_t numChannels = state->numChannels;
434     const uint32_t indCenter = n / TWO_STEPS - 1;
435     const uint32_t decimateFactor = state->decimateFactor;
436     uint32_t i;
437     uint32_t j;
438     uint32_t l;
439     uint32_t ch;
440     float sum[MAX_NUM_CHANNEL];
441     float h;
442     uint32_t rem = indCenter % decimateFactor;
443     uint32_t len = indCenter / decimateFactor;
444 
445     // center index
446     h = coeffs[indCenter];
447     for (ch = 0; ch < numChannels; ch++) {
448         sum[ch] = h * (inputs[indCenter * numChannels + ch]);
449     }
450     // symmetric indices
451     for (j = 0; j < rem; j++) {
452         h = *coeffs++;
453         for (ch = 0; ch < numChannels; ch++) {
454             sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - TWO_STEPS) * numChannels + ch]);
455         }
456     }
457     for (l = 0; l < len; l++) {
458         coeffs++;
459         j++;
460         for (i = 1; i < decimateFactor; i++) {
461             h = *coeffs++;
462             for (ch = 0; ch < numChannels; ch++) {
463                 sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - TWO_STEPS) * numChannels + ch]);
464             }
465             j++;
466         }
467     }
468 
469     for (ch = 0; ch < numChannels; ch++) {
470         *outputs++ = sum[ch];
471     }
472 }
473 
474 
MultiplyFilterSymmetricEvenDownMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)475 static void MultiplyFilterSymmetricEvenDownMono(SingleStagePolyphaseResamplerState* state, const float* coeffs,
476     const float* inputs, float* outputs, int32_t subfilterNum)
477 {
478     const uint32_t n = state->filterLength;
479     const uint32_t decimateFactor = state->decimateFactor;
480     uint32_t i;
481     uint32_t j;
482     uint32_t l;
483     uint32_t rem = (n / TWO_STEPS) % decimateFactor;
484     uint32_t len = (n / TWO_STEPS) / decimateFactor;
485     float h;
486 
487     float sum = 0;
488 
489     // symmetric indices
490     for (j = 0; j < rem; j++) {
491         h = *coeffs++;
492         sum += h * (inputs[j] + inputs[(n - j - 1)]);
493     }
494     for (l = 0; l < len; l++) {
495         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
496             h = *coeffs++;
497             sum += h * (inputs[j] + inputs[(n - j - 1)]);
498             j++;
499         }
500         // Skip zero
501         coeffs++;
502         j++;
503         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
504             h = *coeffs++;
505             sum += h * (inputs[j] + inputs[(n - j - 1)]);
506             j++;
507         }
508     }
509     *outputs = sum;
510 }
511 
MultiplyFilterSymmetricEvenDownStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)512 static void MultiplyFilterSymmetricEvenDownStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
513     const float* inputs, float* outputs, int32_t subfilterNum)
514 {
515     const uint32_t n = state->filterLength;
516     const uint32_t decimateFactor = state->decimateFactor;
517     uint32_t i;
518     uint32_t j;
519     uint32_t l;
520     uint32_t rem = (n / TWO_STEPS) % decimateFactor;
521     uint32_t len = (n / TWO_STEPS) / decimateFactor;
522     float h;
523 
524     float sumL = 0;
525     float sumR = 0;
526 
527     // symmetric indices
528     for (j = 0; j < rem; j++) {
529         h = *coeffs++;
530         sumL += h * (inputs[j * STEREO] + inputs[(n - j - 1) * STEREO]);
531         sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - 1) * STEREO + 1]);
532     }
533     for (l = 0; l < len; l++) {
534         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
535             h = *coeffs++;
536             sumL += h * (inputs[j * STEREO] + inputs[(n - j - 1) * STEREO]);
537             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - 1) * STEREO + 1]);
538             j++;
539         }
540         // Skip zero
541         coeffs++;
542         j++;
543         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
544             h = *coeffs++;
545             sumL += h * (inputs[j * STEREO] + inputs[(n - j - 1) * STEREO]);
546             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - 1) * STEREO + 1]);
547             j++;
548         }
549     }
550     *outputs++ = sumL;
551     *outputs++ = sumR;
552 }
553 
554 
MultiplyFilterSymmetricEvenDownMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)555 static void MultiplyFilterSymmetricEvenDownMultichannel(SingleStagePolyphaseResamplerState* state, const float* coeffs,
556     const float* inputs, float* outputs, int32_t subfilterNum)
557 {
558     const uint32_t n = state->filterLength;
559     const uint32_t numChannels = state->numChannels;
560     const uint32_t decimateFactor = state->decimateFactor;
561     uint32_t i;
562     uint32_t j;
563     uint32_t l;
564     uint32_t ch;
565     float sum[MAX_NUM_CHANNEL];
566     float h;
567     uint32_t rem = (n / TWO_STEPS) % decimateFactor;
568     uint32_t len = (n / TWO_STEPS) / decimateFactor;
569 
570     for (ch = 0; ch < numChannels; ch++) {
571         sum[ch] = 0;
572     }
573     // symmetric indices
574     for (j = 0; j < rem; j++) {
575         h = *coeffs++;
576         for (ch = 0; ch < numChannels; ch++) {
577             sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - 1) * numChannels + ch]);
578         }
579     }
580     for (l = 0; l < len; l++) {
581         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
582             h = *coeffs++;
583             for (ch = 0; ch < numChannels; ch++) {
584                 sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - 1) * numChannels + ch]);
585             }
586             j++;
587         }
588         // Skip zero
589         coeffs++;
590         j++;
591         for (i = 0; i < decimateFactor / TWO_STEPS; i++) {
592             h = *coeffs++;
593             for (ch = 0; ch < numChannels; ch++) {
594                 sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - 1) * numChannels + ch]);
595             }
596             j++;
597         }
598     }
599 
600     for (ch = 0; ch < numChannels; ch++) {
601         *outputs++ = sum[ch];
602     }
603 }
604 
MultiplyFilterDownMono(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)605 static void MultiplyFilterDownMono(SingleStagePolyphaseResamplerState *state, const float *coeffs,
606     const float *inputs, float *outputs, int32_t subfilterNum)
607 {
608     const uint32_t n = state->filterLength;
609     const uint32_t indCenter = n / TWO_STEPS - 1;
610     const uint32_t decimateFactor = state->decimateFactor;
611     uint32_t j;
612     float h;
613     float sum = 0;
614 
615     uint32_t counts = indCenter % decimateFactor - (uint32_t)subfilterNum;
616     if (counts < 0) {
617         counts += state->interpolateFactor;
618     }
619 
620     for (j = 0; j < n; j++) {
621         h = *coeffs++;
622         if (counts == 0) { // Skip zero coefficients
623             counts = decimateFactor - 1;
624             inputs++;
625             continue;
626         }
627         sum += h * (*inputs++);
628         counts--;
629     }
630     *outputs = sum;
631 }
632 
MultiplyFilterDownStereo(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)633 static void MultiplyFilterDownStereo(SingleStagePolyphaseResamplerState* state, const float* coeffs,
634     const float* inputs, float* outputs, int32_t subfilterNum)
635 {
636     const uint32_t n = state->filterLength;
637     const uint32_t indCenter = n / TWO_STEPS - 1;
638     const uint32_t decimateFactor = state->decimateFactor;
639     uint32_t j;
640     float sumL = 0;
641     float sumR = 0;
642     float h;
643 
644     uint32_t counts = indCenter % decimateFactor - (uint32_t)subfilterNum;
645     if (counts < 0) {
646         counts += state->interpolateFactor;
647     }
648 
649     for (j = 0; j < n; j++) {
650         h = *coeffs++;
651         if (counts == 0) { // Skip zero coefficients
652             counts = decimateFactor - 1;
653             inputs += STEREO;
654             continue;
655         }
656         sumL += h * (*inputs++);
657         sumR += h * (*inputs++);
658         counts--;
659     }
660     *outputs++ = sumL;
661     *outputs = sumR;
662 }
663 
MultiplyFilterDownMultichannel(SingleStagePolyphaseResamplerState * state,const float * coeffs,const float * inputs,float * outputs,int32_t subfilterNum)664 static void MultiplyFilterDownMultichannel(SingleStagePolyphaseResamplerState *state, const float *coeffs,
665     const float *inputs, float *outputs, int32_t subfilterNum)
666 {
667     const uint32_t n = state->filterLength;
668     const uint32_t indCenter = n / TWO_STEPS - 1;
669     const uint32_t decimateFactor = state->decimateFactor;
670     const uint32_t numChannels = state->numChannels;
671     uint32_t j;
672     uint32_t ch;
673     float h;
674     float sum[MAX_NUM_CHANNEL];
675 
676     uint32_t counts = indCenter % decimateFactor - (uint32_t)subfilterNum;
677     if (counts < 0) {
678         counts += state->interpolateFactor;
679     }
680 
681     for (ch = 0; ch < numChannels; ch++) {
682         sum[ch] = 0;
683     }
684     for (j = 0; j < n; j++) {
685         h = *coeffs++;
686         if (counts == 0) { // Skip zero coefficients
687             counts = decimateFactor - 1;
688             inputs += numChannels;
689             continue;
690         }
691         for (ch = 0; ch < numChannels; ch++) {
692             sum[ch] += h * (*inputs++);
693         }
694         counts--;
695     }
696     for (ch = 0; ch < numChannels; ch++) {
697         *outputs++ = sum[ch];
698     }
699 }
700 
701 
702 /*===== Function pointers of filter multiplication functions =====*/
703 static MultiplyFilterFun multiplyFilterFunTable[] = {
704     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_UP]          = MultiplyFilterSymmetricOddUpMono,
705     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_UP + 1]      = MultiplyFilterSymmetricOddUpStereo,
706     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_UP + TWO_STEPS] = MultiplyFilterSymmetricOddUpMultichannel,
707     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_UP]         = MultiplyFilterSymmetricEvenUpMono,
708     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_UP + 1]     = MultiplyFilterSymmetricEvenUpStereo,
709     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_UP + TWO_STEPS] = MultiplyFilterSymmetricEvenUpMultichannel,
710     [THREE_STEPS * MULTIPLY_FILTER_FUN_UP]                        = MultiplyFilterMono,
711     [THREE_STEPS * MULTIPLY_FILTER_FUN_UP + 1]                    = MultiplyFilterStereo,
712     [THREE_STEPS * MULTIPLY_FILTER_FUN_UP + TWO_STEPS]            = MultiplyFilterMultichannel,
713     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_DOWN]        = MultiplyFilterSymmetricOddDownMono,
714     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_DOWN + 1]    = MultiplyFilterSymmetricOddDownStereo,
715     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_DOWN + TWO_STEPS] =
716         MultiplyFilterSymmetricOddDownMultichannel,
717     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_DOWN]       = MultiplyFilterSymmetricEvenDownMono,
718     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_DOWN + 1]   = MultiplyFilterSymmetricEvenDownStereo,
719     [THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_DOWN + TWO_STEPS] =
720         MultiplyFilterSymmetricEvenDownMultichannel,
721     [THREE_STEPS * MULTIPLY_FILTER_FUN_DOWN]                      = MultiplyFilterDownMono,
722     [THREE_STEPS * MULTIPLY_FILTER_FUN_DOWN + 1]                  = MultiplyFilterDownStereo,
723     [THREE_STEPS * MULTIPLY_FILTER_FUN_DOWN + TWO_STEPS]          = MultiplyFilterDownMultichannel
724 };
725 
PolyphaseResamplerMono(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)726 static int32_t PolyphaseResamplerMono(SingleStagePolyphaseResamplerState *state, const float *in, uint32_t *inputLength,
727     float *out, uint32_t *outputLength)
728 {
729     const uint32_t n = state->filterLength;
730     uint32_t outSample = 0;
731     uint32_t inputIndex = state->inputIndex;
732     uint32_t subfilterNum = state->subfilterNum;
733     const float* filterCoefficients = state->filterCoefficients;
734     const uint32_t quoSamplerateRatio = state->quoSamplerateRatio;
735     const uint32_t remSamplerateRatio = state->remSamplerateRatio;
736     const uint32_t decimateFactor = state->decimateFactor;
737     const uint32_t interpolateFactor = state->interpolateFactor;
738     uint32_t i;
739 
740     if (inputIndex < (uint32_t)(*inputLength)) {
741         outSample = CompareMin((*outputLength), ((interpolateFactor * ((*inputLength) - inputIndex) -
742             subfilterNum) - 1) / decimateFactor + 1);
743     }
744 
745     for (i = 0; i < outSample; i++) {
746         const float* coeffs = &filterCoefficients[subfilterNum * n];
747         const float* inputs = &in[inputIndex * MONO];
748         MultiplyFilterMono(state, coeffs, inputs, out, subfilterNum);
749         out++;
750 
751         inputIndex += quoSamplerateRatio;
752         subfilterNum += remSamplerateRatio;
753         if (subfilterNum >= interpolateFactor) {
754             subfilterNum -= interpolateFactor;
755             inputIndex++;
756         }
757     }
758 
759     state->inputIndex = inputIndex;
760     state->subfilterNum = subfilterNum;
761     return outSample;
762 }
763 
PolyphaseResamplerStereo(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)764 static int32_t PolyphaseResamplerStereo(SingleStagePolyphaseResamplerState* state, const float* in,
765     uint32_t* inputLength, float* out, uint32_t* outputLength)
766 {
767     const uint32_t n = state->filterLength;
768     uint32_t outSample = 0;
769     uint32_t inputIndex = state->inputIndex;
770     uint32_t subfilterNum = state->subfilterNum;
771     const float* filterCoefficients = state->filterCoefficients;
772     const uint32_t quoSamplerateRatio = state->quoSamplerateRatio;
773     const uint32_t remSamplerateRatio = state->remSamplerateRatio;
774     const uint32_t decimateFactor = state->decimateFactor;
775     const uint32_t interpolateFactor = state->interpolateFactor;
776     uint32_t i;
777 
778     if (inputIndex < (uint32_t) (*inputLength)) {
779         outSample = CompareMin((*outputLength), ((interpolateFactor * ((*inputLength) - inputIndex) -
780             subfilterNum) - 1) / decimateFactor + 1);
781     }
782     for (i = 0; i < outSample; i++) {
783         const float* coeffs = &filterCoefficients[subfilterNum * n];
784         const float* inputs = &in[inputIndex * STEREO];
785         MultiplyFilterStereo(state, coeffs, inputs, out, subfilterNum);
786         out += STEREO;
787 
788         inputIndex += quoSamplerateRatio;
789         subfilterNum += remSamplerateRatio;
790         if (subfilterNum >= interpolateFactor) {
791             subfilterNum -= interpolateFactor;
792             inputIndex++;
793         }
794     }
795 
796     state->inputIndex = inputIndex;
797     state->subfilterNum = subfilterNum;
798     return outSample;
799 }
800 
PolyphaseResamplerMultichannel(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)801 static int32_t PolyphaseResamplerMultichannel(SingleStagePolyphaseResamplerState* state, const float* in,
802     uint32_t* inputLength, float* out, uint32_t* outputLength)
803 {
804     const uint32_t n = state->filterLength;
805     uint32_t outSample = 0;
806     uint32_t inputIndex = state->inputIndex;
807     uint32_t subfilterNum = state->subfilterNum;
808     const float* filterCoefficients = state->filterCoefficients;
809     const uint32_t quoSamplerateRatio = state->quoSamplerateRatio;
810     const uint32_t remSamplerateRatio = state->remSamplerateRatio;
811     const uint32_t decimateFactor = state->decimateFactor;
812     const uint32_t interpolateFactor = state->interpolateFactor;
813     const uint32_t numChannels = state->numChannels;
814     uint32_t i;
815 
816     if (inputIndex < (uint32_t)(*inputLength)) {
817         outSample = CompareMin((*outputLength), ((interpolateFactor *
818             ((*inputLength) - inputIndex) - subfilterNum) - 1) / decimateFactor + 1);
819     }
820 
821     for (i = 0; i < outSample; i++) {
822         const float* coeffs = &filterCoefficients[subfilterNum * n];
823         const float* inputs = &in[inputIndex * numChannels];
824         MultiplyFilterMultichannel(state, coeffs, inputs, out, subfilterNum);
825         out += numChannels;
826 
827         inputIndex += quoSamplerateRatio;
828         subfilterNum += remSamplerateRatio;
829         if (subfilterNum >= interpolateFactor) {
830             subfilterNum -= interpolateFactor;
831             inputIndex++;
832         }
833     }
834 
835     state->inputIndex = inputIndex;
836     state->subfilterNum = subfilterNum;
837     return outSample;
838 }
839 
PolyphaseDownsamplerHalfbandMono(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)840 static int32_t PolyphaseDownsamplerHalfbandMono(SingleStagePolyphaseResamplerState* state, const float* in,
841     uint32_t* inputLength, float* out, uint32_t* outputLength)
842 {
843     const uint32_t n = state->filterLength;
844     const uint32_t indCenter = n / TWO_STEPS - 1;
845     uint32_t outSample = 0;
846     uint32_t inputIndex = state->inputIndex;
847     const float* filterCoefficients = state->filterCoefficients;
848     float hCenter = filterCoefficients[indCenter];
849     uint32_t i;
850     uint32_t j;
851     float sum;
852 
853     if (inputIndex < (uint32_t)(*inputLength)) {
854         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / TWO_STEPS + 1);
855     }
856 
857     for (i = 0; i < outSample; i++) {
858         const float *coeffs = &filterCoefficients[0];
859 
860         const float *inputs = &in[inputIndex];
861         // center
862         sum = hCenter * inputs[indCenter];
863         // symmetric indices
864         for (j = 0; j < indCenter; j += TWO_STEPS) {
865             sum += (*coeffs) * (inputs[j] + inputs[(n - j - TWO_STEPS)]);
866             coeffs += TWO_STEPS;
867         }
868         *out++ = sum;
869         inputIndex += TWO_STEPS;
870     }
871 
872     state->inputIndex = inputIndex;
873     return outSample;
874 }
875 
PolyphaseDownsamplerHalfbandStereo(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)876 static int32_t PolyphaseDownsamplerHalfbandStereo(SingleStagePolyphaseResamplerState* state, const float* in,
877     uint32_t* inputLength, float* out, uint32_t* outputLength)
878 {
879     const uint32_t n = state->filterLength;
880     const uint32_t indCenter = n / TWO_STEPS - 1;
881     uint32_t outSample = 0;
882     uint32_t inputIndex = state->inputIndex;
883     const float* filterCoefficients = state->filterCoefficients;
884     float hCenter = filterCoefficients[indCenter];
885     uint32_t i;
886     uint32_t j;
887     float sumL;
888     float sumR;
889     float h;
890 
891     if (inputIndex < (uint32_t)(*inputLength)) {
892         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / TWO_STEPS + 1);
893     }
894 
895     for (i = 0; i < outSample; i++) {
896         const float* coeffs = &filterCoefficients[0];
897         const float* inputs = &in[inputIndex * STEREO];
898 
899         // center
900         sumL = hCenter * inputs[n - TWO_STEPS];
901         sumR = hCenter * inputs[n - 1];
902         // symmetric indices
903         for (j = 0; j < indCenter; j += TWO_STEPS) {
904             h = *coeffs;
905             sumL += h * (inputs[j * STEREO] + inputs[(n - j - TWO_STEPS) * STEREO]);
906             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - TWO_STEPS) * STEREO + 1]);
907             coeffs += TWO_STEPS;
908         }
909         *out++ = sumL;
910         *out++ = sumR;
911         inputIndex += TWO_STEPS;
912     }
913 
914     state->inputIndex = inputIndex;
915     return outSample;
916 }
917 
PolyphaseDownsamplerHalfbandMultichannel(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)918 static int32_t PolyphaseDownsamplerHalfbandMultichannel(SingleStagePolyphaseResamplerState* state, const float* in,
919     uint32_t* inputLength, float* out, uint32_t* outputLength)
920 {
921     const uint32_t n = state->filterLength;
922     const uint32_t indCenter = n / TWO_STEPS - 1;
923     uint32_t outSample = 0;
924     uint32_t inputIndex = state->inputIndex;
925     const float* filterCoefficients = state->filterCoefficients;
926     float hCenter = filterCoefficients[indCenter];
927     uint32_t i;
928     uint32_t j;
929     uint32_t ch;
930     float h;
931     float sum[MAX_NUM_CHANNEL];
932     const uint32_t numChannels = state->numChannels;
933 
934     if (inputIndex < (uint32_t)(*inputLength)) {
935         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / TWO_STEPS + 1);
936     }
937 
938     for (i = 0; i < outSample; i++) {
939         const float* coeffs = &filterCoefficients[0];
940         const float* inputs = &in[inputIndex * numChannels];
941 
942         // center
943         for (ch = 0; ch < numChannels; ch++) {
944             sum[ch] = hCenter * inputs[indCenter * numChannels + ch];
945         }
946         // symmetric indices
947         for (j = 0; j < indCenter; j += TWO_STEPS) {
948             h = *coeffs;
949             for (ch = 0; ch < numChannels; ch++) {
950                 sum[ch] += h * (inputs[j * numChannels + ch] + inputs[(n - j - TWO_STEPS) * numChannels + ch]);
951             }
952             coeffs += TWO_STEPS;
953         }
954         for (ch = 0; ch < numChannels; ch++) {
955             *out++ = sum[ch];
956         }
957         inputIndex += TWO_STEPS;
958     }
959 
960     state->inputIndex = inputIndex;
961     return outSample;
962 }
963 
PolyphaseDownsamplerThirdbandMono(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)964 static int32_t PolyphaseDownsamplerThirdbandMono(SingleStagePolyphaseResamplerState* state, const float* in,
965     uint32_t* inputLength, float* out, uint32_t* outputLength)
966 {
967     const uint32_t n = state->filterLength;
968     const uint32_t indCenter = n / TWO_STEPS - 1;
969     uint32_t outSample = 0;
970     uint32_t inputIndex = state->inputIndex;
971     const float* filterCoefficients = state->filterCoefficients;
972     uint32_t i;
973     uint32_t j;
974     float sum;
975     uint32_t rem = indCenter % THREE_STEPS;
976     float hCenter = filterCoefficients[indCenter];
977 
978     if (inputIndex < (uint32_t)(*inputLength)) {
979         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / THREE_STEPS + 1);
980     }
981 
982     for (i = 0; i < outSample; i++) {
983         const float* coeffs = &filterCoefficients[0];
984         const float* inputs = &in[inputIndex];
985 
986         // center
987         sum = hCenter * inputs[indCenter];
988         // symmetric indices
989         for (j = 0; j < rem; j++) {
990             sum += (*coeffs++) * (inputs[j] + inputs[(n - j - TWO_STEPS)]);
991         }
992         coeffs++;
993         for (j = rem + 1; j < indCenter; j += THREE_STEPS) {
994             sum += (*coeffs++) * (inputs[j] + inputs[(n - j - TWO_STEPS)]);
995             sum += (*coeffs++) * (inputs[(j + 1)] + inputs[(n - (j + 1) - TWO_STEPS)]);
996             coeffs++;
997         }
998         *out++ = sum;
999         inputIndex += THREE_STEPS;
1000     }
1001 
1002     state->inputIndex = inputIndex;
1003     return outSample;
1004 }
1005 
PolyphaseDownsamplerThirdbandStereo(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)1006 static int32_t PolyphaseDownsamplerThirdbandStereo(SingleStagePolyphaseResamplerState* state, const float* in,
1007     uint32_t* inputLength, float* out, uint32_t* outputLength)
1008 {
1009     const uint32_t n = state->filterLength;
1010     const uint32_t indCenter = n / TWO_STEPS - 1;
1011     uint32_t outSample = 0;
1012     uint32_t inputIndex = state->inputIndex;
1013     const float* filterCoefficients = state->filterCoefficients;
1014     uint32_t i;
1015     uint32_t j;
1016     float sumL;
1017     float sumR;
1018     float h;
1019 
1020     uint32_t rem = indCenter % THREE_STEPS;
1021     float hCenter = filterCoefficients[indCenter];
1022 
1023     if (inputIndex < (uint32_t)(*inputLength)) {
1024         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / THREE_STEPS + 1);
1025     }
1026 
1027     for (i = 0; i < outSample; i++) {
1028         const float* coeffs = &filterCoefficients[0];
1029         const float* inputs = &in[inputIndex * STEREO];
1030 
1031         // center
1032         sumL = hCenter * inputs[n - TWO_STEPS];
1033         sumR = hCenter * inputs[n - 1];
1034 
1035         // symmetric indices
1036         for (j = 0; j < rem; j++) {
1037             h = *coeffs++;
1038             sumL += h * (inputs[j * STEREO] + inputs[(n - j - TWO_STEPS) * STEREO]);
1039             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - TWO_STEPS) * STEREO + 1]);
1040         }
1041         coeffs++;
1042         for (j = rem + 1; j < indCenter; j += THREE_STEPS) {
1043             h = *coeffs++;
1044             sumL += h * (inputs[j * STEREO] + inputs[(n - j - TWO_STEPS) * STEREO]);
1045             sumR += h * (inputs[j * STEREO + 1] + inputs[(n - j - TWO_STEPS) * STEREO + 1]);
1046             h = *coeffs++;
1047             sumL += h * (inputs[(j + 1) * STEREO] + inputs[(n - (j + 1) - TWO_STEPS) * STEREO]);
1048             sumR += h * (inputs[(j + 1) * STEREO + 1] + inputs[(n - (j + 1) - TWO_STEPS) * STEREO + 1]);
1049             coeffs++;
1050         }
1051         *out++ = sumL;
1052         *out++ = sumR;
1053         inputIndex += THREE_STEPS;
1054     }
1055 
1056     state->inputIndex = inputIndex;
1057     return outSample;
1058 }
1059 
PolyphaseDownsamplerThirdbandMultichannel(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)1060 static int32_t PolyphaseDownsamplerThirdbandMultichannel(SingleStagePolyphaseResamplerState* state, const float* in,
1061     uint32_t* inputLength, float* out, uint32_t* outputLength)
1062 {
1063     const uint32_t n = state->filterLength;
1064     const uint32_t indCenter = n / TWO_STEPS - 1;
1065     uint32_t outSample = 0;
1066     uint32_t inputIndex = state->inputIndex;
1067     const float* filterCoefficients = state->filterCoefficients;
1068     uint32_t i;
1069     uint32_t j;
1070     uint32_t ch;
1071     float h1;
1072     float h2;
1073     float sum[MAX_NUM_CHANNEL];
1074     const uint32_t numChannels = state->numChannels;
1075     uint32_t rem = indCenter % THREE_STEPS;
1076     float hCenter = filterCoefficients[indCenter];
1077 
1078     if (inputIndex < (uint32_t)(*inputLength)) {
1079         outSample = CompareMin((*outputLength), (((*inputLength) - inputIndex) - 1) / THREE_STEPS + 1);
1080     }
1081 
1082     for (i = 0; i < outSample; i++) {
1083         const float* coeffs = &filterCoefficients[0];
1084         const float* inputs = &in[inputIndex * numChannels];
1085 
1086         // center
1087         for (ch = 0; ch < numChannels; ch++) {
1088             sum[ch] = hCenter * (inputs[indCenter * numChannels + ch]);
1089         }
1090 
1091         // symmetric indices
1092         for (j = 0; j < rem; j++) {
1093             h1 = *coeffs++;
1094             for (ch = 0; ch < numChannels; ch++) {
1095                 sum[ch] += h1 * (inputs[j * numChannels + ch] + inputs[(n - j - TWO_STEPS) * numChannels + ch]);
1096             }
1097         }
1098         coeffs++;
1099         for (j = rem + 1; j < indCenter; j += THREE_STEPS) {
1100             h1 = *coeffs++;
1101             h2 = *coeffs++;
1102             for (ch = 0; ch < numChannels; ch++) {
1103                 sum[ch] += h1 * (inputs[j * numChannels + ch] + inputs[(n - j - TWO_STEPS) * numChannels + ch]);
1104                 sum[ch] += h2 * (inputs[(j + 1) * numChannels + ch] + inputs[(n - (j + 1) - TWO_STEPS) *
1105                     numChannels + ch]);
1106             }
1107             coeffs++;
1108         }
1109         for (ch = 0; ch < numChannels; ch++) {
1110             *out++ = sum[ch];
1111         }
1112         inputIndex += THREE_STEPS;
1113     }
1114 
1115     state->inputIndex = inputIndex;
1116     return outSample;
1117 }
1118 
PolyphaseResamplerCoarse(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)1119 static int32_t PolyphaseResamplerCoarse(SingleStagePolyphaseResamplerState* state, const float* in,
1120     uint32_t* inputLength, float* out, uint32_t* outputLength)
1121 {
1122     const uint32_t n = state->filterLength;
1123     uint32_t outSample = 0;
1124     uint32_t inputIndex = state->inputIndex;
1125     uint32_t subfilterNum = state->subfilterNum;
1126     const float* filterCoefficients = state->filterCoefficients;
1127     const uint32_t quoSamplerateRatio = state->quoSamplerateRatio;
1128     const uint32_t remSamplerateRatio = state->remSamplerateRatio;
1129     const uint32_t decimateFactor = state->decimateFactor;
1130     const uint32_t interpolateFactor = state->interpolateFactor;
1131     const uint32_t numChannels = state->numChannels;
1132     uint32_t i;
1133 
1134     if (inputIndex < (uint32_t)(*inputLength)) {
1135         outSample = CompareMin((*outputLength), ((interpolateFactor * ((*inputLength) - inputIndex) -
1136             subfilterNum) - 1) / decimateFactor + 1);
1137     }
1138 
1139     for (i = 0; i < outSample; i++) {
1140         const float* coeffs = &filterCoefficients[subfilterNum * n];
1141         const float* inputs = &in[inputIndex * numChannels];
1142 
1143         state->multiplyFunSeq[subfilterNum](state, coeffs, inputs, out, subfilterNum);
1144         out += numChannels;
1145 
1146         inputIndex += quoSamplerateRatio;
1147         subfilterNum += remSamplerateRatio;
1148         if (subfilterNum >= interpolateFactor) {
1149             subfilterNum -= interpolateFactor;
1150             inputIndex++;
1151         }
1152     }
1153 
1154     state->inputIndex = inputIndex;
1155     state->subfilterNum = subfilterNum;
1156     return outSample;
1157 }
1158 
1159 /*
1160  * This resampler is used to produce zero output in situations where memory for the filter could not be allocated.
1161  */
PolyphaseResamplerZero(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)1162 static int32_t PolyphaseResamplerZero(SingleStagePolyphaseResamplerState* state, const float* in,
1163     uint32_t* inputLength, float* out, uint32_t* outputLength)
1164 {
1165     uint32_t outSample = 0;
1166     uint32_t inputIndex = state->inputIndex;
1167     uint32_t subfilterNum = state->subfilterNum;
1168     const uint32_t quoSamplerateRatio = state->quoSamplerateRatio;
1169     const uint32_t remSamplerateRatio = state->remSamplerateRatio;
1170     const uint32_t decimateFactor = state->decimateFactor;
1171     const uint32_t interpolateFactor = state->interpolateFactor;
1172     const uint32_t numChannels = state->numChannels;
1173     uint32_t i;
1174     uint32_t ch;
1175 
1176     (void)in;
1177 
1178     if (inputIndex < (uint32_t)(*inputLength)) {
1179         outSample = CompareMin((*outputLength), ((interpolateFactor * ((*inputLength) - inputIndex) -
1180             subfilterNum) - 1) / decimateFactor + 1);
1181     }
1182 
1183     for (i = 0; i < outSample; i++) {
1184         for (ch = 0; ch < numChannels; ch++) {
1185             *out++ = 0;
1186         }
1187 
1188         inputIndex += quoSamplerateRatio;
1189         subfilterNum += remSamplerateRatio;
1190         if (subfilterNum >= interpolateFactor) {
1191             subfilterNum -= interpolateFactor;
1192             inputIndex++;
1193         }
1194     }
1195 
1196     state->inputIndex = inputIndex;
1197     state->subfilterNum = subfilterNum;
1198     return outSample;
1199 }
1200 
GetMultiplyFilterFun(SingleStagePolyphaseResamplerState * state,uint32_t i)1201 static MultiplyFilterFun GetMultiplyFilterFun(SingleStagePolyphaseResamplerState* state, uint32_t i)
1202 {
1203     uint32_t channelMode = CompareMin(state->numChannels - 1, STEREO);
1204 
1205     if (state->interpolateFactor < state->decimateFactor) { // downsampling
1206         if (i == 0) {
1207             return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_DOWN + channelMode];
1208         }
1209         if ((uint32_t)TWO_STEPS * i == state->interpolateFactor) {
1210             return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_DOWN + channelMode];
1211         }
1212         return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_DOWN + channelMode];
1213     } else { // upsampling
1214         if (i == 0) {
1215             return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_ODD_UP + channelMode];
1216         }
1217         if ((uint32_t)TWO_STEPS * i == state->interpolateFactor) {
1218             return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_SYMMETRIC_EVEN_UP + channelMode];
1219         }
1220         return multiplyFilterFunTable[THREE_STEPS * MULTIPLY_FILTER_FUN_UP + channelMode];
1221     }
1222 }
1223 
SetResamplerFunctionCoarse(SingleStagePolyphaseResamplerState * state)1224 static ResamplerMethod SetResamplerFunctionCoarse(SingleStagePolyphaseResamplerState* state)
1225 {
1226     if (TWO_STEPS * state->interpolateFactor == state->decimateFactor) {
1227         // Specific function for downsample by 2
1228         switch (state->numChannels) {
1229         case MONO:
1230             return PolyphaseDownsamplerHalfbandMono;
1231         case STEREO:
1232             return PolyphaseDownsamplerHalfbandStereo;
1233         default:
1234             return PolyphaseDownsamplerHalfbandMultichannel;
1235         }
1236     }
1237     if (THREE_STEPS * state->interpolateFactor == state->decimateFactor) {
1238         // Specific function for downsample by 3
1239         switch (state->numChannels) {
1240         case MONO:
1241             return PolyphaseDownsamplerThirdbandMono;
1242         case STEREO:
1243             return PolyphaseDownsamplerThirdbandStereo;
1244         default:
1245             return PolyphaseDownsamplerThirdbandMultichannel;
1246         }
1247     }
1248 
1249     for (uint32_t j = 0; j < state->interpolateFactor; j++) {
1250         state->multiplyFunSeq[j] = GetMultiplyFilterFun(state, j);
1251     }
1252     return PolyphaseResamplerCoarse;
1253 }
1254 
UpdateResamplerState(SingleStagePolyphaseResamplerState * state)1255 static int32_t UpdateResamplerState(SingleStagePolyphaseResamplerState* state)
1256 {
1257     uint32_t oldFilterLength = state->filterLength;
1258 
1259     state->quoSamplerateRatio = state->decimateFactor / state->interpolateFactor;
1260     state->remSamplerateRatio = state->decimateFactor % state->interpolateFactor;
1261     state->filterLength = QUALITY_TABLE[state->quality].filterLength;
1262     state->coshParameter = QUALITY_TABLE[state->quality].coshParameter;
1263 
1264     if (state->interpolateFactor < state->decimateFactor) { // downsampling
1265         state->cutoff = (float)state->interpolateFactor / state->decimateFactor;
1266         state->filterLength = state->filterLength * state->decimateFactor / state->interpolateFactor;
1267 
1268         // Round up to make sure filterLength be multiple of 8
1269         state->filterLength = 8 * ((state->filterLength - 1) / 8) + 8;
1270     } else { // upsampling
1271         state->cutoff = 1;
1272     }
1273 
1274     // modified for new requirements (extended i/o sample rate combination) 2025.2.28
1275     if ((CompareMax(state->decimateFactor, state->interpolateFactor) <= MAX_RATIO_INTEGRAL_METHOD) &
1276         ((state->decimateFactor == 1 || state->interpolateFactor == 1) ||
1277             ((float)state->decimateFactor / (float)state->interpolateFactor < 2.0f))) {
1278         state->resamplerFunction = SetResamplerFunctionCoarse(state);
1279     } else { // fine (non-integral) sampling rate ratio
1280         switch (state->numChannels) {
1281         case MONO:
1282             state->resamplerFunction = PolyphaseResamplerMono;
1283             break;
1284         case STEREO:
1285             state->resamplerFunction = PolyphaseResamplerStereo;
1286             break;
1287         default:
1288             state->resamplerFunction = PolyphaseResamplerMultichannel;
1289         }
1290     }
1291 
1292     CalculateFilter(state);
1293 
1294     /* Here's the place where we update the filter memory to take into account
1295        the change in filter length. It's probably the messiest part of the code
1296        due to handling of lots of corner cases. */
1297     return UpdateFilterMemory(state, oldFilterLength);
1298 }
1299 
UpdateFilterMemory(SingleStagePolyphaseResamplerState * state,uint32_t oldFilterLength)1300 static int32_t UpdateFilterMemory(SingleStagePolyphaseResamplerState* state, uint32_t oldFilterLength)
1301 {
1302     /* Adding bufferSize to filterLength won't overflow here because filterLength
1303         could be multiplied by sizeof(float) above. */
1304     uint32_t requiredInputMemorySize = state->filterLength - 1 + state->bufferSize;
1305     if (requiredInputMemorySize > state->inputMemorySize) {
1306         if (state->inputMemory == NULL) { // first time initiaization
1307             state->inputMemory = (float*)malloc(state->numChannels * requiredInputMemorySize * sizeof(float));
1308             CHECK_AND_RETURN_RET_LOG(state->inputMemory, RESAMPLER_ERR_ALLOC_FAILED,
1309                 "first time requiredInputMemorySize %{public}d alloc failed", requiredInputMemorySize);
1310         } else {
1311             float* inputMemory = (float*)malloc(state->numChannels * requiredInputMemorySize * sizeof(float));
1312             CHECK_AND_RETURN_RET_LOG(inputMemory, RESAMPLER_ERR_ALLOC_FAILED,
1313                 "requiredInputMemorySize %{public}d alloc failed", requiredInputMemorySize);
1314             int32_t ret = memcpy_s(inputMemory, requiredInputMemorySize * state->numChannels * sizeof(float),
1315                 state->inputMemory, state->inputMemorySize * state->numChannels * sizeof(float));
1316             if (INT_MAX / sizeof(float) / state->numChannels < requiredInputMemorySize || ret != 0) {
1317                 state->resamplerFunction = PolyphaseResamplerZero;
1318                 /* state->mem may still contain consumed input samples for the filter.
1319                 Restore filterLength so that filterLength - 1 still points to the position after
1320                 the last of these samples. */
1321                 state->filterLength = oldFilterLength;
1322 
1323                 return FreeMemoryAndReturnError(inputMemory);
1324             }
1325             free(state->inputMemory);
1326             state->inputMemory = inputMemory;
1327         }
1328         state->inputMemorySize = requiredInputMemorySize;
1329     }
1330     /* codes for sudden sample rate change are deprecated and deleted so far */
1331     uint32_t i;
1332     for (i = 0; i < state->numChannels * state->inputMemorySize; i++) {
1333         state->inputMemory[i] = 0;
1334     }
1335     return RESAMPLER_ERR_SUCCESS;
1336 }
1337 
SingleStagePolyphaseResamplerSetQuality(SingleStagePolyphaseResamplerState * state,int32_t quality)1338 static int32_t SingleStagePolyphaseResamplerSetQuality(SingleStagePolyphaseResamplerState* state, int32_t quality)
1339 {
1340     if (quality > QUALITY_LEVEL_TEN || quality < 0) {
1341         return RESAMPLER_ERR_INVALID_ARG;
1342     }
1343     if (state->quality == quality) {
1344         return RESAMPLER_ERR_SUCCESS;
1345     }
1346     state->quality = quality;
1347     if (state->isInitialized) {
1348         return UpdateResamplerState(state);
1349     }
1350     return RESAMPLER_ERR_SUCCESS;
1351 }
1352 
SingleStagePolyphaseResamplerInit(uint32_t numChannels,uint32_t decimateFactor,uint32_t interpolateFactor,int32_t quality,int32_t * err)1353 SingleStagePolyphaseResamplerState* SingleStagePolyphaseResamplerInit(uint32_t numChannels,
1354     uint32_t decimateFactor, uint32_t interpolateFactor, int32_t quality, int32_t* err)
1355 {
1356     SingleStagePolyphaseResamplerState* state;
1357     int32_t filterErr;
1358 
1359     if (numChannels == 0 || decimateFactor == 0 || interpolateFactor == 0 || quality > QUALITY_LEVEL_TEN ||
1360         quality < 0) {
1361         if (err) {
1362             *err = RESAMPLER_ERR_INVALID_ARG;
1363         }
1364         return NULL;
1365     }
1366     state = (SingleStagePolyphaseResamplerState*)calloc(sizeof(SingleStagePolyphaseResamplerState), 1);
1367     if (!state) {
1368         if (err) {
1369             *err = RESAMPLER_ERR_ALLOC_FAILED;
1370         }
1371         return NULL;
1372     }
1373     state->isInitialized = 0;
1374     state->isStarted = 0;
1375     state->decimateFactor = 0;
1376     state->interpolateFactor = 0;
1377     state->quality = -1;
1378     state->filterCoefficientsSize = 0;
1379     state->inputMemorySize = 0;
1380     state->filterLength = 0;
1381     state->filterCoefficients = NULL;
1382     state->inputMemory = NULL;
1383     state->resamplerFunction = 0;
1384 
1385     state->cutoff = 1.f;
1386     state->numChannels = numChannels;
1387 
1388     state->bufferSize = BUFFER_SIZE;
1389 
1390     state->inputIndex = 0;
1391     state->magicSamples = 0;
1392     state->subfilterNum = 0;
1393 
1394     SingleStagePolyphaseResamplerSetQuality(state, quality);
1395     filterErr = SingleStagePolyphaseResamplerSetRate(state, decimateFactor, interpolateFactor);
1396     filterErr = UpdateResamplerState(state);
1397     if (filterErr == RESAMPLER_ERR_SUCCESS) {
1398         state->isInitialized = 1;
1399     } else {
1400         SingleStagePolyphaseResamplerFree(state);
1401         state = NULL;
1402     }
1403     if (err) {
1404         *err = filterErr;
1405     }
1406     return state;
1407 }
1408 
ApplyResampler(SingleStagePolyphaseResamplerState * state,uint32_t * inputLength,float * out,uint32_t * outputLength)1409 static void ApplyResampler(SingleStagePolyphaseResamplerState* state, uint32_t* inputLength,
1410     float* out, uint32_t* outputLength)
1411 {
1412     const uint32_t n = state->filterLength;
1413     int32_t outSample = 0;
1414     float* inputMemory = state->inputMemory;
1415     uint32_t inputSize;
1416     const uint32_t numChannels = state->numChannels;
1417     uint32_t j;
1418 
1419     state->isStarted = 1;
1420     /* Call resampler function */
1421     outSample = state->resamplerFunction(state, inputMemory, inputLength, out, outputLength);
1422 
1423     if (state->inputIndex < (int32_t)*inputLength) {
1424         *inputLength = state->inputIndex;
1425     }
1426     *outputLength = outSample;
1427     state->inputIndex -= *inputLength;
1428 
1429     inputSize = (*inputLength) * numChannels;
1430     for (j = 0; j < (n - 1) * numChannels; j++) {
1431         inputMemory[j] = inputMemory[j + inputSize];
1432     }
1433 }
1434 
1435 
ComputeGcd(uint32_t a,uint32_t b)1436 static inline uint32_t ComputeGcd(uint32_t a, uint32_t b)
1437 {
1438     while (b != 0) {
1439         uint32_t temp = a;
1440 
1441         a = b;
1442         b = temp % b;
1443     }
1444     return a;
1445 }
1446 
1447 
SingleStagePolyphaseResamplerProcess(SingleStagePolyphaseResamplerState * state,const float * in,uint32_t * inputLength,float * out,uint32_t * outputLength)1448 int32_t SingleStagePolyphaseResamplerProcess(SingleStagePolyphaseResamplerState* state, const float* in,
1449     uint32_t* inputLength, float* out, uint32_t* outputLength)
1450 {
1451     uint32_t j;
1452     uint32_t remainingInputLength = *inputLength;
1453     uint32_t remainingOutputLength = *outputLength;
1454     const uint32_t filtOffs = state->filterLength - 1;
1455     const uint32_t bufferLen = state->inputMemorySize - filtOffs;
1456     const uint32_t numChannels = state->numChannels;
1457     float* buf = state->inputMemory + filtOffs * numChannels;
1458 
1459     while (remainingInputLength && remainingOutputLength) {
1460         uint32_t processInputLength = (remainingInputLength > bufferLen) ? bufferLen : remainingInputLength;
1461         uint32_t processOutputLength = remainingOutputLength;
1462 
1463         if (in) {
1464             for (j = 0; j < processInputLength * numChannels; j++) {
1465                 buf[j] = in[j];
1466             }
1467         } else {
1468             for (j = 0; j < processInputLength * numChannels; j++) {
1469                 buf[j] = 0;
1470             }
1471         }
1472         ApplyResampler(state, &processInputLength, out, &processOutputLength);
1473         remainingInputLength -= processInputLength;
1474         remainingOutputLength -= processOutputLength;
1475         out += processOutputLength * numChannels;
1476         if (in) {
1477             in += processInputLength * numChannels;
1478         }
1479     }
1480 
1481     *inputLength -= remainingInputLength;
1482     *outputLength -= remainingOutputLength;
1483     return state->resamplerFunction == PolyphaseResamplerZero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
1484 }
1485 
SingleStagePolyphaseResamplerSetRate(SingleStagePolyphaseResamplerState * state,uint32_t decimateFactor,uint32_t interpolateFactor)1486 int32_t SingleStagePolyphaseResamplerSetRate(SingleStagePolyphaseResamplerState* state, uint32_t decimateFactor,
1487     uint32_t interpolateFactor)
1488 {
1489     uint32_t fact;
1490     uint32_t oldInterpolateFactor;
1491 
1492     if (decimateFactor == 0 || interpolateFactor == 0) {
1493         return RESAMPLER_ERR_INVALID_ARG;
1494     }
1495 
1496     if (state->decimateFactor == decimateFactor && state->interpolateFactor == interpolateFactor) {
1497         return RESAMPLER_ERR_SUCCESS;
1498     }
1499 
1500     oldInterpolateFactor = state->interpolateFactor;
1501     state->decimateFactor = decimateFactor;
1502     state->interpolateFactor = interpolateFactor;
1503 
1504     fact = ComputeGcd(state->decimateFactor, state->interpolateFactor);
1505     CHECK_AND_RETURN_RET_LOG(fact != 0, RESAMPLER_ERR_OVERFLOW, "fact is zero, invalid");
1506     state->decimateFactor /= fact;
1507     state->interpolateFactor /= fact;
1508 
1509     if (oldInterpolateFactor > 0) {
1510         state->subfilterNum = state->subfilterNum * state->interpolateFactor / oldInterpolateFactor;
1511 
1512         /* Safety net */
1513         if (state->subfilterNum >= state->interpolateFactor) {
1514             state->subfilterNum = state->interpolateFactor - 1;
1515         }
1516     }
1517 
1518     if (state->isInitialized) {
1519         return UpdateResamplerState(state);
1520     }
1521     return RESAMPLER_ERR_SUCCESS;
1522 }
1523 
SingleStagePolyphaseResamplerSkipHalfTaps(SingleStagePolyphaseResamplerState * state)1524 int32_t SingleStagePolyphaseResamplerSkipHalfTaps(SingleStagePolyphaseResamplerState* state)
1525 {
1526     state->inputIndex = state->filterLength / TWO_STEPS;
1527     return RESAMPLER_ERR_SUCCESS;
1528 }
1529 
1530 
SingleStagePolyphaseResamplerFree(SingleStagePolyphaseResamplerState * state)1531 void SingleStagePolyphaseResamplerFree(SingleStagePolyphaseResamplerState* state)
1532 {
1533     free(state->inputMemory);
1534     state->inputMemory = NULL;
1535     free(state->filterCoefficients);
1536     state->filterCoefficients = NULL;
1537     free(state);
1538     state = NULL;
1539 }
1540 
1541 
SingleStagePolyphaseResamplerResetMem(SingleStagePolyphaseResamplerState * state)1542 int32_t SingleStagePolyphaseResamplerResetMem(SingleStagePolyphaseResamplerState* state)
1543 {
1544     uint32_t i;
1545     state->inputIndex = 0;
1546     state->magicSamples = 0;
1547     state->subfilterNum = 0;
1548 
1549     for (i = 0; i < state->numChannels * state->inputMemorySize; i++) {
1550         state->inputMemory[i] = 0;
1551     }
1552     return RESAMPLER_ERR_SUCCESS;
1553 }