1 /* 2 * Copyright (C) 2007 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 #include <string.h> 18 #include "AudioResamplerSinc.h" 19 20 namespace android { 21 // ---------------------------------------------------------------------------- 22 23 24 /* 25 * These coeficients are computed with the "fir" utility found in 26 * tools/resampler_tools 27 * TODO: A good optimization would be to transpose this matrix, to take 28 * better advantage of the data-cache. 29 */ 30 const int32_t AudioResamplerSinc::mFirCoefsUp[] = { 31 0x7fffffff, 0x7f15d078, 0x7c5e0da6, 0x77ecd867, 0x71e2e251, 0x6a6c304a, 0x61be7269, 0x58170412, 0x4db8ab05, 0x42e92ea6, 0x37eee214, 0x2d0e3bb1, 0x22879366, 0x18951e95, 0x0f693d0d, 0x072d2621, 32 0x00000000, 0xf9f66655, 0xf51a5fd7, 0xf16bbd84, 0xeee0d9ac, 0xed67a922, 0xece70de6, 0xed405897, 0xee50e505, 0xeff3be30, 0xf203370f, 0xf45a6741, 0xf6d67d53, 0xf957db66, 0xfbc2f647, 0xfe00f2b9, 33 0x00000000, 0x01b37218, 0x0313a0c6, 0x041d930d, 0x04d28057, 0x053731b0, 0x05534dff, 0x05309bfd, 0x04da440d, 0x045c1aee, 0x03c1fcdd, 0x03173ef5, 0x02663ae8, 0x01b7f736, 0x0113ec79, 0x007fe6a9, 34 0x00000000, 0xff96b229, 0xff44f99f, 0xff0a86be, 0xfee5f803, 0xfed518fd, 0xfed521fd, 0xfee2f4fd, 0xfefb54f8, 0xff1b159b, 0xff3f4203, 0xff6539e0, 0xff8ac502, 0xffae1ddd, 0xffcdf3f9, 0xffe96798, 35 0x00000000, 0x00119de6, 0x001e6b7e, 0x0026cb7a, 0x002b4830, 0x002c83d6, 0x002b2a82, 0x0027e67a, 0x002356f9, 0x001e098e, 0x001875e4, 0x0012fbbe, 0x000de2d1, 0x00095c10, 0x00058414, 0x00026636, 36 0x00000000, 0xfffe44a9, 0xfffd206d, 0xfffc7b7f, 0xfffc3c8f, 0xfffc4ac2, 0xfffc8f2b, 0xfffcf5c4, 0xfffd6df3, 0xfffdeab2, 0xfffe6275, 0xfffececf, 0xffff2c07, 0xffff788c, 0xffffb471, 0xffffe0f2, 37 0x00000000, 0x000013e6, 0x00001f03, 0x00002396, 0x00002399, 0x000020b6, 0x00001c3c, 0x00001722, 0x00001216, 0x00000d81, 0x0000099c, 0x0000067c, 0x00000419, 0x0000025f, 0x00000131, 0x00000070, 38 0x00000000, 0xffffffc7, 0xffffffb3, 0xffffffb3, 0xffffffbe, 0xffffffcd, 0xffffffdb, 0xffffffe7, 0xfffffff0, 0xfffffff7, 0xfffffffb, 0xfffffffe, 0xffffffff, 0x00000000, 0x00000000, 0x00000000, 39 0x00000000 // this one is needed for lerping the last coefficient 40 }; 41 42 /* 43 * These coefficients are optimized for 48KHz -> 44.1KHz (stop-band at 22.050KHz) 44 * It's possible to use the above coefficient for any down-sampling 45 * at the expense of a slower processing loop (we can interpolate 46 * these coefficient from the above by "Stretching" them in time). 47 */ 48 const int32_t AudioResamplerSinc::mFirCoefsDown[] = { 49 0x7fffffff, 0x7f55e46d, 0x7d5b4c60, 0x7a1b4b98, 0x75a7fb14, 0x7019f0bd, 0x698f875a, 0x622bfd59, 0x5a167256, 0x5178cc54, 0x487e8e6c, 0x3f53aae8, 0x36235ad4, 0x2d17047b, 0x245539ab, 0x1c00d540, 50 0x14383e57, 0x0d14d5ca, 0x06aa910b, 0x0107c38b, 0xfc351654, 0xf835abae, 0xf5076b45, 0xf2a37202, 0xf0fe9faa, 0xf00a3bbd, 0xefb4aa81, 0xefea2b05, 0xf0959716, 0xf1a11e83, 0xf2f6f7a0, 0xf481fff4, 51 0xf62e48ce, 0xf7e98ca5, 0xf9a38b4c, 0xfb4e4bfa, 0xfcde456f, 0xfe4a6d30, 0xff8c2fdf, 0x009f5555, 0x0181d393, 0x0233940f, 0x02b62f06, 0x030ca07d, 0x033afa62, 0x03461725, 0x03334f83, 0x030835fa, 52 0x02ca59cc, 0x027f12d1, 0x022b570d, 0x01d39a49, 0x017bb78f, 0x0126e414, 0x00d7aaaf, 0x008feec7, 0x0050f584, 0x001b73e3, 0xffefa063, 0xffcd46ed, 0xffb3ddcd, 0xffa29aaa, 0xff988691, 0xff949066, 53 0xff959d24, 0xff9a959e, 0xffa27195, 0xffac4011, 0xffb72d2b, 0xffc28569, 0xffcdb706, 0xffd85171, 0xffe20364, 0xffea97e9, 0xfff1f2b2, 0xfff80c06, 0xfffcec92, 0x0000a955, 0x00035fd8, 0x000532cf, 54 0x00064735, 0x0006c1f9, 0x0006c62d, 0x000673ba, 0x0005e68f, 0x00053630, 0x000475a3, 0x0003b397, 0x0002fac1, 0x00025257, 0x0001be9e, 0x0001417a, 0x0000dafd, 0x000089eb, 0x00004c28, 0x00001f1d, 55 0x00000000, 0xffffec10, 0xffffe0be, 0xffffdbc5, 0xffffdb39, 0xffffdd8b, 0xffffe182, 0xffffe638, 0xffffeb0a, 0xffffef8f, 0xfffff38b, 0xfffff6e3, 0xfffff993, 0xfffffba6, 0xfffffd30, 0xfffffe4a, 56 0xffffff09, 0xffffff85, 0xffffffd1, 0xfffffffb, 0x0000000f, 0x00000016, 0x00000015, 0x00000012, 0x0000000d, 0x00000009, 0x00000006, 0x00000003, 0x00000002, 0x00000001, 0x00000000, 0x00000000, 57 0x00000000 // this one is needed for lerping the last coefficient 58 }; 59 60 // ---------------------------------------------------------------------------- 61 62 static inline 63 int32_t mulRL(int left, int32_t in, uint32_t vRL) 64 { 65 #if defined(__arm__) && !defined(__thumb__) 66 int32_t out; 67 if (left) { 68 asm( "smultb %[out], %[in], %[vRL] \n" 69 : [out]"=r"(out) 70 : [in]"%r"(in), [vRL]"r"(vRL) 71 : ); 72 } else { 73 asm( "smultt %[out], %[in], %[vRL] \n" 74 : [out]"=r"(out) 75 : [in]"%r"(in), [vRL]"r"(vRL) 76 : ); 77 } 78 return out; 79 #else 80 if (left) { 81 return int16_t(in>>16) * int16_t(vRL&0xFFFF); 82 } else { 83 return int16_t(in>>16) * int16_t(vRL>>16); 84 } 85 #endif 86 } 87 88 static inline 89 int32_t mulAdd(int16_t in, int32_t v, int32_t a) 90 { 91 #if defined(__arm__) && !defined(__thumb__) 92 int32_t out; 93 asm( "smlawb %[out], %[v], %[in], %[a] \n" 94 : [out]"=r"(out) 95 : [in]"%r"(in), [v]"r"(v), [a]"r"(a) 96 : ); 97 return out; 98 #else 99 return a + in * (v>>16); 100 // improved precision 101 // return a + in * (v>>16) + ((in * (v & 0xffff)) >> 16); 102 #endif 103 } 104 105 static inline 106 int32_t mulAddRL(int left, uint32_t inRL, int32_t v, int32_t a) 107 { 108 #if defined(__arm__) && !defined(__thumb__) 109 int32_t out; 110 if (left) { 111 asm( "smlawb %[out], %[v], %[inRL], %[a] \n" 112 : [out]"=r"(out) 113 : [inRL]"%r"(inRL), [v]"r"(v), [a]"r"(a) 114 : ); 115 } else { 116 asm( "smlawt %[out], %[v], %[inRL], %[a] \n" 117 : [out]"=r"(out) 118 : [inRL]"%r"(inRL), [v]"r"(v), [a]"r"(a) 119 : ); 120 } 121 return out; 122 #else 123 if (left) { 124 return a + (int16_t(inRL&0xFFFF) * (v>>16)); 125 //improved precision 126 // return a + (int16_t(inRL&0xFFFF) * (v>>16)) + ((int16_t(inRL&0xFFFF) * (v & 0xffff)) >> 16); 127 } else { 128 return a + (int16_t(inRL>>16) * (v>>16)); 129 } 130 #endif 131 } 132 133 // ---------------------------------------------------------------------------- 134 135 AudioResamplerSinc::AudioResamplerSinc(int bitDepth, 136 int inChannelCount, int32_t sampleRate) 137 : AudioResampler(bitDepth, inChannelCount, sampleRate), 138 mState(0) 139 { 140 /* 141 * Layout of the state buffer for 32 tap: 142 * 143 * "present" sample beginning of 2nd buffer 144 * v v 145 * 0 01 2 23 3 146 * 0 F0 0 F0 F 147 * [pppppppppppppppInnnnnnnnnnnnnnnnpppppppppppppppInnnnnnnnnnnnnnnn] 148 * ^ ^ head 149 * 150 * p = past samples, convoluted with the (p)ositive side of sinc() 151 * n = future samples, convoluted with the (n)egative side of sinc() 152 * r = extra space for implementing the ring buffer 153 * 154 */ 155 156 const size_t numCoefs = 2*halfNumCoefs; 157 const size_t stateSize = numCoefs * inChannelCount * 2; 158 mState = new int16_t[stateSize]; 159 memset(mState, 0, sizeof(int16_t)*stateSize); 160 mImpulse = mState + (halfNumCoefs-1)*inChannelCount; 161 mRingFull = mImpulse + (numCoefs+1)*inChannelCount; 162 } 163 164 AudioResamplerSinc::~AudioResamplerSinc() 165 { 166 delete [] mState; 167 } 168 169 void AudioResamplerSinc::init() { 170 } 171 172 void AudioResamplerSinc::resample(int32_t* out, size_t outFrameCount, 173 AudioBufferProvider* provider) 174 { 175 mFirCoefs = (mInSampleRate <= mSampleRate) ? mFirCoefsUp : mFirCoefsDown; 176 177 // select the appropriate resampler 178 switch (mChannelCount) { 179 case 1: 180 resample<1>(out, outFrameCount, provider); 181 break; 182 case 2: 183 resample<2>(out, outFrameCount, provider); 184 break; 185 } 186 } 187 188 189 template<int CHANNELS> 190 void AudioResamplerSinc::resample(int32_t* out, size_t outFrameCount, 191 AudioBufferProvider* provider) 192 { 193 int16_t* impulse = mImpulse; 194 uint32_t vRL = mVolumeRL; 195 size_t inputIndex = mInputIndex; 196 uint32_t phaseFraction = mPhaseFraction; 197 uint32_t phaseIncrement = mPhaseIncrement; 198 size_t outputIndex = 0; 199 size_t outputSampleCount = outFrameCount * 2; 200 size_t inFrameCount = (outFrameCount*mInSampleRate)/mSampleRate; 201 202 AudioBufferProvider::Buffer& buffer(mBuffer); 203 while (outputIndex < outputSampleCount) { 204 // buffer is empty, fetch a new one 205 while (buffer.frameCount == 0) { 206 buffer.frameCount = inFrameCount; 207 provider->getNextBuffer(&buffer); 208 if (buffer.raw == NULL) { 209 goto resample_exit; 210 } 211 const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits; 212 if (phaseIndex == 1) { 213 // read one frame 214 read<CHANNELS>(impulse, phaseFraction, buffer.i16, inputIndex); 215 } else if (phaseIndex == 2) { 216 // read 2 frames 217 read<CHANNELS>(impulse, phaseFraction, buffer.i16, inputIndex); 218 inputIndex++; 219 if (inputIndex >= mBuffer.frameCount) { 220 inputIndex -= mBuffer.frameCount; 221 provider->releaseBuffer(&buffer); 222 } else { 223 read<CHANNELS>(impulse, phaseFraction, buffer.i16, inputIndex); 224 } 225 } 226 } 227 int16_t *in = buffer.i16; 228 const size_t frameCount = buffer.frameCount; 229 230 // Always read-in the first samples from the input buffer 231 int16_t* head = impulse + halfNumCoefs*CHANNELS; 232 head[0] = in[inputIndex*CHANNELS + 0]; 233 if (CHANNELS == 2) 234 head[1] = in[inputIndex*CHANNELS + 1]; 235 236 // handle boundary case 237 int32_t l, r; 238 while (outputIndex < outputSampleCount) { 239 filterCoefficient<CHANNELS>(l, r, phaseFraction, impulse); 240 out[outputIndex++] += 2 * mulRL(1, l, vRL); 241 out[outputIndex++] += 2 * mulRL(0, r, vRL); 242 243 phaseFraction += phaseIncrement; 244 const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits; 245 if (phaseIndex == 1) { 246 inputIndex++; 247 if (inputIndex >= frameCount) 248 break; // need a new buffer 249 read<CHANNELS>(impulse, phaseFraction, in, inputIndex); 250 } else if(phaseIndex == 2) { // maximum value 251 inputIndex++; 252 if (inputIndex >= frameCount) 253 break; // 0 frame available, 2 frames needed 254 // read first frame 255 read<CHANNELS>(impulse, phaseFraction, in, inputIndex); 256 inputIndex++; 257 if (inputIndex >= frameCount) 258 break; // 0 frame available, 1 frame needed 259 // read second frame 260 read<CHANNELS>(impulse, phaseFraction, in, inputIndex); 261 } 262 } 263 264 // if done with buffer, save samples 265 if (inputIndex >= frameCount) { 266 inputIndex -= frameCount; 267 provider->releaseBuffer(&buffer); 268 } 269 } 270 271 resample_exit: 272 mImpulse = impulse; 273 mInputIndex = inputIndex; 274 mPhaseFraction = phaseFraction; 275 } 276 277 template<int CHANNELS> 278 /*** 279 * read() 280 * 281 * This function reads only one frame from input buffer and writes it in 282 * state buffer 283 * 284 **/ 285 void AudioResamplerSinc::read( 286 int16_t*& impulse, uint32_t& phaseFraction, 287 int16_t const* in, size_t inputIndex) 288 { 289 const uint32_t phaseIndex = phaseFraction >> kNumPhaseBits; 290 impulse += CHANNELS; 291 phaseFraction -= 1LU<<kNumPhaseBits; 292 if (impulse >= mRingFull) { 293 const size_t stateSize = (halfNumCoefs*2)*CHANNELS; 294 memcpy(mState, mState+stateSize, sizeof(int16_t)*stateSize); 295 impulse -= stateSize; 296 } 297 int16_t* head = impulse + halfNumCoefs*CHANNELS; 298 head[0] = in[inputIndex*CHANNELS + 0]; 299 if (CHANNELS == 2) 300 head[1] = in[inputIndex*CHANNELS + 1]; 301 } 302 303 template<int CHANNELS> 304 void AudioResamplerSinc::filterCoefficient( 305 int32_t& l, int32_t& r, uint32_t phase, int16_t const *samples) 306 { 307 // compute the index of the coefficient on the positive side and 308 // negative side 309 uint32_t indexP = (phase & cMask) >> cShift; 310 uint16_t lerpP = (phase & pMask) >> pShift; 311 uint32_t indexN = (-phase & cMask) >> cShift; 312 uint16_t lerpN = (-phase & pMask) >> pShift; 313 if ((indexP == 0) && (lerpP == 0)) { 314 indexN = cMask >> cShift; 315 lerpN = pMask >> pShift; 316 } 317 318 l = 0; 319 r = 0; 320 int32_t const* coefs = mFirCoefs; 321 int16_t const *sP = samples; 322 int16_t const *sN = samples+CHANNELS; 323 for (unsigned int i=0 ; i<halfNumCoefs/4 ; i++) { 324 interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP); 325 interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN); 326 sP -= CHANNELS; sN += CHANNELS; coefs += 1<<coefsBits; 327 interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP); 328 interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN); 329 sP -= CHANNELS; sN += CHANNELS; coefs += 1<<coefsBits; 330 interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP); 331 interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN); 332 sP -= CHANNELS; sN += CHANNELS; coefs += 1<<coefsBits; 333 interpolate<CHANNELS>(l, r, coefs+indexP, lerpP, sP); 334 interpolate<CHANNELS>(l, r, coefs+indexN, lerpN, sN); 335 sP -= CHANNELS; sN += CHANNELS; coefs += 1<<coefsBits; 336 } 337 } 338 339 template<int CHANNELS> 340 void AudioResamplerSinc::interpolate( 341 int32_t& l, int32_t& r, 342 int32_t const* coefs, int16_t lerp, int16_t const* samples) 343 { 344 int32_t c0 = coefs[0]; 345 int32_t c1 = coefs[1]; 346 int32_t sinc = mulAdd(lerp, (c1-c0)<<1, c0); 347 if (CHANNELS == 2) { 348 uint32_t rl = *reinterpret_cast<uint32_t const*>(samples); 349 l = mulAddRL(1, rl, sinc, l); 350 r = mulAddRL(0, rl, sinc, r); 351 } else { 352 r = l = mulAdd(samples[0], sinc, l); 353 } 354 } 355 356 // ---------------------------------------------------------------------------- 357 }; // namespace android 358 359