resample.c
上传用户:zhongxx05
上传日期:2007-06-06
资源大小:33641k
文件大小:13k
源码类别:

Symbian

开发平台:

C/C++

  1. /* ***** BEGIN LICENSE BLOCK ***** 
  2.  * Version: RCSL 1.0/RPSL 1.0 
  3.  *  
  4.  * Portions Copyright (c) 1995-2002 RealNetworks, Inc. All Rights Reserved. 
  5.  *      
  6.  * The contents of this file, and the files included with this file, are 
  7.  * subject to the current version of the RealNetworks Public Source License 
  8.  * Version 1.0 (the "RPSL") available at 
  9.  * http://www.helixcommunity.org/content/rpsl unless you have licensed 
  10.  * the file under the RealNetworks Community Source License Version 1.0 
  11.  * (the "RCSL") available at http://www.helixcommunity.org/content/rcsl, 
  12.  * in which case the RCSL will apply. You may also obtain the license terms 
  13.  * directly from RealNetworks.  You may not use this file except in 
  14.  * compliance with the RPSL or, if you have a valid RCSL with RealNetworks 
  15.  * applicable to this file, the RCSL.  Please see the applicable RPSL or 
  16.  * RCSL for the rights, obligations and limitations governing use of the 
  17.  * contents of the file.  
  18.  *  
  19.  * This file is part of the Helix DNA Technology. RealNetworks is the 
  20.  * developer of the Original Code and owns the copyrights in the portions 
  21.  * it created. 
  22.  *  
  23.  * This file, and the files included with this file, is distributed and made 
  24.  * available on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER 
  25.  * EXPRESS OR IMPLIED, AND REALNETWORKS HEREBY DISCLAIMS ALL SUCH WARRANTIES, 
  26.  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY, FITNESS 
  27.  * FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT. 
  28.  * 
  29.  * Technology Compatibility Kit Test Suite(s) Location: 
  30.  *    http://www.helixcommunity.org/content/tck 
  31.  * 
  32.  * Contributor(s): 
  33.  *  
  34.  * ***** END LICENSE BLOCK ***** */ 
  35. /*
  36.  * Fixed-point sampling rate conversion library
  37.  * Developed by Ken Cooke (kenc@real.com)
  38.  * May 2003
  39.  *
  40.  * Main resampling functions.
  41.  */
  42. #ifdef _OPENWAVE
  43. #include "hlxclib/stdlib.h"
  44. #include "hlxclib/string.h"
  45. #else
  46. #include <stdlib.h>
  47. #include <string.h>
  48. #endif
  49. #include "resample.h"
  50. #include "core.h"
  51. #include "filter.h"
  52. #ifndef _OPENWAVE
  53. #define MAX(a,b) ((a) > (b) ? (a) : (b))
  54. #define MIN(a,b) ((a) < (b) ? (a) : (b))
  55. #endif
  56. #define MAX_RATE 192000
  57. #define MAX_CHANS 2
  58. #define UP_MAX 640 /* ARB is enabled when up > UP_MAX */
  59. #define UP_ARB 128 /* ARB oversampling factor */
  60. #define NWING_ALIGN 2 /* 2 required for ARMv5E, otherwise 1 */
  61. /* unsigned 32x32->hi32, using mulhi */
  62. #define UMULHI(a,b) (uint)(mulhi(a, b) + (((int)a>>31) & b) + (((int)b>>31) & a));
  63. /* 
  64. __inline int mulhi(int a, int b) {
  65. return (int) (((__int64)a * (__int64)b) >> 32);
  66. }
  67. __inline uint udivhi(uint num, uint den) {
  68. return (uint) (((unsigned __int64)num << 32) / den);
  69. }
  70. */
  71. /* signed 32x32->hi32 */
  72. static int mulhi(int a, int b)
  73. {
  74. int ah, bh, lh, hl, hh;
  75. uint al, bl, ll;
  76. al = a & 0xffff;
  77. ah = a >> 16;
  78. bl = b & 0xffff;
  79. bh = b >> 16;
  80. ll = al * bl;
  81. lh = al * bh + (ll >> 16);
  82. hl = ah * bl + (lh & 0xffff);
  83. hh = ah * bh + (lh >> 16) + (hl >> 16);
  84. return hh;
  85. }
  86. /* unsigned (num<<32)/den, undefined for overflow */
  87. static uint udivhi(uint num, uint den)
  88. {
  89. uint i, carry, quo = 0;
  90. /* bitwise shift-and-subtract */
  91. for (i = 0; i < 32; i++) {
  92. carry = (int)num >> 31;
  93. num <<= 1;
  94. quo <<= 1;
  95. if ((carry | num) >= den) {
  96. num -= den;
  97. quo += 1;
  98. }
  99. }
  100. return quo;
  101. }
  102. /* greatest common divisor */
  103. static int gcd(int a, int b)
  104. {
  105. while (a != b) {
  106. if (a > b)
  107. a -= b;
  108. else
  109. b -= a;
  110. }
  111. return a;
  112. }
  113. static void
  114. Interpolate(int *inbuf, int inlen, int *outbuf, int outlen, int gain)
  115. {
  116. uint stepi, stepf, halfstepi, halfstepf, f;
  117. int x0, x1, x2, x3, x0_3, x3_3, acc, frac;
  118. int i, j, sign;
  119. stepi = inlen / outlen;
  120. stepf = udivhi(inlen - stepi * outlen, outlen);
  121. /* i.f = 0.5 + step/2 */
  122. halfstepf = (stepi << 31) | (stepf >> 1);
  123. halfstepi = stepi >> 1;
  124. f = 0x80000000 + halfstepf;
  125. i = halfstepi + (f < halfstepf); /* add with carry */
  126. for (j = 0; j < outlen; j++) {
  127. x3 = (i-2 < 0 ? inbuf[-i+1] >> 2 : inbuf[i-2] >> 2); /* reflect */
  128. x2 = (i-1 < 0 ? inbuf[-i+0] >> 2 : inbuf[i-1] >> 2); /* reflect */
  129. x1 = (i+0 < inlen ? inbuf[i+0] >> 2 : 0); /* zero pad */
  130. x0 = (i+1 < inlen ? inbuf[i+1] >> 2 : 0); /* zero pad */
  131. x0_3 = mulhi(x0, 0x55555556); /* x0/3 */
  132. x3_3 = mulhi(x3, 0x55555556); /* x3/3 */
  133. frac = f >> 1;
  134. /*
  135.  * 4-tap Lagrange interpolation using Farrow structure.
  136.  * Requires 2 guardbits to prevent overflow.
  137.  */
  138. acc = (x0_3 - x3_3 + x2 - x1) >> 1;
  139. acc = mulhi(acc, frac) << 1;
  140. acc += ((x1 + x3) >> 1) - x2;
  141. acc = mulhi(acc, frac) << 1;
  142. acc += x1 - x3_3 - ((x2 + x0_3) >> 1);
  143. acc = mulhi(acc, frac) << 1;
  144. acc += x2;
  145. acc = mulhi(acc, gain) << 1;
  146. f += stepf;
  147. i += stepi + (f < stepf); /* add with carry */
  148. /* clip to 30-bit */
  149. if ((sign = (acc >> 31)) != (acc >> 29))
  150. acc = sign ^ ((1<<29)-1);
  151. outbuf[j] = acc << 2;
  152. }
  153. }
  154. static short *
  155. MakeFilter(int up, int dn, int quality, int *pnwing)
  156. {
  157. int nwing, nwingk, i, phase, gain;
  158. int nkernel, nfilter, nwinga;
  159. int *kernel, *filtbuf;
  160. short *filter;
  161. /* choose the filter kernel */
  162. switch (quality) {
  163. case 0:
  164. kernel = (int *)kernel8;
  165. nwingk = 4;
  166. break;
  167. case 1:
  168. kernel = (int *)kernel16;
  169. nwingk = 8;
  170. break;
  171. case 2:
  172. kernel = (int *)kernel24;
  173. nwingk = 12;
  174. break;
  175. case 3:
  176. kernel = (int *)kernel32;
  177. nwingk = 16;
  178. break;
  179. default:
  180. return NULL;
  181. }
  182. /* compute the filter specs */
  183. nkernel = nwingk * UP_KERNEL;
  184. nfilter = nwingk * MAX(up, dn);
  185. nwing = nfilter / up;
  186. nfilter = nwing * up;
  187. /* adjust gain when downsampling */
  188. if (nwing > nwingk)
  189. gain = udivhi(nwingk, nwing) >> 1;
  190. else
  191. gain = 0x7fffffff; /* unity */
  192. /* allocate buffers */
  193. nwinga = (nwing + NWING_ALIGN-1) & ~(NWING_ALIGN-1); /* aligned size */
  194. filter = (short *) calloc(nwinga * up, sizeof(short));
  195. if (!filter)
  196. return NULL;
  197. filtbuf = (int *) calloc(nfilter, sizeof(int));
  198. if (!filtbuf) {
  199. free(filter);
  200. return NULL;
  201. }
  202. /* interpolate the full filter */
  203. Interpolate(kernel, nkernel, filtbuf, nfilter, gain);
  204. /* quantize the filter coefs */
  205. for (i = 0; i < nfilter; i++) {
  206. filtbuf[i] = (filtbuf[i] + (1<<15)) >> 16;
  207. }
  208. /* deinterleave into phases */
  209. for (phase = 0; phase < up; phase++) {
  210. for (i = 0; i < nwing; i++) {
  211. filter[phase*nwinga + i] = (short) filtbuf[i*up + phase];
  212. }
  213. }
  214. free(filtbuf);
  215. *pnwing = nwing;
  216. return filter;
  217. }
  218. /*
  219.  * Creates a new resampler.
  220.  *
  221.  * inrate and outrate can be arbitrary sampling rates.
  222.  *
  223.  * nchans must be 1 (mono) or 2 (stereo).
  224.  *
  225.  * quality parameter ranges from 0 (low) to 3 (high).
  226.  * lower quality is faster, and uses less memory.
  227.  *
  228.  * returns the resampler instance, or NULL on error.
  229.  */
  230. void *
  231. InitResampler(int inrate, int outrate, int nchans, int quality)
  232. {
  233. state_t *s;
  234. int divisor, up, dn;
  235. int nwing, nwinga, nhist;
  236. int pcmstep, fltstep;
  237. uint uprate, stepf;
  238. short *filter;
  239. if (inrate < 1 || inrate > MAX_RATE)
  240. return NULL;
  241. if (outrate < 1 || outrate > MAX_RATE)
  242. return NULL;
  243. if (nchans < 1 || nchans > MAX_CHANS)
  244. return NULL;
  245. /* reduce to smallest fraction */
  246. divisor = gcd(inrate, outrate);
  247. up = outrate / divisor;
  248. dn = inrate / divisor;
  249. stepf = 0; /* RAT mode */
  250. /* when too many phases, ARB mode */
  251. if (up > UP_MAX) {
  252. uprate = UP_ARB * inrate;
  253. up = UP_ARB;
  254. dn = uprate / outrate;
  255. stepf = udivhi(uprate - dn * outrate, outrate);
  256. }
  257. /* create the polyphase filter */
  258. filter = MakeFilter(up, dn, quality, &nwing);
  259. if (!filter)
  260. return NULL;
  261. //printf("mode=%s qual=%d ", stepf ? "ARB" : "RAT", quality);
  262. //printf("up=%d down=%.3f taps=%dn", up, dn + stepf/4294967296.0, 2*nwing);
  263. nwinga = (nwing + NWING_ALIGN-1) & ~(NWING_ALIGN-1); /* aligned size */
  264. nhist = nchans * (2 * nwing + (stepf != 0));
  265. /* allocate buffers */
  266. s = (state_t *) calloc(1, sizeof(state_t));
  267. if (!s) {
  268. free(filter);
  269. return NULL;
  270. }
  271. s->histbuf = (short *) calloc(2 * nhist, sizeof(short));
  272. if (!s->histbuf) {
  273. free(s);
  274. free(filter);
  275. return NULL;
  276. }
  277. /* filter init */
  278. s->filter = filter;
  279. s->up = up;
  280. s->dn = dn;
  281. s->nchans = nchans;
  282. s->nwing = nwing;
  283. s->nhist = nhist;
  284. s->nstart = nhist - nchans * (nwing-1);
  285. s->offset = 0;
  286. s->phasef = 0;
  287. s->stepf = stepf;
  288. s->rwing = s->filter;
  289. s->lwing = s->filter + nwinga * (up-1);
  290. /*
  291.  * Create the polyphase stepping tables.
  292.  * NOTE: step-by-N includes -nwing to rewind pointers.
  293.  */
  294. /* step phase by N */
  295. pcmstep = dn / up;
  296. fltstep = dn - up * pcmstep; /* dn % up */
  297. s->stepNptr = s->filter + nwinga * (up - fltstep); /* fwd/bak threshold */
  298. s->stepNfwd[0] = nwinga * fltstep - nwing; /* rwgptr step */
  299. s->stepNfwd[1] = -nwinga * fltstep - nwing; /* lwgptr step */
  300. s->stepNfwd[2] = nchans * (pcmstep - nwing); /* pcmptr step */
  301. pcmstep = (dn + up-1) / up;
  302. fltstep = (dn + up-1) - up * pcmstep;
  303. s->stepNbak[0] = nwinga * (fltstep - (up-1)) - nwing;
  304. s->stepNbak[1] = -nwinga * (fltstep - (up-1)) - nwing;
  305. s->stepNbak[2] = nchans * (pcmstep - nwing);
  306. /* step phase by 1 */
  307. pcmstep = 1 / up;
  308. fltstep = 1 - up * pcmstep;
  309. s->step1ptr = s->filter + nwinga * (up - fltstep);
  310. s->step1fwd[0] = nwinga * fltstep;
  311. s->step1fwd[1] = -nwinga * fltstep;
  312. s->step1fwd[2] = nchans * pcmstep;
  313. pcmstep = (1 + up-1) / up;
  314. fltstep = (1 + up-1) - up * pcmstep;
  315. s->step1bak[0] = nwinga * (fltstep - (up-1));
  316. s->step1bak[1] = -nwinga * (fltstep - (up-1));
  317. s->step1bak[2] = nchans * pcmstep;
  318. /* set the core function pointer */
  319. if (stepf != 0)
  320. s->ResampleCore = (nchans == 1 ? ARBCoreMono : ARBCoreStereo);
  321. else
  322. s->ResampleCore = (nchans == 1 ? RATCoreMono : RATCoreStereo);
  323. return (void *)s;
  324. }
  325. /*
  326.  * Resamples inbuf into outbuf.
  327.  *
  328.  * insamps can be any number of samples, including zero.
  329.  *
  330.  * outbuf must be sufficiently large to hold the output.
  331.  * use GetMaxOutput() to allocate properly.
  332.  *
  333.  * returns the number of output samples.
  334.  */
  335. int
  336. Resample(short *inbuf, int insamps, short *outbuf, void *inst)
  337. {
  338. state_t *s = (state_t *)inst;
  339. short *pcmptr, *pcmend, *outptr;
  340. /* fill history buffer */
  341. memcpy(s->histbuf + s->nhist, inbuf, MIN(s->nhist, insamps) * sizeof(short));
  342. /* process history buffer */
  343. pcmptr = s->histbuf + s->nstart;
  344. pcmend = pcmptr + MIN(s->nhist, insamps);
  345. outptr = s->ResampleCore(pcmptr, pcmend, outbuf, s);
  346. /* process input buffer */
  347. if (insamps > s->nhist) {
  348. pcmptr = inbuf + s->nstart;
  349. pcmend = pcmptr + (insamps - s->nhist);
  350. outptr = s->ResampleCore(pcmptr, pcmend, outptr, s);
  351. }
  352. /* save history buffer */
  353. if (insamps > s->nhist)
  354. memcpy(s->histbuf, inbuf + (insamps - s->nhist), s->nhist * sizeof(short));
  355. else
  356. memmove(s->histbuf, s->histbuf + insamps, s->nhist * sizeof(short));
  357. return (outptr - outbuf);
  358. }
  359. void
  360. FreeResampler(void *inst)
  361. {
  362. state_t *s = (state_t *)inst;
  363. if (s) {
  364. if (s->filter)
  365. free(s->filter);
  366. if (s->histbuf)
  367. free(s->histbuf);
  368. free(s);
  369. }
  370. }
  371. /*
  372.  * Computes the maximum output samples that could be produced by insamps.
  373.  */
  374. int
  375. GetMaxOutput(int insamps, void *inst)
  376. {
  377. state_t *s = (state_t *)inst;
  378. int i, outsamps;
  379. uint f;
  380. if (s->nchans == 2)
  381. insamps = (insamps + 1) >> 1; /* frames */
  382. /* outsamps = ceil(insamps * up / dn.stepf) */
  383. i = f = outsamps = 0;
  384. while (i < s->up * insamps) {
  385. f += s->stepf;
  386. i += s->dn + (f < s->stepf); /* add with carry */
  387. outsamps++;
  388. }
  389. return outsamps * s->nchans;
  390. }
  391. /* faster version, using __int64
  392. int
  393. GetMaxOutput(int insamps, void *inst)
  394. {
  395. state_t *s = (state_t *)inst;
  396. __int64 dn64, out64;
  397. if (s->nchans == 2)
  398. insamps = (insamps + 1) >> 1;
  399. dn64 = ((__int64)s->dn << 32) + s->stepf;
  400. out64 = (__int64)(insamps * s->up) << 32;
  401. out64 = (out64 + (dn64 - 1)) / dn64;
  402. return (int)out64 * s->nchans;
  403. }
  404. */
  405. /*
  406.  * Computes the minimum input samples that will produce at least outsamps.
  407.  */
  408. int
  409. GetMinInput(int outsamps, void *inst)
  410. {
  411. state_t *s = (state_t *)inst;
  412. uint numhi, numlo, quo, rem;
  413. int insamps;
  414. if (s->nchans == 2)
  415. outsamps = (outsamps + 1) >> 1; /* frames */
  416. /* insamps = ceil(outsamps * dn.stepf / up) */
  417. numhi = outsamps * s->dn + UMULHI(outsamps, s->stepf);
  418. numlo = outsamps * s->stepf;
  419. quo = numhi / s->up;
  420. rem = numhi - quo * s->up;
  421. insamps = quo + (rem || numlo); /* round up */
  422. return insamps * s->nchans;
  423. }
  424. /*
  425.  * Computes the approximate group delay.
  426.  * NOTE: return value is frames, not samples...
  427.  */
  428. int
  429. GetDelay(void *inst)
  430. {
  431. state_t *s = (state_t *)inst;
  432. int delay;
  433. delay = GetMaxOutput(s->nwing * s->nchans, inst);
  434. if (s->nchans == 2)
  435. delay >>= 1; /* frames */
  436. return delay;
  437. }