Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 1 | /* $Id$ */ |
| 2 | /* |
| 3 | * Digital Audio Resampling Home Page located at |
| 4 | * http://www-ccrma.stanford.edu/~jos/resample/. |
| 5 | * |
| 6 | * SOFTWARE FOR SAMPLING-RATE CONVERSION AND FIR DIGITAL FILTER DESIGN |
| 7 | * |
| 8 | * Snippet from the resample.1 man page: |
| 9 | * |
| 10 | * HISTORY |
| 11 | * |
| 12 | * The first version of this software was written by Julius O. Smith III |
| 13 | * <jos@ccrma.stanford.edu> at CCRMA <http://www-ccrma.stanford.edu> in |
| 14 | * 1981. It was called SRCONV and was written in SAIL for PDP-10 |
| 15 | * compatible machines. The algorithm was first published in |
| 16 | * |
| 17 | * Smith, Julius O. and Phil Gossett. ``A Flexible Sampling-Rate |
| 18 | * Conversion Method,'' Proceedings (2): 19.4.1-19.4.4, IEEE Conference |
| 19 | * on Acoustics, Speech, and Signal Processing, San Diego, March 1984. |
| 20 | * |
| 21 | * An expanded tutorial based on this paper is available at the Digital |
| 22 | * Audio Resampling Home Page given above. |
| 23 | * |
| 24 | * Circa 1988, the SRCONV program was translated from SAIL to C by |
| 25 | * Christopher Lee Fraley working with Roger Dannenberg at CMU. |
| 26 | * |
| 27 | * Since then, the C version has been maintained by jos. |
| 28 | * |
| 29 | * Sndlib support was added 6/99 by John Gibson <jgg9c@virginia.edu>. |
| 30 | * |
| 31 | * The resample program is free software distributed in accordance |
| 32 | * with the Lesser GNU Public License (LGPL). There is NO warranty; not |
| 33 | * even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
| 34 | */ |
| 35 | |
| 36 | /* PJMEDIA modification: |
| 37 | * - remove resample(), just use SrcUp, SrcUD, and SrcLinear directly. |
| 38 | * - move FilterUp() and FilterUD() from filterkit.c |
| 39 | * - move stddefs.h and resample.h to this file. |
| 40 | * - const correctness. |
| 41 | */ |
| 42 | |
| 43 | #include <resamplesubs.h> |
| 44 | #include "config.h" |
| 45 | #include "stddefs.h" |
| 46 | #include "resample.h" |
| 47 | |
| 48 | |
| 49 | #ifdef _MSC_VER |
| 50 | # pragma warning(push, 3) |
| 51 | //# pragma warning(disable: 4245) // Conversion from uint to ushort |
| 52 | # pragma warning(disable: 4244) // Conversion from double to uint |
| 53 | # pragma warning(disable: 4146) // unary minus operator applied to unsigned type, result still unsigned |
| 54 | # pragma warning(disable: 4761) // integral size mismatch in argument; conversion supplied |
| 55 | #endif |
| 56 | |
| 57 | #if defined(RESAMPLE_HAS_SMALL_FILTER) && RESAMPLE_HAS_SMALL_FILTER!=0 |
| 58 | # include "smallfilter.h" |
| 59 | #else |
| 60 | # define SMALL_FILTER_NMULT 0 |
| 61 | # define SMALL_FILTER_SCALE 0 |
| 62 | # define SMALL_FILTER_NWING 0 |
| 63 | # define SMALL_FILTER_IMP NULL |
| 64 | # define SMALL_FILTER_IMPD NULL |
| 65 | #endif |
| 66 | |
| 67 | #if defined(RESAMPLE_HAS_LARGE_FILTER) && RESAMPLE_HAS_LARGE_FILTER!=0 |
| 68 | # include "largefilter.h" |
| 69 | #else |
| 70 | # define LARGE_FILTER_NMULT 0 |
| 71 | # define LARGE_FILTER_SCALE 0 |
| 72 | # define LARGE_FILTER_NWING 0 |
| 73 | # define LARGE_FILTER_IMP NULL |
| 74 | # define LARGE_FILTER_IMPD NULL |
| 75 | #endif |
| 76 | |
| 77 | |
| 78 | #undef INLINE |
| 79 | #define INLINE |
| 80 | #define HAVE_FILTER 0 |
| 81 | |
| 82 | #ifndef NULL |
| 83 | # define NULL 0 |
| 84 | #endif |
| 85 | |
| 86 | |
| 87 | static INLINE RES_HWORD WordToHword(RES_WORD v, int scl) |
| 88 | { |
| 89 | RES_HWORD out; |
| 90 | RES_WORD llsb = (1<<(scl-1)); |
| 91 | v += llsb; /* round */ |
| 92 | v >>= scl; |
| 93 | if (v>MAX_HWORD) { |
| 94 | v = MAX_HWORD; |
| 95 | } else if (v < MIN_HWORD) { |
| 96 | v = MIN_HWORD; |
| 97 | } |
| 98 | out = (RES_HWORD) v; |
| 99 | return out; |
| 100 | } |
| 101 | |
| 102 | /* Sampling rate conversion using linear interpolation for maximum speed. |
| 103 | */ |
| 104 | static int |
| 105 | SrcLinear(const RES_HWORD X[], RES_HWORD Y[], double pFactor, RES_UHWORD nx) |
| 106 | { |
| 107 | RES_HWORD iconst; |
| 108 | RES_UWORD time = 0; |
| 109 | const RES_HWORD *xp; |
| 110 | RES_HWORD *Ystart, *Yend; |
| 111 | RES_WORD v,x1,x2; |
| 112 | |
| 113 | double dt; /* Step through input signal */ |
| 114 | RES_UWORD dtb; /* Fixed-point version of Dt */ |
| 115 | RES_UWORD endTime; /* When time reaches EndTime, return to user */ |
| 116 | |
| 117 | dt = 1.0/pFactor; /* Output sampling period */ |
| 118 | dtb = dt*(1<<Np) + 0.5; /* Fixed-point representation */ |
| 119 | |
| 120 | Ystart = Y; |
| 121 | Yend = Ystart + (unsigned)(nx * pFactor + 0.5); |
| 122 | endTime = time + (1<<Np)*(RES_WORD)nx; |
| 123 | |
| 124 | // Integer round down in dtb calculation may cause (endTime % dtb > 0), |
| 125 | // so it may cause resample write pass the output buffer (Y >= Yend). |
| 126 | // while (time < endTime) |
| 127 | while (Y < Yend) |
| 128 | { |
| 129 | iconst = (time) & Pmask; |
| 130 | xp = &X[(time)>>Np]; /* Ptr to current input sample */ |
| 131 | x1 = *xp++; |
| 132 | x2 = *xp; |
| 133 | x1 *= ((1<<Np)-iconst); |
| 134 | x2 *= iconst; |
| 135 | v = x1 + x2; |
| 136 | *Y++ = WordToHword(v,Np); /* Deposit output */ |
| 137 | time += dtb; /* Move to next sample by time increment */ |
| 138 | } |
| 139 | return (Y - Ystart); /* Return number of output samples */ |
| 140 | } |
| 141 | |
| 142 | static RES_WORD FilterUp(const RES_HWORD Imp[], const RES_HWORD ImpD[], |
| 143 | RES_UHWORD Nwing, RES_BOOL Interp, |
| 144 | const RES_HWORD *Xp, RES_HWORD Ph, RES_HWORD Inc) |
| 145 | { |
| 146 | const RES_HWORD *Hp; |
| 147 | const RES_HWORD *Hdp = NULL; |
| 148 | const RES_HWORD *End; |
| 149 | RES_HWORD a = 0; |
| 150 | RES_WORD v, t; |
| 151 | |
| 152 | v=0; |
| 153 | Hp = &Imp[Ph>>Na]; |
| 154 | End = &Imp[Nwing]; |
| 155 | if (Interp) { |
| 156 | Hdp = &ImpD[Ph>>Na]; |
| 157 | a = Ph & Amask; |
| 158 | } |
| 159 | if (Inc == 1) /* If doing right wing... */ |
| 160 | { /* ...drop extra coeff, so when Ph is */ |
| 161 | End--; /* 0.5, we don't do too many mult's */ |
| 162 | if (Ph == 0) /* If the phase is zero... */ |
| 163 | { /* ...then we've already skipped the */ |
| 164 | Hp += Npc; /* first sample, so we must also */ |
| 165 | Hdp += Npc; /* skip ahead in Imp[] and ImpD[] */ |
| 166 | } |
| 167 | } |
| 168 | if (Interp) |
| 169 | while (Hp < End) { |
| 170 | t = *Hp; /* Get filter coeff */ |
| 171 | t += (((RES_WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ |
| 172 | Hdp += Npc; /* Filter coeff differences step */ |
| 173 | t *= *Xp; /* Mult coeff by input sample */ |
| 174 | if (t & (1<<(Nhxn-1))) /* Round, if needed */ |
| 175 | t += (1<<(Nhxn-1)); |
| 176 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 177 | v += t; /* The filter output */ |
| 178 | Hp += Npc; /* Filter coeff step */ |
| 179 | |
| 180 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 181 | } |
| 182 | else |
| 183 | while (Hp < End) { |
| 184 | t = *Hp; /* Get filter coeff */ |
| 185 | t *= *Xp; /* Mult coeff by input sample */ |
| 186 | if (t & (1<<(Nhxn-1))) /* Round, if needed */ |
| 187 | t += (1<<(Nhxn-1)); |
| 188 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 189 | v += t; /* The filter output */ |
| 190 | Hp += Npc; /* Filter coeff step */ |
| 191 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 192 | } |
| 193 | return(v); |
| 194 | } |
| 195 | |
| 196 | |
| 197 | static RES_WORD FilterUD(const RES_HWORD Imp[], const RES_HWORD ImpD[], |
| 198 | RES_UHWORD Nwing, RES_BOOL Interp, |
| 199 | const RES_HWORD *Xp, RES_HWORD Ph, RES_HWORD Inc, RES_UHWORD dhb) |
| 200 | { |
| 201 | RES_HWORD a; |
| 202 | const RES_HWORD *Hp, *Hdp, *End; |
| 203 | RES_WORD v, t; |
| 204 | RES_UWORD Ho; |
| 205 | |
| 206 | v=0; |
| 207 | Ho = (Ph*(RES_UWORD)dhb)>>Np; |
| 208 | End = &Imp[Nwing]; |
| 209 | if (Inc == 1) /* If doing right wing... */ |
| 210 | { /* ...drop extra coeff, so when Ph is */ |
| 211 | End--; /* 0.5, we don't do too many mult's */ |
| 212 | if (Ph == 0) /* If the phase is zero... */ |
| 213 | Ho += dhb; /* ...then we've already skipped the */ |
| 214 | } /* first sample, so we must also */ |
| 215 | /* skip ahead in Imp[] and ImpD[] */ |
| 216 | if (Interp) |
| 217 | while ((Hp = &Imp[Ho>>Na]) < End) { |
| 218 | t = *Hp; /* Get IR sample */ |
| 219 | Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table*/ |
| 220 | a = Ho & Amask; /* a is logically between 0 and 1 */ |
| 221 | t += (((RES_WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ |
| 222 | t *= *Xp; /* Mult coeff by input sample */ |
| 223 | if (t & 1<<(Nhxn-1)) /* Round, if needed */ |
| 224 | t += 1<<(Nhxn-1); |
| 225 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 226 | v += t; /* The filter output */ |
| 227 | Ho += dhb; /* IR step */ |
| 228 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 229 | } |
| 230 | else |
| 231 | while ((Hp = &Imp[Ho>>Na]) < End) { |
| 232 | t = *Hp; /* Get IR sample */ |
| 233 | t *= *Xp; /* Mult coeff by input sample */ |
| 234 | if (t & 1<<(Nhxn-1)) /* Round, if needed */ |
| 235 | t += 1<<(Nhxn-1); |
| 236 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 237 | v += t; /* The filter output */ |
| 238 | Ho += dhb; /* IR step */ |
| 239 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 240 | } |
| 241 | return(v); |
| 242 | } |
| 243 | |
| 244 | /* Sampling rate up-conversion only subroutine; |
| 245 | * Slightly faster than down-conversion; |
| 246 | */ |
| 247 | static int SrcUp(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 248 | RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl, |
| 249 | const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp) |
| 250 | { |
| 251 | const RES_HWORD *xp; |
| 252 | RES_HWORD *Ystart, *Yend; |
| 253 | RES_WORD v; |
| 254 | |
| 255 | double dt; /* Step through input signal */ |
| 256 | RES_UWORD dtb; /* Fixed-point version of Dt */ |
| 257 | RES_UWORD time = 0; |
| 258 | RES_UWORD endTime; /* When time reaches EndTime, return to user */ |
| 259 | |
| 260 | dt = 1.0/pFactor; /* Output sampling period */ |
| 261 | dtb = dt*(1<<Np) + 0.5; /* Fixed-point representation */ |
| 262 | |
| 263 | Ystart = Y; |
| 264 | Yend = Ystart + (unsigned)(nx * pFactor + 0.5); |
| 265 | endTime = time + (1<<Np)*(RES_WORD)nx; |
| 266 | |
| 267 | // Integer round down in dtb calculation may cause (endTime % dtb > 0), |
| 268 | // so it may cause resample write pass the output buffer (Y >= Yend). |
| 269 | // while (time < endTime) |
| 270 | while (Y < Yend) |
| 271 | { |
| 272 | xp = &X[time>>Np]; /* Ptr to current input sample */ |
| 273 | /* Perform left-wing inner product */ |
| 274 | v = 0; |
| 275 | v = FilterUp(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask),-1); |
| 276 | |
| 277 | /* Perform right-wing inner product */ |
| 278 | v += FilterUp(pImp, pImpD, pNwing, Interp, xp+1, (RES_HWORD)((-time)&Pmask),1); |
| 279 | |
| 280 | v >>= Nhg; /* Make guard bits */ |
| 281 | v *= pLpScl; /* Normalize for unity filter gain */ |
| 282 | *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ |
| 283 | time += dtb; /* Move to next sample by time increment */ |
| 284 | } |
| 285 | return (Y - Ystart); /* Return the number of output samples */ |
| 286 | } |
| 287 | |
| 288 | |
| 289 | /* Sampling rate conversion subroutine */ |
| 290 | |
| 291 | static int SrcUD(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 292 | RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl, |
| 293 | const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp) |
| 294 | { |
| 295 | const RES_HWORD *xp; |
| 296 | RES_HWORD *Ystart, *Yend; |
| 297 | RES_WORD v; |
| 298 | |
| 299 | double dh; /* Step through filter impulse response */ |
| 300 | double dt; /* Step through input signal */ |
| 301 | RES_UWORD time = 0; |
| 302 | RES_UWORD endTime; /* When time reaches EndTime, return to user */ |
| 303 | RES_UWORD dhb, dtb; /* Fixed-point versions of Dh,Dt */ |
| 304 | |
| 305 | dt = 1.0/pFactor; /* Output sampling period */ |
| 306 | dtb = dt*(1<<Np) + 0.5; /* Fixed-point representation */ |
| 307 | |
| 308 | dh = MIN(Npc, pFactor*Npc); /* Filter sampling period */ |
| 309 | dhb = dh*(1<<Na) + 0.5; /* Fixed-point representation */ |
| 310 | |
| 311 | Ystart = Y; |
| 312 | Yend = Ystart + (unsigned)(nx * pFactor + 0.5); |
| 313 | endTime = time + (1<<Np)*(RES_WORD)nx; |
| 314 | |
| 315 | // Integer round down in dtb calculation may cause (endTime % dtb > 0), |
| 316 | // so it may cause resample write pass the output buffer (Y >= Yend). |
| 317 | // while (time < endTime) |
| 318 | while (Y < Yend) |
| 319 | { |
| 320 | xp = &X[time>>Np]; /* Ptr to current input sample */ |
| 321 | v = FilterUD(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask), |
| 322 | -1, dhb); /* Perform left-wing inner product */ |
| 323 | v += FilterUD(pImp, pImpD, pNwing, Interp, xp+1, (RES_HWORD)((-time)&Pmask), |
| 324 | 1, dhb); /* Perform right-wing inner product */ |
| 325 | v >>= Nhg; /* Make guard bits */ |
| 326 | v *= pLpScl; /* Normalize for unity filter gain */ |
| 327 | *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ |
| 328 | time += dtb; /* Move to next sample by time increment */ |
| 329 | } |
| 330 | return (Y - Ystart); /* Return the number of output samples */ |
| 331 | } |
| 332 | |
| 333 | |
| 334 | DECL(int) res_SrcLinear(const RES_HWORD X[], RES_HWORD Y[], |
| 335 | double pFactor, RES_UHWORD nx) |
| 336 | { |
| 337 | return SrcLinear(X, Y, pFactor, nx); |
| 338 | } |
| 339 | |
| 340 | DECL(int) res_Resample(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 341 | RES_UHWORD nx, RES_BOOL LargeF, RES_BOOL Interp) |
| 342 | { |
| 343 | if (pFactor >= 1) { |
| 344 | |
| 345 | if (LargeF) |
| 346 | return SrcUp(X, Y, pFactor, nx, |
| 347 | LARGE_FILTER_NWING, LARGE_FILTER_SCALE, |
| 348 | LARGE_FILTER_IMP, LARGE_FILTER_IMPD, Interp); |
| 349 | else |
| 350 | return SrcUp(X, Y, pFactor, nx, |
| 351 | SMALL_FILTER_NWING, SMALL_FILTER_SCALE, |
| 352 | SMALL_FILTER_IMP, SMALL_FILTER_IMPD, Interp); |
| 353 | |
| 354 | } else { |
| 355 | |
| 356 | if (LargeF) |
| 357 | return SrcUD(X, Y, pFactor, nx, |
| 358 | LARGE_FILTER_NWING, LARGE_FILTER_SCALE * pFactor + 0.5, |
| 359 | LARGE_FILTER_IMP, LARGE_FILTER_IMPD, Interp); |
| 360 | else |
| 361 | return SrcUD(X, Y, pFactor, nx, |
| 362 | SMALL_FILTER_NWING, SMALL_FILTER_SCALE * pFactor + 0.5, |
| 363 | SMALL_FILTER_IMP, SMALL_FILTER_IMPD, Interp); |
| 364 | |
| 365 | } |
| 366 | } |
| 367 | |
| 368 | DECL(int) res_GetXOFF(double pFactor, RES_BOOL LargeF) |
| 369 | { |
| 370 | if (LargeF) |
| 371 | return (LARGE_FILTER_NMULT + 1) / 2.0 * |
| 372 | MAX(1.0, 1.0/pFactor); |
| 373 | else |
| 374 | return (SMALL_FILTER_NMULT + 1) / 2.0 * |
| 375 | MAX(1.0, 1.0/pFactor); |
| 376 | } |
| 377 | |