resampler.c 23 KB

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