45static constexpr std::array<float, 2464> coeffs = {
46 #include "ResampleCoeffs.ii"
51static constexpr int INDEX_INC = 128;
52static constexpr int COEFF_LEN = int(std::size(coeffs));
53static constexpr int COEFF_HALF_LEN = COEFF_LEN - 1;
64 void getCoeffs(
double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen);
74 static Table calcTable(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen);
83 std::vector<Element> cache;
86ResampleCoeffs::~ResampleCoeffs()
88 assert(cache.empty());
94 return resampleCoeffs;
98 double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen)
100 if (
auto it =
ranges::find(cache, ratio, &Element::ratio);
102 permute = std::span<int16_t, HALF_TAB_LEN>{it->permute.data(), HALF_TAB_LEN};
103 table = it->table.data();
104 filterLen = it->filterLen;
112 auto perm = std::span<int16_t, HALF_TAB_LEN>{elem.permute.data(), HALF_TAB_LEN};
113 elem.table = calcTable(ratio, perm, elem.filterLen);
115 table = elem.table.data();
116 filterLen = elem.filterLen;
117 cache.push_back(std::move(elem));
124 if (it->count == 0) {
236static constexpr unsigned N = TAB_LEN;
237static constexpr unsigned N1 = N - 1;
238static constexpr unsigned N2 = N / 2;
240static constexpr unsigned mapIdx(
unsigned x)
243 return (
t < N2) ?
t : N1 -
t;
246static constexpr std::pair<unsigned, unsigned> next(
unsigned x,
unsigned step)
248 return {mapIdx(x + step), mapIdx(N1 - x + step)};
251static void calcPermute(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute)
253 double r2 = ratio * N;
254 double fract = r2 - floor(r2);
255 auto step = narrow_cast<unsigned>(floor(r2));
270 unsigned restart = incr ? 0 : N2 - 1;
271 unsigned curr = restart;
274 for (
auto i :
xrange(N2)) {
275 auto [nxt1, nxt2] = next(i, step);
276 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
279 for (
unsigned i = N2 - 1; int(i) >= 0; --i) {
280 auto [nxt1, nxt2] = next(i, step);
281 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
288 assert(permute[curr] == -1);
290 permute[curr] = narrow<int16_t>(cnt++);
292 auto [nxt1, nxt2] = next(curr, step);
293 if (permute[nxt1] == -1) {
296 }
else if (permute[nxt2] == -1) {
302 if (cnt == N2)
break;
305 while (permute[restart] != -1) {
308 assert(restart != N2);
310 assert(restart != 0);
318 std::array<int16_t, N2> testPerm;
320 assert(std::is_permutation(permute.begin(), permute.end(), testPerm.begin()));
326 double fraction = index.fractionAsDouble();
327 int indx = index.toInt();
328 return double(coeffs[indx]) +
329 fraction * (double(coeffs[indx + 1]) - double(coeffs[indx]));
332ResampleCoeffs::Table ResampleCoeffs::calcTable(
333 double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen)
335 calcPermute(ratio, permute);
337 double floatIncr = (ratio > 1.0) ? INDEX_INC / ratio : INDEX_INC;
338 double normFactor = floatIncr / INDEX_INC;
342 int min_idx = -maxFilterIndex.divAsInt(increment);
343 int max_idx = 1 + (maxFilterIndex - (increment -
FilterIndex(floatIncr))).divAsInt(increment);
344 int idx_cnt = max_idx - min_idx + 1;
345 filterLen = (idx_cnt + 3) & ~3;
346 min_idx -= (narrow<int>(filterLen) - idx_cnt) / 2;
347 Table table(HALF_TAB_LEN * filterLen);
348 ranges::fill(std::span{table.data(), HALF_TAB_LEN * filterLen}, 0);
350 for (
auto t :
xrange(HALF_TAB_LEN)) {
351 float* tab = &table[permute[
t] * filterLen];
352 double lastPos = (double(
t) + 0.5) / TAB_LEN;
356 int coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
357 filterIndex += increment * coeffCount;
358 int bufIndex = -coeffCount;
360 tab[bufIndex - min_idx] =
361 float(getCoeff(filterIndex) * normFactor);
362 filterIndex -= increment;
366 filterIndex = increment - startFilterIndex;
367 coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
368 filterIndex += increment * coeffCount;
369 bufIndex = 1 + coeffCount;
371 tab[bufIndex - min_idx] =
372 float(getCoeff(filterIndex) * normFactor);
373 filterIndex -= increment;
380static const std::array<int16_t, HALF_TAB_LEN> dummyPermute = {};
382template<
unsigned CHANNELS>
386 , hostClock(hostClock_)
387 , ratio(float(hostClock.getPeriod().toDouble() / getEmuClock().getPeriod().toDouble()))
388 , permute(dummyPermute)
393 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
396 size_t initialSize = 4000;
397 buffer.resize((initialSize + extra) * CHANNELS);
400template<
unsigned CHANNELS>
407template<
bool REVERSE>
408static inline void calcSseMono(
const float* buf_,
const float* tab_,
size_t len,
float* out)
410 assert((len % 4) == 0);
411 assert((uintptr_t(tab_) % 16) == 0);
413 auto x = narrow<ptrdiff_t>((len & ~7) *
sizeof(
float));
414 assert((x % 32) == 0);
415 const char* buf = std::bit_cast<const char*>(buf_) + x;
416 const char* tab = std::bit_cast<const char*>(tab_) + (REVERSE ? -x : x);
419 __m128 a0 = _mm_setzero_ps();
420 __m128 a1 = _mm_setzero_ps();
422 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 0));
423 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 16));
425 if constexpr (REVERSE) {
426 t0 = _mm_loadr_ps(std::bit_cast<const float*>(tab - x - 16));
427 t1 = _mm_loadr_ps(std::bit_cast<const float*>(tab - x - 32));
429 t0 = _mm_load_ps (std::bit_cast<const float*>(tab + x + 0));
430 t1 = _mm_load_ps (std::bit_cast<const float*>(tab + x + 16));
432 __m128 m0 = _mm_mul_ps(b0, t0);
433 __m128 m1 = _mm_mul_ps(b1, t1);
434 a0 = _mm_add_ps(a0, m0);
435 a1 = _mm_add_ps(a1, m1);
436 x += 2 *
sizeof(__m128);
439 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf));
441 if constexpr (REVERSE) {
442 t0 = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
444 t0 = _mm_load_ps (std::bit_cast<const float*>(tab));
446 __m128 m0 = _mm_mul_ps(b0, t0);
447 a0 = _mm_add_ps(a0, m0);
450 __m128 a = _mm_add_ps(a0, a1);
453 __m128
t = _mm_add_ps(a, _mm_movehl_ps(a, a));
454 __m128 s = _mm_add_ss(
t, _mm_shuffle_ps(
t,
t, 1));
456 _mm_store_ss(out, s);
459template<
int N>
static inline __m128 shuffle(__m128 x)
461 return _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(x), N));
463template<
bool REVERSE>
464static inline void calcSseStereo(
const float* buf_,
const float* tab_,
size_t len,
float* out)
466 assert((len % 4) == 0);
467 assert((uintptr_t(tab_) % 16) == 0);
469 auto x = narrow<ptrdiff_t>(2 * (len & ~7) *
sizeof(
float));
470 const auto* buf = std::bit_cast<const char*>(buf_) + x;
471 const auto* tab = std::bit_cast<const char*>(tab_);
474 __m128 a0 = _mm_setzero_ps();
475 __m128 a1 = _mm_setzero_ps();
476 __m128 a2 = _mm_setzero_ps();
477 __m128 a3 = _mm_setzero_ps();
479 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 0));
480 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 16));
481 __m128 b2 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 32));
482 __m128 b3 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 48));
484 if constexpr (REVERSE) {
485 ta = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
486 tb = _mm_loadr_ps(std::bit_cast<const float*>(tab - 32));
487 tab -= 2 *
sizeof(__m128);
489 ta = _mm_load_ps (std::bit_cast<const float*>(tab + 0));
490 tb = _mm_load_ps (std::bit_cast<const float*>(tab + 16));
491 tab += 2 *
sizeof(__m128);
493 __m128 t0 = shuffle<0x50>(ta);
494 __m128 t1 = shuffle<0xFA>(ta);
495 __m128 t2 = shuffle<0x50>(tb);
496 __m128 t3 = shuffle<0xFA>(tb);
497 __m128 m0 = _mm_mul_ps(b0, t0);
498 __m128 m1 = _mm_mul_ps(b1, t1);
499 __m128 m2 = _mm_mul_ps(b2, t2);
500 __m128 m3 = _mm_mul_ps(b3, t3);
501 a0 = _mm_add_ps(a0, m0);
502 a1 = _mm_add_ps(a1, m1);
503 a2 = _mm_add_ps(a2, m2);
504 a3 = _mm_add_ps(a3, m3);
505 x += 4 *
sizeof(__m128);
508 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + 0));
509 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + 16));
511 if constexpr (REVERSE) {
512 ta = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
514 ta = _mm_load_ps (std::bit_cast<const float*>(tab + 0));
516 __m128 t0 = shuffle<0x50>(ta);
517 __m128 t1 = shuffle<0xFA>(ta);
518 __m128 m0 = _mm_mul_ps(b0, t0);
519 __m128 m1 = _mm_mul_ps(b1, t1);
520 a0 = _mm_add_ps(a0, m0);
521 a1 = _mm_add_ps(a1, m1);
524 __m128 a01 = _mm_add_ps(a0, a1);
525 __m128 a23 = _mm_add_ps(a2, a3);
526 __m128 a = _mm_add_ps(a01, a23);
528 __m128 s = _mm_add_ps(a, _mm_movehl_ps(a, a));
529 _mm_store_ss(&out[0], s);
530 _mm_store_ss(&out[1], shuffle<0x55>(s));
535template<
unsigned CHANNELS>
536void ResampleHQ<CHANNELS>::calcOutput(
537 float pos,
float* __restrict output)
539 assert((filterLen & 3) == 0);
541 int bufIdx = int(pos) + bufStart;
542 assert((bufIdx + filterLen) <= bufEnd);
544 const float* buf = &buffer[bufIdx];
546 auto t = size_t(lrintf(pos * TAB_LEN)) % TAB_LEN;
547 if (!(
t & HALF_TAB_LEN)) {
550 const float* tab = &table[
t * filterLen];
553 if constexpr (CHANNELS == 1) {
554 calcSseMono <false>(buf, tab, filterLen, output);
556 calcSseStereo<false>(buf, tab, filterLen, output);
562 for (
auto ch :
xrange(CHANNELS)) {
567 for (
unsigned i = 0; i < filterLen; i += 4) {
568 r0 += tab[i + 0] * buf[CHANNELS * (i + 0)];
569 r1 += tab[i + 1] * buf[CHANNELS * (i + 1)];
570 r2 += tab[i + 2] * buf[CHANNELS * (i + 2)];
571 r3 += tab[i + 3] * buf[CHANNELS * (i + 3)];
573 output[ch] = r0 + r1 + r2 + r3;
578 t = permute[TAB_LEN - 1 -
t];
579 const float* tab = &table[(
t + 1) * filterLen];
582 if constexpr (CHANNELS == 1) {
583 calcSseMono <true>(buf, tab, filterLen, output);
585 calcSseStereo<true>(buf, tab, filterLen, output);
591 for (
auto ch :
xrange(CHANNELS)) {
596 for (
int i = 0; i < int(filterLen); i += 4) {
597 r0 += tab[-i - 1] * buf[CHANNELS * (i + 0)];
598 r1 += tab[-i - 2] * buf[CHANNELS * (i + 1)];
599 r2 += tab[-i - 3] * buf[CHANNELS * (i + 2)];
600 r3 += tab[-i - 4] * buf[CHANNELS * (i + 3)];
602 output[ch] = r0 + r1 + r2 + r3;
608template<
unsigned CHANNELS>
609void ResampleHQ<CHANNELS>::prepareData(
unsigned emuNum)
612 unsigned free = unsigned(buffer.size() / CHANNELS) - bufEnd;
616 unsigned available = bufEnd - bufStart;
617 memmove(&buffer[0], &buffer[bufStart *
size_t(CHANNELS)],
618 available *
size_t(CHANNELS) *
sizeof(
float));
622 free = unsigned(buffer.size() / CHANNELS) - bufEnd;
623 auto missing = narrow_cast<int>(emuNum - free);
624 if (missing > 0) [[unlikely]] {
632 buffer.resize(buffer.size() + missing *
size_t(CHANNELS));
636 auto tmpBuf = tmpBufExtra.subspan(0, emuNum * CHANNELS);
637 if (input.generateInput(tmpBufExtra.data(), emuNum)) {
639 subspan(buffer, bufEnd * CHANNELS));
641 nonzeroSamples = bufEnd - bufStart;
647 assert(bufStart <= bufEnd);
648 assert(bufEnd <= (buffer.size() / CHANNELS));
651template<
unsigned CHANNELS>
653 float* __restrict dataOut,
size_t hostNum, EmuTime::param time)
655 auto& emuClk = getEmuClock();
656 unsigned emuNum = emuClk.getTicksTill(time);
661 bool notMuted = nonzeroSamples > 0;
664 EmuTime host1 = hostClock.getFastAdd(1);
665 assert(host1 > emuClk.getTime());
666 float pos = narrow_cast<float>(emuClk.getTicksTillDouble(host1));
667 assert(pos <= (ratio + 2));
668 for (
auto i :
xrange(hostNum)) {
669 calcOutput(pos, &dataOut[i * CHANNELS]);
675 nonzeroSamples = std::max<int>(0, nonzeroSamples - emuNum);
677 assert(bufStart <= bufEnd);
678 unsigned available = bufEnd - bufStart;
679 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
680 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
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)
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)