blob: 8a615a75da7533c16806b69423aa012ba3f402f9 [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_PROCESS_H
18#define ANDROID_AUDIO_RESAMPLER_FIR_PROCESS_H
19
20namespace android {
21
22// depends on AudioResamplerFirOps.h
23
Andy Hungd5491392014-04-08 18:28:09 -070024/* variant for input type TI = int16_t input samples */
25template<typename TC>
Andy Hung86eae0e2013-12-09 12:12:46 -080026static inline
Andy Hungd5491392014-04-08 18:28:09 -070027void mac(int32_t& l, int32_t& r, TC coef, const int16_t* samples)
Andy Hung86eae0e2013-12-09 12:12:46 -080028{
Andy Hungd5491392014-04-08 18:28:09 -070029 uint32_t rl = *reinterpret_cast<const uint32_t*>(samples);
30 l = mulAddRL(1, rl, coef, l);
31 r = mulAddRL(0, rl, coef, r);
Andy Hung86eae0e2013-12-09 12:12:46 -080032}
33
Andy Hungd5491392014-04-08 18:28:09 -070034template<typename TC>
Andy Hung86eae0e2013-12-09 12:12:46 -080035static inline
Andy Hungd5491392014-04-08 18:28:09 -070036void mac(int32_t& l, TC coef, const int16_t* samples)
Andy Hung86eae0e2013-12-09 12:12:46 -080037{
Andy Hungd5491392014-04-08 18:28:09 -070038 l = mulAdd(samples[0], coef, l);
39}
Andy Hung86eae0e2013-12-09 12:12:46 -080040
Andy Hungd5491392014-04-08 18:28:09 -070041/* 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;
Andy Hung68ffa202014-04-09 19:19:06 -070047 r += *samples * coef;
Andy Hungd5491392014-04-08 18:28:09 -070048}
49
50template<typename TC>
51static inline
52void mac(float& l, TC coef, const float* samples)
53{
Andy Hung68ffa202014-04-09 19:19:06 -070054 l += *samples * coef;
Andy Hungd5491392014-04-08 18:28:09 -070055}
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;
Andy Hung86eae0e2013-12-09 12:12:46 -080069}
70
71/*
Andy Hung68ffa202014-04-09 19:19:06 -070072 * Helper template functions for loop unrolling accumulator operations.
Andy Hung86eae0e2013-12-09 12:12:46 -080073 *
Andy Hung68ffa202014-04-09 19:19:06 -070074 * 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.
Andy Hung86eae0e2013-12-09 12:12:46 -080077 */
78
Andy Hung68ffa202014-04-09 19:19:06 -070079template<int CHANNELS, typename TO>
80class Accumulator : public Accumulator<CHANNELS-1, TO> // recursive
Andy Hung86eae0e2013-12-09 12:12:46 -080081{
Andy Hung68ffa202014-04-09 19:19:06 -070082public:
83 inline void clear() {
84 value = 0;
85 Accumulator<CHANNELS-1, TO>::clear();
Andy Hungd5491392014-04-08 18:28:09 -070086 }
Andy Hung68ffa202014-04-09 19:19:06 -070087 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};
Andy Hung86eae0e2013-12-09 12:12:46 -0800111
Andy Hungd5491392014-04-08 18:28:09 -0700112template<typename TC, typename TINTERP>
Andy Hung42b01112014-07-20 14:04:19 -0700113inline
Andy Hungd5491392014-04-08 18:28:09 -0700114TC interpolate(TC coef_0, TC coef_1, TINTERP lerp)
115{
116 return lerp * (coef_1 - coef_0) + coef_0;
117}
118
Andy Hung42b01112014-07-20 14:04:19 -0700119template<>
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;
Andy Hungd5491392014-04-08 18:28:09 -0700124}
125
Andy Hung42b01112014-07-20 14:04:19 -0700126template<>
127inline
128int32_t interpolate<int32_t, uint32_t>(int32_t coef_0, int32_t coef_1, uint32_t lerp)
Andy Hungd5491392014-04-08 18:28:09 -0700129{
Andy Hung42b01112014-07-20 14:04:19 -0700130 return (lerp * static_cast<int64_t>(coef_1 - coef_0) >> 31) + coef_0;
Andy Hungd5491392014-04-08 18:28:09 -0700131}
132
Andy Hung68ffa202014-04-09 19:19:06 -0700133/* 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
Glenn Kastenb187de12014-12-30 08:18:15 -0800177template <int CHANNELS, int STRIDE, typename TFUNC, typename TC, typename TI, typename TO,
178 typename TINTERP>
Andy Hung68ffa202014-04-09 19:19:06 -0700179static inline
180void ProcessBase(TO* const out,
Glenn Kastena4daf0b2014-07-28 16:34:45 -0700181 size_t count,
Andy Hung68ffa202014-04-09 19:19:06 -0700182 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
246template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO>
247static inline
248void ProcessL(TO* const out,
249 int count,
250 const TC* coefsP,
251 const TC* coefsN,
252 const TI* sP,
253 const TI* sN,
254 const TO* const volumeLR)
255{
256 ProcessBase<CHANNELS, STRIDE, InterpNull>(out, count, coefsP, coefsN, sP, sN, 0, volumeLR);
257}
258
Andy Hungd5491392014-04-08 18:28:09 -0700259template <int CHANNELS, int STRIDE, typename TC, typename TI, typename TO, typename TINTERP>
260static inline
261void Process(TO* const out,
Andy Hung86eae0e2013-12-09 12:12:46 -0800262 int count,
263 const TC* coefsP,
264 const TC* coefsN,
Andy Hungd5491392014-04-08 18:28:09 -0700265 const TC* coefsP1 __unused,
266 const TC* coefsN1 __unused,
267 const TI* sP,
268 const TI* sN,
269 TINTERP lerpP,
270 const TO* const volumeLR)
Andy Hung86eae0e2013-12-09 12:12:46 -0800271{
Glenn Kastenb187de12014-12-30 08:18:15 -0800272 ProcessBase<CHANNELS, STRIDE, InterpCompute>(out, count, coefsP, coefsN, sP, sN, lerpP,
273 volumeLR);
Andy Hung86eae0e2013-12-09 12:12:46 -0800274}
275
276/*
Andy Hungd5491392014-04-08 18:28:09 -0700277 * Calculates a single output frame (two samples) from input sample pointer.
Andy Hung86eae0e2013-12-09 12:12:46 -0800278 *
279 * This sets up the params for the accelerated Process() and ProcessL()
280 * functions to do the appropriate dot products.
281 *
Andy Hungd5491392014-04-08 18:28:09 -0700282 * @param out should point to the output buffer with space for at least one output frame.
Andy Hung86eae0e2013-12-09 12:12:46 -0800283 *
Andy Hungd5491392014-04-08 18:28:09 -0700284 * @param phase is the fractional distance between input frames for interpolation:
Andy Hung86eae0e2013-12-09 12:12:46 -0800285 * phase >= 0 && phase < phaseWrapLimit. It can be thought of as a rational fraction
286 * of phase/phaseWrapLimit.
287 *
288 * @param phaseWrapLimit is #polyphases<<coefShift, where #polyphases is the number of polyphases
289 * in the polyphase filter. Likewise, #polyphases can be obtained as (phaseWrapLimit>>coefShift).
290 *
291 * @param coefShift gives the bit alignment of the polyphase index in the phase parameter.
292 *
293 * @param halfNumCoefs is the half the number of coefficients per polyphase filter. Since the
294 * overall filterbank is odd-length symmetric, only halfNumCoefs need be stored.
295 *
296 * @param coefs is the polyphase filter bank, starting at from polyphase index 0, and ranging to
297 * and including the #polyphases. Each polyphase of the filter has half-length halfNumCoefs
298 * (due to symmetry). The total size of the filter bank in coefficients is
299 * (#polyphases+1)*halfNumCoefs.
300 *
301 * The filter bank coefs should be aligned to a minimum of 16 bytes (preferrably to cache line).
302 *
303 * The coefs should be attenuated (to compensate for passband ripple)
304 * if storing back into the native format.
305 *
306 * @param samples are unaligned input samples. The position is in the "middle" of the
307 * sample array with respect to the FIR filter:
308 * the negative half of the filter is dot product from samples+1 to samples+halfNumCoefs;
309 * the positive half of the filter is dot product from samples to samples-halfNumCoefs+1.
310 *
311 * @param volumeLR is a pointer to an array of two 32 bit volume values, one per stereo channel,
312 * expressed as a S32 integer. A negative value inverts the channel 180 degrees.
313 * The pointer volumeLR should be aligned to a minimum of 8 bytes.
314 * A typical value for volume is 0x1000 to align to a unity gain output of 20.12.
315 *
316 * In between calls to filterCoefficient, the phase is incremented by phaseIncrement, where
317 * phaseIncrement is calculated as inputSampling * phaseWrapLimit / outputSampling.
318 *
319 * The filter polyphase index is given by indexP = phase >> coefShift. Due to
320 * odd length symmetric filter, the polyphase index of the negative half depends on
321 * whether interpolation is used.
322 *
323 * The fractional siting between the polyphase indices is given by the bits below coefShift:
324 *
325 * lerpP = phase << 32 - coefShift >> 1; // for 32 bit unsigned phase multiply
326 * lerpP = phase << 32 - coefShift >> 17; // for 16 bit unsigned phase multiply
327 *
328 * For integer types, this is expressed as:
329 *
330 * lerpP = phase << sizeof(phase)*8 - coefShift
331 * >> (sizeof(phase)-sizeof(*coefs))*8 + 1;
332 *
Andy Hungd5491392014-04-08 18:28:09 -0700333 * For floating point, lerpP is the fractional phase scaled to [0.0, 1.0):
334 *
335 * lerpP = (phase << 32 - coefShift) / (1 << 32); // floating point equivalent
Andy Hung86eae0e2013-12-09 12:12:46 -0800336 */
337
Andy Hungd5491392014-04-08 18:28:09 -0700338template<int CHANNELS, bool LOCKED, int STRIDE, typename TC, typename TI, typename TO>
Andy Hung86eae0e2013-12-09 12:12:46 -0800339static inline
Andy Hungd5491392014-04-08 18:28:09 -0700340void fir(TO* const out,
Andy Hung86eae0e2013-12-09 12:12:46 -0800341 const uint32_t phase, const uint32_t phaseWrapLimit,
342 const int coefShift, const int halfNumCoefs, const TC* const coefs,
Andy Hungd5491392014-04-08 18:28:09 -0700343 const TI* const samples, const TO* const volumeLR)
Andy Hung86eae0e2013-12-09 12:12:46 -0800344{
345 // NOTE: be very careful when modifying the code here. register
346 // pressure is very high and a small change might cause the compiler
347 // to generate far less efficient code.
348 // Always sanity check the result with objdump or test-resample.
349
350 if (LOCKED) {
351 // locked polyphase (no interpolation)
352 // Compute the polyphase filter index on the positive and negative side.
353 uint32_t indexP = phase >> coefShift;
354 uint32_t indexN = (phaseWrapLimit - phase) >> coefShift;
355 const TC* coefsP = coefs + indexP*halfNumCoefs;
356 const TC* coefsN = coefs + indexN*halfNumCoefs;
Andy Hungd5491392014-04-08 18:28:09 -0700357 const TI* sP = samples;
358 const TI* sN = samples + CHANNELS;
Andy Hung86eae0e2013-12-09 12:12:46 -0800359
360 // dot product filter.
361 ProcessL<CHANNELS, STRIDE>(out,
362 halfNumCoefs, coefsP, coefsN, sP, sN, volumeLR);
363 } else {
364 // interpolated polyphase
365 // Compute the polyphase filter index on the positive and negative side.
366 uint32_t indexP = phase >> coefShift;
367 uint32_t indexN = (phaseWrapLimit - phase - 1) >> coefShift; // one's complement.
368 const TC* coefsP = coefs + indexP*halfNumCoefs;
369 const TC* coefsN = coefs + indexN*halfNumCoefs;
370 const TC* coefsP1 = coefsP + halfNumCoefs;
371 const TC* coefsN1 = coefsN + halfNumCoefs;
Andy Hungd5491392014-04-08 18:28:09 -0700372 const TI* sP = samples;
373 const TI* sN = samples + CHANNELS;
Andy Hung86eae0e2013-12-09 12:12:46 -0800374
375 // Interpolation fraction lerpP derived by shifting all the way up and down
376 // to clear the appropriate bits and align to the appropriate level
377 // for the integer multiply. The constants should resolve in compile time.
378 //
379 // The interpolated filter coefficient is derived as follows for the pos/neg half:
380 //
381 // interpolated[P] = index[P]*lerpP + index[P+1]*(1-lerpP)
382 // interpolated[N] = index[N+1]*lerpP + index[N]*(1-lerpP)
Andy Hung86eae0e2013-12-09 12:12:46 -0800383
384 // on-the-fly interpolated dot product filter
Andy Hungd5491392014-04-08 18:28:09 -0700385 if (is_same<TC, float>::value || is_same<TC, double>::value) {
386 static const TC scale = 1. / (65536. * 65536.); // scale phase bits to [0.0, 1.0)
387 TC lerpP = TC(phase << (sizeof(phase)*8 - coefShift)) * scale;
388
389 Process<CHANNELS, STRIDE>(out,
390 halfNumCoefs, coefsP, coefsN, coefsP1, coefsN1, sP, sN, lerpP, volumeLR);
391 } else {
392 uint32_t lerpP = phase << (sizeof(phase)*8 - coefShift)
393 >> ((sizeof(phase)-sizeof(*coefs))*8 + 1);
394
395 Process<CHANNELS, STRIDE>(out,
396 halfNumCoefs, coefsP, coefsN, coefsP1, coefsN1, sP, sN, lerpP, volumeLR);
397 }
Andy Hung86eae0e2013-12-09 12:12:46 -0800398 }
399}
400
Glenn Kasten63238ef2015-03-02 15:50:29 -0800401} // namespace android
Andy Hung86eae0e2013-12-09 12:12:46 -0800402
403#endif /*ANDROID_AUDIO_RESAMPLER_FIR_PROCESS_H*/