blob: fa6ee3e3269162a0fb176998f6140d9ea4e74d70 [file] [log] [blame]
Andy Hung86eae0e2013-12-09 12:12:46 -08001/*
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
20namespace 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
30static inline double sinc(double x) {
31 if (fabs(x) < FLT_MIN) {
32 return 1.;
33 }
34 return sin(x) / x;
35}
36
37static 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
62static 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
69static 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
97template <int N>
98struct I0Term {
99 static const double value = I0Term<N-1>::value/ (4. * N * N);
100};
101
102template <>
103struct I0Term<0> {
104 static const double value = 1.;
105};
106
107template <int N>
108struct I0ATerm {
109 static const double value = I0ATerm<N-1>::value * (2.*N-1.) * (2.*N-1.) / (8. * N);
110};
111
112template <>
113struct I0ATerm<0> { // 1/sqrt(2*PI);
114 static const double value = 0.398942280401432677939946059934381868475858631164934657665925;
115};
116
117static 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 */
154static 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 */
165template <typename T>
166static 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 */
200template <typename T>
201static 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
260template <typename T>
261static 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*/