Andy Hung | 86eae0e | 2013-12-09 12:12:46 -0800 | [diff] [blame^] | 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_GEN_H |
| 18 | #define ANDROID_AUDIO_RESAMPLER_FIR_GEN_H |
| 19 | |
| 20 | namespace android { |
| 21 | |
| 22 | /* |
| 23 | * Sinc function is the traditional variant. |
| 24 | * |
| 25 | * TODO: Investigate optimizations (regular sampling grid, NEON vector accelerations) |
| 26 | * TODO: Remove comparison at 0 and trap at a higher level. |
| 27 | * |
| 28 | */ |
| 29 | |
| 30 | static inline double sinc(double x) { |
| 31 | if (fabs(x) < FLT_MIN) { |
| 32 | return 1.; |
| 33 | } |
| 34 | return sin(x) / x; |
| 35 | } |
| 36 | |
| 37 | static inline double sqr(double x) { |
| 38 | return x * x; |
| 39 | } |
| 40 | |
| 41 | /* |
| 42 | * rounds a double to the nearest integer for FIR coefficients. |
| 43 | * |
| 44 | * One variant uses noise shaping, which must keep error history |
| 45 | * to work (the err parameter, initialized to 0). |
| 46 | * The other variant is a non-noise shaped version for |
| 47 | * S32 coefficients (noise shaping doesn't gain much). |
| 48 | * |
| 49 | * Caution: No bounds saturation is applied, but isn't needed in |
| 50 | * this case. |
| 51 | * |
| 52 | * @param x is the value to round. |
| 53 | * |
| 54 | * @param maxval is the maximum integer scale factor expressed as an int64 (for headroom). |
| 55 | * Typically this may be the maximum positive integer+1 (using the fact that double precision |
| 56 | * FIR coefficients generated here are never that close to 1.0 to pose an overflow condition). |
| 57 | * |
| 58 | * @param err is the previous error (actual - rounded) for the previous rounding op. |
| 59 | * |
| 60 | */ |
| 61 | |
| 62 | static inline int64_t toint(double x, int64_t maxval, double& err) { |
| 63 | double val = x * maxval; |
| 64 | double ival = floor(val + 0.5 + err*0.17); |
| 65 | err = val - ival; |
| 66 | return static_cast<int64_t>(ival); |
| 67 | } |
| 68 | |
| 69 | static inline int64_t toint(double x, int64_t maxval) { |
| 70 | return static_cast<int64_t>(floor(x * maxval + 0.5)); |
| 71 | } |
| 72 | |
| 73 | /* |
| 74 | * Modified Bessel function of the first kind |
| 75 | * http://en.wikipedia.org/wiki/Bessel_function |
| 76 | * |
| 77 | * The formulas are taken from Abramowitz and Stegun: |
| 78 | * |
| 79 | * http://people.math.sfu.ca/~cbm/aands/page_375.htm |
| 80 | * http://people.math.sfu.ca/~cbm/aands/page_378.htm |
| 81 | * |
| 82 | * http://dlmf.nist.gov/10.25 |
| 83 | * http://dlmf.nist.gov/10.40 |
| 84 | * |
| 85 | * Note we assume x is nonnegative (the function is symmetric, |
| 86 | * pass in the absolute value as needed). |
| 87 | * |
| 88 | * Constants are compile time derived with templates I0Term<> and |
| 89 | * I0ATerm<> to the precision of the compiler. The series can be expanded |
| 90 | * to any precision needed, but currently set around 24b precision. |
| 91 | * |
| 92 | * We use a bit of template math here, constexpr would probably be |
| 93 | * more appropriate for a C++11 compiler. |
| 94 | * |
| 95 | */ |
| 96 | |
| 97 | template <int N> |
| 98 | struct I0Term { |
| 99 | static const double value = I0Term<N-1>::value/ (4. * N * N); |
| 100 | }; |
| 101 | |
| 102 | template <> |
| 103 | struct I0Term<0> { |
| 104 | static const double value = 1.; |
| 105 | }; |
| 106 | |
| 107 | template <int N> |
| 108 | struct I0ATerm { |
| 109 | static const double value = I0ATerm<N-1>::value * (2.*N-1.) * (2.*N-1.) / (8. * N); |
| 110 | }; |
| 111 | |
| 112 | template <> |
| 113 | struct I0ATerm<0> { // 1/sqrt(2*PI); |
| 114 | static const double value = 0.398942280401432677939946059934381868475858631164934657665925; |
| 115 | }; |
| 116 | |
| 117 | static inline double I0(double x) { |
| 118 | if (x < 3.75) { // TODO: Estrin's method instead of Horner's method? |
| 119 | x *= x; |
| 120 | return I0Term<0>::value + x*( |
| 121 | I0Term<1>::value + x*( |
| 122 | I0Term<2>::value + x*( |
| 123 | I0Term<3>::value + x*( |
| 124 | I0Term<4>::value + x*( |
| 125 | I0Term<5>::value + x*( |
| 126 | I0Term<6>::value)))))); // e < 1.6e-7 |
| 127 | } |
| 128 | // a bit ugly here - perhaps we expand the top series |
| 129 | // to permit computation to x < 20 (a reasonable range) |
| 130 | double y = 1./x; |
| 131 | return exp(x) * sqrt(y) * ( |
| 132 | // note: reciprocal squareroot may be easier! |
| 133 | // http://en.wikipedia.org/wiki/Fast_inverse_square_root |
| 134 | I0ATerm<0>::value + y*( |
| 135 | I0ATerm<1>::value + y*( |
| 136 | I0ATerm<2>::value + y*( |
| 137 | I0ATerm<3>::value + y*( |
| 138 | I0ATerm<4>::value + y*( |
| 139 | I0ATerm<5>::value + y*( |
| 140 | I0ATerm<6>::value + y*( |
| 141 | I0ATerm<7>::value + y*( |
| 142 | I0ATerm<8>::value))))))))); // (... e) < 1.9e-7 |
| 143 | } |
| 144 | |
| 145 | /* |
| 146 | * calculates the transition bandwidth for a Kaiser filter |
| 147 | * |
| 148 | * Formula 3.2.8, Multirate Systems and Filter Banks, PP Vaidyanathan, pg. 48 |
| 149 | * |
| 150 | * @param halfNumCoef is half the number of coefficients per filter phase. |
| 151 | * @param stopBandAtten is the stop band attenuation desired. |
| 152 | * @return the transition bandwidth in normalized frequency (0 <= f <= 0.5) |
| 153 | */ |
| 154 | static inline double firKaiserTbw(int halfNumCoef, double stopBandAtten) { |
| 155 | return (stopBandAtten - 7.95)/(2.*14.36*halfNumCoef); |
| 156 | } |
| 157 | |
| 158 | /* |
| 159 | * calculates the fir transfer response. |
| 160 | * |
| 161 | * calculates the transfer coefficient H(w) for 0 <= w <= PI. |
| 162 | * Be careful be careful to consider the fact that this is an interpolated filter |
| 163 | * of length L, so normalizing H(w)/L is probably what you expect. |
| 164 | */ |
| 165 | template <typename T> |
| 166 | static inline double firTransfer(const T* coef, int L, int halfNumCoef, double w) { |
| 167 | double accum = static_cast<double>(coef[0])*0.5; |
| 168 | coef += halfNumCoef; // skip first row. |
| 169 | for (int i=1 ; i<=L ; ++i) { |
| 170 | for (int j=0, ix=i ; j<halfNumCoef ; ++j, ix+=L) { |
| 171 | accum += cos(ix*w)*static_cast<double>(*coef++); |
| 172 | } |
| 173 | } |
| 174 | return accum*2.; |
| 175 | } |
| 176 | |
| 177 | /* |
| 178 | * returns the minimum and maximum |H(f)| bounds |
| 179 | * |
| 180 | * @param coef is the designed polyphase filter banks |
| 181 | * |
| 182 | * @param L is the number of phases (for interpolation) |
| 183 | * |
| 184 | * @param halfNumCoef should be half the number of coefficients for a single |
| 185 | * polyphase. |
| 186 | * |
| 187 | * @param fstart is the normalized frequency start. |
| 188 | * |
| 189 | * @param fend is the normalized frequency end. |
| 190 | * |
| 191 | * @param steps is the number of steps to take (sampling) between frequency start and end |
| 192 | * |
| 193 | * @param firMin returns the minimum transfer |H(f)| found |
| 194 | * |
| 195 | * @param firMax returns the maximum transfer |H(f)| found |
| 196 | * |
| 197 | * 0 <= f <= 0.5. |
| 198 | * This is used to test passband and stopband performance. |
| 199 | */ |
| 200 | template <typename T> |
| 201 | static void testFir(const T* coef, int L, int halfNumCoef, |
| 202 | double fstart, double fend, int steps, double &firMin, double &firMax) { |
| 203 | double wstart = fstart*(2.*M_PI); |
| 204 | double wend = fend*(2.*M_PI); |
| 205 | double wstep = (wend - wstart)/steps; |
| 206 | double fmax, fmin; |
| 207 | double trf = firTransfer(coef, L, halfNumCoef, wstart); |
| 208 | if (trf<0) { |
| 209 | trf = -trf; |
| 210 | } |
| 211 | fmin = fmax = trf; |
| 212 | wstart += wstep; |
| 213 | for (int i=1; i<steps; ++i) { |
| 214 | trf = firTransfer(coef, L, halfNumCoef, wstart); |
| 215 | if (trf<0) { |
| 216 | trf = -trf; |
| 217 | } |
| 218 | if (trf>fmax) { |
| 219 | fmax = trf; |
| 220 | } |
| 221 | else if (trf<fmin) { |
| 222 | fmin = trf; |
| 223 | } |
| 224 | wstart += wstep; |
| 225 | } |
| 226 | // renormalize - this is only needed for integer filter types |
| 227 | double norm = 1./((1ULL<<(sizeof(T)*8-1))*L); |
| 228 | |
| 229 | firMin = fmin * norm; |
| 230 | firMax = fmax * norm; |
| 231 | } |
| 232 | |
| 233 | /* |
| 234 | * Calculates the polyphase filter banks based on a windowed sinc function. |
| 235 | * |
| 236 | * The windowed sinc is an odd length symmetric filter of exactly L*halfNumCoef*2+1 |
| 237 | * taps for the entire kernel. This is then decomposed into L+1 polyphase filterbanks. |
| 238 | * The last filterbank is used for interpolation purposes (and is mostly composed |
| 239 | * of the first bank shifted by one sample), and is unnecessary if one does |
| 240 | * not do interpolation. |
| 241 | * |
| 242 | * @param coef is the caller allocated space for coefficients. This should be |
| 243 | * exactly (L+1)*halfNumCoef in size. |
| 244 | * |
| 245 | * @param L is the number of phases (for interpolation) |
| 246 | * |
| 247 | * @param halfNumCoef should be half the number of coefficients for a single |
| 248 | * polyphase. |
| 249 | * |
| 250 | * @param stopBandAtten is the stopband value, should be >50dB. |
| 251 | * |
| 252 | * @param fcr is cutoff frequency/sampling rate (<0.5). At this point, the energy |
| 253 | * should be 6dB less. (fcr is where the amplitude drops by half). Use the |
| 254 | * firKaiserTbw() to calculate the transition bandwidth. fcr is the midpoint |
| 255 | * between the stop band and the pass band (fstop+fpass)/2. |
| 256 | * |
| 257 | * @param atten is the attenuation (generally slightly less than 1). |
| 258 | */ |
| 259 | |
| 260 | template <typename T> |
| 261 | static inline void firKaiserGen(T* coef, int L, int halfNumCoef, |
| 262 | double stopBandAtten, double fcr, double atten) { |
| 263 | // |
| 264 | // Formula 3.2.5, 3.2.7, Multirate Systems and Filter Banks, PP Vaidyanathan, pg. 48 |
| 265 | // |
| 266 | // See also: http://melodi.ee.washington.edu/courses/ee518/notes/lec17.pdf |
| 267 | // |
| 268 | // Kaiser window and beta parameter |
| 269 | // |
| 270 | // | 0.1102*(A - 8.7) A > 50 |
| 271 | // beta = | 0.5842*(A - 21)^0.4 + 0.07886*(A - 21) 21 <= A <= 50 |
| 272 | // | 0. A < 21 |
| 273 | // |
| 274 | // with A is the desired stop-band attenuation in dBFS |
| 275 | // |
| 276 | // 30 dB 2.210 |
| 277 | // 40 dB 3.384 |
| 278 | // 50 dB 4.538 |
| 279 | // 60 dB 5.658 |
| 280 | // 70 dB 6.764 |
| 281 | // 80 dB 7.865 |
| 282 | // 90 dB 8.960 |
| 283 | // 100 dB 10.056 |
| 284 | |
| 285 | const int N = L * halfNumCoef; // non-negative half |
| 286 | const double beta = 0.1102 * (stopBandAtten - 8.7); // >= 50dB always |
| 287 | const double yscale = 2. * atten * fcr / I0(beta); |
| 288 | const double xstep = 2. * M_PI * fcr / L; |
| 289 | const double xfrac = 1. / N; |
| 290 | double err = 0; // for noise shaping on int16_t coefficients |
| 291 | for (int i=0 ; i<=L ; ++i) { // generate an extra set of coefs for interpolation |
| 292 | for (int j=0, ix=i ; j<halfNumCoef ; ++j, ix+=L) { |
| 293 | double y = I0(beta * sqrt(1.0 - sqr(ix * xfrac))) * sinc(ix * xstep) * yscale; |
| 294 | |
| 295 | // (caution!) float version does not need rounding |
| 296 | if (is_same<T, int16_t>::value) { // int16_t needs noise shaping |
| 297 | *coef++ = static_cast<T>(toint(y, 1ULL<<(sizeof(T)*8-1), err)); |
| 298 | } else { |
| 299 | *coef++ = static_cast<T>(toint(y, 1ULL<<(sizeof(T)*8-1))); |
| 300 | } |
| 301 | } |
| 302 | } |
| 303 | } |
| 304 | |
| 305 | }; // namespace android |
| 306 | |
| 307 | #endif /*ANDROID_AUDIO_RESAMPLER_FIR_GEN_H*/ |