41static constexpr std::array<float, 2464> coeffs = {
42 #include "ResampleCoeffs.ii"
47static constexpr int INDEX_INC = 128;
48static constexpr int COEFF_LEN = int(
std::size(coeffs));
49static constexpr int COEFF_HALF_LEN = COEFF_LEN - 1;
60 void getCoeffs(
double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen);
69 static Table calcTable(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen);
73 std::array<int16_t, HALF_TAB_LEN> permute;
78 std::vector<Element> cache;
81ResampleCoeffs::~ResampleCoeffs()
83 assert(cache.empty());
89 return resampleCoeffs;
93 double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen)
95 if (
auto it =
ranges::find(cache, ratio, &Element::ratio);
97 permute = it->permute;
98 table = it->table.data();
99 filterLen = it->filterLen;
106 elem.table = calcTable(ratio, elem.permute, elem.filterLen);
107 permute = elem.permute;
108 table = elem.table.data();
109 filterLen = elem.filterLen;
110 cache.push_back(std::move(elem));
117 if (it->count == 0) {
229static constexpr unsigned N = TAB_LEN;
230static constexpr unsigned N1 = N - 1;
231static constexpr unsigned N2 = N / 2;
233static constexpr unsigned mapIdx(
unsigned x)
236 return (
t < N2) ?
t : N1 -
t;
239static constexpr std::pair<unsigned, unsigned> next(
unsigned x,
unsigned step)
241 return {mapIdx(x + step), mapIdx(N1 - x + step)};
244static void calcPermute(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute)
246 double r2 = ratio * N;
247 double fract = r2 - floor(r2);
248 auto step = narrow_cast<unsigned>(floor(r2));
263 unsigned restart = incr ? 0 : N2 - 1;
264 unsigned curr = restart;
267 for (
auto i :
xrange(N2)) {
268 auto [nxt1, nxt2] = next(i, step);
269 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
272 for (
unsigned i = N2 - 1; int(i) >= 0; --i) {
273 auto [nxt1, nxt2] = next(i, step);
274 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
281 assert(permute[curr] == -1);
283 permute[curr] = narrow<int16_t>(cnt++);
285 auto [nxt1, nxt2] = next(curr, step);
286 if (permute[nxt1] == -1) {
289 }
else if (permute[nxt2] == -1) {
295 if (cnt == N2)
break;
298 while (permute[restart] != -1) {
301 assert(restart != N2);
303 assert(restart != 0);
311 std::array<int16_t, N2> testPerm;
313 assert(std::is_permutation(permute.begin(), permute.end(), testPerm.begin()));
319 double fraction = index.fractionAsDouble();
320 int indx = index.toInt();
321 return double(coeffs[indx]) +
322 fraction * (double(coeffs[indx + 1]) - double(coeffs[indx]));
325ResampleCoeffs::Table ResampleCoeffs::calcTable(
326 double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen)
328 calcPermute(ratio, permute);
330 double floatIncr = (ratio > 1.0) ? INDEX_INC / ratio : INDEX_INC;
331 double normFactor = floatIncr / INDEX_INC;
335 int min_idx = -maxFilterIndex.divAsInt(increment);
336 int max_idx = 1 + (maxFilterIndex - (increment -
FilterIndex(floatIncr))).divAsInt(increment);
337 int idx_cnt = max_idx - min_idx + 1;
338 filterLen = (idx_cnt + 3) & ~3;
339 min_idx -= (narrow<int>(filterLen) - idx_cnt) / 2;
340 Table table(HALF_TAB_LEN * filterLen);
341 ranges::fill(std::span{table.data(), HALF_TAB_LEN * filterLen}, 0);
343 for (
auto t :
xrange(HALF_TAB_LEN)) {
344 float* tab = &table[permute[
t] * filterLen];
345 double lastPos = (double(
t) + 0.5) / TAB_LEN;
349 int coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
350 filterIndex += increment * coeffCount;
351 int bufIndex = -coeffCount;
353 tab[bufIndex - min_idx] =
354 float(getCoeff(filterIndex) * normFactor);
355 filterIndex -= increment;
359 filterIndex = increment - startFilterIndex;
360 coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
361 filterIndex += increment * coeffCount;
362 bufIndex = 1 + coeffCount;
364 tab[bufIndex - min_idx] =
365 float(getCoeff(filterIndex) * normFactor);
366 filterIndex -= increment;
373static const std::array<int16_t, HALF_TAB_LEN> dummyPermute = {};
375template<
unsigned CHANNELS>
379 , hostClock(hostClock_)
380 , ratio(float(hostClock.getPeriod().toDouble() / getEmuClock().getPeriod().toDouble()))
381 , permute(dummyPermute)
386 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
389 size_t initialSize = 4000;
390 buffer.resize((initialSize + extra) * CHANNELS);
393template<
unsigned CHANNELS>
400template<
bool REVERSE>
401static inline void calcSseMono(
const float* buf_,
const float* tab_,
size_t len,
float* out)
403 assert((len % 4) == 0);
404 assert((uintptr_t(tab_) % 16) == 0);
406 auto x = narrow<ptrdiff_t>((len & ~7) *
sizeof(
float));
407 assert((x % 32) == 0);
408 const char* buf =
reinterpret_cast<const char*
>(buf_) + x;
409 const char* tab =
reinterpret_cast<const char*
>(tab_) + (REVERSE ? -x : x);
412 __m128 a0 = _mm_setzero_ps();
413 __m128 a1 = _mm_setzero_ps();
415 __m128 b0 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 0));
416 __m128 b1 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 16));
418 if constexpr (REVERSE) {
419 t0 = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - x - 16));
420 t1 = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - x - 32));
422 t0 = _mm_load_ps (
reinterpret_cast<const float*
>(tab + x + 0));
423 t1 = _mm_load_ps (
reinterpret_cast<const float*
>(tab + x + 16));
425 __m128 m0 = _mm_mul_ps(b0, t0);
426 __m128 m1 = _mm_mul_ps(b1, t1);
427 a0 = _mm_add_ps(a0, m0);
428 a1 = _mm_add_ps(a1, m1);
429 x += 2 *
sizeof(__m128);
432 __m128 b0 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf));
434 if constexpr (REVERSE) {
435 t0 = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - 16));
437 t0 = _mm_load_ps (
reinterpret_cast<const float*
>(tab));
439 __m128 m0 = _mm_mul_ps(b0, t0);
440 a0 = _mm_add_ps(a0, m0);
443 __m128 a = _mm_add_ps(a0, a1);
446 __m128
t = _mm_add_ps(a, _mm_movehl_ps(a, a));
447 __m128 s = _mm_add_ss(
t, _mm_shuffle_ps(
t,
t, 1));
449 _mm_store_ss(out, s);
452template<
int N>
static inline __m128 shuffle(__m128 x)
454 return _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(x), N));
456template<
bool REVERSE>
457static inline void calcSseStereo(
const float* buf_,
const float* tab_,
size_t len,
float* out)
459 assert((len % 4) == 0);
460 assert((uintptr_t(tab_) % 16) == 0);
462 auto x = narrow<ptrdiff_t>(2 * (len & ~7) *
sizeof(
float));
463 const char* buf =
reinterpret_cast<const char*
>(buf_) + x;
464 const char* tab =
reinterpret_cast<const char*
>(tab_);
467 __m128 a0 = _mm_setzero_ps();
468 __m128 a1 = _mm_setzero_ps();
469 __m128 a2 = _mm_setzero_ps();
470 __m128 a3 = _mm_setzero_ps();
472 __m128 b0 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 0));
473 __m128 b1 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 16));
474 __m128 b2 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 32));
475 __m128 b3 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + x + 48));
477 if constexpr (REVERSE) {
478 ta = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - 16));
479 tb = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - 32));
480 tab -= 2 *
sizeof(__m128);
482 ta = _mm_load_ps (
reinterpret_cast<const float*
>(tab + 0));
483 tb = _mm_load_ps (
reinterpret_cast<const float*
>(tab + 16));
484 tab += 2 *
sizeof(__m128);
486 __m128 t0 = shuffle<0x50>(ta);
487 __m128 t1 = shuffle<0xFA>(ta);
488 __m128 t2 = shuffle<0x50>(tb);
489 __m128 t3 = shuffle<0xFA>(tb);
490 __m128 m0 = _mm_mul_ps(b0, t0);
491 __m128 m1 = _mm_mul_ps(b1, t1);
492 __m128 m2 = _mm_mul_ps(b2, t2);
493 __m128 m3 = _mm_mul_ps(b3, t3);
494 a0 = _mm_add_ps(a0, m0);
495 a1 = _mm_add_ps(a1, m1);
496 a2 = _mm_add_ps(a2, m2);
497 a3 = _mm_add_ps(a3, m3);
498 x += 4 *
sizeof(__m128);
501 __m128 b0 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + 0));
502 __m128 b1 = _mm_loadu_ps(
reinterpret_cast<const float*
>(buf + 16));
504 if constexpr (REVERSE) {
505 ta = _mm_loadr_ps(
reinterpret_cast<const float*
>(tab - 16));
507 ta = _mm_load_ps (
reinterpret_cast<const float*
>(tab + 0));
509 __m128 t0 = shuffle<0x50>(ta);
510 __m128 t1 = shuffle<0xFA>(ta);
511 __m128 m0 = _mm_mul_ps(b0, t0);
512 __m128 m1 = _mm_mul_ps(b1, t1);
513 a0 = _mm_add_ps(a0, m0);
514 a1 = _mm_add_ps(a1, m1);
517 __m128 a01 = _mm_add_ps(a0, a1);
518 __m128 a23 = _mm_add_ps(a2, a3);
519 __m128 a = _mm_add_ps(a01, a23);
521 __m128 s = _mm_add_ps(a, _mm_movehl_ps(a, a));
522 _mm_store_ss(&out[0], s);
523 _mm_store_ss(&out[1], shuffle<0x55>(s));
528template<
unsigned CHANNELS>
529void ResampleHQ<CHANNELS>::calcOutput(
530 float pos,
float* __restrict output)
532 assert((filterLen & 3) == 0);
534 int bufIdx = int(pos) + bufStart;
535 assert((bufIdx + filterLen) <= bufEnd);
537 const float* buf = &buffer[bufIdx];
539 auto t = size_t(lrintf(pos * TAB_LEN)) % TAB_LEN;
540 if (!(
t & HALF_TAB_LEN)) {
543 const float* tab = &table[
t * filterLen];
546 if constexpr (CHANNELS == 1) {
547 calcSseMono <false>(buf, tab, filterLen, output);
549 calcSseStereo<false>(buf, tab, filterLen, output);
555 for (
auto ch :
xrange(CHANNELS)) {
560 for (
unsigned i = 0; i < filterLen; i += 4) {
561 r0 += tab[i + 0] * buf[CHANNELS * (i + 0)];
562 r1 += tab[i + 1] * buf[CHANNELS * (i + 1)];
563 r2 += tab[i + 2] * buf[CHANNELS * (i + 2)];
564 r3 += tab[i + 3] * buf[CHANNELS * (i + 3)];
566 output[ch] = r0 + r1 + r2 + r3;
571 t = permute[TAB_LEN - 1 -
t];
572 const float* tab = &table[(
t + 1) * filterLen];
575 if constexpr (CHANNELS == 1) {
576 calcSseMono <true>(buf, tab, filterLen, output);
578 calcSseStereo<true>(buf, tab, filterLen, output);
584 for (
auto ch :
xrange(CHANNELS)) {
589 for (
int i = 0; i < int(filterLen); i += 4) {
590 r0 += tab[-i - 1] * buf[CHANNELS * (i + 0)];
591 r1 += tab[-i - 2] * buf[CHANNELS * (i + 1)];
592 r2 += tab[-i - 3] * buf[CHANNELS * (i + 2)];
593 r3 += tab[-i - 4] * buf[CHANNELS * (i + 3)];
595 output[ch] = r0 + r1 + r2 + r3;
601template<
unsigned CHANNELS>
602void ResampleHQ<CHANNELS>::prepareData(
unsigned emuNum)
605 unsigned free = unsigned(buffer.size() / CHANNELS) - bufEnd;
609 unsigned available = bufEnd - bufStart;
610 memmove(&buffer[0], &buffer[bufStart *
size_t(CHANNELS)],
611 available *
size_t(CHANNELS) *
sizeof(
float));
615 free = unsigned(buffer.size() / CHANNELS) - bufEnd;
616 auto missing = narrow_cast<int>(emuNum - free);
617 if (missing > 0) [[unlikely]] {
625 buffer.resize(buffer.size() + missing *
size_t(CHANNELS));
629 auto tmpBuf = tmpBufExtra.subspan(0, emuNum * CHANNELS);
630 if (input.generateInput(tmpBufExtra.data(), emuNum)) {
632 subspan(buffer, bufEnd * CHANNELS));
634 nonzeroSamples = bufEnd - bufStart;
640 assert(bufStart <= bufEnd);
641 assert(bufEnd <= (buffer.size() / CHANNELS));
644template<
unsigned CHANNELS>
646 float* __restrict dataOut,
size_t hostNum, EmuTime::param time)
648 auto& emuClk = getEmuClock();
649 unsigned emuNum = emuClk.getTicksTill(time);
654 bool notMuted = nonzeroSamples > 0;
657 EmuTime host1 = hostClock.getFastAdd(1);
658 assert(host1 > emuClk.getTime());
659 float pos = narrow_cast<float>(emuClk.getTicksTillDouble(host1));
660 assert(pos <= (ratio + 2));
661 for (
auto i :
xrange(hostNum)) {
662 calcOutput(pos, &dataOut[i * CHANNELS]);
668 nonzeroSamples = std::max<int>(0, nonzeroSamples - emuNum);
670 assert(bufStart <= bufEnd);
671 unsigned available = bufEnd - bufStart;
672 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
673 assert(available == extra); (void)available; (void)extra;
Represents a clock with a variable frequency.
static ResampleCoeffs & instance()
ResampleCoeffs(const ResampleCoeffs &)=delete
void getCoeffs(double ratio, std::span< const int16_t, HALF_TAB_LEN > &permute, float *&table, unsigned &filterLen)
ResampleCoeffs & operator=(const ResampleCoeffs &)=delete
void releaseCoeffs(double ratio)
ResampleHQ(ResampledSoundDevice &input, const DynamicClock &hostClock)
bool generateOutputImpl(float *dataOut, size_t num, EmuTime::param time) override
ALWAYS_INLINE unsigned count(const uint8_t *pIn, const uint8_t *pMatch, const uint8_t *pInLimit)
This file implemented 3 utility functions:
FixedPoint< 16 > FilterIndex
constexpr void fill(ForwardRange &&range, const T &value)
auto copy(InputRange &&range, OutputIter out)
constexpr void iota(ForwardIt first, ForwardIt last, T value)
auto find(InputRange &&range, const T &value)
size_t size(std::string_view utf8)
constexpr auto subspan(Range &&range, size_t offset, size_t count=std::dynamic_extent)
void move_pop_back(VECTOR &v, typename VECTOR::iterator it)
Erase the pointed to element from the given vector.
auto rfind_unguarded(RANGE &range, const VAL &val, Proj proj={})
Similar to the find(_if)_unguarded functions above, but searches from the back to front.
#define VLA_SSE_ALIGNED(TYPE, NAME, LENGTH)
constexpr auto xrange(T e)
constexpr auto end(const zstring_view &x)