Home | History | Annotate | Download | only in libaudioprocessing
      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 
     20 namespace android {
     21 
     22 // depends on AudioResamplerFirOps.h
     23 
     24 /* variant for input type TI = int16_t input samples */
     25 template<typename TC>
     26 static inline
     27 void 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 
     34 template<typename TC>
     35 static inline
     36 void 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 */
     42 template<typename TC>
     43 static inline
     44 void mac(float& l, float& r, TC coef,  const float* samples)
     45 {
     46     l += *samples++ * coef;
     47     r += *samples * coef;
     48 }
     49 
     50 template<typename TC>
     51 static inline
     52 void 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 */
     58 static inline
     59 int32_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 */
     65 static inline
     66 float 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 
     79 template<int CHANNELS, typename TO>
     80 class Accumulator : public Accumulator<CHANNELS-1, TO> // recursive
     81 {
     82 public:
     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 
    100 template<typename TO>
    101 class Accumulator<0, TO> {
    102 public:
    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 
    112 template<typename TC, typename TINTERP>
    113 inline
    114 TC interpolate(TC coef_0, TC coef_1, TINTERP lerp)
    115 {
    116     return lerp * (coef_1 - coef_0) + coef_0;
    117 }
    118 
    119 template<>
    120 inline
    121 int16_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 
    126 template<>
    127 inline
    128 int32_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 */
    134 struct 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 
    148 struct 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 
    177 template <int CHANNELS, int STRIDE, typename TFUNC, typename TC, typename TI, typename TO,
    178         typename TINTERP>
    179 static inline
    180 void 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     static_assert(CHANNELS > 0, "CHANNELS must be > 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  */
    249 template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO>
    250 static inline
    251 void 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  */
    295 template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO, typename TINTERP>
    296 static inline
    297 void 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 
    374 template<int CHANNELS, bool LOCKED, int STRIDE, typename TC, typename TI, typename TO>
    375 static inline
    376 void 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