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;
66 void getCoeffs(
double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen);
76 static Table calcTable(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen);
85 std::vector<Element> cache;
88ResampleCoeffs::~ResampleCoeffs()
90 assert(cache.empty());
96 return resampleCoeffs;
100 double ratio, std::span<const int16_t, HALF_TAB_LEN>& permute,
float*& table,
unsigned& filterLen)
102 if (
auto it =
ranges::find(cache, ratio, &Element::ratio);
104 permute = std::span<int16_t, HALF_TAB_LEN>{it->permute.data(), HALF_TAB_LEN};
105 table = it->table.data();
106 filterLen = it->filterLen;
114 auto perm = std::span<int16_t, HALF_TAB_LEN>{elem.permute.data(), HALF_TAB_LEN};
115 elem.table = calcTable(ratio, perm, elem.filterLen);
117 table = elem.table.data();
118 filterLen = elem.filterLen;
119 cache.push_back(std::move(elem));
126 if (it->count == 0) {
238static constexpr unsigned N = TAB_LEN;
239static constexpr unsigned N1 = N - 1;
240static constexpr unsigned N2 = N / 2;
242static constexpr unsigned mapIdx(
unsigned x)
245 return (
t < N2) ?
t : N1 -
t;
248static constexpr std::pair<unsigned, unsigned> next(
unsigned x,
unsigned step)
250 return {mapIdx(x + step), mapIdx(N1 - x + step)};
253static void calcPermute(
double ratio, std::span<int16_t, HALF_TAB_LEN> permute)
255 double r2 = ratio * N;
256 double fract = r2 - floor(r2);
257 auto step = narrow_cast<unsigned>(floor(r2));
272 unsigned restart = incr ? 0 : N2 - 1;
273 unsigned curr = restart;
276 for (
auto i :
xrange(N2)) {
277 auto [nxt1, nxt2] = next(i, step);
278 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
281 for (
unsigned i = N2 - 1; int(i) >= 0; --i) {
282 auto [nxt1, nxt2] = next(i, step);
283 if ((nxt1 == i) || (nxt2 == i)) { curr = i;
break; }
290 assert(permute[curr] == -1);
292 permute[curr] = narrow<int16_t>(cnt++);
294 auto [nxt1, nxt2] = next(curr, step);
295 if (permute[nxt1] == -1) {
298 }
else if (permute[nxt2] == -1) {
304 if (cnt == N2)
break;
307 while (permute[restart] != -1) {
310 assert(restart != N2);
312 assert(restart != 0);
320 std::array<int16_t, N2> testPerm;
322 assert(std::is_permutation(permute.begin(), permute.end(), testPerm.begin()));
328 double fraction = index.fractionAsDouble();
329 int indx = index.toInt();
330 return double(coeffs[indx]) +
331 fraction * (double(coeffs[indx + 1]) - double(coeffs[indx]));
334ResampleCoeffs::Table ResampleCoeffs::calcTable(
335 double ratio, std::span<int16_t, HALF_TAB_LEN> permute,
unsigned& filterLen)
337 calcPermute(ratio, permute);
339 double floatIncr = (ratio > 1.0) ? INDEX_INC / ratio : INDEX_INC;
340 double normFactor = floatIncr / INDEX_INC;
344 int min_idx = -maxFilterIndex.divAsInt(increment);
345 int max_idx = 1 + (maxFilterIndex - (increment -
FilterIndex(floatIncr))).divAsInt(increment);
346 int idx_cnt = max_idx - min_idx + 1;
347 filterLen = (idx_cnt + 3) & ~3;
348 min_idx -= (narrow<int>(filterLen) - idx_cnt) / 2;
349 Table table(HALF_TAB_LEN * filterLen);
350 ranges::fill(std::span{table.data(), HALF_TAB_LEN * filterLen}, 0);
352 for (
auto t :
xrange(HALF_TAB_LEN)) {
353 float* tab = &table[permute[
t] * filterLen];
354 double lastPos = (double(
t) + 0.5) / TAB_LEN;
358 int coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
359 filterIndex += increment * coeffCount;
360 int bufIndex = -coeffCount;
362 tab[bufIndex - min_idx] =
363 float(getCoeff(filterIndex) * normFactor);
364 filterIndex -= increment;
368 filterIndex = increment - startFilterIndex;
369 coeffCount = (maxFilterIndex - filterIndex).divAsInt(increment);
370 filterIndex += increment * coeffCount;
371 bufIndex = 1 + coeffCount;
373 tab[bufIndex - min_idx] =
374 float(getCoeff(filterIndex) * normFactor);
375 filterIndex -= increment;
382static const std::array<int16_t, HALF_TAB_LEN> dummyPermute = {};
384template<
unsigned CHANNELS>
388 , hostClock(hostClock_)
389 , ratio(float(hostClock.getPeriod().toDouble() / getEmuClock().getPeriod().toDouble()))
390 , permute(dummyPermute)
395 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
398 size_t initialSize = 4000;
399 buffer.resize((initialSize + extra) * CHANNELS);
402template<
unsigned CHANNELS>
409template<
bool REVERSE>
410static inline void calcSseMono(
const float* buf_,
const float* tab_,
size_t len,
float* out)
412 assert((len % 4) == 0);
413 assert((uintptr_t(tab_) % 16) == 0);
415 auto x = narrow<ptrdiff_t>((len & ~7) *
sizeof(
float));
416 assert((x % 32) == 0);
417 const char* buf = std::bit_cast<const char*>(buf_) + x;
418 const char* tab = std::bit_cast<const char*>(tab_) + (REVERSE ? -x : x);
421 __m128 a0 = _mm_setzero_ps();
422 __m128 a1 = _mm_setzero_ps();
424 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 0));
425 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 16));
427 if constexpr (REVERSE) {
428 t0 = _mm_loadr_ps(std::bit_cast<const float*>(tab - x - 16));
429 t1 = _mm_loadr_ps(std::bit_cast<const float*>(tab - x - 32));
431 t0 = _mm_load_ps (std::bit_cast<const float*>(tab + x + 0));
432 t1 = _mm_load_ps (std::bit_cast<const float*>(tab + x + 16));
434 __m128 m0 = _mm_mul_ps(b0, t0);
435 __m128 m1 = _mm_mul_ps(b1, t1);
436 a0 = _mm_add_ps(a0, m0);
437 a1 = _mm_add_ps(a1, m1);
438 x += 2 *
sizeof(__m128);
441 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf));
443 if constexpr (REVERSE) {
444 t0 = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
446 t0 = _mm_load_ps (std::bit_cast<const float*>(tab));
448 __m128 m0 = _mm_mul_ps(b0, t0);
449 a0 = _mm_add_ps(a0, m0);
452 __m128 a = _mm_add_ps(a0, a1);
455 __m128
t = _mm_add_ps(a, _mm_movehl_ps(a, a));
456 __m128 s = _mm_add_ss(
t, _mm_shuffle_ps(
t,
t, 1));
458 _mm_store_ss(out, s);
461template<
int N>
static inline __m128 shuffle(__m128 x)
463 return _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(x), N));
465template<
bool REVERSE>
466static inline void calcSseStereo(
const float* buf_,
const float* tab_,
size_t len,
float* out)
468 assert((len % 4) == 0);
469 assert((uintptr_t(tab_) % 16) == 0);
471 auto x = narrow<ptrdiff_t>(2 * (len & ~7) *
sizeof(
float));
472 const auto* buf = std::bit_cast<const char*>(buf_) + x;
473 const auto* tab = std::bit_cast<const char*>(tab_);
476 __m128 a0 = _mm_setzero_ps();
477 __m128 a1 = _mm_setzero_ps();
478 __m128 a2 = _mm_setzero_ps();
479 __m128 a3 = _mm_setzero_ps();
481 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 0));
482 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 16));
483 __m128 b2 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 32));
484 __m128 b3 = _mm_loadu_ps(std::bit_cast<const float*>(buf + x + 48));
486 if constexpr (REVERSE) {
487 ta = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
488 tb = _mm_loadr_ps(std::bit_cast<const float*>(tab - 32));
489 tab -= 2 *
sizeof(__m128);
491 ta = _mm_load_ps (std::bit_cast<const float*>(tab + 0));
492 tb = _mm_load_ps (std::bit_cast<const float*>(tab + 16));
493 tab += 2 *
sizeof(__m128);
495 __m128 t0 = shuffle<0x50>(ta);
496 __m128 t1 = shuffle<0xFA>(ta);
497 __m128 t2 = shuffle<0x50>(tb);
498 __m128 t3 = shuffle<0xFA>(tb);
499 __m128 m0 = _mm_mul_ps(b0, t0);
500 __m128 m1 = _mm_mul_ps(b1, t1);
501 __m128 m2 = _mm_mul_ps(b2, t2);
502 __m128 m3 = _mm_mul_ps(b3, t3);
503 a0 = _mm_add_ps(a0, m0);
504 a1 = _mm_add_ps(a1, m1);
505 a2 = _mm_add_ps(a2, m2);
506 a3 = _mm_add_ps(a3, m3);
507 x += 4 *
sizeof(__m128);
510 __m128 b0 = _mm_loadu_ps(std::bit_cast<const float*>(buf + 0));
511 __m128 b1 = _mm_loadu_ps(std::bit_cast<const float*>(buf + 16));
513 if constexpr (REVERSE) {
514 ta = _mm_loadr_ps(std::bit_cast<const float*>(tab - 16));
516 ta = _mm_load_ps (std::bit_cast<const float*>(tab + 0));
518 __m128 t0 = shuffle<0x50>(ta);
519 __m128 t1 = shuffle<0xFA>(ta);
520 __m128 m0 = _mm_mul_ps(b0, t0);
521 __m128 m1 = _mm_mul_ps(b1, t1);
522 a0 = _mm_add_ps(a0, m0);
523 a1 = _mm_add_ps(a1, m1);
526 __m128 a01 = _mm_add_ps(a0, a1);
527 __m128 a23 = _mm_add_ps(a2, a3);
528 __m128 a = _mm_add_ps(a01, a23);
530 __m128 s = _mm_add_ps(a, _mm_movehl_ps(a, a));
531 _mm_store_ss(&out[0], s);
532 _mm_store_ss(&out[1], shuffle<0x55>(s));
537template<
unsigned CHANNELS>
538void ResampleHQ<CHANNELS>::calcOutput(
539 float pos,
float* __restrict output)
541 assert((filterLen & 3) == 0);
543 int bufIdx = int(pos) + bufStart;
544 assert((bufIdx + filterLen) <= bufEnd);
546 const float* buf = &buffer[bufIdx];
548 auto t = size_t(lrintf(pos * TAB_LEN)) % TAB_LEN;
549 if (!(
t & HALF_TAB_LEN)) {
552 const float* tab = &table[
t * filterLen];
555 if constexpr (CHANNELS == 1) {
556 calcSseMono <false>(buf, tab, filterLen, output);
558 calcSseStereo<false>(buf, tab, filterLen, output);
564 for (
auto ch :
xrange(CHANNELS)) {
569 for (
unsigned i = 0; i < filterLen; i += 4) {
570 r0 += tab[i + 0] * buf[CHANNELS * (i + 0)];
571 r1 += tab[i + 1] * buf[CHANNELS * (i + 1)];
572 r2 += tab[i + 2] * buf[CHANNELS * (i + 2)];
573 r3 += tab[i + 3] * buf[CHANNELS * (i + 3)];
575 output[ch] = r0 + r1 + r2 + r3;
580 t = permute[TAB_LEN - 1 -
t];
581 const float* tab = &table[(
t + 1) * filterLen];
584 if constexpr (CHANNELS == 1) {
585 calcSseMono <true>(buf, tab, filterLen, output);
587 calcSseStereo<true>(buf, tab, filterLen, output);
593 for (
auto ch :
xrange(CHANNELS)) {
598 for (
int i = 0; i < int(filterLen); i += 4) {
599 r0 += tab[-i - 1] * buf[CHANNELS * (i + 0)];
600 r1 += tab[-i - 2] * buf[CHANNELS * (i + 1)];
601 r2 += tab[-i - 3] * buf[CHANNELS * (i + 2)];
602 r3 += tab[-i - 4] * buf[CHANNELS * (i + 3)];
604 output[ch] = r0 + r1 + r2 + r3;
610template<
unsigned CHANNELS>
611void ResampleHQ<CHANNELS>::prepareData(
unsigned emuNum)
614 unsigned free = unsigned(buffer.size() / CHANNELS) - bufEnd;
618 unsigned available = bufEnd - bufStart;
619 memmove(&buffer[0], &buffer[bufStart *
size_t(CHANNELS)],
620 available *
size_t(CHANNELS) *
sizeof(
float));
624 free = unsigned(buffer.size() / CHANNELS) - bufEnd;
625 auto missing = narrow_cast<int>(emuNum - free);
626 if (missing > 0) [[unlikely]] {
634 buffer.resize(buffer.size() + missing *
size_t(CHANNELS));
638 auto tmpBuf = tmpBufExtra.subspan(0, emuNum * CHANNELS);
639 if (input.generateInput(tmpBufExtra.data(), emuNum)) {
641 subspan(buffer, bufEnd * CHANNELS));
643 nonzeroSamples = bufEnd - bufStart;
649 assert(bufStart <= bufEnd);
650 assert(bufEnd <= (buffer.size() / CHANNELS));
653template<
unsigned CHANNELS>
655 float* __restrict dataOut,
size_t hostNum, EmuTime::param time)
657 auto& emuClk = getEmuClock();
658 unsigned emuNum = emuClk.getTicksTill(time);
663 bool notMuted = nonzeroSamples > 0;
666 EmuTime host1 = hostClock.getFastAdd(1);
667 assert(host1 > emuClk.getTime());
668 float pos = narrow_cast<float>(emuClk.getTicksTillDouble(host1));
669 assert(pos <= (ratio + 2));
670 for (
auto i :
xrange(hostNum)) {
671 calcOutput(pos, &dataOut[i * CHANNELS]);
677 nonzeroSamples = std::max<int>(0, nonzeroSamples - emuNum);
679 assert(bufStart <= bufEnd);
680 unsigned available = bufEnd - bufStart;
681 unsigned extra = filterLen + 1 + narrow_cast<int>(ratio) + 1;
682 assert(available == extra); (void)available; (void)extra;
Represents a clock with a variable frequency.
static ResampleCoeffs & instance()
ResampleCoeffs(const ResampleCoeffs &)=delete
ResampleCoeffs & operator=(ResampleCoeffs &&)=delete
void getCoeffs(double ratio, std::span< const int16_t, HALF_TAB_LEN > &permute, float *&table, unsigned &filterLen)
ResampleCoeffs & operator=(const ResampleCoeffs &)=delete
ResampleCoeffs(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)
constexpr void iota(ForwardIt first, ForwardIt last, T value)
auto find(InputRange &&range, const T &value)
constexpr auto copy(InputRange &&range, OutputIter out)
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)