resample.c 43 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239
  1. /* Copyright (C) 2007-2008 Jean-Marc Valin
  2. Copyright (C) 2008 Thorvald Natvig
  3. File: resample.c
  4. Arbitrary resampling code
  5. Redistribution and use in source and binary forms, with or without
  6. modification, are permitted provided that the following conditions are
  7. met:
  8. 1. Redistributions of source code must retain the above copyright notice,
  9. this list of conditions and the following disclaimer.
  10. 2. Redistributions in binary form must reproduce the above copyright
  11. notice, this list of conditions and the following disclaimer in the
  12. documentation and/or other materials provided with the distribution.
  13. 3. The name of the author may not be used to endorse or promote products
  14. derived from this software without specific prior written permission.
  15. THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  16. IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  17. OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  18. DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
  19. INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  20. (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  21. SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  22. HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
  23. STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  24. ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  25. POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. /*
  28. The design goals of this code are:
  29. - Very fast algorithm
  30. - SIMD-friendly algorithm
  31. - Low memory requirement
  32. - Good *perceptual* quality (and not best SNR)
  33. Warning: This resampler is relatively new. Although I think I got rid of
  34. all the major bugs and I don't expect the API to change anymore, there
  35. may be something I've missed. So use with caution.
  36. This algorithm is based on this original resampling algorithm:
  37. Smith, Julius O. Digital Audio Resampling Home Page
  38. Center for Computer Research in Music and Acoustics (CCRMA),
  39. Stanford University, 2007.
  40. Web published at https://ccrma.stanford.edu/~jos/resample/.
  41. There is one main difference, though. This resampler uses cubic
  42. interpolation instead of linear interpolation in the above paper. This
  43. makes the table much smaller and makes it possible to compute that table
  44. on a per-stream basis. In turn, being able to tweak the table for each
  45. stream makes it possible to both reduce complexity on simple ratios
  46. (e.g. 2/3), and get rid of the rounding operations in the inner loop.
  47. The latter both reduces CPU time and makes the algorithm more SIMD-friendly.
  48. */
  49. #ifdef HAVE_CONFIG_H
  50. #include "config.h"
  51. #endif
  52. #ifdef OUTSIDE_SPEEX
  53. #include <stdlib.h>
  54. static void *speex_alloc(int size) {return calloc(size,1);}
  55. static void *speex_realloc(void *ptr, int size) {return realloc(ptr, size);}
  56. static void speex_free(void *ptr) {free(ptr);}
  57. #ifndef EXPORT
  58. #define EXPORT
  59. #endif
  60. #include "speex_resampler.h"
  61. #include "arch.h"
  62. #else /* OUTSIDE_SPEEX */
  63. #include "speex/speex_resampler.h"
  64. #include "arch.h"
  65. #include "os_support.h"
  66. #endif /* OUTSIDE_SPEEX */
  67. #include <math.h>
  68. #include <limits.h>
  69. #ifndef M_PI
  70. #define M_PI 3.14159265358979323846
  71. #endif
  72. #define IMAX(a,b) ((a) > (b) ? (a) : (b))
  73. #define IMIN(a,b) ((a) < (b) ? (a) : (b))
  74. #ifndef NULL
  75. #define NULL 0
  76. #endif
  77. #ifndef UINT32_MAX
  78. #define UINT32_MAX 4294967295U
  79. #endif
  80. #ifdef USE_SSE
  81. #include "resample_sse.h"
  82. #endif
  83. #ifdef USE_NEON
  84. #include "resample_neon.h"
  85. #endif
  86. /* Numer of elements to allocate on the stack */
  87. #ifdef VAR_ARRAYS
  88. #define FIXED_STACK_ALLOC 8192
  89. #else
  90. #define FIXED_STACK_ALLOC 1024
  91. #endif
  92. typedef int (*resampler_basic_func)(SpeexResamplerState *, spx_uint32_t , const spx_word16_t *, spx_uint32_t *, spx_word16_t *, spx_uint32_t *);
  93. struct SpeexResamplerState_ {
  94. spx_uint32_t in_rate;
  95. spx_uint32_t out_rate;
  96. spx_uint32_t num_rate;
  97. spx_uint32_t den_rate;
  98. int quality;
  99. spx_uint32_t nb_channels;
  100. spx_uint32_t filt_len;
  101. spx_uint32_t mem_alloc_size;
  102. spx_uint32_t buffer_size;
  103. int int_advance;
  104. int frac_advance;
  105. float cutoff;
  106. spx_uint32_t oversample;
  107. int initialised;
  108. int started;
  109. /* These are per-channel */
  110. spx_int32_t *last_sample;
  111. spx_uint32_t *samp_frac_num;
  112. spx_uint32_t *magic_samples;
  113. spx_word16_t *mem;
  114. spx_word16_t *sinc_table;
  115. spx_uint32_t sinc_table_length;
  116. resampler_basic_func resampler_ptr;
  117. int in_stride;
  118. int out_stride;
  119. } ;
  120. static const double kaiser12_table[68] = {
  121. 0.99859849, 1.00000000, 0.99859849, 0.99440475, 0.98745105, 0.97779076,
  122. 0.96549770, 0.95066529, 0.93340547, 0.91384741, 0.89213598, 0.86843014,
  123. 0.84290116, 0.81573067, 0.78710866, 0.75723148, 0.72629970, 0.69451601,
  124. 0.66208321, 0.62920216, 0.59606986, 0.56287762, 0.52980938, 0.49704014,
  125. 0.46473455, 0.43304576, 0.40211431, 0.37206735, 0.34301800, 0.31506490,
  126. 0.28829195, 0.26276832, 0.23854851, 0.21567274, 0.19416736, 0.17404546,
  127. 0.15530766, 0.13794294, 0.12192957, 0.10723616, 0.09382272, 0.08164178,
  128. 0.07063950, 0.06075685, 0.05193064, 0.04409466, 0.03718069, 0.03111947,
  129. 0.02584161, 0.02127838, 0.01736250, 0.01402878, 0.01121463, 0.00886058,
  130. 0.00691064, 0.00531256, 0.00401805, 0.00298291, 0.00216702, 0.00153438,
  131. 0.00105297, 0.00069463, 0.00043489, 0.00025272, 0.00013031, 0.0000527734,
  132. 0.00001000, 0.00000000};
  133. /*
  134. static const double kaiser12_table[36] = {
  135. 0.99440475, 1.00000000, 0.99440475, 0.97779076, 0.95066529, 0.91384741,
  136. 0.86843014, 0.81573067, 0.75723148, 0.69451601, 0.62920216, 0.56287762,
  137. 0.49704014, 0.43304576, 0.37206735, 0.31506490, 0.26276832, 0.21567274,
  138. 0.17404546, 0.13794294, 0.10723616, 0.08164178, 0.06075685, 0.04409466,
  139. 0.03111947, 0.02127838, 0.01402878, 0.00886058, 0.00531256, 0.00298291,
  140. 0.00153438, 0.00069463, 0.00025272, 0.0000527734, 0.00000500, 0.00000000};
  141. */
  142. static const double kaiser10_table[36] = {
  143. 0.99537781, 1.00000000, 0.99537781, 0.98162644, 0.95908712, 0.92831446,
  144. 0.89005583, 0.84522401, 0.79486424, 0.74011713, 0.68217934, 0.62226347,
  145. 0.56155915, 0.50119680, 0.44221549, 0.38553619, 0.33194107, 0.28205962,
  146. 0.23636152, 0.19515633, 0.15859932, 0.12670280, 0.09935205, 0.07632451,
  147. 0.05731132, 0.04193980, 0.02979584, 0.02044510, 0.01345224, 0.00839739,
  148. 0.00488951, 0.00257636, 0.00115101, 0.00035515, 0.00000000, 0.00000000};
  149. static const double kaiser8_table[36] = {
  150. 0.99635258, 1.00000000, 0.99635258, 0.98548012, 0.96759014, 0.94302200,
  151. 0.91223751, 0.87580811, 0.83439927, 0.78875245, 0.73966538, 0.68797126,
  152. 0.63451750, 0.58014482, 0.52566725, 0.47185369, 0.41941150, 0.36897272,
  153. 0.32108304, 0.27619388, 0.23465776, 0.19672670, 0.16255380, 0.13219758,
  154. 0.10562887, 0.08273982, 0.06335451, 0.04724088, 0.03412321, 0.02369490,
  155. 0.01563093, 0.00959968, 0.00527363, 0.00233883, 0.00050000, 0.00000000};
  156. static const double kaiser6_table[36] = {
  157. 0.99733006, 1.00000000, 0.99733006, 0.98935595, 0.97618418, 0.95799003,
  158. 0.93501423, 0.90755855, 0.87598009, 0.84068475, 0.80211977, 0.76076565,
  159. 0.71712752, 0.67172623, 0.62508937, 0.57774224, 0.53019925, 0.48295561,
  160. 0.43647969, 0.39120616, 0.34752997, 0.30580127, 0.26632152, 0.22934058,
  161. 0.19505503, 0.16360756, 0.13508755, 0.10953262, 0.08693120, 0.06722600,
  162. 0.05031820, 0.03607231, 0.02432151, 0.01487334, 0.00752000, 0.00000000};
  163. struct FuncDef {
  164. const double *table;
  165. int oversample;
  166. };
  167. static const struct FuncDef kaiser12_funcdef = {kaiser12_table, 64};
  168. #define KAISER12 (&kaiser12_funcdef)
  169. static const struct FuncDef kaiser10_funcdef = {kaiser10_table, 32};
  170. #define KAISER10 (&kaiser10_funcdef)
  171. static const struct FuncDef kaiser8_funcdef = {kaiser8_table, 32};
  172. #define KAISER8 (&kaiser8_funcdef)
  173. static const struct FuncDef kaiser6_funcdef = {kaiser6_table, 32};
  174. #define KAISER6 (&kaiser6_funcdef)
  175. struct QualityMapping {
  176. int base_length;
  177. int oversample;
  178. float downsample_bandwidth;
  179. float upsample_bandwidth;
  180. const struct FuncDef *window_func;
  181. };
  182. /* This table maps conversion quality to internal parameters. There are two
  183. reasons that explain why the up-sampling bandwidth is larger than the
  184. down-sampling bandwidth:
  185. 1) When up-sampling, we can assume that the spectrum is already attenuated
  186. close to the Nyquist rate (from an A/D or a previous resampling filter)
  187. 2) Any aliasing that occurs very close to the Nyquist rate will be masked
  188. by the sinusoids/noise just below the Nyquist rate (guaranteed only for
  189. up-sampling).
  190. */
  191. static const struct QualityMapping quality_map[11] = {
  192. { 8, 4, 0.830f, 0.860f, KAISER6 }, /* Q0 */
  193. { 16, 4, 0.850f, 0.880f, KAISER6 }, /* Q1 */
  194. { 32, 4, 0.882f, 0.910f, KAISER6 }, /* Q2 */ /* 82.3% cutoff ( ~60 dB stop) 6 */
  195. { 48, 8, 0.895f, 0.917f, KAISER8 }, /* Q3 */ /* 84.9% cutoff ( ~80 dB stop) 8 */
  196. { 64, 8, 0.921f, 0.940f, KAISER8 }, /* Q4 */ /* 88.7% cutoff ( ~80 dB stop) 8 */
  197. { 80, 16, 0.922f, 0.940f, KAISER10}, /* Q5 */ /* 89.1% cutoff (~100 dB stop) 10 */
  198. { 96, 16, 0.940f, 0.945f, KAISER10}, /* Q6 */ /* 91.5% cutoff (~100 dB stop) 10 */
  199. {128, 16, 0.950f, 0.950f, KAISER10}, /* Q7 */ /* 93.1% cutoff (~100 dB stop) 10 */
  200. {160, 16, 0.960f, 0.960f, KAISER10}, /* Q8 */ /* 94.5% cutoff (~100 dB stop) 10 */
  201. {192, 32, 0.968f, 0.968f, KAISER12}, /* Q9 */ /* 95.5% cutoff (~100 dB stop) 10 */
  202. {256, 32, 0.975f, 0.975f, KAISER12}, /* Q10 */ /* 96.6% cutoff (~100 dB stop) 10 */
  203. };
  204. /*8,24,40,56,80,104,128,160,200,256,320*/
  205. static double compute_func(float x, const struct FuncDef *func)
  206. {
  207. float y, frac;
  208. double interp[4];
  209. int ind;
  210. y = x*func->oversample;
  211. ind = (int)floor(y);
  212. frac = (y-ind);
  213. /* CSE with handle the repeated powers */
  214. interp[3] = -0.1666666667*frac + 0.1666666667*(frac*frac*frac);
  215. interp[2] = frac + 0.5*(frac*frac) - 0.5*(frac*frac*frac);
  216. /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
  217. interp[0] = -0.3333333333*frac + 0.5*(frac*frac) - 0.1666666667*(frac*frac*frac);
  218. /* Just to make sure we don't have rounding problems */
  219. interp[1] = 1.f-interp[3]-interp[2]-interp[0];
  220. /*sum = frac*accum[1] + (1-frac)*accum[2];*/
  221. return interp[0]*func->table[ind] + interp[1]*func->table[ind+1] + interp[2]*func->table[ind+2] + interp[3]*func->table[ind+3];
  222. }
  223. #if 0
  224. #include <stdio.h>
  225. int main(int argc, char **argv)
  226. {
  227. int i;
  228. for (i=0;i<256;i++)
  229. {
  230. printf ("%f\n", compute_func(i/256., KAISER12));
  231. }
  232. return 0;
  233. }
  234. #endif
  235. #ifdef FIXED_POINT
  236. /* The slow way of computing a sinc for the table. Should improve that some day */
  237. static spx_word16_t sinc(float cutoff, float x, int N, const struct FuncDef *window_func)
  238. {
  239. /*fprintf (stderr, "%f ", x);*/
  240. float xx = x * cutoff;
  241. if (fabs(x)<1e-6f)
  242. return WORD2INT(32768.*cutoff);
  243. else if (fabs(x) > .5f*N)
  244. return 0;
  245. /*FIXME: Can it really be any slower than this? */
  246. return WORD2INT(32768.*cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func));
  247. }
  248. #else
  249. /* The slow way of computing a sinc for the table. Should improve that some day */
  250. static spx_word16_t sinc(float cutoff, float x, int N, const struct FuncDef *window_func)
  251. {
  252. /*fprintf (stderr, "%f ", x);*/
  253. float xx = x * cutoff;
  254. if (fabs(x)<1e-6)
  255. return cutoff;
  256. else if (fabs(x) > .5*N)
  257. return 0;
  258. /*FIXME: Can it really be any slower than this? */
  259. return cutoff*sin(M_PI*xx)/(M_PI*xx) * compute_func(fabs(2.*x/N), window_func);
  260. }
  261. #endif
  262. #ifdef FIXED_POINT
  263. static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
  264. {
  265. /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
  266. but I know it's MMSE-optimal on a sinc */
  267. spx_word16_t x2, x3;
  268. x2 = MULT16_16_P15(x, x);
  269. x3 = MULT16_16_P15(x, x2);
  270. interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
  271. interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
  272. interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
  273. /* Just to make sure we don't have rounding problems */
  274. interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
  275. if (interp[2]<32767)
  276. interp[2]+=1;
  277. }
  278. #else
  279. static void cubic_coef(spx_word16_t frac, spx_word16_t interp[4])
  280. {
  281. /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
  282. but I know it's MMSE-optimal on a sinc */
  283. interp[0] = -0.16667f*frac + 0.16667f*frac*frac*frac;
  284. interp[1] = frac + 0.5f*frac*frac - 0.5f*frac*frac*frac;
  285. /*interp[2] = 1.f - 0.5f*frac - frac*frac + 0.5f*frac*frac*frac;*/
  286. interp[3] = -0.33333f*frac + 0.5f*frac*frac - 0.16667f*frac*frac*frac;
  287. /* Just to make sure we don't have rounding problems */
  288. interp[2] = 1.-interp[0]-interp[1]-interp[3];
  289. }
  290. #endif
  291. static int resampler_basic_direct_single(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  292. {
  293. const int N = st->filt_len;
  294. int out_sample = 0;
  295. int last_sample = st->last_sample[channel_index];
  296. spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
  297. const spx_word16_t *sinc_table = st->sinc_table;
  298. const int out_stride = st->out_stride;
  299. const int int_advance = st->int_advance;
  300. const int frac_advance = st->frac_advance;
  301. const spx_uint32_t den_rate = st->den_rate;
  302. spx_word32_t sum;
  303. while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
  304. {
  305. const spx_word16_t *sinct = & sinc_table[samp_frac_num*N];
  306. const spx_word16_t *iptr = & in[last_sample];
  307. #ifndef OVERRIDE_INNER_PRODUCT_SINGLE
  308. int j;
  309. sum = 0;
  310. for(j=0;j<N;j++) sum += MULT16_16(sinct[j], iptr[j]);
  311. /* This code is slower on most DSPs which have only 2 accumulators.
  312. Plus this this forces truncation to 32 bits and you lose the HW guard bits.
  313. I think we can trust the compiler and let it vectorize and/or unroll itself.
  314. spx_word32_t accum[4] = {0,0,0,0};
  315. for(j=0;j<N;j+=4) {
  316. accum[0] += MULT16_16(sinct[j], iptr[j]);
  317. accum[1] += MULT16_16(sinct[j+1], iptr[j+1]);
  318. accum[2] += MULT16_16(sinct[j+2], iptr[j+2]);
  319. accum[3] += MULT16_16(sinct[j+3], iptr[j+3]);
  320. }
  321. sum = accum[0] + accum[1] + accum[2] + accum[3];
  322. */
  323. sum = SATURATE32PSHR(sum, 15, 32767);
  324. #else
  325. sum = inner_product_single(sinct, iptr, N);
  326. #endif
  327. out[out_stride * out_sample++] = sum;
  328. last_sample += int_advance;
  329. samp_frac_num += frac_advance;
  330. if (samp_frac_num >= den_rate)
  331. {
  332. samp_frac_num -= den_rate;
  333. last_sample++;
  334. }
  335. }
  336. st->last_sample[channel_index] = last_sample;
  337. st->samp_frac_num[channel_index] = samp_frac_num;
  338. return out_sample;
  339. }
  340. #ifdef FIXED_POINT
  341. #else
  342. /* This is the same as the previous function, except with a double-precision accumulator */
  343. static int resampler_basic_direct_double(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  344. {
  345. const int N = st->filt_len;
  346. int out_sample = 0;
  347. int last_sample = st->last_sample[channel_index];
  348. spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
  349. const spx_word16_t *sinc_table = st->sinc_table;
  350. const int out_stride = st->out_stride;
  351. const int int_advance = st->int_advance;
  352. const int frac_advance = st->frac_advance;
  353. const spx_uint32_t den_rate = st->den_rate;
  354. double sum;
  355. while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
  356. {
  357. const spx_word16_t *sinct = & sinc_table[samp_frac_num*N];
  358. const spx_word16_t *iptr = & in[last_sample];
  359. #ifndef OVERRIDE_INNER_PRODUCT_DOUBLE
  360. int j;
  361. double accum[4] = {0,0,0,0};
  362. for(j=0;j<N;j+=4) {
  363. accum[0] += sinct[j]*iptr[j];
  364. accum[1] += sinct[j+1]*iptr[j+1];
  365. accum[2] += sinct[j+2]*iptr[j+2];
  366. accum[3] += sinct[j+3]*iptr[j+3];
  367. }
  368. sum = accum[0] + accum[1] + accum[2] + accum[3];
  369. #else
  370. sum = inner_product_double(sinct, iptr, N);
  371. #endif
  372. out[out_stride * out_sample++] = PSHR32(sum, 15);
  373. last_sample += int_advance;
  374. samp_frac_num += frac_advance;
  375. if (samp_frac_num >= den_rate)
  376. {
  377. samp_frac_num -= den_rate;
  378. last_sample++;
  379. }
  380. }
  381. st->last_sample[channel_index] = last_sample;
  382. st->samp_frac_num[channel_index] = samp_frac_num;
  383. return out_sample;
  384. }
  385. #endif
  386. static int resampler_basic_interpolate_single(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  387. {
  388. const int N = st->filt_len;
  389. int out_sample = 0;
  390. int last_sample = st->last_sample[channel_index];
  391. spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
  392. const int out_stride = st->out_stride;
  393. const int int_advance = st->int_advance;
  394. const int frac_advance = st->frac_advance;
  395. const spx_uint32_t den_rate = st->den_rate;
  396. spx_word32_t sum;
  397. while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
  398. {
  399. const spx_word16_t *iptr = & in[last_sample];
  400. const int offset = samp_frac_num*st->oversample/st->den_rate;
  401. #ifdef FIXED_POINT
  402. const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
  403. #else
  404. const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
  405. #endif
  406. spx_word16_t interp[4];
  407. #ifndef OVERRIDE_INTERPOLATE_PRODUCT_SINGLE
  408. int j;
  409. spx_word32_t accum[4] = {0,0,0,0};
  410. for(j=0;j<N;j++) {
  411. const spx_word16_t curr_in=iptr[j];
  412. accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
  413. accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
  414. accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
  415. accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
  416. }
  417. cubic_coef(frac, interp);
  418. sum = MULT16_32_Q15(interp[0],SHR32(accum[0], 1)) + MULT16_32_Q15(interp[1],SHR32(accum[1], 1)) + MULT16_32_Q15(interp[2],SHR32(accum[2], 1)) + MULT16_32_Q15(interp[3],SHR32(accum[3], 1));
  419. sum = SATURATE32PSHR(sum, 15, 32767);
  420. #else
  421. cubic_coef(frac, interp);
  422. sum = interpolate_product_single(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
  423. #endif
  424. out[out_stride * out_sample++] = sum;
  425. last_sample += int_advance;
  426. samp_frac_num += frac_advance;
  427. if (samp_frac_num >= den_rate)
  428. {
  429. samp_frac_num -= den_rate;
  430. last_sample++;
  431. }
  432. }
  433. st->last_sample[channel_index] = last_sample;
  434. st->samp_frac_num[channel_index] = samp_frac_num;
  435. return out_sample;
  436. }
  437. #ifdef FIXED_POINT
  438. #else
  439. /* This is the same as the previous function, except with a double-precision accumulator */
  440. static int resampler_basic_interpolate_double(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  441. {
  442. const int N = st->filt_len;
  443. int out_sample = 0;
  444. int last_sample = st->last_sample[channel_index];
  445. spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
  446. const int out_stride = st->out_stride;
  447. const int int_advance = st->int_advance;
  448. const int frac_advance = st->frac_advance;
  449. const spx_uint32_t den_rate = st->den_rate;
  450. spx_word32_t sum;
  451. while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
  452. {
  453. const spx_word16_t *iptr = & in[last_sample];
  454. const int offset = samp_frac_num*st->oversample/st->den_rate;
  455. #ifdef FIXED_POINT
  456. const spx_word16_t frac = PDIV32(SHL32((samp_frac_num*st->oversample) % st->den_rate,15),st->den_rate);
  457. #else
  458. const spx_word16_t frac = ((float)((samp_frac_num*st->oversample) % st->den_rate))/st->den_rate;
  459. #endif
  460. spx_word16_t interp[4];
  461. #ifndef OVERRIDE_INTERPOLATE_PRODUCT_DOUBLE
  462. int j;
  463. double accum[4] = {0,0,0,0};
  464. for(j=0;j<N;j++) {
  465. const double curr_in=iptr[j];
  466. accum[0] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-2]);
  467. accum[1] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset-1]);
  468. accum[2] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset]);
  469. accum[3] += MULT16_16(curr_in,st->sinc_table[4+(j+1)*st->oversample-offset+1]);
  470. }
  471. cubic_coef(frac, interp);
  472. sum = MULT16_32_Q15(interp[0],accum[0]) + MULT16_32_Q15(interp[1],accum[1]) + MULT16_32_Q15(interp[2],accum[2]) + MULT16_32_Q15(interp[3],accum[3]);
  473. #else
  474. cubic_coef(frac, interp);
  475. sum = interpolate_product_double(iptr, st->sinc_table + st->oversample + 4 - offset - 2, N, st->oversample, interp);
  476. #endif
  477. out[out_stride * out_sample++] = PSHR32(sum,15);
  478. last_sample += int_advance;
  479. samp_frac_num += frac_advance;
  480. if (samp_frac_num >= den_rate)
  481. {
  482. samp_frac_num -= den_rate;
  483. last_sample++;
  484. }
  485. }
  486. st->last_sample[channel_index] = last_sample;
  487. st->samp_frac_num[channel_index] = samp_frac_num;
  488. return out_sample;
  489. }
  490. #endif
  491. /* This resampler is used to produce zero output in situations where memory
  492. for the filter could not be allocated. The expected numbers of input and
  493. output samples are still processed so that callers failing to check error
  494. codes are not surprised, possibly getting into infinite loops. */
  495. static int resampler_basic_zero(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_word16_t *in, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  496. {
  497. int out_sample = 0;
  498. int last_sample = st->last_sample[channel_index];
  499. spx_uint32_t samp_frac_num = st->samp_frac_num[channel_index];
  500. const int out_stride = st->out_stride;
  501. const int int_advance = st->int_advance;
  502. const int frac_advance = st->frac_advance;
  503. const spx_uint32_t den_rate = st->den_rate;
  504. (void)in;
  505. while (!(last_sample >= (spx_int32_t)*in_len || out_sample >= (spx_int32_t)*out_len))
  506. {
  507. out[out_stride * out_sample++] = 0;
  508. last_sample += int_advance;
  509. samp_frac_num += frac_advance;
  510. if (samp_frac_num >= den_rate)
  511. {
  512. samp_frac_num -= den_rate;
  513. last_sample++;
  514. }
  515. }
  516. st->last_sample[channel_index] = last_sample;
  517. st->samp_frac_num[channel_index] = samp_frac_num;
  518. return out_sample;
  519. }
  520. static int multiply_frac(spx_uint32_t *result, spx_uint32_t value, spx_uint32_t num, spx_uint32_t den)
  521. {
  522. spx_uint32_t major = value / den;
  523. spx_uint32_t remain = value % den;
  524. /* TODO: Could use 64 bits operation to check for overflow. But only guaranteed in C99+ */
  525. if (remain > UINT32_MAX / num || major > UINT32_MAX / num
  526. || major * num > UINT32_MAX - remain * num / den)
  527. return RESAMPLER_ERR_OVERFLOW;
  528. *result = remain * num / den + major * num;
  529. return RESAMPLER_ERR_SUCCESS;
  530. }
  531. static int update_filter(SpeexResamplerState *st)
  532. {
  533. spx_uint32_t old_length = st->filt_len;
  534. spx_uint32_t old_alloc_size = st->mem_alloc_size;
  535. int use_direct;
  536. spx_uint32_t min_sinc_table_length;
  537. spx_uint32_t min_alloc_size;
  538. st->int_advance = st->num_rate/st->den_rate;
  539. st->frac_advance = st->num_rate%st->den_rate;
  540. st->oversample = quality_map[st->quality].oversample;
  541. st->filt_len = quality_map[st->quality].base_length;
  542. if (st->num_rate > st->den_rate)
  543. {
  544. /* down-sampling */
  545. st->cutoff = quality_map[st->quality].downsample_bandwidth * st->den_rate / st->num_rate;
  546. if (multiply_frac(&st->filt_len,st->filt_len,st->num_rate,st->den_rate) != RESAMPLER_ERR_SUCCESS)
  547. goto fail;
  548. /* Round up to make sure we have a multiple of 8 for SSE */
  549. st->filt_len = ((st->filt_len-1)&(~0x7))+8;
  550. if (2*st->den_rate < st->num_rate)
  551. st->oversample >>= 1;
  552. if (4*st->den_rate < st->num_rate)
  553. st->oversample >>= 1;
  554. if (8*st->den_rate < st->num_rate)
  555. st->oversample >>= 1;
  556. if (16*st->den_rate < st->num_rate)
  557. st->oversample >>= 1;
  558. if (st->oversample < 1)
  559. st->oversample = 1;
  560. } else {
  561. /* up-sampling */
  562. st->cutoff = quality_map[st->quality].upsample_bandwidth;
  563. }
  564. #ifdef RESAMPLE_FULL_SINC_TABLE
  565. use_direct = 1;
  566. if (INT_MAX/sizeof(spx_word16_t)/st->den_rate < st->filt_len)
  567. goto fail;
  568. #else
  569. /* Choose the resampling type that requires the least amount of memory */
  570. use_direct = st->filt_len*st->den_rate <= st->filt_len*st->oversample+8
  571. && INT_MAX/sizeof(spx_word16_t)/st->den_rate >= st->filt_len;
  572. #endif
  573. if (use_direct)
  574. {
  575. min_sinc_table_length = st->filt_len*st->den_rate;
  576. } else {
  577. if ((INT_MAX/sizeof(spx_word16_t)-8)/st->oversample < st->filt_len)
  578. goto fail;
  579. min_sinc_table_length = st->filt_len*st->oversample+8;
  580. }
  581. if (st->sinc_table_length < min_sinc_table_length)
  582. {
  583. spx_word16_t *sinc_table = (spx_word16_t *)speex_realloc(st->sinc_table,min_sinc_table_length*sizeof(spx_word16_t));
  584. if (!sinc_table)
  585. goto fail;
  586. st->sinc_table = sinc_table;
  587. st->sinc_table_length = min_sinc_table_length;
  588. }
  589. if (use_direct)
  590. {
  591. spx_uint32_t i;
  592. for (i=0;i<st->den_rate;i++)
  593. {
  594. spx_int32_t j;
  595. for (j=0;j<st->filt_len;j++)
  596. {
  597. st->sinc_table[i*st->filt_len+j] = sinc(st->cutoff,((j-(spx_int32_t)st->filt_len/2+1)-((float)i)/st->den_rate), st->filt_len, quality_map[st->quality].window_func);
  598. }
  599. }
  600. #ifdef FIXED_POINT
  601. st->resampler_ptr = resampler_basic_direct_single;
  602. #else
  603. if (st->quality>8)
  604. st->resampler_ptr = resampler_basic_direct_double;
  605. else
  606. st->resampler_ptr = resampler_basic_direct_single;
  607. #endif
  608. /*fprintf (stderr, "resampler uses direct sinc table and normalised cutoff %f\n", cutoff);*/
  609. } else {
  610. spx_int32_t i;
  611. for (i=-4;i<(spx_int32_t)(st->oversample*st->filt_len+4);i++)
  612. st->sinc_table[i+4] = sinc(st->cutoff,(i/(float)st->oversample - st->filt_len/2), st->filt_len, quality_map[st->quality].window_func);
  613. #ifdef FIXED_POINT
  614. st->resampler_ptr = resampler_basic_interpolate_single;
  615. #else
  616. if (st->quality>8)
  617. st->resampler_ptr = resampler_basic_interpolate_double;
  618. else
  619. st->resampler_ptr = resampler_basic_interpolate_single;
  620. #endif
  621. /*fprintf (stderr, "resampler uses interpolated sinc table and normalised cutoff %f\n", cutoff);*/
  622. }
  623. /* Here's the place where we update the filter memory to take into account
  624. the change in filter length. It's probably the messiest part of the code
  625. due to handling of lots of corner cases. */
  626. /* Adding buffer_size to filt_len won't overflow here because filt_len
  627. could be multiplied by sizeof(spx_word16_t) above. */
  628. min_alloc_size = st->filt_len-1 + st->buffer_size;
  629. if (min_alloc_size > st->mem_alloc_size)
  630. {
  631. spx_word16_t *mem;
  632. if (INT_MAX/sizeof(spx_word16_t)/st->nb_channels < min_alloc_size)
  633. goto fail;
  634. else if (!(mem = (spx_word16_t*)speex_realloc(st->mem, st->nb_channels*min_alloc_size * sizeof(*mem))))
  635. goto fail;
  636. st->mem = mem;
  637. st->mem_alloc_size = min_alloc_size;
  638. }
  639. if (!st->started)
  640. {
  641. spx_uint32_t i;
  642. for (i=0;i<st->nb_channels*st->mem_alloc_size;i++)
  643. st->mem[i] = 0;
  644. /*speex_warning("reinit filter");*/
  645. } else if (st->filt_len > old_length)
  646. {
  647. spx_uint32_t i;
  648. /* Increase the filter length */
  649. /*speex_warning("increase filter size");*/
  650. for (i=st->nb_channels;i--;)
  651. {
  652. spx_uint32_t j;
  653. spx_uint32_t olen = old_length;
  654. /*if (st->magic_samples[i])*/
  655. {
  656. /* Try and remove the magic samples as if nothing had happened */
  657. /* FIXME: This is wrong but for now we need it to avoid going over the array bounds */
  658. olen = old_length + 2*st->magic_samples[i];
  659. for (j=old_length-1+st->magic_samples[i];j--;)
  660. st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]] = st->mem[i*old_alloc_size+j];
  661. for (j=0;j<st->magic_samples[i];j++)
  662. st->mem[i*st->mem_alloc_size+j] = 0;
  663. st->magic_samples[i] = 0;
  664. }
  665. if (st->filt_len > olen)
  666. {
  667. /* If the new filter length is still bigger than the "augmented" length */
  668. /* Copy data going backward */
  669. for (j=0;j<olen-1;j++)
  670. st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = st->mem[i*st->mem_alloc_size+(olen-2-j)];
  671. /* Then put zeros for lack of anything better */
  672. for (;j<st->filt_len-1;j++)
  673. st->mem[i*st->mem_alloc_size+(st->filt_len-2-j)] = 0;
  674. /* Adjust last_sample */
  675. st->last_sample[i] += (st->filt_len - olen)/2;
  676. } else {
  677. /* Put back some of the magic! */
  678. st->magic_samples[i] = (olen - st->filt_len)/2;
  679. for (j=0;j<st->filt_len-1+st->magic_samples[i];j++)
  680. st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
  681. }
  682. }
  683. } else if (st->filt_len < old_length)
  684. {
  685. spx_uint32_t i;
  686. /* Reduce filter length, this a bit tricky. We need to store some of the memory as "magic"
  687. samples so they can be used directly as input the next time(s) */
  688. for (i=0;i<st->nb_channels;i++)
  689. {
  690. spx_uint32_t j;
  691. spx_uint32_t old_magic = st->magic_samples[i];
  692. st->magic_samples[i] = (old_length - st->filt_len)/2;
  693. /* We must copy some of the memory that's no longer used */
  694. /* Copy data going backward */
  695. for (j=0;j<st->filt_len-1+st->magic_samples[i]+old_magic;j++)
  696. st->mem[i*st->mem_alloc_size+j] = st->mem[i*st->mem_alloc_size+j+st->magic_samples[i]];
  697. st->magic_samples[i] += old_magic;
  698. }
  699. }
  700. return RESAMPLER_ERR_SUCCESS;
  701. fail:
  702. st->resampler_ptr = resampler_basic_zero;
  703. /* st->mem may still contain consumed input samples for the filter.
  704. Restore filt_len so that filt_len - 1 still points to the position after
  705. the last of these samples. */
  706. st->filt_len = old_length;
  707. return RESAMPLER_ERR_ALLOC_FAILED;
  708. }
  709. EXPORT SpeexResamplerState *speex_resampler_init(spx_uint32_t nb_channels, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
  710. {
  711. return speex_resampler_init_frac(nb_channels, in_rate, out_rate, in_rate, out_rate, quality, err);
  712. }
  713. EXPORT SpeexResamplerState *speex_resampler_init_frac(spx_uint32_t nb_channels, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate, int quality, int *err)
  714. {
  715. SpeexResamplerState *st;
  716. int filter_err;
  717. if (nb_channels == 0 || ratio_num == 0 || ratio_den == 0 || quality > 10 || quality < 0)
  718. {
  719. if (err)
  720. *err = RESAMPLER_ERR_INVALID_ARG;
  721. return NULL;
  722. }
  723. st = (SpeexResamplerState *)speex_alloc(sizeof(SpeexResamplerState));
  724. if (!st)
  725. {
  726. if (err)
  727. *err = RESAMPLER_ERR_ALLOC_FAILED;
  728. return NULL;
  729. }
  730. st->initialised = 0;
  731. st->started = 0;
  732. st->in_rate = 0;
  733. st->out_rate = 0;
  734. st->num_rate = 0;
  735. st->den_rate = 0;
  736. st->quality = -1;
  737. st->sinc_table_length = 0;
  738. st->mem_alloc_size = 0;
  739. st->filt_len = 0;
  740. st->mem = 0;
  741. st->resampler_ptr = 0;
  742. st->cutoff = 1.f;
  743. st->nb_channels = nb_channels;
  744. st->in_stride = 1;
  745. st->out_stride = 1;
  746. st->buffer_size = 160;
  747. /* Per channel data */
  748. if (!(st->last_sample = (spx_int32_t*)speex_alloc(nb_channels*sizeof(spx_int32_t))))
  749. goto fail;
  750. if (!(st->magic_samples = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t))))
  751. goto fail;
  752. if (!(st->samp_frac_num = (spx_uint32_t*)speex_alloc(nb_channels*sizeof(spx_uint32_t))))
  753. goto fail;
  754. speex_resampler_set_quality(st, quality);
  755. speex_resampler_set_rate_frac(st, ratio_num, ratio_den, in_rate, out_rate);
  756. filter_err = update_filter(st);
  757. if (filter_err == RESAMPLER_ERR_SUCCESS)
  758. {
  759. st->initialised = 1;
  760. } else {
  761. speex_resampler_destroy(st);
  762. st = NULL;
  763. }
  764. if (err)
  765. *err = filter_err;
  766. return st;
  767. fail:
  768. if (err)
  769. *err = RESAMPLER_ERR_ALLOC_FAILED;
  770. speex_resampler_destroy(st);
  771. return NULL;
  772. }
  773. EXPORT void speex_resampler_destroy(SpeexResamplerState *st)
  774. {
  775. speex_free(st->mem);
  776. speex_free(st->sinc_table);
  777. speex_free(st->last_sample);
  778. speex_free(st->magic_samples);
  779. speex_free(st->samp_frac_num);
  780. speex_free(st);
  781. }
  782. static int speex_resampler_process_native(SpeexResamplerState *st, spx_uint32_t channel_index, spx_uint32_t *in_len, spx_word16_t *out, spx_uint32_t *out_len)
  783. {
  784. int j=0;
  785. const int N = st->filt_len;
  786. int out_sample = 0;
  787. spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
  788. spx_uint32_t ilen;
  789. st->started = 1;
  790. /* Call the right resampler through the function ptr */
  791. out_sample = st->resampler_ptr(st, channel_index, mem, in_len, out, out_len);
  792. if (st->last_sample[channel_index] < (spx_int32_t)*in_len)
  793. *in_len = st->last_sample[channel_index];
  794. *out_len = out_sample;
  795. st->last_sample[channel_index] -= *in_len;
  796. ilen = *in_len;
  797. for(j=0;j<N-1;++j)
  798. mem[j] = mem[j+ilen];
  799. return RESAMPLER_ERR_SUCCESS;
  800. }
  801. static int speex_resampler_magic(SpeexResamplerState *st, spx_uint32_t channel_index, spx_word16_t **out, spx_uint32_t out_len) {
  802. spx_uint32_t tmp_in_len = st->magic_samples[channel_index];
  803. spx_word16_t *mem = st->mem + channel_index * st->mem_alloc_size;
  804. const int N = st->filt_len;
  805. speex_resampler_process_native(st, channel_index, &tmp_in_len, *out, &out_len);
  806. st->magic_samples[channel_index] -= tmp_in_len;
  807. /* If we couldn't process all "magic" input samples, save the rest for next time */
  808. if (st->magic_samples[channel_index])
  809. {
  810. spx_uint32_t i;
  811. for (i=0;i<st->magic_samples[channel_index];i++)
  812. mem[N-1+i]=mem[N-1+i+tmp_in_len];
  813. }
  814. *out += out_len*st->out_stride;
  815. return out_len;
  816. }
  817. #ifdef FIXED_POINT
  818. EXPORT int speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
  819. #else
  820. EXPORT int speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
  821. #endif
  822. {
  823. int j;
  824. spx_uint32_t ilen = *in_len;
  825. spx_uint32_t olen = *out_len;
  826. spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
  827. const int filt_offs = st->filt_len - 1;
  828. const spx_uint32_t xlen = st->mem_alloc_size - filt_offs;
  829. const int istride = st->in_stride;
  830. if (st->magic_samples[channel_index])
  831. olen -= speex_resampler_magic(st, channel_index, &out, olen);
  832. if (! st->magic_samples[channel_index]) {
  833. while (ilen && olen) {
  834. spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
  835. spx_uint32_t ochunk = olen;
  836. if (in) {
  837. for(j=0;j<ichunk;++j)
  838. x[j+filt_offs]=in[j*istride];
  839. } else {
  840. for(j=0;j<ichunk;++j)
  841. x[j+filt_offs]=0;
  842. }
  843. speex_resampler_process_native(st, channel_index, &ichunk, out, &ochunk);
  844. ilen -= ichunk;
  845. olen -= ochunk;
  846. out += ochunk * st->out_stride;
  847. if (in)
  848. in += ichunk * istride;
  849. }
  850. }
  851. *in_len -= ilen;
  852. *out_len -= olen;
  853. return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
  854. }
  855. #ifdef FIXED_POINT
  856. EXPORT int speex_resampler_process_float(SpeexResamplerState *st, spx_uint32_t channel_index, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
  857. #else
  858. EXPORT int speex_resampler_process_int(SpeexResamplerState *st, spx_uint32_t channel_index, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
  859. #endif
  860. {
  861. int j;
  862. const int istride_save = st->in_stride;
  863. const int ostride_save = st->out_stride;
  864. spx_uint32_t ilen = *in_len;
  865. spx_uint32_t olen = *out_len;
  866. spx_word16_t *x = st->mem + channel_index * st->mem_alloc_size;
  867. const spx_uint32_t xlen = st->mem_alloc_size - (st->filt_len - 1);
  868. #ifdef VAR_ARRAYS
  869. const unsigned int ylen = (olen < FIXED_STACK_ALLOC) ? olen : FIXED_STACK_ALLOC;
  870. spx_word16_t ystack[ylen];
  871. #else
  872. const unsigned int ylen = FIXED_STACK_ALLOC;
  873. spx_word16_t ystack[FIXED_STACK_ALLOC];
  874. #endif
  875. st->out_stride = 1;
  876. while (ilen && olen) {
  877. spx_word16_t *y = ystack;
  878. spx_uint32_t ichunk = (ilen > xlen) ? xlen : ilen;
  879. spx_uint32_t ochunk = (olen > ylen) ? ylen : olen;
  880. spx_uint32_t omagic = 0;
  881. if (st->magic_samples[channel_index]) {
  882. omagic = speex_resampler_magic(st, channel_index, &y, ochunk);
  883. ochunk -= omagic;
  884. olen -= omagic;
  885. }
  886. if (! st->magic_samples[channel_index]) {
  887. if (in) {
  888. for(j=0;j<ichunk;++j)
  889. #ifdef FIXED_POINT
  890. x[j+st->filt_len-1]=WORD2INT(in[j*istride_save]);
  891. #else
  892. x[j+st->filt_len-1]=in[j*istride_save];
  893. #endif
  894. } else {
  895. for(j=0;j<ichunk;++j)
  896. x[j+st->filt_len-1]=0;
  897. }
  898. speex_resampler_process_native(st, channel_index, &ichunk, y, &ochunk);
  899. } else {
  900. ichunk = 0;
  901. ochunk = 0;
  902. }
  903. for (j=0;j<ochunk+omagic;++j)
  904. #ifdef FIXED_POINT
  905. out[j*ostride_save] = ystack[j];
  906. #else
  907. out[j*ostride_save] = WORD2INT(ystack[j]);
  908. #endif
  909. ilen -= ichunk;
  910. olen -= ochunk;
  911. out += (ochunk+omagic) * ostride_save;
  912. if (in)
  913. in += ichunk * istride_save;
  914. }
  915. st->out_stride = ostride_save;
  916. *in_len -= ilen;
  917. *out_len -= olen;
  918. return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
  919. }
  920. EXPORT int speex_resampler_process_interleaved_float(SpeexResamplerState *st, const float *in, spx_uint32_t *in_len, float *out, spx_uint32_t *out_len)
  921. {
  922. spx_uint32_t i;
  923. int istride_save, ostride_save;
  924. spx_uint32_t bak_out_len = *out_len;
  925. spx_uint32_t bak_in_len = *in_len;
  926. istride_save = st->in_stride;
  927. ostride_save = st->out_stride;
  928. st->in_stride = st->out_stride = st->nb_channels;
  929. for (i=0;i<st->nb_channels;i++)
  930. {
  931. *out_len = bak_out_len;
  932. *in_len = bak_in_len;
  933. if (in != NULL)
  934. speex_resampler_process_float(st, i, in+i, in_len, out+i, out_len);
  935. else
  936. speex_resampler_process_float(st, i, NULL, in_len, out+i, out_len);
  937. }
  938. st->in_stride = istride_save;
  939. st->out_stride = ostride_save;
  940. return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
  941. }
  942. EXPORT int speex_resampler_process_interleaved_int(SpeexResamplerState *st, const spx_int16_t *in, spx_uint32_t *in_len, spx_int16_t *out, spx_uint32_t *out_len)
  943. {
  944. spx_uint32_t i;
  945. int istride_save, ostride_save;
  946. spx_uint32_t bak_out_len = *out_len;
  947. spx_uint32_t bak_in_len = *in_len;
  948. istride_save = st->in_stride;
  949. ostride_save = st->out_stride;
  950. st->in_stride = st->out_stride = st->nb_channels;
  951. for (i=0;i<st->nb_channels;i++)
  952. {
  953. *out_len = bak_out_len;
  954. *in_len = bak_in_len;
  955. if (in != NULL)
  956. speex_resampler_process_int(st, i, in+i, in_len, out+i, out_len);
  957. else
  958. speex_resampler_process_int(st, i, NULL, in_len, out+i, out_len);
  959. }
  960. st->in_stride = istride_save;
  961. st->out_stride = ostride_save;
  962. return st->resampler_ptr == resampler_basic_zero ? RESAMPLER_ERR_ALLOC_FAILED : RESAMPLER_ERR_SUCCESS;
  963. }
  964. EXPORT int speex_resampler_set_rate(SpeexResamplerState *st, spx_uint32_t in_rate, spx_uint32_t out_rate)
  965. {
  966. return speex_resampler_set_rate_frac(st, in_rate, out_rate, in_rate, out_rate);
  967. }
  968. EXPORT void speex_resampler_get_rate(SpeexResamplerState *st, spx_uint32_t *in_rate, spx_uint32_t *out_rate)
  969. {
  970. *in_rate = st->in_rate;
  971. *out_rate = st->out_rate;
  972. }
  973. static inline spx_uint32_t compute_gcd(spx_uint32_t a, spx_uint32_t b)
  974. {
  975. while (b != 0)
  976. {
  977. spx_uint32_t temp = a;
  978. a = b;
  979. b = temp % b;
  980. }
  981. return a;
  982. }
  983. EXPORT int speex_resampler_set_rate_frac(SpeexResamplerState *st, spx_uint32_t ratio_num, spx_uint32_t ratio_den, spx_uint32_t in_rate, spx_uint32_t out_rate)
  984. {
  985. spx_uint32_t fact;
  986. spx_uint32_t old_den;
  987. spx_uint32_t i;
  988. if (ratio_num == 0 || ratio_den == 0)
  989. return RESAMPLER_ERR_INVALID_ARG;
  990. if (st->in_rate == in_rate && st->out_rate == out_rate && st->num_rate == ratio_num && st->den_rate == ratio_den)
  991. return RESAMPLER_ERR_SUCCESS;
  992. old_den = st->den_rate;
  993. st->in_rate = in_rate;
  994. st->out_rate = out_rate;
  995. st->num_rate = ratio_num;
  996. st->den_rate = ratio_den;
  997. fact = compute_gcd(st->num_rate, st->den_rate);
  998. st->num_rate /= fact;
  999. st->den_rate /= fact;
  1000. if (old_den > 0)
  1001. {
  1002. for (i=0;i<st->nb_channels;i++)
  1003. {
  1004. if (multiply_frac(&st->samp_frac_num[i],st->samp_frac_num[i],st->den_rate,old_den) != RESAMPLER_ERR_SUCCESS)
  1005. return RESAMPLER_ERR_OVERFLOW;
  1006. /* Safety net */
  1007. if (st->samp_frac_num[i] >= st->den_rate)
  1008. st->samp_frac_num[i] = st->den_rate-1;
  1009. }
  1010. }
  1011. if (st->initialised)
  1012. return update_filter(st);
  1013. return RESAMPLER_ERR_SUCCESS;
  1014. }
  1015. EXPORT void speex_resampler_get_ratio(SpeexResamplerState *st, spx_uint32_t *ratio_num, spx_uint32_t *ratio_den)
  1016. {
  1017. *ratio_num = st->num_rate;
  1018. *ratio_den = st->den_rate;
  1019. }
  1020. EXPORT int speex_resampler_set_quality(SpeexResamplerState *st, int quality)
  1021. {
  1022. if (quality > 10 || quality < 0)
  1023. return RESAMPLER_ERR_INVALID_ARG;
  1024. if (st->quality == quality)
  1025. return RESAMPLER_ERR_SUCCESS;
  1026. st->quality = quality;
  1027. if (st->initialised)
  1028. return update_filter(st);
  1029. return RESAMPLER_ERR_SUCCESS;
  1030. }
  1031. EXPORT void speex_resampler_get_quality(SpeexResamplerState *st, int *quality)
  1032. {
  1033. *quality = st->quality;
  1034. }
  1035. EXPORT void speex_resampler_set_input_stride(SpeexResamplerState *st, spx_uint32_t stride)
  1036. {
  1037. st->in_stride = stride;
  1038. }
  1039. EXPORT void speex_resampler_get_input_stride(SpeexResamplerState *st, spx_uint32_t *stride)
  1040. {
  1041. *stride = st->in_stride;
  1042. }
  1043. EXPORT void speex_resampler_set_output_stride(SpeexResamplerState *st, spx_uint32_t stride)
  1044. {
  1045. st->out_stride = stride;
  1046. }
  1047. EXPORT void speex_resampler_get_output_stride(SpeexResamplerState *st, spx_uint32_t *stride)
  1048. {
  1049. *stride = st->out_stride;
  1050. }
  1051. EXPORT int speex_resampler_get_input_latency(SpeexResamplerState *st)
  1052. {
  1053. return st->filt_len / 2;
  1054. }
  1055. EXPORT int speex_resampler_get_output_latency(SpeexResamplerState *st)
  1056. {
  1057. return ((st->filt_len / 2) * st->den_rate + (st->num_rate >> 1)) / st->num_rate;
  1058. }
  1059. EXPORT int speex_resampler_skip_zeros(SpeexResamplerState *st)
  1060. {
  1061. spx_uint32_t i;
  1062. for (i=0;i<st->nb_channels;i++)
  1063. st->last_sample[i] = st->filt_len/2;
  1064. return RESAMPLER_ERR_SUCCESS;
  1065. }
  1066. EXPORT int speex_resampler_reset_mem(SpeexResamplerState *st)
  1067. {
  1068. spx_uint32_t i;
  1069. for (i=0;i<st->nb_channels;i++)
  1070. {
  1071. st->last_sample[i] = 0;
  1072. st->magic_samples[i] = 0;
  1073. st->samp_frac_num[i] = 0;
  1074. }
  1075. for (i=0;i<st->nb_channels*(st->filt_len-1);i++)
  1076. st->mem[i] = 0;
  1077. return RESAMPLER_ERR_SUCCESS;
  1078. }
  1079. EXPORT const char *speex_resampler_strerror(int err)
  1080. {
  1081. switch (err)
  1082. {
  1083. case RESAMPLER_ERR_SUCCESS:
  1084. return "Success.";
  1085. case RESAMPLER_ERR_ALLOC_FAILED:
  1086. return "Memory allocation failed.";
  1087. case RESAMPLER_ERR_BAD_STATE:
  1088. return "Bad resampler state.";
  1089. case RESAMPLER_ERR_INVALID_ARG:
  1090. return "Invalid argument.";
  1091. case RESAMPLER_ERR_PTR_OVERLAP:
  1092. return "Input and output buffers overlap.";
  1093. default:
  1094. return "Unknown error. Bad error code or strange version mismatch.";
  1095. }
  1096. }