resampler.c 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958
  1. #include <stdlib.h>
  2. #include <string.h>
  3. #define _USE_MATH_DEFINES
  4. #include <math.h>
  5. #if (defined(_M_IX86) || defined(__i386__) || defined(_M_X64) || defined(__amd64__))
  6. #include <xmmintrin.h>
  7. #define RESAMPLER_SSE
  8. #endif
  9. #ifdef _MSC_VER
  10. #define ALIGNED _declspec(align(16))
  11. #else
  12. #define ALIGNED __attribute__((aligned(16)))
  13. #endif
  14. #ifndef M_PI
  15. #define M_PI 3.14159265358979323846
  16. #endif
  17. #include "resampler.h"
  18. enum { RESAMPLER_SHIFT = 10 };
  19. enum { RESAMPLER_RESOLUTION = 1 << RESAMPLER_SHIFT };
  20. enum { SINC_WIDTH = 16 };
  21. enum { SINC_SAMPLES = RESAMPLER_RESOLUTION * SINC_WIDTH };
  22. enum { CUBIC_SAMPLES = RESAMPLER_RESOLUTION * 4 };
  23. ALIGNED static float cubic_lut[CUBIC_SAMPLES];
  24. static float sinc_lut[SINC_SAMPLES + 1];
  25. static float window_lut[SINC_SAMPLES + 1];
  26. enum { resampler_buffer_size = SINC_WIDTH * 4 };
  27. static int fEqual(const float b, const float a)
  28. {
  29. return fabs(a - b) < 1.0e-6;
  30. }
  31. static float sinc(float x)
  32. {
  33. return fEqual(x, 0.0f) ? 1.0f : (float)(sin(x * M_PI) / (x * M_PI));
  34. }
  35. #ifdef RESAMPLER_SSE
  36. #ifdef _MSC_VER
  37. #include <intrin.h>
  38. #elif defined(__clang__) || defined(__GNUC__)
  39. static inline void
  40. __cpuid(int *data, int selector)
  41. {
  42. #if defined(__PIC__) && defined(__i386__)
  43. asm("xchgl %%ebx, %%esi; cpuid; xchgl %%ebx, %%esi"
  44. : "=a" (data[0]),
  45. "=S" (data[1]),
  46. "=c" (data[2]),
  47. "=d" (data[3])
  48. : "0" (selector));
  49. #elif defined(__PIC__) && defined(__amd64__)
  50. asm("xchg{q} {%%}rbx, %q1; cpuid; xchg{q} {%%}rbx, %q1"
  51. : "=a" (data[0]),
  52. "=&r" (data[1]),
  53. "=c" (data[2]),
  54. "=d" (data[3])
  55. : "0" (selector));
  56. #else
  57. asm("cpuid"
  58. : "=a" (data[0]),
  59. "=b" (data[1]),
  60. "=c" (data[2]),
  61. "=d" (data[3])
  62. : "0" (selector));
  63. #endif
  64. }
  65. #else
  66. #define __cpuid(a,b) memset((a), 0, sizeof(int) * 4)
  67. #endif
  68. static int query_cpu_feature_sse() {
  69. int buffer[4];
  70. __cpuid(buffer,1);
  71. if ((buffer[3]&(1<<25)) == 0) return 0;
  72. return 1;
  73. }
  74. static int resampler_has_sse = 0;
  75. #endif
  76. void resampler_init(void)
  77. {
  78. unsigned i;
  79. double dx = (float)(SINC_WIDTH) / SINC_SAMPLES, x = 0.0;
  80. for (i = 0; i < SINC_SAMPLES + 1; ++i, x += dx)
  81. {
  82. float y = x / SINC_WIDTH;
  83. #if 0
  84. // Blackman
  85. float window = 0.42659 - 0.49656 * cos(M_PI + M_PI * y) + 0.076849 * cos(2.0 * M_PI * y);
  86. #elif 1
  87. // Nuttal 3 term
  88. float window = 0.40897 + 0.5 * cos(M_PI * y) + 0.09103 * cos(2.0 * M_PI * y);
  89. #elif 0
  90. // C.R.Helmrich's 2 term window
  91. float window = 0.79445 * cos(0.5 * M_PI * y) + 0.20555 * cos(1.5 * M_PI * y);
  92. #elif 0
  93. // Lanczos
  94. float window = sinc(y);
  95. #endif
  96. sinc_lut[i] = fabs(x) < SINC_WIDTH ? sinc(x) : 0.0;
  97. window_lut[i] = window;
  98. }
  99. dx = 1.0 / (float)(RESAMPLER_RESOLUTION);
  100. x = 0.0;
  101. for (i = 0; i < RESAMPLER_RESOLUTION; ++i, x += dx)
  102. {
  103. cubic_lut[i*4] = (float)(-0.5 * x * x * x + x * x - 0.5 * x);
  104. cubic_lut[i*4+1] = (float)( 1.5 * x * x * x - 2.5 * x * x + 1.0);
  105. cubic_lut[i*4+2] = (float)(-1.5 * x * x * x + 2.0 * x * x + 0.5 * x);
  106. cubic_lut[i*4+3] = (float)( 0.5 * x * x * x - 0.5 * x * x);
  107. }
  108. #ifdef RESAMPLER_SSE
  109. resampler_has_sse = query_cpu_feature_sse();
  110. #endif
  111. }
  112. typedef struct resampler
  113. {
  114. int write_pos, write_filled;
  115. int read_pos, read_filled;
  116. unsigned int phase;
  117. unsigned int phase_inc;
  118. unsigned int inv_phase;
  119. unsigned int inv_phase_inc;
  120. unsigned char quality;
  121. signed char delay_added;
  122. signed char delay_removed;
  123. float last_amp;
  124. float accumulator;
  125. float buffer_in[resampler_buffer_size * 2];
  126. float buffer_out[resampler_buffer_size + SINC_WIDTH * 2 - 1];
  127. } resampler;
  128. void * resampler_create(void)
  129. {
  130. resampler * r = ( resampler * ) malloc( sizeof(resampler) );
  131. if ( !r ) return 0;
  132. r->write_pos = SINC_WIDTH - 1;
  133. r->write_filled = 0;
  134. r->read_pos = 0;
  135. r->read_filled = 0;
  136. r->phase = 0;
  137. r->phase_inc = 0;
  138. r->inv_phase = 0;
  139. r->inv_phase_inc = 0;
  140. r->quality = RESAMPLER_QUALITY_MAX;
  141. r->delay_added = -1;
  142. r->delay_removed = -1;
  143. r->last_amp = 0;
  144. r->accumulator = 0;
  145. memset( r->buffer_in, 0, sizeof(r->buffer_in) );
  146. memset( r->buffer_out, 0, sizeof(r->buffer_out) );
  147. return r;
  148. }
  149. void resampler_delete(void * _r)
  150. {
  151. free( _r );
  152. }
  153. void * resampler_dup(const void * _r)
  154. {
  155. const resampler * r_in = ( const resampler * ) _r;
  156. resampler * r_out = ( resampler * ) malloc( sizeof(resampler) );
  157. if ( !r_out ) return 0;
  158. r_out->write_pos = r_in->write_pos;
  159. r_out->write_filled = r_in->write_filled;
  160. r_out->read_pos = r_in->read_pos;
  161. r_out->read_filled = r_in->read_filled;
  162. r_out->phase = r_in->phase;
  163. r_out->phase_inc = r_in->phase_inc;
  164. r_out->inv_phase = r_in->inv_phase;
  165. r_out->inv_phase_inc = r_in->inv_phase_inc;
  166. r_out->quality = r_in->quality;
  167. r_out->delay_added = r_in->delay_added;
  168. r_out->delay_removed = r_in->delay_removed;
  169. r_out->last_amp = r_in->last_amp;
  170. r_out->accumulator = r_in->accumulator;
  171. memcpy( r_out->buffer_in, r_in->buffer_in, sizeof(r_in->buffer_in) );
  172. memcpy( r_out->buffer_out, r_in->buffer_out, sizeof(r_in->buffer_out) );
  173. return r_out;
  174. }
  175. void resampler_dup_inplace(void *_d, const void *_s)
  176. {
  177. const resampler * r_in = ( const resampler * ) _s;
  178. resampler * r_out = ( resampler * ) _d;
  179. r_out->write_pos = r_in->write_pos;
  180. r_out->write_filled = r_in->write_filled;
  181. r_out->read_pos = r_in->read_pos;
  182. r_out->read_filled = r_in->read_filled;
  183. r_out->phase = r_in->phase;
  184. r_out->phase_inc = r_in->phase_inc;
  185. r_out->inv_phase = r_in->inv_phase;
  186. r_out->inv_phase_inc = r_in->inv_phase_inc;
  187. r_out->quality = r_in->quality;
  188. r_out->delay_added = r_in->delay_added;
  189. r_out->delay_removed = r_in->delay_removed;
  190. r_out->last_amp = r_in->last_amp;
  191. r_out->accumulator = r_in->accumulator;
  192. memcpy( r_out->buffer_in, r_in->buffer_in, sizeof(r_in->buffer_in) );
  193. memcpy( r_out->buffer_out, r_in->buffer_out, sizeof(r_in->buffer_out) );
  194. }
  195. void resampler_set_quality(void *_r, int quality)
  196. {
  197. resampler * r = ( resampler * ) _r;
  198. if (quality < RESAMPLER_QUALITY_MIN)
  199. quality = RESAMPLER_QUALITY_MIN;
  200. else if (quality > RESAMPLER_QUALITY_MAX)
  201. quality = RESAMPLER_QUALITY_MAX;
  202. if ( r->quality != quality )
  203. {
  204. if ( quality == RESAMPLER_QUALITY_BLEP || r->quality == RESAMPLER_QUALITY_BLEP )
  205. {
  206. r->read_pos = 0;
  207. r->read_filled = 0;
  208. r->last_amp = 0;
  209. r->accumulator = 0;
  210. memset( r->buffer_out, 0, sizeof(r->buffer_out) );
  211. }
  212. r->delay_added = -1;
  213. r->delay_removed = -1;
  214. }
  215. r->quality = (unsigned char)quality;
  216. }
  217. int resampler_get_free_count(void *_r)
  218. {
  219. resampler * r = ( resampler * ) _r;
  220. return resampler_buffer_size - r->write_filled;
  221. }
  222. static int resampler_min_filled(resampler *r)
  223. {
  224. switch (r->quality)
  225. {
  226. default:
  227. case RESAMPLER_QUALITY_ZOH:
  228. case RESAMPLER_QUALITY_BLEP:
  229. return 1;
  230. case RESAMPLER_QUALITY_LINEAR:
  231. return 2;
  232. case RESAMPLER_QUALITY_CUBIC:
  233. return 4;
  234. case RESAMPLER_QUALITY_SINC:
  235. return SINC_WIDTH * 2;
  236. }
  237. }
  238. static int resampler_input_delay(resampler *r)
  239. {
  240. switch (r->quality)
  241. {
  242. default:
  243. case RESAMPLER_QUALITY_ZOH:
  244. case RESAMPLER_QUALITY_BLEP:
  245. case RESAMPLER_QUALITY_LINEAR:
  246. return 0;
  247. case RESAMPLER_QUALITY_CUBIC:
  248. return 1;
  249. case RESAMPLER_QUALITY_SINC:
  250. return SINC_WIDTH - 1;
  251. }
  252. }
  253. static int resampler_output_delay(resampler *r)
  254. {
  255. switch (r->quality)
  256. {
  257. default:
  258. case RESAMPLER_QUALITY_ZOH:
  259. case RESAMPLER_QUALITY_LINEAR:
  260. case RESAMPLER_QUALITY_CUBIC:
  261. case RESAMPLER_QUALITY_SINC:
  262. return 0;
  263. case RESAMPLER_QUALITY_BLEP:
  264. return SINC_WIDTH - 1;
  265. }
  266. }
  267. int resampler_ready(void *_r)
  268. {
  269. resampler * r = ( resampler * ) _r;
  270. return r->write_filled > resampler_min_filled(r);
  271. }
  272. void resampler_clear(void *_r)
  273. {
  274. resampler * r = ( resampler * ) _r;
  275. r->write_pos = SINC_WIDTH - 1;
  276. r->write_filled = 0;
  277. r->read_pos = 0;
  278. r->read_filled = 0;
  279. r->phase = 0;
  280. r->delay_added = -1;
  281. r->delay_removed = -1;
  282. memset(r->buffer_in, 0, (SINC_WIDTH - 1) * sizeof(r->buffer_in[0]));
  283. memset(r->buffer_in + resampler_buffer_size, 0, (SINC_WIDTH - 1) * sizeof(r->buffer_in[0]));
  284. if (r->quality == RESAMPLER_QUALITY_BLEP)
  285. memset(r->buffer_out, 0, sizeof(r->buffer_out));
  286. }
  287. void resampler_set_rate(void *_r, double new_factor)
  288. {
  289. resampler * r = ( resampler * ) _r;
  290. r->phase_inc = (int)( new_factor * RESAMPLER_RESOLUTION );
  291. new_factor = 1.0 / new_factor;
  292. r->inv_phase_inc = (int)( new_factor * RESAMPLER_RESOLUTION );
  293. }
  294. void resampler_write_sample(void *_r, short s)
  295. {
  296. resampler * r = ( resampler * ) _r;
  297. if ( r->delay_added < 0 )
  298. {
  299. r->delay_added = 0;
  300. r->write_filled = resampler_input_delay( r );
  301. }
  302. if ( r->write_filled < resampler_buffer_size )
  303. {
  304. float s32 = s;
  305. s32 *= 256.0;
  306. r->buffer_in[ r->write_pos ] = s32;
  307. r->buffer_in[ r->write_pos + resampler_buffer_size ] = s32;
  308. ++r->write_filled;
  309. r->write_pos = ( r->write_pos + 1 ) % resampler_buffer_size;
  310. }
  311. }
  312. void resampler_write_sample_fixed(void *_r, int s, unsigned char depth)
  313. {
  314. resampler * r = ( resampler * ) _r;
  315. if ( r->delay_added < 0 )
  316. {
  317. r->delay_added = 0;
  318. r->write_filled = resampler_input_delay( r );
  319. }
  320. if ( r->write_filled < resampler_buffer_size )
  321. {
  322. float s32 = s;
  323. s32 /= (double)(1 << (depth - 1));
  324. r->buffer_in[ r->write_pos ] = s32;
  325. r->buffer_in[ r->write_pos + resampler_buffer_size ] = s32;
  326. ++r->write_filled;
  327. r->write_pos = ( r->write_pos + 1 ) % resampler_buffer_size;
  328. }
  329. }
  330. static int resampler_run_zoh(resampler * r, float ** out_, float * out_end)
  331. {
  332. int in_size = r->write_filled;
  333. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  334. int used = 0;
  335. in_size -= 1;
  336. if ( in_size > 0 )
  337. {
  338. float* out = *out_;
  339. float const* in = in_;
  340. float const* const in_end = in + in_size;
  341. int phase = r->phase;
  342. int phase_inc = r->phase_inc;
  343. do
  344. {
  345. float sample;
  346. if ( out >= out_end )
  347. break;
  348. sample = *in;
  349. *out++ = sample;
  350. phase += phase_inc;
  351. in += phase >> RESAMPLER_SHIFT;
  352. phase &= RESAMPLER_RESOLUTION-1;
  353. }
  354. while ( in < in_end );
  355. r->phase = (unsigned short) phase;
  356. *out_ = out;
  357. used = (int)(in - in_);
  358. r->write_filled -= used;
  359. }
  360. return used;
  361. }
  362. static int resampler_run_blep(resampler * r, float ** out_, float * out_end)
  363. {
  364. int in_size = r->write_filled;
  365. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  366. int used = 0;
  367. in_size -= 1;
  368. if ( in_size > 0 )
  369. {
  370. float* out = *out_;
  371. float const* in = in_;
  372. float const* const in_end = in + in_size;
  373. float last_amp = r->last_amp;
  374. int inv_phase = r->inv_phase;
  375. int inv_phase_inc = r->inv_phase_inc;
  376. const int step = RESAMPLER_RESOLUTION;
  377. do
  378. {
  379. float kernel[SINC_WIDTH * 2], kernel_sum = 0.0;
  380. int i = SINC_WIDTH;
  381. float sample;
  382. if ( out + SINC_WIDTH * 2 > out_end )
  383. break;
  384. for (; i >= -SINC_WIDTH + 1; --i)
  385. {
  386. int pos = i * step;
  387. int abs_pos = abs(inv_phase - pos);
  388. kernel_sum += kernel[i + SINC_WIDTH - 1] = sinc_lut[abs_pos] * window_lut[abs_pos];
  389. }
  390. sample = *in++ - last_amp;
  391. last_amp += sample;
  392. sample /= kernel_sum;
  393. for (sample = 0, i = 0; i < SINC_WIDTH * 2; ++i)
  394. out[i] += sample * kernel[i];
  395. inv_phase += inv_phase_inc;
  396. out += inv_phase >> RESAMPLER_SHIFT;
  397. inv_phase &= RESAMPLER_RESOLUTION-1;
  398. }
  399. while ( in < in_end );
  400. r->inv_phase = inv_phase;
  401. r->last_amp = last_amp;
  402. *out_ = out;
  403. used = (int)(in - in_);
  404. r->write_filled -= used;
  405. }
  406. return used;
  407. }
  408. #ifdef RESAMPLER_SSE
  409. static int resampler_run_blep_sse(resampler * r, float ** out_, float * out_end)
  410. {
  411. int in_size = r->write_filled;
  412. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  413. int used = 0;
  414. in_size -= 1;
  415. if ( in_size > 0 )
  416. {
  417. float* out = *out_;
  418. float const* in = in_;
  419. float const* const in_end = in + in_size;
  420. float last_amp = r->last_amp;
  421. int inv_phase = r->inv_phase;
  422. int inv_phase_inc = r->inv_phase_inc;
  423. const int step = RESAMPLER_RESOLUTION;
  424. do
  425. {
  426. // accumulate in extended precision
  427. float kernel_sum = 0.0;
  428. __m128 kernel[SINC_WIDTH / 2];
  429. __m128 temp1, temp2;
  430. __m128 samplex;
  431. float sample;
  432. float *kernelf = (float*)(&kernel);
  433. int i = SINC_WIDTH;
  434. if ( out + SINC_WIDTH * 2 > out_end )
  435. break;
  436. for (; i >= -SINC_WIDTH + 1; --i)
  437. {
  438. int pos = i * step;
  439. int abs_pos = abs(inv_phase - pos);
  440. kernel_sum += kernelf[i + SINC_WIDTH - 1] = sinc_lut[abs_pos] * window_lut[abs_pos];
  441. }
  442. sample = *in++ - last_amp;
  443. last_amp += sample;
  444. sample /= kernel_sum;
  445. samplex = _mm_set1_ps( sample );
  446. for (i = 0; i < SINC_WIDTH / 2; ++i)
  447. {
  448. temp1 = _mm_load_ps( (const float *)( kernel + i ) );
  449. temp1 = _mm_mul_ps( temp1, samplex );
  450. temp2 = _mm_loadu_ps( (const float *) out + i * 4 );
  451. temp1 = _mm_add_ps( temp1, temp2 );
  452. _mm_storeu_ps( (float *) out + i * 4, temp1 );
  453. }
  454. inv_phase += inv_phase_inc;
  455. out += inv_phase >> RESAMPLER_SHIFT;
  456. inv_phase &= RESAMPLER_RESOLUTION - 1;
  457. }
  458. while ( in < in_end );
  459. r->inv_phase = inv_phase;
  460. r->last_amp = last_amp;
  461. *out_ = out;
  462. used = (int)(in - in_);
  463. r->write_filled -= used;
  464. }
  465. return used;
  466. }
  467. #endif
  468. static int resampler_run_linear(resampler * r, float ** out_, float * out_end)
  469. {
  470. int in_size = r->write_filled;
  471. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  472. int used = 0;
  473. in_size -= 2;
  474. if ( in_size > 0 )
  475. {
  476. float* out = *out_;
  477. float const* in = in_;
  478. float const* const in_end = in + in_size;
  479. int phase = r->phase;
  480. int phase_inc = r->phase_inc;
  481. do
  482. {
  483. float sample;
  484. if ( out >= out_end )
  485. break;
  486. sample = in[0] + (in[1] - in[0]) * ((float)phase / RESAMPLER_RESOLUTION);
  487. *out++ = sample;
  488. phase += phase_inc;
  489. in += phase >> RESAMPLER_SHIFT;
  490. phase &= RESAMPLER_RESOLUTION-1;
  491. }
  492. while ( in < in_end );
  493. r->phase = phase;
  494. *out_ = out;
  495. used = (int)(in - in_);
  496. r->write_filled -= used;
  497. }
  498. return used;
  499. }
  500. static int resampler_run_cubic(resampler * r, float ** out_, float * out_end)
  501. {
  502. int in_size = r->write_filled;
  503. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  504. int used = 0;
  505. in_size -= 4;
  506. if ( in_size > 0 )
  507. {
  508. float* out = *out_;
  509. float const* in = in_;
  510. float const* const in_end = in + in_size;
  511. int phase = r->phase;
  512. int phase_inc = r->phase_inc;
  513. do
  514. {
  515. float * kernel;
  516. int i;
  517. float sample;
  518. if ( out >= out_end )
  519. break;
  520. kernel = cubic_lut + phase * 4;
  521. for (sample = 0, i = 0; i < 4; ++i)
  522. sample += in[i] * kernel[i];
  523. *out++ = sample;
  524. phase += phase_inc;
  525. in += phase >> RESAMPLER_SHIFT;
  526. phase &= RESAMPLER_RESOLUTION-1;
  527. }
  528. while ( in < in_end );
  529. r->phase = phase;
  530. *out_ = out;
  531. used = (int)(in - in_);
  532. r->write_filled -= used;
  533. }
  534. return used;
  535. }
  536. #ifdef RESAMPLER_SSE
  537. static int resampler_run_cubic_sse(resampler * r, float ** out_, float * out_end)
  538. {
  539. int in_size = r->write_filled;
  540. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  541. int used = 0;
  542. in_size -= 4;
  543. if ( in_size > 0 )
  544. {
  545. float* out = *out_;
  546. float const* in = in_;
  547. float const* const in_end = in + in_size;
  548. int phase = r->phase;
  549. int phase_inc = r->phase_inc;
  550. do
  551. {
  552. __m128 temp1, temp2;
  553. __m128 samplex = _mm_setzero_ps();
  554. if ( out >= out_end )
  555. break;
  556. temp1 = _mm_loadu_ps( (const float *)( in ) );
  557. temp2 = _mm_load_ps( (const float *)( cubic_lut + phase * 4 ) );
  558. temp1 = _mm_mul_ps( temp1, temp2 );
  559. samplex = _mm_add_ps( samplex, temp1 );
  560. temp1 = _mm_movehl_ps( temp1, samplex );
  561. samplex = _mm_add_ps( samplex, temp1 );
  562. temp1 = samplex;
  563. temp1 = _mm_shuffle_ps( temp1, samplex, _MM_SHUFFLE(0, 0, 0, 1) );
  564. samplex = _mm_add_ps( samplex, temp1 );
  565. _mm_store_ss( out, samplex );
  566. ++out;
  567. phase += phase_inc;
  568. in += phase >> RESAMPLER_SHIFT;
  569. phase &= RESAMPLER_RESOLUTION - 1;
  570. }
  571. while ( in < in_end );
  572. r->phase = phase;
  573. *out_ = out;
  574. used = (int)(in - in_);
  575. r->write_filled -= used;
  576. }
  577. return used;
  578. }
  579. #endif
  580. static int resampler_run_sinc(resampler * r, float ** out_, float * out_end)
  581. {
  582. int in_size = r->write_filled;
  583. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  584. int used = 0;
  585. in_size -= SINC_WIDTH * 2;
  586. if ( in_size > 0 )
  587. {
  588. float* out = *out_;
  589. float const* in = in_;
  590. float const* const in_end = in + in_size;
  591. int phase = r->phase;
  592. int phase_inc = r->phase_inc;
  593. int step = phase_inc > RESAMPLER_RESOLUTION ? RESAMPLER_RESOLUTION * RESAMPLER_RESOLUTION / phase_inc : RESAMPLER_RESOLUTION;
  594. int window_step = RESAMPLER_RESOLUTION;
  595. do
  596. {
  597. float kernel[SINC_WIDTH * 2], kernel_sum = 0.0;
  598. int i = SINC_WIDTH;
  599. int phase_adj = phase * step / RESAMPLER_RESOLUTION;
  600. float sample;
  601. if ( out >= out_end )
  602. break;
  603. for (; i >= -SINC_WIDTH + 1; --i)
  604. {
  605. int pos = i * step;
  606. int window_pos = i * window_step;
  607. kernel_sum += kernel[i + SINC_WIDTH - 1] = sinc_lut[abs(phase_adj - pos)] * window_lut[abs(phase - window_pos)];
  608. }
  609. for (sample = 0, i = 0; i < SINC_WIDTH * 2; ++i)
  610. sample += in[i] * kernel[i];
  611. *out++ = (float)(sample / kernel_sum);
  612. phase += phase_inc;
  613. in += phase >> RESAMPLER_SHIFT;
  614. phase &= RESAMPLER_RESOLUTION-1;
  615. }
  616. while ( in < in_end );
  617. r->phase = phase;
  618. *out_ = out;
  619. used = (int)(in - in_);
  620. r->write_filled -= used;
  621. }
  622. return used;
  623. }
  624. #ifdef RESAMPLER_SSE
  625. static int resampler_run_sinc_sse(resampler * r, float ** out_, float * out_end)
  626. {
  627. int in_size = r->write_filled;
  628. float const* in_ = r->buffer_in + resampler_buffer_size + r->write_pos - r->write_filled;
  629. int used = 0;
  630. in_size -= SINC_WIDTH * 2;
  631. if ( in_size > 0 )
  632. {
  633. float* out = *out_;
  634. float const* in = in_;
  635. float const* const in_end = in + in_size;
  636. int phase = r->phase;
  637. int phase_inc = r->phase_inc;
  638. int step = phase_inc > RESAMPLER_RESOLUTION ? RESAMPLER_RESOLUTION * RESAMPLER_RESOLUTION / phase_inc : RESAMPLER_RESOLUTION;
  639. int window_step = RESAMPLER_RESOLUTION;
  640. do
  641. {
  642. // accumulate in extended precision
  643. float kernel_sum = 0.0;
  644. __m128 kernel[SINC_WIDTH / 2];
  645. __m128 temp1, temp2;
  646. __m128 samplex = _mm_setzero_ps();
  647. float *kernelf = (float*)(&kernel);
  648. int i = SINC_WIDTH;
  649. int phase_adj = phase * step / RESAMPLER_RESOLUTION;
  650. if ( out >= out_end )
  651. break;
  652. for (; i >= -SINC_WIDTH + 1; --i)
  653. {
  654. int pos = i * step;
  655. int window_pos = i * window_step;
  656. kernel_sum += kernelf[i + SINC_WIDTH - 1] = sinc_lut[abs(phase_adj - pos)] * window_lut[abs(phase - window_pos)];
  657. }
  658. for (i = 0; i < SINC_WIDTH / 2; ++i)
  659. {
  660. temp1 = _mm_loadu_ps( (const float *)( in + i * 4 ) );
  661. temp2 = _mm_load_ps( (const float *)( kernel + i ) );
  662. temp1 = _mm_mul_ps( temp1, temp2 );
  663. samplex = _mm_add_ps( samplex, temp1 );
  664. }
  665. kernel_sum = 1.0f / kernel_sum;
  666. temp1 = _mm_movehl_ps( temp1, samplex );
  667. samplex = _mm_add_ps( samplex, temp1 );
  668. temp1 = samplex;
  669. temp1 = _mm_shuffle_ps( temp1, samplex, _MM_SHUFFLE(0, 0, 0, 1) );
  670. samplex = _mm_add_ps( samplex, temp1 );
  671. temp1 = _mm_set_ss( kernel_sum );
  672. samplex = _mm_mul_ps( samplex, temp1 );
  673. _mm_store_ss( out, samplex );
  674. ++out;
  675. phase += phase_inc;
  676. in += phase >> RESAMPLER_SHIFT;
  677. phase &= RESAMPLER_RESOLUTION - 1;
  678. }
  679. while ( in < in_end );
  680. r->phase = phase;
  681. *out_ = out;
  682. used = (int)(in - in_);
  683. r->write_filled -= used;
  684. }
  685. return used;
  686. }
  687. #endif
  688. static void resampler_fill(resampler * r)
  689. {
  690. int min_filled = resampler_min_filled(r);
  691. int quality = r->quality;
  692. while ( r->write_filled > min_filled &&
  693. r->read_filled < resampler_buffer_size )
  694. {
  695. int write_pos = ( r->read_pos + r->read_filled ) % resampler_buffer_size;
  696. int write_size = resampler_buffer_size - write_pos;
  697. float * out = r->buffer_out + write_pos;
  698. if ( write_size > ( resampler_buffer_size - r->read_filled ) )
  699. write_size = resampler_buffer_size - r->read_filled;
  700. switch (quality)
  701. {
  702. case RESAMPLER_QUALITY_ZOH:
  703. resampler_run_zoh( r, &out, out + write_size );
  704. break;
  705. case RESAMPLER_QUALITY_BLEP:
  706. {
  707. int used;
  708. int write_extra = 0;
  709. if ( write_pos >= r->read_pos )
  710. write_extra = r->read_pos;
  711. if ( write_extra > SINC_WIDTH * 2 - 1 )
  712. write_extra = SINC_WIDTH * 2 - 1;
  713. memcpy( r->buffer_out + resampler_buffer_size, r->buffer_out, write_extra * sizeof(r->buffer_out[0]) );
  714. #ifdef RESAMPLER_SSE
  715. if ( resampler_has_sse )
  716. used = resampler_run_blep_sse( r, &out, out + write_size + write_extra );
  717. else
  718. #endif
  719. used = resampler_run_blep( r, &out, out + write_size + write_extra );
  720. memcpy( r->buffer_out, r->buffer_out + resampler_buffer_size, write_extra * sizeof(r->buffer_out[0]) );
  721. if (!used)
  722. return;
  723. break;
  724. }
  725. case RESAMPLER_QUALITY_LINEAR:
  726. resampler_run_linear( r, &out, out + write_size );
  727. break;
  728. case RESAMPLER_QUALITY_CUBIC:
  729. #ifdef RESAMPLER_SSE
  730. if ( resampler_has_sse )
  731. resampler_run_cubic_sse( r, &out, out + write_size );
  732. else
  733. #endif
  734. resampler_run_cubic( r, &out, out + write_size );
  735. break;
  736. case RESAMPLER_QUALITY_SINC:
  737. #ifdef RESAMPLER_SSE
  738. if ( resampler_has_sse )
  739. resampler_run_sinc_sse( r, &out, out + write_size );
  740. else
  741. #endif
  742. resampler_run_sinc( r, &out, out + write_size );
  743. break;
  744. }
  745. r->read_filled += out - r->buffer_out - write_pos;
  746. }
  747. }
  748. static void resampler_fill_and_remove_delay(resampler * r)
  749. {
  750. resampler_fill( r );
  751. if ( r->delay_removed < 0 )
  752. {
  753. int delay = resampler_output_delay( r );
  754. r->delay_removed = 0;
  755. while ( delay-- )
  756. resampler_remove_sample( r );
  757. }
  758. }
  759. int resampler_get_sample_count(void *_r)
  760. {
  761. resampler * r = ( resampler * ) _r;
  762. if ( r->read_filled < 1 && (r->quality != RESAMPLER_QUALITY_BLEP || r->inv_phase_inc))
  763. resampler_fill_and_remove_delay( r );
  764. return r->read_filled;
  765. }
  766. int resampler_get_sample(void *_r)
  767. {
  768. resampler * r = ( resampler * ) _r;
  769. if ( r->read_filled < 1 && r->phase_inc)
  770. resampler_fill_and_remove_delay( r );
  771. if ( r->read_filled < 1 )
  772. return 0;
  773. if ( r->quality == RESAMPLER_QUALITY_BLEP )
  774. return (int)(r->buffer_out[ r->read_pos ] + r->accumulator);
  775. else
  776. return (int)r->buffer_out[ r->read_pos ];
  777. }
  778. void resampler_remove_sample(void *_r)
  779. {
  780. resampler * r = ( resampler * ) _r;
  781. if ( r->read_filled > 0 )
  782. {
  783. if ( r->quality == RESAMPLER_QUALITY_BLEP )
  784. {
  785. r->accumulator += r->buffer_out[ r->read_pos ];
  786. r->buffer_out[ r->read_pos ] = 0;
  787. r->accumulator -= r->accumulator * (1.0 / 8192.0);
  788. if (fabs(r->accumulator) < 1e-20)
  789. r->accumulator = 0;
  790. }
  791. --r->read_filled;
  792. r->read_pos = ( r->read_pos + 1 ) % resampler_buffer_size;
  793. }
  794. }
  795. /* Get a 16-bit sample without overflow */
  796. short resampler_get_and_remove_sample(void *_r)
  797. {
  798. int sample = (int)round(resampler_get_sample(_r) / 256.0);
  799. resampler_remove_sample(_r);
  800. if (sample >= 32767)
  801. return 32767;
  802. else if (sample <= -32767)
  803. return -32767;
  804. else
  805. return (short)sample;
  806. }