Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [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); |
| 122 | endTime = time + (1<<Np)*(RES_WORD)nx; |
| 123 | while (time < endTime) |
| 124 | { |
| 125 | iconst = (time) & Pmask; |
| 126 | xp = &X[(time)>>Np]; /* Ptr to current input sample */ |
| 127 | x1 = *xp++; |
| 128 | x2 = *xp; |
| 129 | x1 *= ((1<<Np)-iconst); |
| 130 | x2 *= iconst; |
| 131 | v = x1 + x2; |
| 132 | *Y++ = WordToHword(v,Np); /* Deposit output */ |
| 133 | time += dtb; /* Move to next sample by time increment */ |
| 134 | } |
| 135 | return (Y - Ystart); /* Return number of output samples */ |
| 136 | } |
| 137 | |
| 138 | static RES_WORD FilterUp(const RES_HWORD Imp[], const RES_HWORD ImpD[], |
| 139 | RES_UHWORD Nwing, RES_BOOL Interp, |
| 140 | const RES_HWORD *Xp, RES_HWORD Ph, RES_HWORD Inc) |
| 141 | { |
| 142 | const RES_HWORD *Hp; |
| 143 | const RES_HWORD *Hdp = NULL; |
| 144 | const RES_HWORD *End; |
| 145 | RES_HWORD a = 0; |
| 146 | RES_WORD v, t; |
| 147 | |
| 148 | v=0; |
| 149 | Hp = &Imp[Ph>>Na]; |
| 150 | End = &Imp[Nwing]; |
| 151 | if (Interp) { |
| 152 | Hdp = &ImpD[Ph>>Na]; |
| 153 | a = Ph & Amask; |
| 154 | } |
| 155 | if (Inc == 1) /* If doing right wing... */ |
| 156 | { /* ...drop extra coeff, so when Ph is */ |
| 157 | End--; /* 0.5, we don't do too many mult's */ |
| 158 | if (Ph == 0) /* If the phase is zero... */ |
| 159 | { /* ...then we've already skipped the */ |
| 160 | Hp += Npc; /* first sample, so we must also */ |
| 161 | Hdp += Npc; /* skip ahead in Imp[] and ImpD[] */ |
| 162 | } |
| 163 | } |
| 164 | if (Interp) |
| 165 | while (Hp < End) { |
| 166 | t = *Hp; /* Get filter coeff */ |
| 167 | t += (((RES_WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ |
| 168 | Hdp += Npc; /* Filter coeff differences step */ |
| 169 | t *= *Xp; /* Mult coeff by input sample */ |
| 170 | if (t & (1<<(Nhxn-1))) /* Round, if needed */ |
| 171 | t += (1<<(Nhxn-1)); |
| 172 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 173 | v += t; /* The filter output */ |
| 174 | Hp += Npc; /* Filter coeff step */ |
| 175 | |
| 176 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 177 | } |
| 178 | else |
| 179 | while (Hp < End) { |
| 180 | t = *Hp; /* Get filter coeff */ |
| 181 | t *= *Xp; /* Mult coeff by input sample */ |
| 182 | if (t & (1<<(Nhxn-1))) /* Round, if needed */ |
| 183 | t += (1<<(Nhxn-1)); |
| 184 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 185 | v += t; /* The filter output */ |
| 186 | Hp += Npc; /* Filter coeff step */ |
| 187 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 188 | } |
| 189 | return(v); |
| 190 | } |
| 191 | |
| 192 | |
| 193 | static RES_WORD FilterUD(const RES_HWORD Imp[], const RES_HWORD ImpD[], |
| 194 | RES_UHWORD Nwing, RES_BOOL Interp, |
| 195 | const RES_HWORD *Xp, RES_HWORD Ph, RES_HWORD Inc, RES_UHWORD dhb) |
| 196 | { |
| 197 | RES_HWORD a; |
| 198 | const RES_HWORD *Hp, *Hdp, *End; |
| 199 | RES_WORD v, t; |
| 200 | RES_UWORD Ho; |
| 201 | |
| 202 | v=0; |
| 203 | Ho = (Ph*(RES_UWORD)dhb)>>Np; |
| 204 | End = &Imp[Nwing]; |
| 205 | if (Inc == 1) /* If doing right wing... */ |
| 206 | { /* ...drop extra coeff, so when Ph is */ |
| 207 | End--; /* 0.5, we don't do too many mult's */ |
| 208 | if (Ph == 0) /* If the phase is zero... */ |
| 209 | Ho += dhb; /* ...then we've already skipped the */ |
| 210 | } /* first sample, so we must also */ |
| 211 | /* skip ahead in Imp[] and ImpD[] */ |
| 212 | if (Interp) |
| 213 | while ((Hp = &Imp[Ho>>Na]) < End) { |
| 214 | t = *Hp; /* Get IR sample */ |
| 215 | Hdp = &ImpD[Ho>>Na]; /* get interp (lower Na) bits from diff table*/ |
| 216 | a = Ho & Amask; /* a is logically between 0 and 1 */ |
| 217 | t += (((RES_WORD)*Hdp)*a)>>Na; /* t is now interp'd filter coeff */ |
| 218 | t *= *Xp; /* Mult coeff by input sample */ |
| 219 | if (t & 1<<(Nhxn-1)) /* Round, if needed */ |
| 220 | t += 1<<(Nhxn-1); |
| 221 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 222 | v += t; /* The filter output */ |
| 223 | Ho += dhb; /* IR step */ |
| 224 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 225 | } |
| 226 | else |
| 227 | while ((Hp = &Imp[Ho>>Na]) < End) { |
| 228 | t = *Hp; /* Get IR sample */ |
| 229 | t *= *Xp; /* Mult coeff by input sample */ |
| 230 | if (t & 1<<(Nhxn-1)) /* Round, if needed */ |
| 231 | t += 1<<(Nhxn-1); |
| 232 | t >>= Nhxn; /* Leave some guard bits, but come back some */ |
| 233 | v += t; /* The filter output */ |
| 234 | Ho += dhb; /* IR step */ |
| 235 | Xp += Inc; /* Input signal step. NO CHECK ON BOUNDS */ |
| 236 | } |
| 237 | return(v); |
| 238 | } |
| 239 | |
| 240 | /* Sampling rate up-conversion only subroutine; |
| 241 | * Slightly faster than down-conversion; |
| 242 | */ |
| 243 | static int SrcUp(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 244 | RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl, |
| 245 | const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp) |
| 246 | { |
| 247 | const RES_HWORD *xp; |
| 248 | RES_HWORD *Ystart, *Yend; |
| 249 | RES_WORD v; |
| 250 | |
| 251 | double dt; /* Step through input signal */ |
| 252 | RES_UWORD dtb; /* Fixed-point version of Dt */ |
| 253 | RES_UWORD time = 0; |
| 254 | RES_UWORD endTime; /* When time reaches EndTime, return to user */ |
| 255 | |
| 256 | dt = 1.0/pFactor; /* Output sampling period */ |
| 257 | dtb = dt*(1<<Np) + 0.5; /* Fixed-point representation */ |
| 258 | |
| 259 | Ystart = Y; |
| 260 | Yend = Ystart + (unsigned)(nx * pFactor); |
| 261 | endTime = time + (1<<Np)*(RES_WORD)nx; |
Nanang Izzuddin | ec19826 | 2008-05-28 17:26:30 +0000 | [diff] [blame] | 262 | |
| 263 | // Integer round down in dtb calculation may cause (endTime % dtb > 0), |
| 264 | // so it may cause resample write pass the output buffer (Y >= Yend). |
| 265 | // while (time < endTime) |
| 266 | while (Y < Yend) |
Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [diff] [blame] | 267 | { |
| 268 | xp = &X[time>>Np]; /* Ptr to current input sample */ |
| 269 | /* Perform left-wing inner product */ |
| 270 | v = 0; |
| 271 | v = FilterUp(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask),-1); |
| 272 | |
| 273 | /* Perform right-wing inner product */ |
| 274 | v += FilterUp(pImp, pImpD, pNwing, Interp, xp+1, (RES_HWORD)((-time)&Pmask),1); |
| 275 | |
| 276 | v >>= Nhg; /* Make guard bits */ |
| 277 | v *= pLpScl; /* Normalize for unity filter gain */ |
| 278 | *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ |
| 279 | time += dtb; /* Move to next sample by time increment */ |
| 280 | } |
| 281 | return (Y - Ystart); /* Return the number of output samples */ |
| 282 | } |
| 283 | |
| 284 | |
| 285 | /* Sampling rate conversion subroutine */ |
| 286 | |
| 287 | static int SrcUD(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 288 | RES_UHWORD nx, RES_UHWORD pNwing, RES_UHWORD pLpScl, |
| 289 | const RES_HWORD pImp[], const RES_HWORD pImpD[], RES_BOOL Interp) |
| 290 | { |
| 291 | const RES_HWORD *xp; |
| 292 | RES_HWORD *Ystart, *Yend; |
| 293 | RES_WORD v; |
| 294 | |
| 295 | double dh; /* Step through filter impulse response */ |
| 296 | double dt; /* Step through input signal */ |
| 297 | RES_UWORD time = 0; |
| 298 | RES_UWORD endTime; /* When time reaches EndTime, return to user */ |
| 299 | RES_UWORD dhb, dtb; /* Fixed-point versions of Dh,Dt */ |
| 300 | |
| 301 | dt = 1.0/pFactor; /* Output sampling period */ |
| 302 | dtb = dt*(1<<Np) + 0.5; /* Fixed-point representation */ |
| 303 | |
| 304 | dh = MIN(Npc, pFactor*Npc); /* Filter sampling period */ |
| 305 | dhb = dh*(1<<Na) + 0.5; /* Fixed-point representation */ |
| 306 | |
| 307 | Ystart = Y; |
| 308 | Yend = Ystart + (unsigned)(nx * pFactor); |
| 309 | endTime = time + (1<<Np)*(RES_WORD)nx; |
Nanang Izzuddin | 50947e7 | 2008-05-28 19:15:31 +0000 | [diff] [blame] | 310 | |
| 311 | // Integer round down in dtb calculation may cause (endTime % dtb > 0), |
| 312 | // so it may cause resample write pass the output buffer (Y >= Yend). |
| 313 | // while (time < endTime) |
| 314 | while (Y < Yend) |
Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [diff] [blame] | 315 | { |
| 316 | xp = &X[time>>Np]; /* Ptr to current input sample */ |
| 317 | v = FilterUD(pImp, pImpD, pNwing, Interp, xp, (RES_HWORD)(time&Pmask), |
| 318 | -1, dhb); /* Perform left-wing inner product */ |
| 319 | v += FilterUD(pImp, pImpD, pNwing, Interp, xp+1, (RES_HWORD)((-time)&Pmask), |
| 320 | 1, dhb); /* Perform right-wing inner product */ |
| 321 | v >>= Nhg; /* Make guard bits */ |
| 322 | v *= pLpScl; /* Normalize for unity filter gain */ |
| 323 | *Y++ = WordToHword(v,NLpScl); /* strip guard bits, deposit output */ |
| 324 | time += dtb; /* Move to next sample by time increment */ |
| 325 | } |
| 326 | return (Y - Ystart); /* Return the number of output samples */ |
| 327 | } |
| 328 | |
| 329 | |
Benny Prijono | 4fb32b5 | 2007-04-30 09:02:46 +0000 | [diff] [blame] | 330 | DECL(int) res_SrcLinear(const RES_HWORD X[], RES_HWORD Y[], |
| 331 | double pFactor, RES_UHWORD nx) |
Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [diff] [blame] | 332 | { |
| 333 | return SrcLinear(X, Y, pFactor, nx); |
| 334 | } |
| 335 | |
Benny Prijono | 4fb32b5 | 2007-04-30 09:02:46 +0000 | [diff] [blame] | 336 | DECL(int) res_Resample(const RES_HWORD X[], RES_HWORD Y[], double pFactor, |
| 337 | RES_UHWORD nx, RES_BOOL LargeF, RES_BOOL Interp) |
Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [diff] [blame] | 338 | { |
| 339 | if (pFactor >= 1) { |
| 340 | |
| 341 | if (LargeF) |
| 342 | return SrcUp(X, Y, pFactor, nx, |
| 343 | LARGE_FILTER_NWING, LARGE_FILTER_SCALE, |
| 344 | LARGE_FILTER_IMP, LARGE_FILTER_IMPD, Interp); |
| 345 | else |
| 346 | return SrcUp(X, Y, pFactor, nx, |
| 347 | SMALL_FILTER_NWING, SMALL_FILTER_SCALE, |
| 348 | SMALL_FILTER_IMP, SMALL_FILTER_IMPD, Interp); |
| 349 | |
| 350 | } else { |
| 351 | |
| 352 | if (LargeF) |
| 353 | return SrcUD(X, Y, pFactor, nx, |
| 354 | LARGE_FILTER_NWING, LARGE_FILTER_SCALE * pFactor + 0.5, |
| 355 | LARGE_FILTER_IMP, LARGE_FILTER_IMPD, Interp); |
| 356 | else |
| 357 | return SrcUD(X, Y, pFactor, nx, |
| 358 | SMALL_FILTER_NWING, SMALL_FILTER_SCALE * pFactor + 0.5, |
| 359 | SMALL_FILTER_IMP, SMALL_FILTER_IMPD, Interp); |
| 360 | |
| 361 | } |
| 362 | } |
| 363 | |
Benny Prijono | 4fb32b5 | 2007-04-30 09:02:46 +0000 | [diff] [blame] | 364 | DECL(int) res_GetXOFF(double pFactor, RES_BOOL LargeF) |
Benny Prijono | c95a0f0 | 2007-04-09 07:06:08 +0000 | [diff] [blame] | 365 | { |
| 366 | if (LargeF) |
| 367 | return (LARGE_FILTER_NMULT + 1) / 2.0 * |
| 368 | MAX(1.0, 1.0/pFactor); |
| 369 | else |
| 370 | return (SMALL_FILTER_NMULT + 1) / 2.0 * |
| 371 | MAX(1.0, 1.0/pFactor); |
| 372 | } |
| 373 | |