1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
|
#pragma once
#include <array>
#include <algorithm>
#include <cmath>
#include <type_traits>
#include <limits>
#include "resample.h"
#include "filter.h"
namespace {
template<typename result_type, typename type>
constexpr result_type dither_truncate(type value, unsigned dither)
{
using limits = std::numeric_limits<result_type>;
using dither_limits = std::numeric_limits<decltype(dither)>;
if constexpr(std::is_floating_point_v<type>)
{
value += dither * type{1} / type{1ul << dither_limits::digits};
value = std::floor(value);
}
else
{
static_assert(std::is_integral_v<type>, "Unsupported acc_type");
value += dither >> (dither_limits::digits - limits::digits); // 17 for short
value >>= limits::digits; // 15 for short
}
return std::clamp<type>(value, limits::min(), limits::max());
}
}
template<typename sample_type_in, typename sample_type_out, typename filter_type, typename acc_type, size_t channels = 1, size_t INTERP_BIT = 14>
void lfr_resample(lfr_fixed_t& pos, const lfr_fixed_t inv_ratio, unsigned& dither, void* out, std::size_t outlen, const void* in, std::size_t inlen, const lfr_filter& filter)
{
static_assert(channels > 0, "0 channels do not make any sense");
auto inp = reinterpret_cast<const sample_type_in*>(in);
auto outp = reinterpret_cast<sample_type_out*>(out);
auto fd = reinterpret_cast<const filter_type*>(filter.data.data());
const auto flen = filter.nsamp;
const auto log2nfilt = filter.log2nfilt;
auto x = pos;
std::array<unsigned, channels> ds{dither};
for(std::size_t i = 1; i < channels; ++i)
{
ds[i] = LCG_A * ds[i - 1] + LCG_C;
}
for(std::size_t i = 0; i < outlen; ++i)
{
/* fn: filter number
ff0: filter factor for filter fn
ff1: filter factor for filter fn+1 */
const auto fn = (static_cast<unsigned>(x >> 1) >> (31 - log2nfilt)) & ((1u << log2nfilt) - 1);
int ff1 = (static_cast<unsigned>(x >> (32 - log2nfilt - INTERP_BIT))) & ((1u << INTERP_BIT) - 1);
int ff0 = (1u << INTERP_BIT) - ff1;
/* off: offset in input corresponding to first sample in filter */
auto off = static_cast<int>(x >> 32);
/* fidx0, fidx1: start, end indexes in FIR data */
int fidx0 = std::max(-off, 0);
int fidx1 = std::min<int>(inlen - off, flen);
/* acc: FIR accumulator */
std::array<acc_type, channels> acc{0};
for(int j = fidx0; j < fidx1; ++j)
{
acc_type f = (fd[(fn + 0) * flen + j] * ff0 + fd[(fn + 1) * flen + j] * ff1) / (1 << INTERP_BIT);
for(std::size_t c = 0; c < channels; ++c)
{
acc[c] += inp[(j + off) * channels + c] * f;
}
}
for(std::size_t c = 0; c < channels; ++c)
{
outp[i * channels + c] = dither_truncate<sample_type_out>(acc[c], ds[c]);
ds[c] = LCG_A2 * ds[c] + LCG_C2;
}
x += inv_ratio;
}
pos = x;
dither = ds[0];
}
|
