1 /** @addtogroup dsp
2 * @{
3 */
4 /*
5 Copyright (C) 2016 D Levin (https://www.kfrlib.com)
6 This file is part of KFR
7
8 KFR is free software: you can redistribute it and/or modify
9 it under the terms of the GNU General Public License as published by
10 the Free Software Foundation, either version 2 of the License, or
11 (at your option) any later version.
12
13 KFR is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU General Public License for more details.
17
18 You should have received a copy of the GNU General Public License
19 along with KFR.
20
21 If GPL is not suitable for your project, you must purchase a commercial license to use KFR.
22 Buying a commercial license is mandatory as soon as you develop commercial activities without
23 disclosing the source code of your own applications.
24 See https://www.kfrlib.com for details.
25 */
26 #pragma once
27
28 #include "../base/memory.hpp"
29 #include "../base/reduce.hpp"
30 #include "../simd/impl/function.hpp"
31 #include "../simd/vec.hpp"
32 #include "window.hpp"
33
34 namespace kfr
35 {
36
37 enum class sample_rate_conversion_quality : int
38 {
39 draft = 4,
40 low = 6,
41 normal = 8,
42 high = 10,
43 perfect = 12,
44 };
45
46 inline namespace CMT_ARCH_NAME
47 {
48
49 using resample_quality = sample_rate_conversion_quality;
50
51 /// @brief Sample Rate converter
52 template <typename T>
53 struct samplerate_converter
54 {
55 using itype = i64;
56 using ftype = subtype<T>;
57
58 private:
windowkfr::CMT_ARCH_NAME::samplerate_converter59 KFR_MEM_INTRINSIC ftype window(ftype n) const
60 {
61 return modzerobessel(kaiser_beta * sqrt(1 - sqr(2 * n - 1))) * reciprocal(modzerobessel(kaiser_beta));
62 }
sidelobe_attkfr::CMT_ARCH_NAME::samplerate_converter63 KFR_MEM_INTRINSIC ftype sidelobe_att() const { return kaiser_beta / 0.1102 + 8.7; }
transition_widthkfr::CMT_ARCH_NAME::samplerate_converter64 KFR_MEM_INTRINSIC ftype transition_width() const { return (sidelobe_att() - 8) / (depth - 1) / 2.285; }
65
66 public:
filter_orderkfr::CMT_ARCH_NAME::samplerate_converter67 static KFR_MEM_INTRINSIC size_t filter_order(sample_rate_conversion_quality quality)
68 {
69 return size_t(1) << (static_cast<int>(quality) + 1);
70 }
71
72 /// @brief Returns sidelobe attenuation for the given quality (in dB)
sidelobe_attenuationkfr::CMT_ARCH_NAME::samplerate_converter73 static KFR_MEM_INTRINSIC ftype sidelobe_attenuation(sample_rate_conversion_quality quality)
74 {
75 return (static_cast<int>(quality) - 3) * ftype(20);
76 }
77
78 /// @brief Returns transition width for the given quality (in rad)
transition_widthkfr::CMT_ARCH_NAME::samplerate_converter79 static KFR_MEM_INTRINSIC ftype transition_width(sample_rate_conversion_quality quality)
80 {
81 return (sidelobe_attenuation(quality) - 8) / (filter_order(quality) - 1) / ftype(2.285);
82 }
83
window_paramkfr::CMT_ARCH_NAME::samplerate_converter84 static KFR_MEM_INTRINSIC ftype window_param(sample_rate_conversion_quality quality)
85 {
86 const ftype att = sidelobe_attenuation(quality);
87 if (att > 50)
88 return ftype(0.1102) * (att - ftype(8.7));
89 if (att >= 21)
90 return ftype(0.5842) * pow(att - 21, ftype(0.4)) + ftype(0.07886) * (att - 21);
91 return 0;
92 }
93
samplerate_converterkfr::CMT_ARCH_NAME::samplerate_converter94 samplerate_converter(sample_rate_conversion_quality quality, itype interpolation_factor,
95 itype decimation_factor, ftype scale = ftype(1), ftype cutoff = 0.5f)
96 : kaiser_beta(window_param(quality)), depth(static_cast<itype>(filter_order(quality))),
97 input_position(0), output_position(0)
98 {
99 const i64 gcf = gcd(interpolation_factor, decimation_factor);
100 interpolation_factor /= gcf;
101 decimation_factor /= gcf;
102
103 taps = depth * interpolation_factor;
104 order = size_t(depth * interpolation_factor - 1);
105
106 this->interpolation_factor = interpolation_factor;
107 this->decimation_factor = decimation_factor;
108
109 const itype halftaps = taps / 2;
110 filter = univector<T>(size_t(taps), T());
111 delay = univector<T>(size_t(depth), T());
112
113 cutoff = cutoff - transition_width() / c_pi<ftype, 4>;
114
115 cutoff = cutoff / std::max(decimation_factor, interpolation_factor);
116
117 for (itype j = 0, jj = 0; j < taps; j++)
118 {
119 filter[size_t(j)] =
120 sinc((jj - halftaps) * cutoff * c_pi<ftype, 2>) * window(ftype(jj) / ftype(taps - 1));
121 jj += size_t(interpolation_factor);
122 if (jj >= taps)
123 jj = jj - taps + 1;
124 }
125
126 const T s = reciprocal(sum(filter)) * static_cast<ftype>(interpolation_factor * scale);
127 filter = filter * s;
128 }
129
input_position_to_intermediatekfr::CMT_ARCH_NAME::samplerate_converter130 KFR_MEM_INTRINSIC itype input_position_to_intermediate(itype in_pos) const
131 {
132 return in_pos * interpolation_factor;
133 }
output_position_to_intermediatekfr::CMT_ARCH_NAME::samplerate_converter134 KFR_MEM_INTRINSIC itype output_position_to_intermediate(itype out_pos) const
135 {
136 return out_pos * decimation_factor;
137 }
138
input_position_to_outputkfr::CMT_ARCH_NAME::samplerate_converter139 KFR_MEM_INTRINSIC itype input_position_to_output(itype in_pos) const
140 {
141 return floor_div(input_position_to_intermediate(in_pos), decimation_factor).quot;
142 }
output_position_to_inputkfr::CMT_ARCH_NAME::samplerate_converter143 KFR_MEM_INTRINSIC itype output_position_to_input(itype out_pos) const
144 {
145 return floor_div(output_position_to_intermediate(out_pos), interpolation_factor).quot;
146 }
147
output_size_for_inputkfr::CMT_ARCH_NAME::samplerate_converter148 KFR_MEM_INTRINSIC itype output_size_for_input(itype input_size) const
149 {
150 return input_position_to_output(input_position + input_size - 1) -
151 input_position_to_output(input_position - 1);
152 }
153
input_size_for_outputkfr::CMT_ARCH_NAME::samplerate_converter154 KFR_MEM_INTRINSIC itype input_size_for_output(itype output_size) const
155 {
156 return output_position_to_input(output_position + output_size - 1) -
157 output_position_to_input(output_position - 1);
158 }
159
skipkfr::CMT_ARCH_NAME::samplerate_converter160 size_t skip(size_t output_size, univector_ref<const T> input)
161 {
162 const itype required_input_size = input_size_for_output(output_size);
163
164 if (required_input_size >= depth)
165 {
166 delay.slice(0, delay.size()) = padded(input.slice(size_t(required_input_size - depth)));
167 }
168 else
169 {
170 delay.truncate(size_t(depth - required_input_size)) = delay.slice(size_t(required_input_size));
171 delay.slice(size_t(depth - required_input_size)) = padded(input);
172 }
173
174 input_position += required_input_size;
175 output_position += output_size;
176
177 return required_input_size;
178 }
179
180 /// @brief Writes output.size() samples to output reading at most input.size(), then consuming zeros as
181 /// input.
182 /// @returns Number of processed input samples (may be less than input.size()).
183 template <univector_tag Tag>
processkfr::CMT_ARCH_NAME::samplerate_converter184 size_t process(univector<T, Tag>& output, univector_ref<const T> input)
185 {
186 const itype required_input_size = input_size_for_output(output.size());
187
188 const itype input_size = input.size();
189 for (size_t i = 0; i < output.size(); i++)
190 {
191 const itype intermediate_index =
192 output_position_to_intermediate(static_cast<itype>(i) + output_position);
193 const itype intermediate_start = intermediate_index - taps + 1;
194 const std::lldiv_t input_pos =
195 floor_div(intermediate_start + interpolation_factor - 1, interpolation_factor);
196 const itype input_start = input_pos.quot; // first input sample
197 const itype tap_start = interpolation_factor - 1 - input_pos.rem;
198 const univector_ref<T> tap_ptr = filter.slice(static_cast<size_t>(tap_start * depth));
199
200 if (input_start >= input_position + input_size)
201 {
202 output[i] = T(0);
203 }
204 else if (input_start >= input_position)
205 {
206 output[i] = dotproduct(input.slice(input_start - input_position, depth), tap_ptr);
207 }
208 else
209 {
210 const itype prev_count = input_position - input_start;
211 output[i] = dotproduct(delay.slice(size_t(depth - prev_count)), tap_ptr) +
212 dotproduct(input.slice(0, size_t(depth - prev_count)),
213 tap_ptr.slice(size_t(prev_count), size_t(depth - prev_count)));
214 }
215 }
216
217 if (required_input_size >= depth)
218 {
219 delay.slice(0, delay.size()) = padded(input.slice(size_t(required_input_size - depth)));
220 }
221 else
222 {
223 delay.truncate(size_t(depth - required_input_size)) = delay.slice(size_t(required_input_size));
224 delay.slice(size_t(depth - required_input_size)) = padded(input);
225 }
226
227 input_position += required_input_size;
228 output_position += output.size();
229
230 return required_input_size;
231 }
get_fractional_delaykfr::CMT_ARCH_NAME::samplerate_converter232 KFR_MEM_INTRINSIC double get_fractional_delay() const { return (taps - 1) * 0.5 / decimation_factor; }
get_delaykfr::CMT_ARCH_NAME::samplerate_converter233 KFR_MEM_INTRINSIC size_t get_delay() const { return static_cast<size_t>(get_fractional_delay()); }
234
235 ftype kaiser_beta;
236 itype depth;
237 itype taps;
238 size_t order;
239 itype interpolation_factor;
240 itype decimation_factor;
241 univector<T> filter;
242 univector<T> delay;
243 itype input_position;
244 itype output_position;
245 };
246
247 namespace internal
248 {
249
250 template <size_t factor, typename E>
251 struct expression_upsample;
252
253 template <size_t factor, size_t offset, typename E>
254 struct expression_downsample;
255
256 template <typename E>
257 struct expression_upsample<2, E> : expression_with_arguments<E>
258 {
259 using expression_with_arguments<E>::expression_with_arguments;
260 using value_type = value_type_of<E>;
261 using T = value_type;
262
sizekfr::CMT_ARCH_NAME::internal::expression_upsample263 KFR_MEM_INTRINSIC size_t size() const CMT_NOEXCEPT { return expression_with_arguments<E>::size() * 2; }
264
265 template <size_t N>
get_elements(const expression_upsample & self,cinput_t cinput,size_t index,vec_shape<T,N>)266 KFR_INTRINSIC friend vec<T, N> get_elements(const expression_upsample& self, cinput_t cinput,
267 size_t index, vec_shape<T, N>)
268 {
269 const vec<T, N / 2> x = self.argument_first(cinput, index / 2, vec_shape<T, N / 2>());
270 return interleave(x, zerovector(x));
271 }
get_elements(const expression_upsample & self,cinput_t cinput,size_t index,vec_shape<T,1>)272 KFR_INTRINSIC friend vec<T, 1> get_elements(const expression_upsample& self, cinput_t cinput,
273 size_t index, vec_shape<T, 1>)
274 {
275 if (index & 1)
276 return 0;
277 else
278 return self.argument_first(cinput, index / 2, vec_shape<T, 1>());
279 }
280 };
281
282 template <typename E>
283 struct expression_upsample<4, E> : expression_with_arguments<E>
284 {
285 using expression_with_arguments<E>::expression_with_arguments;
286 using value_type = value_type_of<E>;
287 using T = value_type;
288
sizekfr::CMT_ARCH_NAME::internal::expression_upsample289 KFR_MEM_INTRINSIC size_t size() const CMT_NOEXCEPT { return expression_with_arguments<E>::size() * 4; }
290
291 template <size_t N>
get_elements(const expression_upsample & self,cinput_t cinput,size_t index,vec_shape<T,N>)292 KFR_INTRINSIC friend vec<T, N> get_elements(const expression_upsample& self, cinput_t cinput,
293 size_t index, vec_shape<T, N>) CMT_NOEXCEPT
294 {
295 const vec<T, N / 4> x = self.argument_first(cinput, index / 4, vec_shape<T, N / 4>());
296 const vec<T, N / 2> xx = interleave(x, zerovector(x));
297 return interleave(xx, zerovector(xx));
298 }
get_elements(const expression_upsample & self,cinput_t cinput,size_t index,vec_shape<T,2>)299 KFR_INTRINSIC friend vec<T, 2> get_elements(const expression_upsample& self, cinput_t cinput,
300 size_t index, vec_shape<T, 2>) CMT_NOEXCEPT
301 {
302 switch (index & 3)
303 {
304 case 0:
305 return interleave(self.argument_first(cinput, index / 4, vec_shape<T, 1>()), zerovector<T, 1>());
306 case 3:
307 return interleave(zerovector<T, 1>(), self.argument_first(cinput, index / 4, vec_shape<T, 1>()));
308 default:
309 return 0;
310 }
311 }
get_elements(const expression_upsample & self,cinput_t cinput,size_t index,vec_shape<T,1>)312 KFR_INTRINSIC friend vec<T, 1> get_elements(const expression_upsample& self, cinput_t cinput,
313 size_t index, vec_shape<T, 1>) CMT_NOEXCEPT
314 {
315 if (index & 3)
316 return 0;
317 else
318 return self.argument_first(cinput, index / 4, vec_shape<T, 1>());
319 }
320 };
321
322 template <typename E, size_t offset>
323 struct expression_downsample<2, offset, E> : expression_with_arguments<E>
324 {
325 using expression_with_arguments<E>::expression_with_arguments;
326 using value_type = value_type_of<E>;
327 using T = value_type;
328
sizekfr::CMT_ARCH_NAME::internal::expression_downsample329 KFR_MEM_INTRINSIC size_t size() const CMT_NOEXCEPT { return expression_with_arguments<E>::size() / 2; }
330
331 template <size_t N>
get_elements(const expression_downsample & self,cinput_t cinput,size_t index,vec_shape<T,N>)332 KFR_INTRINSIC friend vec<T, N> get_elements(const expression_downsample& self, cinput_t cinput,
333 size_t index, vec_shape<T, N>) CMT_NOEXCEPT
334 {
335 const vec<T, N* 2> x = self.argument_first(cinput, index * 2, vec_shape<T, N * 2>());
336 return x.shuffle(csizeseq<N, offset, 2>);
337 }
338 };
339
340 template <typename E, size_t offset>
341 struct expression_downsample<4, offset, E> : expression_with_arguments<E>
342 {
343 using expression_with_arguments<E>::expression_with_arguments;
344 using value_type = value_type_of<E>;
345 using T = value_type;
346
sizekfr::CMT_ARCH_NAME::internal::expression_downsample347 KFR_MEM_INTRINSIC size_t size() const CMT_NOEXCEPT { return expression_with_arguments<E>::size() / 4; }
348
349 template <size_t N>
get_elements(const expression_downsample & self,cinput_t cinput,size_t index,vec_shape<T,N>)350 KFR_INTRINSIC friend vec<T, N> get_elements(const expression_downsample& self, cinput_t cinput,
351 size_t index, vec_shape<T, N>) CMT_NOEXCEPT
352 {
353 const vec<T, N* 4> x = self.argument_first(cinput, index * 4, vec_shape<T, N * 4>());
354 return x.shuffle(csizeseq<N, offset, 4>);
355 }
356 };
357 } // namespace internal
358
359 template <typename E1, size_t offset = 0>
downsample2(E1 && e1,csize_t<offset>=csize_t<0> ())360 KFR_FUNCTION internal::expression_downsample<2, offset, E1> downsample2(E1&& e1,
361 csize_t<offset> = csize_t<0>())
362 {
363 return internal::expression_downsample<2, offset, E1>(std::forward<E1>(e1));
364 }
365
366 template <typename E1, size_t offset = 0>
downsample4(E1 && e1,csize_t<offset>=csize_t<0> ())367 KFR_FUNCTION internal::expression_downsample<4, offset, E1> downsample4(E1&& e1,
368 csize_t<offset> = csize_t<0>())
369 {
370 return internal::expression_downsample<4, offset, E1>(std::forward<E1>(e1));
371 }
372
373 template <typename E1>
upsample2(E1 && e1)374 KFR_FUNCTION internal::expression_upsample<2, E1> upsample2(E1&& e1)
375 {
376 return internal::expression_upsample<2, E1>(std::forward<E1>(e1));
377 }
378
379 template <typename E1>
upsample4(E1 && e1)380 KFR_FUNCTION internal::expression_upsample<4, E1> upsample4(E1&& e1)
381 {
382 return internal::expression_upsample<4, E1>(std::forward<E1>(e1));
383 }
384
385 template <typename T = fbase>
sample_rate_converter(sample_rate_conversion_quality quality,size_t interpolation_factor,size_t decimation_factor,subtype<T> scale=subtype<T> (1),subtype<T> cutoff=0.5f)386 KFR_FUNCTION samplerate_converter<T> sample_rate_converter(sample_rate_conversion_quality quality,
387 size_t interpolation_factor,
388 size_t decimation_factor,
389 subtype<T> scale = subtype<T>(1),
390 subtype<T> cutoff = 0.5f)
391 {
392 using itype = typename samplerate_converter<T>::itype;
393 return samplerate_converter<T>(quality, itype(interpolation_factor), itype(decimation_factor), scale,
394 cutoff);
395 }
396
397 // Deprecated in 0.9.2
398 template <typename T = fbase>
resampler(sample_rate_conversion_quality quality,size_t interpolation_factor,size_t decimation_factor,subtype<T> scale=subtype<T> (1),subtype<T> cutoff=0.5f)399 KFR_FUNCTION samplerate_converter<T> resampler(sample_rate_conversion_quality quality,
400 size_t interpolation_factor, size_t decimation_factor,
401 subtype<T> scale = subtype<T>(1), subtype<T> cutoff = 0.5f)
402 {
403 using itype = typename samplerate_converter<T>::itype;
404 return samplerate_converter<T>(quality, itype(interpolation_factor), itype(decimation_factor), scale,
405 cutoff);
406 }
407 } // namespace CMT_ARCH_NAME
408 } // namespace kfr
409