1/*
2 * Copyright (C) 2013 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 *      http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17#ifndef ANDROID_AUDIO_RESAMPLER_FIR_PROCESS_H
18#define ANDROID_AUDIO_RESAMPLER_FIR_PROCESS_H
19
20namespace android {
21
22// depends on AudioResamplerFirOps.h
23
24/* variant for input type TI = int16_t input samples */
25template<typename TC>
26static inline
27void mac(int32_t& l, int32_t& r, TC coef, const int16_t* samples)
28{
29    uint32_t rl = *reinterpret_cast<const uint32_t*>(samples);
30    l = mulAddRL(1, rl, coef, l);
31    r = mulAddRL(0, rl, coef, r);
32}
33
34template<typename TC>
35static inline
36void mac(int32_t& l, TC coef, const int16_t* samples)
37{
38    l = mulAdd(samples[0], coef, l);
39}
40
41/* variant for input type TI = float input samples */
42template<typename TC>
43static inline
44void mac(float& l, float& r, TC coef,  const float* samples)
45{
46    l += *samples++ * coef;
47    r += *samples * coef;
48}
49
50template<typename TC>
51static inline
52void mac(float& l, TC coef,  const float* samples)
53{
54    l += *samples * coef;
55}
56
57/* variant for output type TO = int32_t output samples */
58static inline
59int32_t volumeAdjust(int32_t value, int32_t volume)
60{
61    return 2 * mulRL(0, value, volume);  // Note: only use top 16b
62}
63
64/* variant for output type TO = float output samples */
65static inline
66float volumeAdjust(float value, float volume)
67{
68    return value * volume;
69}
70
71/*
72 * Helper template functions for loop unrolling accumulator operations.
73 *
74 * Unrolling the loops achieves about 2x gain.
75 * Using a recursive template rather than an array of TO[] for the accumulator
76 * values is an additional 10-20% gain.
77 */
78
79template<int CHANNELS, typename TO>
80class Accumulator : public Accumulator<CHANNELS-1, TO> // recursive
81{
82public:
83    inline void clear() {
84        value = 0;
85        Accumulator<CHANNELS-1, TO>::clear();
86    }
87    template<typename TC, typename TI>
88    inline void acc(TC coef, const TI*& data) {
89        mac(value, coef, data++);
90        Accumulator<CHANNELS-1, TO>::acc(coef, data);
91    }
92    inline void volume(TO*& out, TO gain) {
93        *out++ = volumeAdjust(value, gain);
94        Accumulator<CHANNELS-1, TO>::volume(out, gain);
95    }
96
97    TO value; // one per recursive inherited base class
98};
99
100template<typename TO>
101class Accumulator<0, TO> {
102public:
103    inline void clear() {
104    }
105    template<typename TC, typename TI>
106    inline void acc(TC coef __unused, const TI*& data __unused) {
107    }
108    inline void volume(TO*& out __unused, TO gain __unused) {
109    }
110};
111
112template<typename TC, typename TINTERP>
113inline
114TC interpolate(TC coef_0, TC coef_1, TINTERP lerp)
115{
116    return lerp * (coef_1 - coef_0) + coef_0;
117}
118
119template<>
120inline
121int16_t interpolate<int16_t, uint32_t>(int16_t coef_0, int16_t coef_1, uint32_t lerp)
122{   // in some CPU architectures 16b x 16b multiplies are faster.
123    return (static_cast<int16_t>(lerp) * static_cast<int16_t>(coef_1 - coef_0) >> 15) + coef_0;
124}
125
126template<>
127inline
128int32_t interpolate<int32_t, uint32_t>(int32_t coef_0, int32_t coef_1, uint32_t lerp)
129{
130    return (lerp * static_cast<int64_t>(coef_1 - coef_0) >> 31) + coef_0;
131}
132
133/* class scope for passing in functions into templates */
134struct InterpCompute {
135    template<typename TC, typename TINTERP>
136    static inline
137    TC interpolatep(TC coef_0, TC coef_1, TINTERP lerp) {
138        return interpolate(coef_0, coef_1, lerp);
139    }
140
141    template<typename TC, typename TINTERP>
142    static inline
143    TC interpolaten(TC coef_0, TC coef_1, TINTERP lerp) {
144        return interpolate(coef_0, coef_1, lerp);
145    }
146};
147
148struct InterpNull {
149    template<typename TC, typename TINTERP>
150    static inline
151    TC interpolatep(TC coef_0, TC coef_1 __unused, TINTERP lerp __unused) {
152        return coef_0;
153    }
154
155    template<typename TC, typename TINTERP>
156    static inline
157    TC interpolaten(TC coef_0 __unused, TC coef_1, TINTERP lerp __unused) {
158        return coef_1;
159    }
160};
161
162/*
163 * Calculates a single output frame (two samples).
164 *
165 * The Process*() functions compute both the positive half FIR dot product and
166 * the negative half FIR dot product, accumulates, and then applies the volume.
167 *
168 * Use fir() to compute the proper coefficient pointers for a polyphase
169 * filter bank.
170 *
171 * ProcessBase() is the fundamental processing template function.
172 *
173 * ProcessL() calls ProcessBase() with TFUNC = InterpNull, for fixed/locked phase.
174 * Process() calls ProcessBase() with TFUNC = InterpCompute, for interpolated phase.
175 */
176
177template <int CHANNELS, int STRIDE, typename TFUNC, typename TC, typename TI, typename TO,
178        typename TINTERP>
179static inline
180void ProcessBase(TO* const out,
181        size_t count,
182        const TC* coefsP,
183        const TC* coefsN,
184        const TI* sP,
185        const TI* sN,
186        TINTERP lerpP,
187        const TO* const volumeLR)
188{
189    COMPILE_TIME_ASSERT_FUNCTION_SCOPE(CHANNELS > 0)
190
191    if (CHANNELS > 2) {
192        // TO accum[CHANNELS];
193        Accumulator<CHANNELS, TO> accum;
194
195        // for (int j = 0; j < CHANNELS; ++j) accum[j] = 0;
196        accum.clear();
197        for (size_t i = 0; i < count; ++i) {
198            TC c = TFUNC::interpolatep(coefsP[0], coefsP[count], lerpP);
199
200            // for (int j = 0; j < CHANNELS; ++j) mac(accum[j], c, sP + j);
201            const TI *tmp_data = sP; // tmp_ptr seems to work better
202            accum.acc(c, tmp_data);
203
204            coefsP++;
205            sP -= CHANNELS;
206            c = TFUNC::interpolaten(coefsN[count], coefsN[0], lerpP);
207
208            // for (int j = 0; j < CHANNELS; ++j) mac(accum[j], c, sN + j);
209            tmp_data = sN; // tmp_ptr seems faster than directly using sN
210            accum.acc(c, tmp_data);
211
212            coefsN++;
213            sN += CHANNELS;
214        }
215        // for (int j = 0; j < CHANNELS; ++j) out[j] += volumeAdjust(accum[j], volumeLR[0]);
216        TO *tmp_out = out; // may remove if const out definition changes.
217        accum.volume(tmp_out, volumeLR[0]);
218    } else if (CHANNELS == 2) {
219        TO l = 0;
220        TO r = 0;
221        for (size_t i = 0; i < count; ++i) {
222            mac(l, r, TFUNC::interpolatep(coefsP[0], coefsP[count], lerpP), sP);
223            coefsP++;
224            sP -= CHANNELS;
225            mac(l, r, TFUNC::interpolaten(coefsN[count], coefsN[0], lerpP), sN);
226            coefsN++;
227            sN += CHANNELS;
228        }
229        out[0] += volumeAdjust(l, volumeLR[0]);
230        out[1] += volumeAdjust(r, volumeLR[1]);
231    } else { /* CHANNELS == 1 */
232        TO l = 0;
233        for (size_t i = 0; i < count; ++i) {
234            mac(l, TFUNC::interpolatep(coefsP[0], coefsP[count], lerpP), sP);
235            coefsP++;
236            sP -= CHANNELS;
237            mac(l, TFUNC::interpolaten(coefsN[count], coefsN[0], lerpP), sN);
238            coefsN++;
239            sN += CHANNELS;
240        }
241        out[0] += volumeAdjust(l, volumeLR[0]);
242        out[1] += volumeAdjust(l, volumeLR[1]);
243    }
244}
245
246/* Calculates a single output frame from a polyphase resampling filter.
247 * See Process() for parameter details.
248 */
249template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO>
250static inline
251void ProcessL(TO* const out,
252        int count,
253        const TC* coefsP,
254        const TC* coefsN,
255        const TI* sP,
256        const TI* sN,
257        const TO* const volumeLR)
258{
259    ProcessBase<CHANNELS, STRIDE, InterpNull>(out, count, coefsP, coefsN, sP, sN, 0, volumeLR);
260}
261
262/*
263 * Calculates a single output frame from a polyphase resampling filter,
264 * with filter phase interpolation.
265 *
266 * @param out should point to the output buffer with space for at least one output frame.
267 *
268 * @param count should be half the size of the total filter length (halfNumCoefs), as we
269 * use symmetry in filter coefficients to evaluate two dot products.
270 *
271 * @param coefsP is one phase of the polyphase filter bank of size halfNumCoefs, corresponding
272 * to the positive sP.
273 *
274 * @param coefsN is one phase of the polyphase filter bank of size halfNumCoefs, corresponding
275 * to the negative sN.
276 *
277 * @param coefsP1 is the next phase of coefsP (used for interpolation).
278 *
279 * @param coefsN1 is the next phase of coefsN (used for interpolation).
280 *
281 * @param sP is the positive half of the coefficients (as viewed by a convolution),
282 * starting at the original samples pointer and decrementing (by CHANNELS).
283 *
284 * @param sN is the negative half of the samples (as viewed by a convolution),
285 * starting at the original samples pointer + CHANNELS and incrementing (by CHANNELS).
286 *
287 * @param lerpP The fractional siting between the polyphase indices is given by the bits
288 * below coefShift. See fir() for details.
289 *
290 * @param volumeLR is a pointer to an array of two 32 bit volume values, one per stereo channel,
291 * expressed as a S32 integer or float.  A negative value inverts the channel 180 degrees.
292 * The pointer volumeLR should be aligned to a minimum of 8 bytes.
293 * A typical value for volume is 0x1000 to align to a unity gain output of 20.12.
294 */
295template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO, typename TINTERP>
296static inline
297void Process(TO* const out,
298        int count,
299        const TC* coefsP,
300        const TC* coefsN,
301        const TC* coefsP1 __unused,
302        const TC* coefsN1 __unused,
303        const TI* sP,
304        const TI* sN,
305        TINTERP lerpP,
306        const TO* const volumeLR)
307{
308    ProcessBase<CHANNELS, STRIDE, InterpCompute>(out, count, coefsP, coefsN, sP, sN, lerpP,
309            volumeLR);
310}
311
312/*
313 * Calculates a single output frame from input sample pointer.
314 *
315 * This sets up the params for the accelerated Process() and ProcessL()
316 * functions to do the appropriate dot products.
317 *
318 * @param out should point to the output buffer with space for at least one output frame.
319 *
320 * @param phase is the fractional distance between input frames for interpolation:
321 * phase >= 0  && phase < phaseWrapLimit.  It can be thought of as a rational fraction
322 * of phase/phaseWrapLimit.
323 *
324 * @param phaseWrapLimit is #polyphases<<coefShift, where #polyphases is the number of polyphases
325 * in the polyphase filter. Likewise, #polyphases can be obtained as (phaseWrapLimit>>coefShift).
326 *
327 * @param coefShift gives the bit alignment of the polyphase index in the phase parameter.
328 *
329 * @param halfNumCoefs is the half the number of coefficients per polyphase filter. Since the
330 * overall filterbank is odd-length symmetric, only halfNumCoefs need be stored.
331 *
332 * @param coefs is the polyphase filter bank, starting at from polyphase index 0, and ranging to
333 * and including the #polyphases.  Each polyphase of the filter has half-length halfNumCoefs
334 * (due to symmetry).  The total size of the filter bank in coefficients is
335 * (#polyphases+1)*halfNumCoefs.
336 *
337 * The filter bank coefs should be aligned to a minimum of 16 bytes (preferrably to cache line).
338 *
339 * The coefs should be attenuated (to compensate for passband ripple)
340 * if storing back into the native format.
341 *
342 * @param samples are unaligned input samples.  The position is in the "middle" of the
343 * sample array with respect to the FIR filter:
344 * the negative half of the filter is dot product from samples+1 to samples+halfNumCoefs;
345 * the positive half of the filter is dot product from samples to samples-halfNumCoefs+1.
346 *
347 * @param volumeLR is a pointer to an array of two 32 bit volume values, one per stereo channel,
348 * expressed as a S32 integer or float.  A negative value inverts the channel 180 degrees.
349 * The pointer volumeLR should be aligned to a minimum of 8 bytes.
350 * A typical value for volume is 0x1000 to align to a unity gain output of 20.12.
351 *
352 * In between calls to filterCoefficient, the phase is incremented by phaseIncrement, where
353 * phaseIncrement is calculated as inputSampling * phaseWrapLimit / outputSampling.
354 *
355 * The filter polyphase index is given by indexP = phase >> coefShift. Due to
356 * odd length symmetric filter, the polyphase index of the negative half depends on
357 * whether interpolation is used.
358 *
359 * The fractional siting between the polyphase indices is given by the bits below coefShift:
360 *
361 * lerpP = phase << 32 - coefShift >> 1;  // for 32 bit unsigned phase multiply
362 * lerpP = phase << 32 - coefShift >> 17; // for 16 bit unsigned phase multiply
363 *
364 * For integer types, this is expressed as:
365 *
366 * lerpP = phase << sizeof(phase)*8 - coefShift
367 *              >> (sizeof(phase)-sizeof(*coefs))*8 + 1;
368 *
369 * For floating point, lerpP is the fractional phase scaled to [0.0, 1.0):
370 *
371 * lerpP = (phase << 32 - coefShift) / (1 << 32); // floating point equivalent
372 */
373
374template<int CHANNELS, bool LOCKED, int STRIDE, typename TC, typename TI, typename TO>
375static inline
376void fir(TO* const out,
377        const uint32_t phase, const uint32_t phaseWrapLimit,
378        const int coefShift, const int halfNumCoefs, const TC* const coefs,
379        const TI* const samples, const TO* const volumeLR)
380{
381    // NOTE: be very careful when modifying the code here. register
382    // pressure is very high and a small change might cause the compiler
383    // to generate far less efficient code.
384    // Always sanity check the result with objdump or test-resample.
385
386    if (LOCKED) {
387        // locked polyphase (no interpolation)
388        // Compute the polyphase filter index on the positive and negative side.
389        uint32_t indexP = phase >> coefShift;
390        uint32_t indexN = (phaseWrapLimit - phase) >> coefShift;
391        const TC* coefsP = coefs + indexP*halfNumCoefs;
392        const TC* coefsN = coefs + indexN*halfNumCoefs;
393        const TI* sP = samples;
394        const TI* sN = samples + CHANNELS;
395
396        // dot product filter.
397        ProcessL<CHANNELS, STRIDE>(out,
398                halfNumCoefs, coefsP, coefsN, sP, sN, volumeLR);
399    } else {
400        // interpolated polyphase
401        // Compute the polyphase filter index on the positive and negative side.
402        uint32_t indexP = phase >> coefShift;
403        uint32_t indexN = (phaseWrapLimit - phase - 1) >> coefShift; // one's complement.
404        const TC* coefsP = coefs + indexP*halfNumCoefs;
405        const TC* coefsN = coefs + indexN*halfNumCoefs;
406        const TC* coefsP1 = coefsP + halfNumCoefs;
407        const TC* coefsN1 = coefsN + halfNumCoefs;
408        const TI* sP = samples;
409        const TI* sN = samples + CHANNELS;
410
411        // Interpolation fraction lerpP derived by shifting all the way up and down
412        // to clear the appropriate bits and align to the appropriate level
413        // for the integer multiply.  The constants should resolve in compile time.
414        //
415        // The interpolated filter coefficient is derived as follows for the pos/neg half:
416        //
417        // interpolated[P] = index[P]*lerpP + index[P+1]*(1-lerpP)
418        // interpolated[N] = index[N+1]*lerpP + index[N]*(1-lerpP)
419
420        // on-the-fly interpolated dot product filter
421        if (is_same<TC, float>::value || is_same<TC, double>::value) {
422            static const TC scale = 1. / (65536. * 65536.); // scale phase bits to [0.0, 1.0)
423            TC lerpP = TC(phase << (sizeof(phase)*8 - coefShift)) * scale;
424
425            Process<CHANNELS, STRIDE>(out,
426                    halfNumCoefs, coefsP, coefsN, coefsP1, coefsN1, sP, sN, lerpP, volumeLR);
427        } else {
428            uint32_t lerpP = phase << (sizeof(phase)*8 - coefShift)
429                    >> ((sizeof(phase)-sizeof(*coefs))*8 + 1);
430
431            Process<CHANNELS, STRIDE>(out,
432                    halfNumCoefs, coefsP, coefsN, coefsP1, coefsN1, sP, sN, lerpP, volumeLR);
433        }
434    }
435}
436
437} // namespace android
438
439#endif /*ANDROID_AUDIO_RESAMPLER_FIR_PROCESS_H*/
440