resampler.c 24 KB

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