Debian Codegraph @

src/libsyn123/resample.c

raw
1 /*
2 resample: low-latency usable and quick resampler
3
4 copyright 2018-2023 by the mpg123 project
5 licensed under the terms of the LGPL 2.1
6 see COPYING and AUTHORS files in distribution or http://mpg123.org
7
8 initially written by Thomas Orgis
9
10 The resampler consists of these steps:
11
12 1. If output rate < 1/4 input rate:
13 1.1 Repeat until output >= 1/4 input rate:
14 1.1.1 Low pass with transition band from 1/4 to 1/2 input bandwidth.
15 1.1.2 Drop samples to halve the input sample rate.
16 1.2 Pre-emphasis to compensate for interpolator response.
17
18 2. If output rate > 1/2 input rate:
19 2.1 Pre-emphasis to compensate for interpolator response.
20 2.2 2x oversampling (zero-stuffing).
21
22 3. Low pass filter with >84% bandwidth to cutoff of the smaller of
23 target rate or input rate, between 1/4 and 1/2 of intermediate
24 sample rate.
25
26 4. Interpolate to target rate.
27
28 The filters are IIR-type. The interpolators are based on Olli Niemitalo's
29 2001 paper titled 'Polynomial Interpolators for High-Quality Resampling of
30 Oversampled Audio', available as download at
31 http://yehar.com/blog/wp-content/uploads/2009/08/deip.pdf . It is these
32 interpolators that drastically improve the quality compared to cubic spline
33 interpolation.
34
35 There are different pre-emphasis filters for the case with and without
36 oversampling. At all times, the signal band before interpolation only
37 extends up to 1/2 of Nyquist, enabling the use of interpolators designed
38 for oversampled input. Without this spectral headroom, things would look
39 much more distorted.
40
41 The higher quality mode achieves distortion below 108 dB with a bandwith
42 above 84%, while the lower quality mode weakens the final low pass to
43 72 dB, and has distortion around that, with slightly better bandwidth.
44
45 The downside of this all is serious phase distortion, but that does
46 not matter much to the human ear for resampling before playback of the
47 stream, as opposed to mixing resampled streams with some phase relations
48 in mind. If you want to resample your instrument samples before combining
49 them into music, you have the full signal at hand and can just use the
50 best method regardless of latency. This here is not your resampler, then.
51
52 The upside is that there is virtually no latency compared to FIR-based
53 resamplers and it should be rather easy to use. You have a direct
54 relation between input buffer and output buffer and do not have to
55 think about draining internal windowing buffers. Data with one sample
56 rate in, data with another sample rate out. Simple as that.
57
58 The performance is very good compared to windowed-sinc interpolators,
59 but is behind the FFT-based approach of libsoxr. You trade in massive
60 latency for a performance gain, there. Vectorization of the code does
61 not seem to bring much. A test with SSE intrinsics yielded about 10%
62 speedup for single-channel operation. This is a basic problem of recursive
63 computations. There is not that much dumb parallelism --- and the SIMD
64 parallelism offered in current CPUs is rather dumb and useless without
65 the specific data flow to be efficient. Or maybe I am just doing it
66 wrong. Perhaps many channels may benefit from SIMD.
67
68 If you want to optimize: Most time is spent in the low pass filtering. The
69 interpolation itself is rather cheap. The decimation filter is also worth
70 considering. Currently, it is not even optimized for efficient coefficient
71 access (that's a TODO), let alone using the more efficient Direct Form II,
72 or anything more fancy.
73
74 TODO: efficient ringbuffer access for the decimation filter
75 TODO: initialize with first sample or zero? is there an actual benefit? impulse
76 response?
77 */
78
79 #define NO_GROW_BUF
80 #define NO_SMAX
81 #define NO_SMIN
82 // SSIZE_MAX is not standard C:-/
83 #define _POSIX_C_SOURCE 200809L
84 #include "syn123_int.h"
85 #include "../common/debug.h"
86
87 // coefficient tables generated from coeff-ellip-6-0.01-36-16.txd
88 // (coeff-{filter_type}-{order}-{passband_ripple}-{rejection_dB}-{points})
89
90 // static const unsigned int lpf_db = 36;
91
92 // Order of the lowpass filter.
93 // You'd wish to have that as a multiple of 4 to help vectorization, but
94 // even 12 is too small to have real benefit from SSE (let alone wider stuff),
95 // and 6 seems to be a limit for stability. The filter needs to be of some minimal
96 // order to be steep enough and becomes unstable, especially with interpolated
97 // coefficients, with higher orders. This is a value I settled on after a long
98 // and painful process. This works between 1/4 and 1/2 of Nyquist. I used to
99 // believe that this can be extended down to 1/16 or even less, but this is
100 // noisy at best.
101 #define LPF_ORDER 6
102
103 // points in filter coefficient interpolation tables
104 #define LPF_POINTS 16
105
106 // filter critical frequency in fractions of Nyquist
107 static const float lpf_w_c[LPF_POINTS] =
108 {
109 +1.9500000000e-01f
110 , +1.9867850841e-01f
111 , +2.0682630822e-01f
112 , +2.1914822332e-01f
113 , +2.3516664873e-01f
114 , +2.5424242424e-01f
115 , +2.7560276877e-01f
116 , +2.9837505460e-01f
117 , +3.2162494540e-01f
118 , +3.4439723123e-01f
119 , +3.6575757576e-01f
120 , +3.8483335127e-01f
121 , +4.0085177668e-01f
122 , +4.1317369178e-01f
123 , +4.2132149159e-01f
124 , +4.2500000000e-01f
125 };
126
127 // actual cutoff frequency in fractions of Nyquist
128 static const float lpf_cutoff[LPF_POINTS] =
129 {
130 +2.4618202393e-01f
131 , +2.5064642208e-01f
132 , +2.6050157602e-01f
133 , +2.7531601376e-01f
134 , +2.9440817636e-01f
135 , +3.1688954073e-01f
136 , +3.4172256063e-01f
137 , +3.6778811727e-01f
138 , +3.9395465344e-01f
139 , +4.1914062138e-01f
140 , +4.4236385073e-01f
141 , +4.6277512916e-01f
142 , +4.7967708029e-01f
143 , +4.9253194430e-01f
144 , +5.0096263453e-01f
145 , +5.0475083567e-01f
146 };
147
148 // filter bandwidth (passband fraction in percent)
149 static const unsigned char lpf_bw[LPF_POINTS] =
150 {
151 85
152 , 85
153 , 85
154 , 85
155 , 85
156 , 86
157 , 86
158 , 86
159 , 87
160 , 87
161 , 88
162 , 88
163 , 88
164 , 89
165 , 89
166 , 89
167 };
168
169 // coefficient b_0
170 static const float lpf_b0[LPF_POINTS] =
171 {
172 +2.5060910358e-02f
173 , +2.5545546150e-02f
174 , +2.6658960397e-02f
175 , +2.8449469363e-02f
176 , +3.0975593417e-02f
177 , +3.4290689410e-02f
178 , +3.8423542124e-02f
179 , +4.3355834055e-02f
180 , +4.8998511458e-02f
181 , +5.5171358029e-02f
182 , +6.1592382077e-02f
183 , +6.7884279531e-02f
184 , +7.3603031799e-02f
185 , +7.8288471817e-02f
186 , +8.1529628810e-02f
187 , +8.3031304399e-02f
188 };
189
190 // derivative d b_0/d w_c
191 static const float lpf_mb0[LPF_POINTS] =
192 {
193 +1.3023622859e-01f
194 , +1.3326373417e-01f
195 , +1.4006275119e-01f
196 , +1.5061692736e-01f
197 , +1.6489815146e-01f
198 , +1.8285716898e-01f
199 , +2.0437030679e-01f
200 , +2.2914833420e-01f
201 , +2.5663256095e-01f
202 , +2.8591120057e-01f
203 , +3.1568709259e-01f
204 , +3.4431969261e-01f
205 , +3.6995119359e-01f
206 , +3.9070858956e-01f
207 , +4.0495210591e-01f
208 , +4.1152140765e-01f
209 };
210
211 // coefficients b_1 to b_n
212 static const float lpf_b[LPF_POINTS][LPF_ORDER] =
213 {
214 { -5.1762847048e-02f, +8.4153207389e-02f, -8.3511150073e-02f
215 , +8.4153207389e-02f, -5.1762847048e-02f, +2.5060910358e-02f }
216 , { -5.0602124846e-02f, +8.2431045673e-02f, -8.0086433599e-02f
217 , +8.2431045673e-02f, -5.0602124846e-02f, +2.5545546150e-02f }
218 , { -4.7865340043e-02f, +7.8920478243e-02f, -7.2561589822e-02f
219 , +7.8920478243e-02f, -4.7865340043e-02f, +2.6658960397e-02f }
220 , { -4.3255354022e-02f, +7.4480386867e-02f, -6.1251009130e-02f
221 , +7.4480386867e-02f, -4.3255354022e-02f, +2.8449469363e-02f }
222 , { -3.6312512281e-02f, +7.0487561023e-02f, -4.6392636636e-02f
223 , +7.0487561023e-02f, -3.6312512281e-02f, +3.0975593417e-02f }
224 , { -2.6445440921e-02f, +6.8811704409e-02f, -2.7845395780e-02f
225 , +6.8811704409e-02f, -2.6445440921e-02f, +3.4290689410e-02f }
226 , { -1.3021846935e-02f, +7.1687243737e-02f, -4.8530219060e-03f
227 , +7.1687243737e-02f, -1.3021846935e-02f, +3.8423542124e-02f }
228 , { +4.4706427843e-03f, +8.1414913409e-02f, +2.3941615853e-02f
229 , +8.1414913409e-02f, +4.4706427843e-03f, +4.3355834055e-02f }
230 , { +2.6209675772e-02f, +9.9864368285e-02f, +6.0088123327e-02f
231 , +9.9864368285e-02f, +2.6209675772e-02f, +4.8998511458e-02f }
232 , { +5.1814692315e-02f, +1.2783927823e-01f, +1.0454069691e-01f
233 , +1.2783927823e-01f, +5.1814692315e-02f, +5.5171358029e-02f }
234 , { +8.0182308668e-02f, +1.6447692384e-01f, +1.5672311528e-01f
235 , +1.6447692384e-01f, +8.0182308668e-02f, +6.1592382077e-02f }
236 , { +1.0945240695e-01f, +2.0692267707e-01f, +2.1383540251e-01f
237 , +2.0692267707e-01f, +1.0945240695e-01f, +6.7884279531e-02f }
238 , { +1.3715527898e-01f, +2.5049018459e-01f, +2.7077975696e-01f
239 , +2.5049018459e-01f, +1.3715527898e-01f, +7.3603031799e-02f }
240 , { +1.6054360691e-01f, +2.8938069305e-01f, +3.2088440740e-01f
241 , +2.8938069305e-01f, +1.6054360691e-01f, +7.8288471817e-02f }
242 , { +1.7705368035e-01f, +3.1783679836e-01f, +3.5729711842e-01f
243 , +3.1783679836e-01f, +1.7705368035e-01f, +8.1529628810e-02f }
244 , { +1.8478918080e-01f, +3.3142891364e-01f, +3.7464012836e-01f
245 , +3.3142891364e-01f, +1.8478918080e-01f, +8.3031304399e-02f }
246 };
247
248 // derivatives (d b_1/d w_c) to (d b_n/d w_c)
249 static const float lpf_mb[LPF_POINTS][LPF_ORDER] =
250 {
251 { +3.0947524511e-01f, -4.7927490698e-01f, +9.3391532013e-01f
252 , -4.7927490702e-01f, +3.0947524507e-01f, +1.3023622860e-01f }
253 , { +3.2168974799e-01f, -4.5690983225e-01f, +9.2827504448e-01f
254 , -4.5690983214e-01f, +3.2168974795e-01f, +1.3326373418e-01f }
255 , { +3.5049895985e-01f, -4.0402713817e-01f, +9.1976126033e-01f
256 , -4.0402713816e-01f, +3.5049895984e-01f, +1.4006275120e-01f }
257 , { +3.9869096973e-01f, -3.1471026348e-01f, +9.1858562318e-01f
258 , -3.1471026347e-01f, +3.9869096972e-01f, +1.5061692736e-01f }
259 , { +4.6976534041e-01f, -1.8018044797e-01f, +9.4154172198e-01f
260 , -1.8018044798e-01f, +4.6976534042e-01f, +1.6489815146e-01f }
261 , { +5.6706828511e-01f, +1.0247608319e-02f, +1.0113425453e+00f
262 , +1.0247608337e-02f, +5.6706828510e-01f, +1.8285716898e-01f }
263 , { +6.9280120305e-01f, +2.6709404538e-01f, +1.1535785914e+00f
264 , +2.6709404540e-01f, +6.9280120305e-01f, +2.0437030678e-01f }
265 , { +8.4703144845e-01f, +5.9748112137e-01f, +1.3910603904e+00f
266 , +5.9748112138e-01f, +8.4703144845e-01f, +2.2914833420e-01f }
267 , { +1.0268432839e+00f, +1.0012919713e+00f, +1.7366400230e+00f
268 , +1.0012919713e+00f, +1.0268432839e+00f, +2.5663256095e-01f }
269 , { +1.2257925890e+00f, +1.4678100718e+00f, +2.1866645243e+00f
270 , +1.4678100718e+00f, +1.2257925891e+00f, +2.8591120056e-01f }
271 , { +1.4338418956e+00f, +1.9740692730e+00f, +2.7173819083e+00f
272 , +1.9740692730e+00f, +1.4338418956e+00f, +3.1568709258e-01f }
273 , { +1.6379144032e+00f, +2.4857766605e+00f, +3.2858109638e+00f
274 , +2.4857766605e+00f, +1.6379144032e+00f, +3.4431969261e-01f }
275 , { +1.8231025322e+00f, +2.9609624523e+00f, +3.8352350558e+00f
276 , +2.9609624523e+00f, +1.8231025322e+00f, +3.6995119359e-01f }
277 , { +1.9744172816e+00f, +3.3557576586e+00f, +4.3041807299e+00f
278 , +3.3557576587e+00f, +1.9744172816e+00f, +3.9070858955e-01f }
279 , { +2.0788103417e+00f, +3.6311428529e+00f, +4.6368982737e+00f
280 , +3.6311428526e+00f, +2.0788103416e+00f, +4.0495210591e-01f }
281 , { +2.1270907033e+00f, +3.7592676993e+00f, +4.7930965582e+00f
282 , +3.7592676992e+00f, +2.1270907033e+00f, +4.1152140769e-01f }
283 };
284
285 // coefficients a_1 to a_n
286 static const float lpf_a[LPF_POINTS][LPF_ORDER] =
287 {
288 { -3.9678160947e+00f, +7.1582705272e+00f, -7.3103432700e+00f
289 , +4.4271625605e+00f, -1.4968822486e+00f, +2.2103607837e-01f }
290 , { -3.9217586689e+00f, +7.0245007818e+00f, -7.1375017960e+00f
291 , +4.3093638600e+00f, -1.4547982662e+00f, +2.1489651918e-01f }
292 , { -3.8190188341e+00f, +6.7322820225e+00f, -6.7643407898e+00f
293 , +4.0573843887e+00f, -1.3653052102e+00f, +2.0191441068e-01f }
294 , { -3.6618359076e+00f, +6.3015570638e+00f, -6.2251488099e+00f
295 , +3.6990273202e+00f, -1.2392343109e+00f, +1.8379956602e-01f }
296 , { -3.4544704643e+00f, +5.7631160859e+00f, -5.5687366968e+00f
297 , +3.2721548186e+00f, -1.0907981304e+00f, +1.6273969374e-01f }
298 , { -3.2034885530e+00f, +5.1559829152e+00f, -4.8508981080e+00f
299 , +2.8177192974e+00f, -9.3461709876e-01f, +1.4091459119e-01f }
300 , { -2.9178775764e+00f, +4.5233648501e+00f, -4.1259259822e+00f
301 , +2.3730384158e+00f, -7.8317396639e-01f, +1.2011720880e-01f }
302 , { -2.6088711387e+00f, +3.9075669790e+00f, -3.4391290636e+00f
303 , +1.9669306242e+00f, -6.4531233315e-01f, +1.0156466880e-01f }
304 , { -2.2894468741e+00f, +3.3447965777e+00f, -2.8221523134e+00f
305 , +1.6175400427e+00f, -5.2592289748e-01f, +8.5891269383e-02f }
306 , { -1.9735577122e+00f, +2.8610227169e+00f, -2.2920156678e+00f
307 , +1.3327010249e+00f, -4.2655597907e-01f, +7.3258414200e-02f }
308 , { -1.6752255568e+00f, +2.4698591405e+00f, -1.8535096177e+00f
309 , +1.1119902037e+00f, -3.4650830575e-01f, +6.3506595124e-02f }
310 , { -1.4076456238e+00f, +2.1728886190e+00f, -1.5035402243e+00f
311 , +9.4944185607e-01f, -2.8395564864e-01f, +5.6296779542e-02f }
312 , { -1.1824218415e+00f, +1.9621904134e+00f, -1.2356636112e+00f
313 , +8.3617447471e-01f, -2.3684730680e-01f, +5.1219220854e-02f }
314 , { -1.0089985054e+00f, +1.8243200388e+00f, -1.0434432700e+00f
315 , +7.6259361700e-01f, -2.0344554024e-01f, +4.7870210629e-02f }
316 , { -8.9429544087e-01f, +1.7447560305e+00f, -9.2208898364e-01f
317 , +7.2012730057e-01f, -1.8252811740e-01f, +4.5906155400e-02f }
318 , { -8.4251214238e-01f, +1.7118654093e+00f, -8.6865163772e-01f
319 , +7.0252569491e-01f, -1.7335864376e-01f, +4.5082431755e-02f }
320 };
321
322 // derivatives (d a_1/d w_c) to (d a_n/d w_c)
323 static const float lpf_ma[LPF_POINTS][LPF_ORDER] =
324 {
325 { +1.2492433161e+01f, -3.6517018434e+01f, +4.7357395713e+01f
326 , -3.2369784989e+01f, +1.1586055408e+01f, -1.6933077254e+00f }
327 , { +1.2548734957e+01f, -3.6212194682e+01f, +4.6616582888e+01f
328 , -3.1678742548e+01f, +1.1296037667e+01f, -1.6449911143e+00f }
329 , { +1.2669371015e+01f, -3.5511519947e+01f, +4.4983356850e+01f
330 , -3.0180967586e+01f, +1.0676583147e+01f, -1.5427341805e+00f }
331 , { +1.2841272347e+01f, -3.4388021134e+01f, +4.2540425267e+01f
332 , -2.8002157377e+01f, +9.7976496751e+00f, -1.3997892255e+00f }
333 , { +1.3046121049e+01f, -3.2820302039e+01f, +3.9430990541e+01f
334 , -2.5324586959e+01f, +8.7534918962e+00f, -1.2330749768e+00f }
335 , { +1.3263302462e+01f, -3.0810041034e+01f, +3.5857000371e+01f
336 , -2.2361001245e+01f, +7.6447176195e+00f, -1.0593731098e+00f }
337 , { +1.3473094693e+01f, -2.8396265557e+01f, +3.2063474406e+01f
338 , -1.9323493868e+01f, +6.5616850615e+00f, -8.9237379829e-01f }
339 , { +1.3659446627e+01f, -2.5661488138e+01f, +2.8308712196e+01f
340 , -1.6394785270e+01f, +5.5731925958e+00f, -7.4123740640e-01f }
341 , { +1.3811818531e+01f, -2.2727774254e+01f, +2.4827395931e+01f
342 , -1.3709404604e+01f, +4.7220289931e+00f, -6.1062450746e-01f }
343 , { +1.3925825330e+01f, -1.9744316114e+01f, +2.1797535816e+01f
344 , -1.1348774538e+01f, +4.0264909835e+00f, -5.0168575497e-01f }
345 , { +1.4002735155e+01f, -1.6870564740e+01f, +1.9320924822e+01f
346 , -9.3489487064e+00f, +3.4855436350e+00f, -4.1340259191e-01f }
347 , { +1.4048120627e+01f, -1.4259541011e+01f, +1.7421558040e+01f
348 , -7.7156336592e+00f, +3.0852238544e+00f, -3.4381636627e-01f }
349 , { +1.4070068384e+01f, -1.2044779182e+01f, +1.6060419989e+01f
350 , -6.4399486362e+00f, +2.8047048140e+00f, -2.9090554179e-01f }
351 , { +1.4077328201e+01f, -1.0332356944e+01f, +1.5160803823e+01f
352 , -5.5100288590e+00f, +2.6213968366e+00f, -2.5305663714e-01f }
353 , { +1.4077683041e+01f, -9.1976058009e+00f, +1.4636862941e+01f
354 , -4.9166414668e+00f, +2.5150332199e+00f, -2.2919336158e-01f }
355 , { +1.4076709161e+01f, -8.6849570278e+00f, +1.4418874781e+01f
356 , -4.6538327513e+00f, +2.4707840042e+00f, -2.1869079956e-01f }
357 };
358
359 // Low pass with 1/4 pass band, 1/4 transition band on top, for
360 // instant decimation by 2 and further processing to a rate below
361 // 1/4 of original sampling rate.
362 // This tuned instance needs only order 12 once and gives more
363 // attenuation than the regular resampling lowpass filter. Also, the
364 // pass band is maximaly preserved for no additional bandwidth loss.
365 // It is an Chebychev type 2 filter with clean pass band but of course
366 // wild phase shifts. Designed with octave's cheby2(12,112,0.5).
367
368 #define LPF_4_ORDER 12
369
370 static const float lpf_4_coeff[2][LPF_4_ORDER+1] =
371 {
372 { +4.942274456389e-04f, +2.456588519653e-03f, +7.333608109998e-03f
373 , +1.564190404912e-02f, +2.595749312226e-02f, +3.475411633752e-02f
374 , +3.823783863736e-02f, +3.475411633752e-02f, +2.595749312226e-02f
375 , +1.564190404912e-02f, +7.333608109998e-03f, +2.456588519652e-03f
376 , +4.942274456389e-04f }
377 , { +1.000000000000e+00f, -3.414853772012e+00f, +6.812049477837e+00f
378 , -8.988639858221e+00f, +8.737226725937e+00f, -6.382745001626e+00f
379 , +3.590542256812e+00f, -1.543464216385e+00f, +5.036659057430e-01f
380 , -1.203614775500e-01f, +2.006360736124e-02f, -2.070940463771e-03f
381 , +1.010063725617e-04f }
382 };
383
384 // 4th-order pre-emphasis filters for the differing interpolations
385
386 #define PREEMP_ORDER 5
387
388 static const float preemp_1xopt4p4o_coeff[2][6] =
389 {
390 { +1.4537908355e+00f, -1.5878441906e-01f, +7.6588945557e-01f
391 , -1.1187528142e-01f, +4.9169512092e-02f, -6.1692905372e-03f }
392 , { +1.0000000000e+00f, +1.9208553603e-01f, +6.4223479412e-01f
393 , +8.4340248522e-02f, +6.9072561856e-02f, +4.2876716239e-03f }
394 };
395 static const float preemp_1xopt6p5o_coeff[2][6] =
396 {
397 { +1.7736168976e+00f, -2.9739831101e-01f, +8.3183026784e-01f
398 , -1.5434578823e-01f, +3.0236127398e-02f, -2.1054630699e-03f }
399 , { +1.0000000000e+00f, +2.9206686404e-01f, +6.5605633523e-01f
400 , +1.4922672324e-01f, +7.4792388477e-02f, +9.6914195316e-03f }
401 };
402 static const float preemp_2xopt4p4o_coeff[2][6] =
403 {
404 { +1.1866573548e+00f, +1.1973031478e+00f, -9.4241923873e-02f
405 , -4.0863707262e-01f, -8.0536389300e-02f, -3.8911856607e-04f }
406 , { +1.0000000000e+00f, +1.2122990222e+00f, +1.0444255630e-01f
407 , -3.7674911658e-01f, -1.3106237366e-01f, -8.7740900285e-03f }
408 };
409 static const float preemp_2xopt6p5o_coeff[2][6] =
410 {
411 { +1.2995411641e+00f, +1.3148944902e+00f, -4.3762040619e-02f
412 , -3.7595940266e-01f, -6.2452466903e-02f, +8.5987740998e-04f }
413 , { +1.0000000000e+00f, +1.3227482221e+00f, +2.6592565819e-01f
414 , -3.1166281821e-01f, -1.3266510256e-01f, -1.1224337963e-02f }
415 };
416
417 // TODO: switch the decimation filter to Direct Form II
418 struct lpf4_hist
419 {
420 // Filter history (to be switched to Direct Form 2).
421 float x[LPF_4_ORDER];
422 float y[LPF_4_ORDER];
423 };
424
425 #ifndef NO_DE_DENORM
426 // Battling denormals. They naturally occur in IIR filters and
427 // need to be avoided since they hurt performance on many CPUs.
428 // Usually, gcc -ffast-math would also avoid them, but we cannot
429 // count on that and debugging performance issues becomes confusing.
430 // The idea is to add an alternatingly positive/negative small number
431 // that is orders of magnitude above denormals to freshly computed
432 // samples.
433 // Downside: You never see a true zero if ther is true zero on input.
434 // Upside would be that you'd convert to 24 or 32 bit integer anyway,
435 // where you'd get your zero back (1 in 32 bit is around 5e-10.
436 static const float denorm_base = 1e-15f;
437 #define DE_DENORM(val, base) (val) += (base);
438 #define DE_DENORM_FLIP(base) (base) = -(base);
439 #define DE_DENORM_INIT(base, sign) (base) = (sign)*denorm_base;
440 #else
441 #define DE_DENORM(val, base)
442 #define DE_DENORM_FLIP(base)
443 #define DE_DENORM_INIT(base, sign)
444 #endif
445
446 struct decimator_state
447 {
448 unsigned int sflags; // also stores decimation position (0 or 1)
449 unsigned int n1;
450 struct lpf4_hist *ch; // one for each channel, pointer into common array
451 float *out_hist; // channels*STAGE_HISTORY, pointer to common block
452 #ifndef NO_DE_DENORM
453 float dede; // current de-denormalization offset
454 #endif
455 };
456
457 #ifdef SYN123_HIGH_PREC
458 typedef double lpf_sum_type;
459 #else
460 typedef float lpf_sum_type;
461 #endif
462
463 // The amount of lowpass runs determines much of the quality.
464 // Each run gives lpf_db reduction. The lowpass also includes the
465 // pre-emphasis and oversampling.
466 #define LPF_TIMES 3
467 #define DIRTY_LPF_TIMES 2
468 #define LOWPASS lowpass3_df2
469 #define LOWPASS_PREEMP lowpass3_df2_preemp
470 #define LOWPASS_PREEMP_2X lowpass3_df2_preemp_2x
471 #define DIRTY_LOWPASS lowpass2_df2
472 #define DIRTY_LOWPASS_PREEMP lowpass2_df2_preemp
473 #define DIRTY_LOWPASS_PREEMP_2X lowpass2_df2_preemp_2x
474 // The number of points used in the interpolators.
475 #define FINE_POINTS 6
476 #define DIRTY_POINTS 4
477
478 // The data structure accomodates a fixed amount of low pass applications.
479 #define LPF_MAX_TIMES LPF_TIMES
480
481 // Let's define what we see a a reasonable history size for an IIR lowpass.
482 // On arrival of a new sample, <order> previous samples are needed, both
483 // input and output (recursive). Let's simply double that to cater for
484 // the recursive nature, ensuring that each recursive input at least already
485 // contains something from the new samples
486 #define LPF_HISTORY(order) (2*(order))
487
488 // Number of samples to keep for smooth rate changes on
489 // addition/removal of stages.
490 #define STAGE_HISTORY 48
491
492 enum state_flags
493 {
494 inter_flow = 1<<0
495 , preemp_configured = 1<<1
496 , preemp_flow = 1<<2
497 , decimate_store = 1<<3 // one skipped sample from last buffer
498 , oversample_2x = 1<<4 // x2 oversampling or not
499 , lowpass_configured = 1<<5
500 , lowpass_flow = 1<<6
501 , dirty_method = 1<<7
502 , smooth_change = 1<<8
503 };
504
505 // TODO: tune that to the smallest sensible value.
506 #ifndef SYN123_BATCH
507 #define BATCH 128
508 #else
509 #define BATCH SYN123_BATCH
510 #endif
511
512 #if (BATCH < STAGE_HISTORY)
513 #error "Batch size cannot be smaller than the stage history."
514 #endif
515
516 struct channel_history
517 {
518 // Direct Form II histories for preemp and lowpass
519 float pre_w[PREEMP_ORDER]; // history for Direct Form II
520 float lpf_w[LPF_MAX_TIMES][LPF_ORDER];
521 float x[5]; // past input samples for interpolator
522 float c[6]; // interpolator coefficients
523 };
524
525 struct resample_data
526 {
527 unsigned int sflags;
528 // The configured resampling function to call.
529 // Decides: oversampling/nothing/decimation, fine/dirty
530 size_t (*resample_func)(struct resample_data *, float *, size_t, float *);
531 // The lowpass functions used before interpolation, including preemp.
532 void (*lowpass_2x_func)(struct resample_data *, float *, size_t, float *);
533 void (*lowpass_func) (struct resample_data *, float *, size_t);
534 // Maximum number of input samples that can be fed in one go to have the
535 // number of output samples safely within the limit of size_t.
536 size_t input_limit;
537 // Decimator state. Each stage has a an instance of lpf_4 and the decimator.
538 unsigned int decim_stages;
539 struct decimator_state *decim;
540 // storage for the above to avoid nested malloc
541 struct lpf4_hist *decim_hist;
542 struct channel_history *ch;
543 // Blocks of STAGE_HISTORY*channels samples.
544 // 0: plain input for first decimation, oversampling or direct interpolation
545 // 1: output of first decimation
546 // 2: output of second decimation, etc.
547 float *stage_history; // storage for past input of final (2x) lowpass
548 float *frame; // One PCM frame to work on (one sample for each channel).
549 float *prebuf; // [channels*BATCH]
550 float *upbuf; // [channels*2*BATCH]
551 #ifndef NO_DE_DENORM
552 float dede; // de-denormalization offset, alternatingly used both in preemp and lpf
553 #endif
554 // Final lowpass filter setup.
555 float lpf_cutoff;
556 float lpf_w_c;
557 unsigned char lpf_bw; // Bandwidth in percent, rounded to integer.
558 // Pre-emphasis filter coefficients and history.
559 float pre_b0;
560 unsigned char pre_n1;
561 float pre_b[PREEMP_ORDER][PREEMP_ORDER];
562 float pre_a[PREEMP_ORDER][PREEMP_ORDER];
563 // Same for the low pass.
564 float lpf_b0;
565 // Low pass filter history in ringbuffer. For each ringbuffer state, there is
566 // one mirroring set of coefficients to avoid re-sorting during application.
567 unsigned char lpf_n1; // Current index of the most recent past.
568 // It may help to have these aligned, but since LPF_ORDER is not 4 nor 8, things
569 // just don't add up for vector instructions.
570 float lpf_b[LPF_ORDER][LPF_ORDER];
571 float lpf_a[LPF_ORDER][LPF_ORDER];
572 // Interpolator state.
573 // In future, I could pull the same trick with ringbuffer and copies
574 // of coefficients (matrices, even) as for lpf.
575 // TODO: maybe store more of these for better filter adaption on rate changes, or
576 // at least always the full set of 5.
577 long offset; // interpolator offset
578 unsigned int channels;
579 long inrate;
580 long vinrate;
581 long outrate;
582 long voutrate;
583 };
584
585 // Interpolation of low pass coefficients for specified cutoff, using
586 // the precomputed table. Instead of properly computing the filter
587 // at runtime, a cheap spline interpolation in the stored tables
588 // does the job, too. This works for filters that are not too unstable.
589 // The employed 6th-order filter still works with the associated rounding
590 // errors, in the limited range of cutoff between 1/4 and 1/2 of Nyquist.
591
592 // Constructing the low pass filter for a bit less than claimed cutoff,
593 // numerics shift things a bit.
594 static const float lpf_cut_scale = 0.995f;
595
596 // Reference implementation of the cubic spline evaluation.
597 // x: time coordinate to evaluate at
598 // xa: lower interval boundary
599 // xb: upper interval boundary
600 // a: value at xa
601 // b: value at xb
602 // da: derivative at xa
603 // db: derivative at db
604 static float spline_val( float x, float xa, float xb
605 , float a, float b, float da, float db )
606 {
607 float d = xb-xa;
608 float t = (x-xa)/d;
609 float t2 = t*t;
610 float t3 = t2*t;
611 float h00 = 2*t3-3*t2+1;
612 float h10 = -2*t3+3*t2;
613 float h01d = (t3-2*t2+t)*d;
614 float h11d = (t3-t2)*d;
615 return h00*a + h10*b + h01d*da + h11d*db;
616 }
617
618 // Find coordiante x in interpolation table val.
619 // Returns index for the lower boundary. Monotonic increase
620 // is assumed.
621 static unsigned int lpf_entry(const float *val, float x)
622 {
623 unsigned int i = 0;
624 while(i<(LPF_POINTS-2) && val[i+1] <= x)
625 ++i;
626 return i;
627 }
628
629 // Derivative of value with respect to w_c at given
630 // index. Intervals are not of equal width ... so central
631 // difference is not straightforward. I'll simply use a
632 // inverse-weighted mean of both sides.
633 static float lpf_deriv( const float x[LPF_POINTS]
634 , const float val[LPF_POINTS], unsigned int i )
635 {
636 float w = 0;
637 float m = 0;
638 // lpf_w_c assumed to be monotonically increasing
639 if(i<LPF_POINTS-1)
640 {
641 float w1 = x[i+1]-x[i];
642 m += (val[i+1]-val[i])/(w1*w1);
643 w += 1.f/w1;
644 }
645 if(i>0)
646 {
647 float w0 = x[i]-x[i-1];
648 m += (val[i]-val[i-1])/(w0*w0);
649 w += 1.f/w0;
650 }
651 return m/w;
652 }
653
654 // Static ring buffer index used for filter history and coefficients.
655 // i: offset
656 // n1: current position of the first entry
657 // s: size of the ring buffer
658 #define RING_INDEX(i, n1, s) ( ((n1)+(i)) % s )
659
660 // Compute actual low pass coefficients for the cutoff in the handle.
661 // This interpolates in the tables without subsequent scaling for zero
662 // DC gain, which has been shown not to be useful.
663 static void lpf_init(struct resample_data *rd)
664 {
665 rd->sflags &= ~lowpass_configured;
666 float real_cutoff = rd->lpf_cutoff * lpf_cut_scale;
667 // It does not make sense to return an error here. It is an internal
668 // implementation error if the table is too small. Just run with the
669 // best value we have.
670 if(real_cutoff < lpf_cutoff[0])
671 real_cutoff = lpf_cutoff[0];
672 if(real_cutoff > lpf_cutoff[LPF_POINTS-1])
673 real_cutoff = lpf_cutoff[LPF_POINTS-1];
674 // The coefficient interpolation needs w_c != cutoff.
675 unsigned int lpfi = lpf_entry(lpf_cutoff, real_cutoff);
676 rd->lpf_w_c = spline_val
677 (
678 real_cutoff, lpf_cutoff[lpfi], lpf_cutoff[lpfi+1]
679 , lpf_w_c[lpfi], lpf_w_c[lpfi+1]
680 , lpf_deriv(lpf_cutoff, lpf_w_c, lpfi)
681 , lpf_deriv(lpf_cutoff, lpf_w_c, lpfi+1)
682 );
683 mdebug("lowpass cutoff %g; w_c %g", rd->lpf_cutoff, rd->lpf_w_c);
684 lpfi = lpf_entry(lpf_w_c, rd->lpf_w_c);
685 rd->lpf_bw = (unsigned char)( 0.5 + lpf_cut_scale*(lpf_bw[lpfi]
686 + (rd->lpf_w_c-lpf_w_c[lpfi])/(lpf_w_c[lpfi+1]-lpf_w_c[lpfi])
687 * (lpf_bw[lpfi+1]-lpf_bw[lpfi])) );
688 mdebug("lowpass bandwidth: %u%%", rd->lpf_bw);
689 rd->lpf_b0 = spline_val( rd->lpf_w_c, lpf_w_c[lpfi], lpf_w_c[lpfi+1]
690 , lpf_b0[lpfi], lpf_b0[lpfi+1]
691 , lpf_mb0[lpfi], lpf_mb0[lpfi+1] );
692 float b[LPF_ORDER];
693 float a[LPF_ORDER];
694 for(int i=0; i<LPF_ORDER; ++i)
695 {
696 b[i] = spline_val( rd->lpf_w_c
697 , lpf_w_c[lpfi], lpf_w_c[lpfi+1]
698 , lpf_b[lpfi][i], lpf_b[lpfi+1][i]
699 , lpf_mb[lpfi][i], lpf_mb[lpfi+1][i] );
700 a[i] = spline_val( rd->lpf_w_c
701 , lpf_w_c[lpfi], lpf_w_c[lpfi+1]
702 , lpf_a[lpfi][i], lpf_a[lpfi+1][i]
703 , lpf_ma[lpfi][i], lpf_ma[lpfi+1][i] );
704 }
705 memset(rd->lpf_a, 0, sizeof(float)*LPF_ORDER*LPF_ORDER);
706 memset(rd->lpf_b, 0, sizeof(float)*LPF_ORDER*LPF_ORDER);
707 for(int i=0; i<LPF_ORDER; ++i)
708 {
709 for(int j=0; j<LPF_ORDER; ++j)
710 {
711 rd->lpf_b[j][RING_INDEX(i,j,LPF_ORDER)] = b[i];
712 rd->lpf_a[j][RING_INDEX(i,j,LPF_ORDER)] = a[i];
713 }
714 }
715 debug("lpf coefficients [b,a]:");
716 mdebug("%.15e", rd->lpf_b0);
717 for(int i=0; i<LPF_ORDER; ++i)
718 mdebug("%.15e", rd->lpf_b[0][i]);
719 mdebug("%.15e", 1.);
720 for(int i=0; i<LPF_ORDER; ++i)
721 mdebug("%.15e", rd->lpf_a[0][i]);
722 rd->sflags |= lowpass_configured;
723 }
724
725 // Compute an estimate of the magnitude of lowpass history values.
726 // This is used to re-scale those values on sample ratio changes to
727 // try to keep a smooth ride. Relate this to df2_initval().
728 // First approach: b_0 / (1+sum(a))
729 static float lpf_history_scale(struct resample_data *rd)
730 {
731 float asum = 1;
732 for(unsigned int i=0; i<LPF_ORDER; ++i)
733 asum += rd->lpf_a[0][i];
734 return rd->lpf_b0/asum;
735 }
736
737 static void preemp_init(struct resample_data *rd)
738 {
739 const float* pre_b;
740 const float* pre_a;
741 rd->sflags &= ~preemp_configured;
742
743 #define PREEMP_COEFF(name) \
744 pre_b = preemp_ ## name ## _coeff[0]; \
745 pre_a = preemp_ ## name ## _coeff[1];
746 if(rd->sflags & oversample_2x)
747 {
748 if(rd->sflags & dirty_method)
749 {
750 PREEMP_COEFF(2xopt4p4o);
751 } else
752 {
753 PREEMP_COEFF(2xopt6p5o);
754 }
755 }
756 else
757 {
758 if(rd->sflags & dirty_method)
759 {
760 PREEMP_COEFF(1xopt4p4o);
761 } else
762 {
763 PREEMP_COEFF(1xopt6p5o);
764 }
765 }
766
767 rd->pre_b0 = pre_b[0];
768 for(int i=0; i<PREEMP_ORDER; ++i)
769 {
770 for(int j=0; j<PREEMP_ORDER; ++j)
771 {
772 rd->pre_b[j][RING_INDEX(i,j,PREEMP_ORDER)] = pre_b[i+1];
773 rd->pre_a[j][RING_INDEX(i,j,PREEMP_ORDER)] = pre_a[i+1];
774 }
775 }
776
777 rd->sflags |= preemp_configured;
778 }
779
780
781 // Store the last samples of given input data in the respective
782 // stage history buffer.
783 static void stage_history( struct resample_data *rd
784 , unsigned int stage, float *in, size_t ins )
785 {
786 if(!rd->stage_history)
787 return;
788 // Case 1: We got enough and can fill it all.
789 // Case 2: Just a bit, have to shift existing
790 // history to append to.
791 float *hist = rd->stage_history + stage*STAGE_HISTORY*rd->channels;
792 if(ins >= STAGE_HISTORY)
793 memcpy( hist, in+(ins-STAGE_HISTORY)*rd->channels
794 , STAGE_HISTORY*rd->channels*sizeof(float) );
795 else
796 {
797 memmove( hist, hist+ins*rd->channels
798 , (STAGE_HISTORY-ins)*rd->channels*sizeof(float) );
799 memcpy( hist+(STAGE_HISTORY-ins)*rd->channels, in
800 , ins*rd->channels*sizeof(float) );
801 }
802 }
803
804
805 static void stage_history_init( struct resample_data *rd
806 , unsigned int stage, float *in )
807 {
808 if(!rd->stage_history)
809 return;
810 float *hist = rd->stage_history + stage*STAGE_HISTORY*rd->channels;
811 for(size_t i=0; i<STAGE_HISTORY; ++i)
812 for(unsigned int c=0; c<rd->channels; ++c)
813 hist[i*rd->channels+c] = in[c];
814 }
815
816 // Actual resampling workers.
817
818 // To make things easier to compilers, things like the number of times
819 // a low pass filter is repeatedly applied are hardcoded in functions
820 // generated with a set of macros to avoid code duplication.
821
822 // One decimation step: low pass with transition band from 1/2 to 1/4,
823 // drop every second sample. This works within the input buffer.
824 static size_t decimate( struct resample_data *rrd, unsigned int dstage
825 , float *in, size_t ins )
826 {
827 struct decimator_state *rd = rrd->decim+dstage;
828 if(!ins)
829 return 0;
830 mdebug("decimating %zu samples", ins);
831 if(!(rd->sflags & lowpass_flow))
832 {
833 for(unsigned int c=0; c<rrd->channels; ++c)
834 for(unsigned int j=0; j<LPF_4_ORDER; ++j)
835 rd->ch[c].x[j] = rd->ch[c].y[j] = in[c];
836 rd->n1 = 0;
837 rd->sflags |= lowpass_flow|decimate_store;
838 DE_DENORM_INIT(rd->dede, dstage % 2 ? -1 : +1)
839 stage_history_init(rrd, dstage+1, in);
840 }
841 float *out = in; // output worker
842 float *oout = in; // for history storage
843 size_t outs = 0;
844 #ifndef SYN123_NO_CASES
845 switch(rrd->channels)
846 {
847 case 1: for(size_t i=0; i<ins; ++i)
848 {
849 int ni[LPF_4_ORDER];
850 for(int j=0; j<LPF_4_ORDER; ++j)
851 ni[j] = RING_INDEX(j, rd->n1, LPF_4_ORDER);
852 rd->n1 = ni[LPF_4_ORDER-1];
853 // y0 = b0x0 + b1x1 + ... + bxyn - a1y1 - ... - anyn
854 // Switching y to double pushes roundoff hf noise down (?).
855 lpf_sum_type ny = lpf_4_coeff[0][0] * in[i]; // a0 == 1 implicit here
856 for(int j=0; j<LPF_4_ORDER; ++j)
857 {
858 // This direct summing without intermediate asum and bsum
859 // pushs roundoff hf noise down.
860 ny += rd->ch[0].x[ni[j]]*lpf_4_coeff[0][j+1];
861 ny -= rd->ch[0].y[ni[j]]*lpf_4_coeff[1][j+1];
862 }
863 DE_DENORM(ny, rd->dede)
864 rd->ch[0].x[rd->n1] = in[i];
865 rd->ch[0].y[rd->n1] = ny;
866 // Drop every second sample.
867 // Maybe its faster doing this in a separate step after all?
868 if(rd->sflags & decimate_store)
869 {
870 DE_DENORM_FLIP(rd->dede)
871 *(out++) = ny;
872 rd->sflags &= ~decimate_store;
873 ++outs;
874 } else
875 rd->sflags |= decimate_store;
876 }
877 break;
878 case 2: for(size_t i=0; i<ins; ++i)
879 {
880 int ni[LPF_4_ORDER];
881 for(int j=0; j<LPF_4_ORDER; ++j)
882 ni[j] = RING_INDEX(j, rd->n1, LPF_4_ORDER);
883 rd->n1 = ni[LPF_4_ORDER-1];
884 float frame[2];
885 for(unsigned int c=0; c<2; ++c)
886 {
887 // y0 = b0x0 + b1x1 + ... + bxyn - a1y1 - ... - anyn
888 // Switching y to double pushes roundoff hf noise down (?).
889 lpf_sum_type ny = lpf_4_coeff[0][0] * in[c]; // a0 == 1 implicit here
890 for(int j=0; j<LPF_4_ORDER; ++j)
891 {
892 // This direct summing without intermediate asum and bsum
893 // pushs roundoff hf noise down.
894 ny += rd->ch[c].x[ni[j]]*lpf_4_coeff[0][j+1];
895 ny -= rd->ch[c].y[ni[j]]*lpf_4_coeff[1][j+1];
896 }
897 DE_DENORM(ny, rd->dede)
898 rd->ch[c].x[rd->n1] = in[c];
899 rd->ch[c].y[rd->n1] = ny;
900 frame[c] = ny;
901 }
902 in += 2;
903 // Drop every second sample.
904 // Maybe its faster doing this in a separate step after all?
905 if(rd->sflags & decimate_store)
906 {
907 DE_DENORM_FLIP(rd->dede)
908 *(out++) = frame[0];
909 *(out++) = frame[1];
910 rd->sflags &= ~decimate_store;
911 ++outs;
912 } else
913 rd->sflags |= decimate_store;
914 }
915 break;
916 default:
917 #endif
918 for(size_t i=0; i<ins; ++i)
919 {
920 int ni[LPF_4_ORDER];
921 for(int j=0; j<LPF_4_ORDER; ++j)
922 ni[j] = RING_INDEX(j, rd->n1, LPF_4_ORDER);
923 rd->n1 = ni[LPF_4_ORDER-1];
924 for(unsigned int c=0; c<rrd->channels; ++c)
925 {
926 // y0 = b0x0 + b1x1 + ... + bxyn - a1y1 - ... - anyn
927 // Switching y to double pushes roundoff hf noise down (?).
928 lpf_sum_type ny = lpf_4_coeff[0][0] * in[c]; // a0 == 1 implicit here
929 for(int j=0; j<LPF_4_ORDER; ++j)
930 {
931 // This direct summing without intermediate asum and bsum
932 // pushs roundoff hf noise down.
933 ny += rd->ch[c].x[ni[j]]*lpf_4_coeff[0][j+1];
934 ny -= rd->ch[c].y[ni[j]]*lpf_4_coeff[1][j+1];
935 }
936 DE_DENORM(ny, rd->dede)
937 rd->ch[c].x[rd->n1] = in[c];
938 rd->ch[c].y[rd->n1] = ny;
939 rrd->frame[c] = ny;
940 }
941 in += rrd->channels;
942 // Drop every second sample.
943 // Maybe its faster doing this in a separate step after all?
944 if(rd->sflags & decimate_store)
945 {
946 DE_DENORM_FLIP(rd->dede)
947 for(unsigned int c=0; c<rrd->channels; ++c)
948 *(out++) = rrd->frame[c];
949 rd->sflags &= ~decimate_store;
950 ++outs;
951 } else
952 rd->sflags |= decimate_store;
953 }
954 #ifndef SYN123_NO_CASES
955 }
956 #endif
957 stage_history(rrd, dstage+1, oout, outs);
958 return outs;
959 }
960
961 // Initialization of Direct Form 2 history.
962 // Return a constant value for w[n] that represents an endless history of given insample.
963 // Given the recursive definition: w[n] = x[n] - sum_i(a[i] * w[n-i])
964 // Given my ansatz of endless stream of x[n-i] = x[n] and hence w[n-i] = w[n], it follows
965 // that
966 // w[n] = x[n] - sum_i(a[i] * w[n]) = x[n] - sum_i(a[i]) * w[n]
967 // <=> (1 + sum_i(a[i])) w[n] = x[n]
968 // <=> w[n] = 1 / (1 + sum_i(a[i])) * x[n]
969 // Looks simple. Just a scale value.
970 static float df2_initval(unsigned int order, float *filter_a, float insample)
971 {
972 float asum = 1.;
973 for(unsigned int i=0; i<order; ++i)
974 asum += filter_a[i];
975 float scale = 1.f/asum;
976 return scale * insample;
977 }
978
979 #define PREEMP_DF2_BEGIN(initval, channels, ret) \
980 if(!(rd->sflags & preemp_flow)) \
981 { \
982 if(!(rd->sflags & preemp_configured)) \
983 { \
984 fprintf(stderr, "unconfigured pre-emphasis\n"); \
985 return ret; \
986 } \
987 for(unsigned int c=0; c<channels; ++c) \
988 { \
989 float iv = df2_initval(PREEMP_ORDER, rd->pre_a[0], initval[c]); \
990 for(int i=0; i<PREEMP_ORDER; ++i) \
991 rd->ch[c].pre_w[i] = iv; \
992 } \
993 rd->pre_n1 = 0; \
994 rd->sflags |= preemp_flow; \
995 }
996
997 // Might not be faster than DFI, but for sure needs less storage.
998 #define PREEMP_DF2_SAMPLE(in, out, channels) \
999 { \
1000 unsigned char n1 = rd->pre_n1; \
1001 rd->pre_n1 = RING_INDEX(PREEMP_ORDER-1, rd->pre_n1, PREEMP_ORDER); \
1002 for(unsigned int c=0; c<channels; ++c) \
1003 { \
1004 lpf_sum_type ny = 0; \
1005 lpf_sum_type nw = 0; \
1006 for(unsigned char i=0; i<PREEMP_ORDER; ++i) \
1007 { \
1008 ny += rd->ch[c].pre_w[i]*rd->pre_b[n1][i]; \
1009 nw -= rd->ch[c].pre_w[i]*rd->pre_a[n1][i]; \
1010 } \
1011 nw += in[c]; \
1012 DE_DENORM(nw, rd->dede) \
1013 ny += rd->pre_b0 * nw; \
1014 rd->ch[c].pre_w[rd->pre_n1] = nw; \
1015 out[c] = ny; \
1016 } \
1017 }
1018
1019 // Beginning of a low pass function with on-the-fly initialization
1020 // of filter history.
1021 // For oversampling lowpass with zero-stuffing, initialize to half of first
1022 // input sample.
1023 #define LPF_DF2_BEGIN(times,initval,channels,ret) \
1024 if(!(rd->sflags & lowpass_flow)) \
1025 { \
1026 if(!(rd->sflags & lowpass_configured)) \
1027 { \
1028 fprintf(stderr, "unconfigured lowpass\n"); \
1029 return ret; \
1030 } \
1031 rd->lpf_n1 = 0; \
1032 for(unsigned int c=0; c<channels; ++c) \
1033 { \
1034 float iv = df2_initval(LPF_ORDER, rd->lpf_a[0], initval[c]); \
1035 for(int j=0; j<times; ++j) \
1036 for(int i=0; i<LPF_ORDER; ++i) \
1037 rd->ch[c].lpf_w[j][i] = iv; \
1038 } \
1039 rd->sflags |= lowpass_flow; \
1040 } \
1041 int n1 = rd->lpf_n1; \
1042 int n1n;
1043
1044 #define LPF_DF2_SAMPLE(times, in, out, channels) \
1045 n1n = RING_INDEX(LPF_ORDER-1, n1, LPF_ORDER); \
1046 for(unsigned int c=0; c<channels; ++c) \
1047 { \
1048 float old_y = in[c]; \
1049 for(int j=0; j<times; ++j) \
1050 { \
1051 lpf_sum_type w = old_y; \
1052 lpf_sum_type y = 0; \
1053 for(int k=0; k<LPF_ORDER; ++k) \
1054 { \
1055 y += rd->ch[c].lpf_w[j][k]*rd->lpf_b[n1][k]; \
1056 w -= rd->ch[c].lpf_w[j][k]*rd->lpf_a[n1][k]; \
1057 } \
1058 DE_DENORM(w, rd->dede) \
1059 rd->ch[c].lpf_w[j][n1n] = w; \
1060 y += rd->lpf_b0*w; \
1061 old_y = y; \
1062 } \
1063 out[c] = old_y; \
1064 } \
1065 n1 = n1n;
1066
1067 #define LPF_DF2_END \
1068 rd->lpf_n1 = n1;
1069
1070 // Define normal and oversampling low pass with pre-emphasis in direct form 2.
1071 // The parameter determines the number of repeated applications of the same
1072 // low pass.
1073
1074 #ifndef SYN123_NO_CASES
1075 #define LOWPASS_DF2_FUNCSX(times) \
1076 \
1077 static void lowpass##times##_df2_preemp_2x(struct resample_data *rd, float *in, size_t ins, float *out) \
1078 { \
1079 if(!ins) \
1080 return; \
1081 LPF_DF2_BEGIN(times,in,rd->channels,) \
1082 PREEMP_DF2_BEGIN(in,rd->channels,); \
1083 switch(rd->channels) \
1084 { \
1085 case 1: for(size_t i=0; i<ins; ++i) \
1086 { \
1087 float sample; \
1088 PREEMP_DF2_SAMPLE(in, (&sample), 1) \
1089 /* Zero-stuffing! Insert zero after making up for energy loss. */ \
1090 sample *= 2; \
1091 LPF_DF2_SAMPLE(times, (&sample), out, 1) \
1092 out++; \
1093 sample = 0; \
1094 LPF_DF2_SAMPLE(times, (&sample), out, 1) \
1095 DE_DENORM_FLIP(rd->dede) \
1096 out++; \
1097 in++; \
1098 } \
1099 break; \
1100 case 2: for(size_t i=0; i<ins; ++i) \
1101 { \
1102 float frame[2]; \
1103 PREEMP_DF2_SAMPLE(in, frame, 2) \
1104 /* Zero-stuffing! Insert zero after making up for energy loss. */ \
1105 frame[0] *= 2; \
1106 frame[1] *= 2; \
1107 LPF_DF2_SAMPLE(times, frame, out, 2) \
1108 out += 2; \
1109 frame[1] = frame[0] = 0; \
1110 LPF_DF2_SAMPLE(times, frame, out, 2) \
1111 DE_DENORM_FLIP(rd->dede) \
1112 out += 2; \
1113 in += 2; \
1114 } \
1115 break; \
1116 default: for(size_t i=0; i<ins; ++i) \
1117 { \
1118 PREEMP_DF2_SAMPLE(in, rd->frame, rd->channels) \
1119 /* Zero-stuffing! Insert zero after making up for energy loss. */ \
1120 for(unsigned int c=0; c<rd->channels; ++c) \
1121 rd->frame[c] *= 2; \
1122 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1123 out += rd->channels; \
1124 for(unsigned int c=0; c<rd->channels; ++c) \
1125 rd->frame[c] = 0; \
1126 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1127 DE_DENORM_FLIP(rd->dede) \
1128 out += rd->channels; \
1129 in += rd->channels; \
1130 } \
1131 } \
1132 LPF_DF2_END \
1133 } \
1134 \
1135 static void lowpass##times##_df2_preemp(struct resample_data *rd, float *in, size_t ins) \
1136 { \
1137 if(!ins) \
1138 return; \
1139 float *out = in; \
1140 LPF_DF2_BEGIN(times, in, rd->channels,) \
1141 PREEMP_DF2_BEGIN(in, rd->channels,); \
1142 switch(rd->channels) \
1143 { \
1144 case 1: for(size_t i=0; i<ins; ++i) \
1145 { \
1146 float sample; \
1147 PREEMP_DF2_SAMPLE(in, (&sample), 1) \
1148 LPF_DF2_SAMPLE(times, (&sample), out, 1) \
1149 DE_DENORM_FLIP(rd->dede) \
1150 in++; \
1151 out++; \
1152 } \
1153 break; \
1154 case 2: for(size_t i=0; i<ins; ++i) \
1155 { \
1156 float frame[2]; \
1157 PREEMP_DF2_SAMPLE(in, frame, 2) \
1158 LPF_DF2_SAMPLE(times, frame, out, 2) \
1159 DE_DENORM_FLIP(rd->dede) \
1160 in += 2; \
1161 out += 2; \
1162 } \
1163 break; \
1164 default: for(size_t i=0; i<ins; ++i) \
1165 { \
1166 PREEMP_DF2_SAMPLE(in, rd->frame, rd->channels) \
1167 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1168 DE_DENORM_FLIP(rd->dede) \
1169 in += rd->channels; \
1170 out += rd->channels; \
1171 } \
1172 } \
1173 LPF_DF2_END \
1174 }
1175 #else
1176 #define LOWPASS_DF2_FUNCSX(times) \
1177 \
1178 static void lowpass##times##_df2_preemp_2x(struct resample_data *rd, float *in, size_t ins, float *out) \
1179 { \
1180 if(!ins) \
1181 return; \
1182 LPF_DF2_BEGIN(times,in,rd->channels,) \
1183 PREEMP_DF2_BEGIN(in,rd->channels,); \
1184 for(size_t i=0; i<ins; ++i) \
1185 { \
1186 PREEMP_DF2_SAMPLE(in, rd->frame, rd->channels) \
1187 /* Zero-stuffing! Insert zero after making up for energy loss. */ \
1188 for(unsigned int c=0; c<rd->channels; ++c) \
1189 rd->frame[c] *= 2; \
1190 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1191 out += rd->channels; \
1192 for(unsigned int c=0; c<rd->channels; ++c) \
1193 rd->frame[c] = 0; \
1194 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1195 DE_DENORM_FLIP(rd->dede) \
1196 out += rd->channels; \
1197 in += rd->channels; \
1198 } \
1199 LPF_DF2_END \
1200 } \
1201 \
1202 static void lowpass##times##_df2_preemp(struct resample_data *rd, float *in, size_t ins) \
1203 { \
1204 if(!ins) \
1205 return; \
1206 float *out = in; \
1207 LPF_DF2_BEGIN(times, in, rd->channels,) \
1208 PREEMP_DF2_BEGIN(in, rd->channels,); \
1209 for(size_t i=0; i<ins; ++i) \
1210 { \
1211 PREEMP_DF2_SAMPLE(in, rd->frame, rd->channels) \
1212 LPF_DF2_SAMPLE(times, rd->frame, out, rd->channels) \
1213 DE_DENORM_FLIP(rd->dede) \
1214 in += rd->channels; \
1215 out += rd->channels; \
1216 } \
1217 LPF_DF2_END \
1218 }
1219 #endif
1220
1221
1222 // Need that indirection so that times is expanded properly for concatenation.
1223 #define LOWPASS_DF2_FUNCS(times) \
1224 LOWPASS_DF2_FUNCSX(times)
1225
1226 LOWPASS_DF2_FUNCS(LPF_TIMES)
1227 LOWPASS_DF2_FUNCS(DIRTY_LPF_TIMES)
1228
1229 // Some interpolators based on the deip paper:
1230 // Polynomial Interpolators for High-Quality Resampling of Oversampled Audio
1231 // REVISED VERSION
1232 // by Olli Niemitalo in October 2001
1233 // http://www.student.oulu.fi/~oniemita/DSP/INDEX.HTM
1234 // http://yehar.com/blog/wp-content/uploads/2009/08/deip.pdf
1235
1236 #define OPT4P4O_BEGIN(in) \
1237 if(!(rd->sflags & inter_flow)) \
1238 { \
1239 for(unsigned int c=0; c<rd->channels; ++c) \
1240 rd->ch[c].x[0] = rd->ch[c].x[1] = rd->ch[c].x[2] = in[c]; \
1241 rd->offset = -rd->vinrate; \
1242 rd->sflags |= inter_flow; \
1243 }
1244
1245 /* See OPT6P5O_INTERPOL for logic hints. */
1246 #define OPT4P4O_INTERPOL(in, out, outs, channels) \
1247 { \
1248 long sampleoff = rd->offset; \
1249 if(sampleoff+rd->vinrate < rd->voutrate) \
1250 { \
1251 for(unsigned int c=0; c<channels; ++c) \
1252 { \
1253 float even1 = rd->ch[c].x[2]+rd->ch[c].x[1]; \
1254 float odd1 = rd->ch[c].x[2]-rd->ch[c].x[1]; \
1255 float even2 = in[c]+rd->ch[c].x[0]; \
1256 float odd2 = in[c]-rd->ch[c].x[0]; \
1257 rd->ch[c].c[0] = even1*0.45645918406487612f \
1258 + even2*0.04354173901996461f; \
1259 rd->ch[c].c[1] = odd1*0.47236675362442071f \
1260 + odd2*0.17686613581136501f; \
1261 rd->ch[c].c[2] = even1*-0.253674794204558521f \
1262 + even2*0.25371918651882464f; \
1263 rd->ch[c].c[3] = odd1*-0.37917091811631082f \
1264 + odd2*0.11952965967158000f; \
1265 rd->ch[c].c[4] = even1*0.04252164479749607f \
1266 + even2*-0.04289144034653719f; \
1267 } \
1268 do { \
1269 sampleoff += rd->vinrate; \
1270 float z = ((float)sampleoff/rd->voutrate) - (float)0.5; \
1271 for(unsigned int c=0; c<channels; ++c) \
1272 out[c] = \
1273 ( ( ( \
1274 rd->ch[c].c[4] \
1275 *z + rd->ch[c].c[3] \
1276 )*z + rd->ch[c].c[2] \
1277 )*z + rd->ch[c].c[1] \
1278 )*z + rd->ch[c].c[0]; \
1279 outs++; \
1280 out += channels; \
1281 } while(sampleoff+rd->vinrate < rd->voutrate); \
1282 } \
1283 for(unsigned int c=0; c<channels; ++c) \
1284 { \
1285 rd->ch[c].x[0] = rd->ch[c].x[1]; \
1286 rd->ch[c].x[1] = rd->ch[c].x[2]; \
1287 rd->ch[c].x[2] = in[c]; \
1288 } \
1289 rd->offset = sampleoff - rd->voutrate; \
1290 }
1291
1292 #define OPT4P4O_END
1293
1294 // Optimal 2x (4-point, 4th-order) (z-form)
1295 static size_t resample_opt4p4o(struct resample_data *rd, float*in
1296 , size_t ins, float *out)
1297 {
1298 size_t outs = 0;
1299 if(!ins)
1300 return outs;
1301 OPT4P4O_BEGIN(in)
1302 #ifndef SYN123_NO_CASES
1303 switch(rd->channels)
1304 {
1305 case 1: for(size_t i=0; i<ins; ++i)
1306 {
1307 OPT4P4O_INTERPOL(in, out, outs, 1)
1308 in++;
1309 }
1310 break;
1311 case 2: for(size_t i=0; i<ins; ++i)
1312 {
1313 OPT4P4O_INTERPOL(in, out, outs, 2)
1314 in += 2;
1315 }
1316 break;
1317 default:
1318 #endif
1319 for(size_t i=0; i<ins; ++i)
1320 {
1321 OPT4P4O_INTERPOL(in, out, outs, rd->channels)
1322 in += rd->channels;
1323 }
1324 #ifndef SYN123_NO_CASES
1325 }
1326 #endif
1327 OPT4P4O_END
1328 return outs;
1329 }
1330
1331 static size_t resample_opt4p4o_2batch(struct resample_data *rd, float*in, float *out)
1332 {
1333 size_t outs = 0;
1334 OPT4P4O_BEGIN(in)
1335 #ifndef SYN123_NO_CASES
1336 switch(rd->channels)
1337 {
1338 case 1: for(size_t i=0; i<2*BATCH; ++i)
1339 {
1340 OPT4P4O_INTERPOL(in, out, outs, 1)
1341 in++;
1342 }
1343 break;
1344 case 2: for(size_t i=0; i<2*BATCH; ++i)
1345 {
1346 OPT4P4O_INTERPOL(in, out, outs, 2)
1347 in += 2;
1348 }
1349 break;
1350 default:
1351 #endif
1352 for(size_t i=0; i<2*BATCH; ++i)
1353 {
1354 OPT4P4O_INTERPOL(in, out, outs, rd->channels)
1355 in += rd->channels;
1356 }
1357 #ifndef SYN123_NO_CASES
1358 }
1359 #endif
1360 OPT4P4O_END
1361 return outs;
1362 }
1363
1364 // This one has low distortion, somewhat more problematic response
1365 // Optimal 2x (6-point, 5th-order) (z-form)
1366
1367 #define OPT6P5O_BEGIN(in) \
1368 if(!(rd->sflags & inter_flow)) \
1369 { \
1370 for(unsigned int c=0; c<rd->channels; ++c) \
1371 { \
1372 rd->ch[c].x[0] = rd->ch[c].x[1] = rd->ch[c].x[2] \
1373 = rd->ch[c].x[3] = rd->ch[c].x[4] = in[c]; \
1374 } \
1375 rd->offset = -rd->vinrate; \
1376 rd->sflags |= inter_flow; \
1377 }
1378
1379 #define OPT6P5O_INTERPOL(in, out, outs, channels) \
1380 { \
1381 /* Offset is how long ago the last sample occured. */ \
1382 long sampleoff = rd->offset; \
1383 /* First check if this interval is interesting at all. */ \
1384 if(sampleoff < rd->voutrate-rd->vinrate) \
1385 { \
1386 /* Not sure if one big loop over the channels would be better. SIMD?! */ \
1387 for(unsigned int c=0; c<channels; ++c) \
1388 { \
1389 float even1 = rd->ch[c].x[3]+rd->ch[c].x[2]; \
1390 float odd1 = rd->ch[c].x[3]-rd->ch[c].x[2]; \
1391 float even2 = rd->ch[c].x[4]+rd->ch[c].x[1]; \
1392 float odd2 = rd->ch[c].x[4]-rd->ch[c].x[1]; \
1393 float even3 = in[c]+rd->ch[c].x[0]; \
1394 float odd3 = in[c]-rd->ch[c].x[0]; \
1395 rd->ch[c].c[0] = even1*0.40513396007145713f + even2*0.09251794438424393f \
1396 + even3*0.00234806603570670f; \
1397 rd->ch[c].c[1] = odd1*0.28342806338906690f + odd2*0.21703277024054901f \
1398 + odd3*0.01309294748731515f; \
1399 rd->ch[c].c[2] = even1*-0.191337682540351941f + even2*0.16187844487943592f \
1400 + even3*0.02946017143111912f; \
1401 rd->ch[c].c[3] = odd1*-0.16471626190554542f + odd2*-0.00154547203542499f \
1402 + odd3*0.03399271444851909f; \
1403 rd->ch[c].c[4] = even1*0.03845798729588149f + even2*-0.05712936104242644f \
1404 + even3*0.01866750929921070f; \
1405 rd->ch[c].c[5] = odd1*0.04317950185225609f + odd2*-0.01802814255926417f \
1406 + odd3*0.00152170021558204f; \
1407 } \
1408 /* Only reaching this code if at least one evaluation is due. */ \
1409 do { \
1410 sampleoff += rd->vinrate; \
1411 /* Evaluating as vector sum may be worthwhile for strong upsampling, */ \
1412 /* possibly many channels. Otherwise, it's just more muls. */ \
1413 float z = ((float)sampleoff/rd->voutrate) - (float)0.5; \
1414 for(unsigned int c=0; c<channels; ++c) \
1415 out[c] = \
1416 ( ( ( ( \
1417 rd->ch[c].c[5] \
1418 *z + rd->ch[c].c[4] \
1419 )*z + rd->ch[c].c[3] \
1420 )*z + rd->ch[c].c[2] \
1421 )*z + rd->ch[c].c[1] \
1422 )*z + rd->ch[c].c[0]; \
1423 outs++; \
1424 out += channels; \
1425 } while(sampleoff < rd->voutrate-rd->vinrate); \
1426 } \
1427 for(unsigned int c=0; c<channels; ++c) \
1428 { \
1429 /* Store shifted history. Think about using RING_INDEX here, too, */ \
1430 rd->ch[c].x[0] = rd->ch[c].x[1]; \
1431 rd->ch[c].x[1] = rd->ch[c].x[2]; \
1432 rd->ch[c].x[2] = rd->ch[c].x[3]; \
1433 rd->ch[c].x[3] = rd->ch[c].x[4]; \
1434 rd->ch[c].x[4] = in[c]; \
1435 } \
1436 rd->offset = sampleoff - rd->voutrate; \
1437 }
1438
1439 #define OPT6P5O_END
1440
1441 static size_t resample_opt6p5o(struct resample_data *rd, float*in
1442 , size_t ins, float *out)
1443 {
1444 size_t outs = 0;
1445 if(!ins)
1446 return outs;
1447 OPT6P5O_BEGIN(in)
1448 #ifndef SYN123_NO_CASES
1449 switch(rd->channels)
1450 {
1451 case 1: for(size_t i=0; i<ins; ++i)
1452 {
1453 OPT6P5O_INTERPOL(in, out, outs, 1)
1454 in++;
1455 }
1456 break;
1457 case 2: for(size_t i=0; i<ins; ++i)
1458 {
1459 OPT6P5O_INTERPOL(in, out, outs, 2)
1460 in += 2;
1461 }
1462 break;
1463 default:
1464 #endif
1465 for(size_t i=0; i<ins; ++i)
1466 {
1467 OPT6P5O_INTERPOL(in, out, outs, rd->channels)
1468 in += rd->channels;
1469 }
1470 #ifndef SYN123_NO_CASES
1471 }
1472 #endif
1473 OPT6P5O_END
1474 return outs;
1475 }
1476
1477 // TODO: Remove the 2batch version after verifying that it doesn't improve things.
1478 static size_t resample_opt6p5o_2batch(struct resample_data *rd, float*in
1479 , float *out)
1480 {
1481 size_t outs = 0;
1482 OPT6P5O_BEGIN(in)
1483 #ifndef SYN123_NO_CASES
1484 switch(rd->channels)
1485 {
1486 case 1: for(size_t i=0; i<2*BATCH; ++i)
1487 {
1488 OPT6P5O_INTERPOL(in, out, outs, 1)
1489 in++;
1490 }
1491 break;
1492 case 2: for(size_t i=0; i<2*BATCH; ++i)
1493 {
1494 OPT6P5O_INTERPOL(in, out, outs, 2)
1495 in += 2;
1496 }
1497 break;
1498 default:
1499 #endif
1500 for(size_t i=0; i<2*BATCH; ++i)
1501 {
1502 OPT6P5O_INTERPOL(in, out, outs, rd->channels)
1503 in += rd->channels;
1504 }
1505 #ifndef SYN123_NO_CASES
1506 }
1507 #endif
1508 OPT6P5O_END
1509 return outs;
1510 }
1511
1512 // Full resample function with oversampling.
1513 #define RESAMPLE_FULL_2X(name, lpf_2x, lpf, interpolate_2batch, interpolate) \
1514 static size_t name( struct resample_data *rd \
1515 , float *in, size_t ins, float *out ) \
1516 { \
1517 mxdebug(#name " in %zu", ins); \
1518 float *iin = in; \
1519 size_t iins = ins; \
1520 if(!(rd->sflags & inter_flow) && ins) \
1521 stage_history_init(rd, 0, iin); \
1522 size_t outs = 0; \
1523 size_t nouts = 0; \
1524 size_t blocks = ins/BATCH; \
1525 for(size_t bi = 0; bi<blocks; ++bi) \
1526 { \
1527 /* One batch in, two batches out. */ \
1528 mxdebug(#name " batch %zu lowpass " #lpf_2x, bi); \
1529 lpf_2x(rd, in, BATCH, rd->upbuf); \
1530 mxdebug(#name " batch %zu interpol " #interpolate_2batch, bi); \
1531 nouts = interpolate_2batch(rd, rd->upbuf, out); \
1532 outs += nouts; \
1533 out += nouts*rd->channels; \
1534 in += BATCH*rd->channels; \
1535 } \
1536 ins -= blocks*BATCH; \
1537 xdebug(#name " end lowpass " #lpf_2x); \
1538 lpf_2x(rd, in, ins, rd->upbuf); \
1539 xdebug(#name " end interpol " #interpolate); \
1540 nouts = interpolate(rd, rd->upbuf, 2*ins, out); \
1541 outs += nouts; \
1542 mxdebug(#name " out %zu", outs); \
1543 stage_history(rd, 0, iin, iins); \
1544 return outs; \
1545 }
1546
1547 // Full resample method without oversampling.
1548 #define RESAMPLE_FULL_1X(name, lpf, interpolate) \
1549 static size_t name( struct resample_data *rd \
1550 , float *in, size_t ins, float *out ) \
1551 { \
1552 mxdebug(#name " in %zu", ins); \
1553 float *iin = in; \
1554 size_t iins = ins; \
1555 if(!(rd->sflags & inter_flow) && ins) \
1556 stage_history_init(rd, 0, iin); \
1557 size_t outs = 0; \
1558 size_t nouts = 0; \
1559 size_t blocks = ins/BATCH; \
1560 for(size_t bi = 0; bi<blocks; ++bi) \
1561 { \
1562 mxdebug(#name " batch %zu lowpass " #lpf, bi); \
1563 memcpy(rd->prebuf, in, BATCH*sizeof(*in)*rd->channels); \
1564 lpf(rd, rd->prebuf, BATCH); \
1565 mxdebug(#name " batch %zu interpol " #interpolate, bi); \
1566 nouts = interpolate(rd, rd->prebuf, BATCH, out); \
1567 outs += nouts; \
1568 out += nouts*rd->channels; \
1569 in += BATCH*rd->channels; \
1570 } \
1571 ins -= blocks*BATCH; \
1572 xdebug(#name " end lowpass " #lpf); \
1573 memcpy(rd->prebuf, in, ins*sizeof(*in)*rd->channels); \
1574 lpf(rd, rd->prebuf, ins); \
1575 xdebug(#name " end interpol " #interpolate); \
1576 nouts = interpolate(rd, rd->prebuf, ins, out); \
1577 outs += nouts; \
1578 mxdebug(#name " out %zu", outs); \
1579 stage_history(rd, 0, iin, iins); \
1580 return outs; \
1581 }
1582
1583 // Full resample method with decimation.
1584 #define RESAMPLE_FULL_0X(name, lpf, interpolate) \
1585 static size_t name( struct resample_data *rd \
1586 , float *in, size_t ins, float *out ) \
1587 { \
1588 mxdebug(#name " in %zu", ins); \
1589 float *iin = in; \
1590 size_t iins = ins; \
1591 if(!(rd->sflags & inter_flow) && ins) \
1592 stage_history_init(rd, 0, iin); \
1593 size_t outs = 0; \
1594 size_t nouts = 0; \
1595 size_t blocks = ins/BATCH; \
1596 for(size_t bi = 0; bi<blocks; ++bi) \
1597 { \
1598 mxdebug( #name " batch %zu decim %d and lowpass " #lpf \
1599 , bi, rd->decim_stages ); \
1600 int fill = BATCH; \
1601 memcpy(rd->prebuf, in, fill*sizeof(*in)*rd->channels); \
1602 for(unsigned int ds = 0; ds<rd->decim_stages; ++ds) \
1603 fill = decimate(rd, ds, rd->prebuf, fill); \
1604 lpf(rd, rd->prebuf, fill); \
1605 mxdebug(#name " batch %zu interpol " #interpolate, bi); \
1606 nouts = interpolate(rd, rd->prebuf, fill, out); \
1607 outs += nouts; \
1608 out += nouts*rd->channels; \
1609 in += BATCH*rd->channels; \
1610 } \
1611 ins -= blocks*BATCH; \
1612 int fill = ins; \
1613 mxdebug( #name " end decim %d and lowpass " #lpf \
1614 , rd->decim_stages ); \
1615 memcpy(rd->prebuf, in, fill*sizeof(*in)*rd->channels); \
1616 for(unsigned int ds = 0; ds<rd->decim_stages; ++ds) \
1617 fill = decimate(rd, ds, rd->prebuf, fill); \
1618 lpf(rd, rd->prebuf, fill); \
1619 xdebug(#name " end interpol " #interpolate); \
1620 nouts = interpolate(rd, rd->prebuf, fill, out); \
1621 outs += nouts; \
1622 mxdebug(#name " out %zu", outs); \
1623 stage_history(rd, 0, iin, iins); \
1624 return outs; \
1625 }
1626
1627 // Define the set of resamplers for a given set of lowpasses and interpolators.
1628 #define RESAMPLE_FULL( name_2x, name_1x, name_0x \
1629 , lpf_2x, lpf, interpolate_2batch, interpolate ) \
1630 RESAMPLE_FULL_2X(name_2x, lpf_2x, lpf, interpolate_2batch, interpolate) \
1631 RESAMPLE_FULL_1X(name_1x, lpf, interpolate) \
1632 RESAMPLE_FULL_0X(name_0x, lpf, interpolate)
1633
1634 // Finally define our dirty and fine resamplers.
1635 RESAMPLE_FULL( resample_2x_dirty, resample_1x_dirty, resample_0x_dirty
1636 , DIRTY_LOWPASS_PREEMP_2X, DIRTY_LOWPASS_PREEMP
1637 , resample_opt4p4o_2batch, resample_opt4p4o )
1638 RESAMPLE_FULL( resample_2x_fine, resample_1x_fine, resample_0x_fine
1639 , LOWPASS_PREEMP_2X, LOWPASS_PREEMP
1640 , resample_opt6p5o_2batch, resample_opt6p5o )
1641
1642 // Here I go and implement a bit of big number functionality.
1643 // The task is just to truly be able to compute a*b/c with unsigned
1644 // integers for cases where the result does not overflow but the intermediate
1645 // product may not fit into the type.
1646 // I do not want to rely on a 128 bit type being availbe, so I take
1647 // uint64_t and run with it.
1648
1649 // Multiply two 64 bit unsigned integers and return the result as two
1650 // halves of 128 bit.
1651 static void mul64(uint64_t a, uint64_t b, uint64_t * m1, uint64_t * m0)
1652 {
1653 uint64_t lowmask = ((uint64_t)1 << 32) - 1;
1654 uint64_t a1 = a >> 32;
1655 uint64_t a0 = a & lowmask;
1656 uint64_t b1 = b >> 32;
1657 uint64_t b0 = b & lowmask;
1658 uint64_t prod1 = a1 * b1;
1659 uint64_t prod0 = a0 * b0;
1660 uint64_t a1b0 = a1 * b0;
1661 uint64_t a0b1 = a0 * b1;
1662 prod1 += a0b1 >> 32;
1663 prod1 += a1b0 >> 32;
1664 uint64_t prod0t = prod0 + ((a0b1 & lowmask) << 32);
1665 if(prod0t < prod0)
1666 prod1 += 1;
1667 prod0 = prod0t + ((a1b0 & lowmask) << 32);
1668 if(prod0 < prod0t)
1669 prod1 += 1;
1670 *m1 = prod1;
1671 *m0 = prod0;
1672 }
1673
1674 // Multiply two unsigned integers numbers and divide by a third one,
1675 // returning the result in the same integer width, if no overflow
1676 // occurs. Also catches division by zero. On error, zero is returned
1677 // and the error code set to a non-zero value (1==overflow, 2==divbyzero).
1678 // The division result is computed on two halves of 128 bit internally,
1679 // and discarded if the higher half contains set bits.
1680 // Also, if m != NULL, the remainder is stored there.
1681 // The actually interesting function for the interpolation:
1682 // e = (a*b+off)/d
1683 // This is bounded by zero: If (a*b+off) < 0, this returns just
1684 // zero, also zero remainder.
1685 static uint64_t muloffdiv64( uint64_t a, uint64_t b, int64_t off, uint64_t c
1686 , int *err, uint64_t *m )
1687 {
1688 uint64_t prod1, prod0;
1689 uint64_t div1, div0, div0old;
1690 if(c < 1)
1691 {
1692 if (err)
1693 *err = 2;
1694 return 0;
1695 }
1696 mul64(a, b, &prod1, &prod0);
1697 if(off)
1698 {
1699 uint64_t prod0old = prod0;
1700 prod0 += off;
1701 // Offset can be positive or negative, small or large.
1702 // When adding to prod0, these cases can happen:
1703 // offset > 0, result > prod0: All fine.
1704 // offset > 0, result < prod0: Overflow.
1705 // offset < 0, result < prod0: All fine.
1706 // offset < 0, result > prod0: Underflow.
1707 if(off > 0 && prod0 < prod0old)
1708 {
1709 if(prod1 == UINT64_MAX)
1710 {
1711 if(err)
1712 *err = 1;
1713 return 0;
1714 }
1715 ++prod1;
1716 }
1717 if(off < 0 && prod0 > prod0old)
1718 {
1719 // Pin to zero on total underflow.
1720 if(prod1 == 0)
1721 prod0 = 0;
1722 else
1723 --prod1;
1724 }
1725 }
1726 if(c == 1)
1727 {
1728 div1 = prod1;
1729 div0 = prod0;
1730 if(m)
1731 *m = 0;
1732 } else
1733 {
1734 div1 = 0;
1735 div0 = 0;
1736 uint64_t ctimes = ((uint64_t) - 1) / c;
1737 uint64_t cblock = ctimes * c;
1738 while(prod1)
1739 {
1740 uint64_t cmult1, cmult0;
1741 mul64(ctimes, prod1, &cmult1, &cmult0);
1742 div1 += cmult1;
1743 div0old = div0;
1744 div0 += cmult0;
1745 if(div0 < div0old)
1746 div1++;
1747 mul64(cblock, prod1, &cmult1, &cmult0);
1748 prod1 -= cmult1;
1749 if(prod0 < cmult0)
1750 prod1--;
1751 prod0 -= cmult0;
1752 }
1753 div0old = div0;
1754 div0 += prod0 / c;
1755 if(m)
1756 *m = prod0 % c;
1757 if(div0 < div0old)
1758 div1++;
1759 }
1760 if(div1)
1761 {
1762 if(err)
1763 *err = 1;
1764 return 0;
1765 }
1766 if(err)
1767 *err = 0;
1768 return div0;
1769 }
1770
1771 // Ensure that the sample rates are not above half of the
1772 // possible range of the used data type, including a safeguard
1773 // that 64 bits will always be enough to work with the values.
1774 // This limits you to 63 bits precision in your sampling rate
1775 // ratio for all times, or a maximum speedup/slowdown by 4.6e18.
1776 // This is still a rather big number.
1777 #if LONG_MAX > INT64_MAX
1778 #define RATE_LIMIT INT64_MAX/2
1779 #else
1780 #define RATE_LIMIT LONG_MAX/2
1781 #endif
1782
1783 long attribute_align_arg
1784 syn123_resample_maxrate(void)
1785 {
1786 return RATE_LIMIT;
1787 }
1788
1789 // Settle oversampling and decimation stages for the given
1790 // input/output rate ratio.
1791 static int rate_setup( long inrate, long outrate
1792 , int *oversample, unsigned int *decim_stages )
1793 {
1794 // Want enough headroom so that the sum of two rates can be represented.
1795 // And of course we don't do reverse or frozen time.
1796 if( inrate < 1 || inrate > RATE_LIMIT ||
1797 outrate < 1 || outrate > RATE_LIMIT )
1798 return -1;
1799 *oversample = outrate*2 > inrate ? 1 : 0;
1800 *decim_stages = 0;
1801 if(outrate <= LONG_MAX/4) while(outrate*4 < inrate)
1802 {
1803 (*decim_stages)++;
1804 outrate *= 2;
1805 }
1806 return 0;
1807 }
1808
1809 // Revisit the cases:
1810 // 2x: to keep the implementation free, one must assume to be able to hold
1811 // 2*ins in the data type, then 2*ins/2*inrate*outrate+1 = ins*outrate/inrate+1
1812 // 1x: no doubling, just ins/inrate*outrate+1
1813 // but since the rate to interpolate to is <= 1/2 input, outsamples
1814 // always will be less, so SIZE_MAX
1815 // 0x: output < 1/4 input. Rounding error of 2 does not matter. Things get smaller,
1816 // so SIZE_MAX.
1817
1818 size_t attribute_align_arg
1819 syn123_resample_maxincount(long inrate, long outrate)
1820 {
1821 int oversample;
1822 unsigned int decim_stages;
1823 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
1824 return 0;
1825 if(oversample)
1826 {
1827 uint64_t outlimit;
1828 // I'm crazy. I am planning for size_t being more than 64 bits.
1829 if(SIZE_MAX-1 > UINT64_MAX)
1830 outlimit = UINT64_MAX;
1831 else
1832 outlimit = SIZE_MAX-1;
1833 // SIZE_MAX = ins*outrate/inrate+1
1834 // SIZE_MAX-1 = ins*outrate/inrate
1835 // (SIZE_MAX-1)*inrate = ins*outrate
1836 // ins = (SIZE_MAX-1)*inrate/outrate
1837 // Integer math always rounding down, this must be below the limit.
1838 int err;
1839 uint64_t maxin = muloffdiv64(outlimit, inrate, 0, outrate, &err, NULL);
1840 // The only possible error is genuine overflow. Meaning: We can
1841 // give as much input as can be represented in the variable.
1842 if(err)
1843 maxin = UINT64_MAX;
1844 return maxin > SIZE_MAX ? SIZE_MAX : (size_t)maxin;
1845 }
1846 else
1847 return SIZE_MAX;
1848 }
1849
1850 // The history is returned as size_t, which usually provide plenty of range.
1851 // The idea is that the history needs to fit into memory, although that is not
1852 // really the case. It is just impractical to consider the full history of
1853 // extreme resampling ratios.
1854 size_t attribute_align_arg
1855 syn123_resample_history(long inrate, long outrate, int dirty)
1856 {
1857 int oversample;
1858 unsigned int decim_stages;
1859 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
1860 return 0;
1861 if(oversample && decim_stages)
1862 return 0;
1863 // Either 4p4o or 6p5o interpolation at the end. We only need the points
1864 // before the current sample in the history, so one less.
1865 size_t history = (dirty ? DIRTY_POINTS : FINE_POINTS) - 1;
1866 // For the oldest of these, we need a history of input before it to
1867 // fill the low pass histories. The number of previous low pass points
1868 // simply comes on top. The number of low pass applications does not matter.
1869 history += LPF_HISTORY(LPF_ORDER);
1870
1871 if(oversample)
1872 {
1873 // Need only half the input, but no less.
1874 history = (history+1)/2;
1875 }
1876 // Here, the numbers can potentially become very large.
1877 // Each decimation step doubles the needed input samples and
1878 // adds the state of the decimation filter.
1879 while(decim_stages--)
1880 {
1881 // Reverse decimation ...
1882 if(history > SIZE_MAX/2+1)
1883 return SIZE_MAX;
1884 history = (history-1) + history;
1885 if(history > SIZE_MAX-LPF_HISTORY(LPF_4_ORDER))
1886 return SIZE_MAX;
1887 history += LPF_HISTORY(LPF_4_ORDER);
1888 }
1889 return history;
1890 }
1891
1892 #if 0
1893 // This should be the essence of what the interpolation does regarding
1894 // input and output sample count.
1895 int64_t simulate_interpolate(long vinrate, long voutrate, int64_t ins)
1896 {
1897 int64_t outs = 0;
1898 long offset = -vinrate;
1899 for(int64_t n = 0; n<ins; ++n)
1900 {
1901 while(offset+vinrate < voutrate)
1902 {
1903 ++outs;
1904 offset += vinrate;
1905 }
1906 offset -= voutrate;
1907 }
1908 return outs;
1909 }
1910 #endif
1911
1912 // The exact output sample count given total input size.
1913 // It assumes zero history.
1914 // This returns a negative code on error.
1915 // The combination of interpolation and stages of decimation calls
1916 // for more than simple integer division. The overall rounding error
1917 // is limited at 2 samples, btw.
1918 int64_t attribute_align_arg
1919 syn123_resample_total64(long inrate, long outrate, int64_t ins)
1920 {
1921 if(ins < 0)
1922 return -1;
1923 int oversample;
1924 unsigned int decim_stages;
1925 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
1926 return SYN123_BAD_FMT;
1927 if(oversample && decim_stages)
1928 return SYN123_WEIRD;
1929
1930 uint64_t vins = ins;
1931 uint64_t vinrate = inrate;
1932 uint64_t voutrate = outrate;
1933 if(decim_stages)
1934 {
1935 // The interpolation uses the virtual output rate to get the same
1936 // ratio as with decimated input rate.
1937 voutrate <<= decim_stages;
1938 // The first sample of a block gets used, the following are dropped.
1939 // So I get one sample out of the first input sample, regardless of
1940 // decimation stage count. But the next sample comes only after
1941 // 2^decim_stages further input samples.
1942 // The formula for one stage: y = (x+1)/2
1943 // For n stages ... my guess is wrong. Heck, n is small, less than
1944 // 64 (see RATE_LIMIT). Let's just mimick the decimations in a loop.
1945 // Since vins < UINT64_MAX/2, adding 1 is no issue.
1946 for(unsigned int i=0; i<decim_stages; ++i)
1947 vins = (vins+1)/2;
1948 }
1949 if(oversample)
1950 {
1951 vins *= 2;
1952 vinrate *= 2;
1953 }
1954 int err;
1955 // Interpolation model:
1956 // outs = (vins*voutrate - offset - 1)/vinrate
1957 // with offset = -vinrate at the beginning. This is the rounding, no
1958 // need to deal with the remainder.
1959 uint64_t tot = muloffdiv64(vins, voutrate, vinrate-1, vinrate, &err, NULL);
1960 // Any error is treated as overflow (div by zero -> infinity).
1961 if(err)
1962 return SYN123_OVERFLOW;
1963 return (tot <= INT64_MAX) ? (int64_t)tot : SYN123_OVERFLOW;
1964 }
1965
1966 // The inverse function: How many input samples are needed to get at least
1967 // the desired amount of output?
1968 int64_t attribute_align_arg
1969 syn123_resample_intotal64(long inrate, long outrate, int64_t outs)
1970 {
1971 if(outs < 1)
1972 return outs == 0 ? 0 : -1;
1973 int oversample;
1974 unsigned int decim_stages;
1975 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
1976 return SYN123_BAD_FMT;
1977 if(oversample && decim_stages)
1978 return SYN123_WEIRD;
1979 uint64_t voutrate = outrate;
1980 voutrate <<= decim_stages;
1981 uint64_t vinrate = inrate;
1982 if(oversample)
1983 vinrate *= 2;
1984 // This works for outs > 0:
1985 // ins = (outs*inrate+offset)/outrate+1
1986 // First offset is -inrate.
1987 // You may want to work it out for yourself. Or trust me;-)
1988 int err;
1989 uint64_t vtot = muloffdiv64(outs, vinrate, -(int64_t)vinrate, voutrate, &err, NULL);
1990 if(err)
1991 return SYN123_OVERFLOW;
1992 if(vtot == UINT64_MAX)
1993 return SYN123_OVERFLOW;
1994 ++vtot; // Must be at least 1 now!
1995 uint64_t tot = vtot;
1996 // Un-do oversampling, taking care not to hit overflow.
1997 if(oversample)
1998 {
1999 tot /= 2;
2000 if(tot*2 < vtot)
2001 ++tot;
2002 }
2003 // Now tot is the minimum needed input sample count before interpolation.
2004 // Decimation still needs to be accounted for.
2005 // With decimation, I need the smallest input that fulfills out=(2*in+1)/2.
2006 for(unsigned int i=0; i<decim_stages; ++i)
2007 {
2008 if(!tot)
2009 return 0;
2010 if(tot > UINT64_MAX/2+1)
2011 return SYN123_OVERFLOW;
2012 tot = (tot - 1) + tot;
2013 }
2014 return (tot <= INT64_MAX) ? (int64_t)tot : SYN123_OVERFLOW;
2015 }
2016
2017 #ifndef PORTABLE_API
2018 // Again the dreadful business of dealing with shape-shifting off_t API.
2019 // The real implementation is the function with suffix 64, using int64_t.
2020 // Depending on your OS, there is a native off_t with 32 or 64 bits, and
2021 // possibly a 64 bit off64_t. The macros here define the appropriate wrappers
2022 // for the _32 and _64 suffixes promised in the API header, depending on
2023 // _FILE_OFFSET_BITS value.
2024 // A special case is a system that does ignore _FILE_OFFSET BITS but has
2025 // 64 bit offsets: That should still get the plain alias and one with _64,
2026 // making the header agnostic to that fact, not depending on any build properties
2027
2028 #define resample_total_wrap(type, limit, name, name64) \
2029 type attribute_align_arg name(long inrate, long outrate, type io) \
2030 { \
2031 int64_t tot = name64(inrate, outrate, io); \
2032 return (tot <= limit) ? (type)tot : SYN123_OVERFLOW; \
2033 }
2034 #define resample_total_alias(type, name, name64) \
2035 type attribute_align_arg name(long inrate, long outrate, type io) \
2036 { \
2037 return name64(inrate, outrate, io); \
2038 }
2039
2040 #if SIZEOF_OFF_T == 8
2041 #ifndef FORCED_OFF_64
2042 resample_total_alias(off_t, syn123_resample_total, syn123_resample_total64)
2043 resample_total_alias(off_t, syn123_resample_intotal, syn123_resample_intotal64)
2044 #endif
2045 resample_total_alias(off_t, syn123_resample_total_64, syn123_resample_total64)
2046 resample_total_alias(off_t, syn123_resample_intotal_64, syn123_resample_intotal64)
2047 #elif SIZEOF_OFF_T == 4
2048 resample_total_wrap(off_t, INT32_MAX, syn123_resample_total, syn123_resample_total64)
2049 resample_total_wrap(off_t, INT32_MAX, syn123_resample_intotal, syn123_resample_intotal64)
2050 resample_total_wrap(off_t, INT32_MAX, syn123_resample_total_32, syn123_resample_total64)
2051 resample_total_wrap(off_t, INT32_MAX, syn123_resample_intotal_32, syn123_resample_intotal64)
2052 #ifdef LFS_LARGEFILE_64
2053 resample_total_alias(off64_t, syn123_resample_total_64, syn123_resample_total64)
2054 resample_total_alias(off64_t, syn123_resample_intotal_64, syn123_resample_intotal64)
2055 #endif
2056 #else
2057 #error "Unexpected off_t size."
2058 #endif
2059
2060 #endif
2061
2062 // As any sensible return value is at least 1, this uses the unsigned
2063 // type and 0 for error/pathological input.
2064 // This function could be simplified to
2065 // uintmax_t count = prod/inrate + (prod%inrate ? 1 : 0) + 1;
2066 // to accomodate for possible rounding during decimation and interpolation,
2067 // but it would be a teensy bit less accurate. The computation time
2068 // should not matter much. The simple formula can be used as a test
2069 // to check if the elaborate function comes to a value that only
2070 // sometimes is less by 1, but not more.
2071 size_t attribute_align_arg
2072 syn123_resample_count(long inrate, long outrate, size_t ins)
2073 {
2074 // The largest output you get from the beginning. So this can just treat ins
2075 // as offset from there.
2076 if( ins > syn123_resample_maxincount(inrate, outrate)
2077 #if SIZE_MAX > INT64_MAX
2078 || ins > INT64_MAX
2079 #endif
2080 )
2081 return 0;
2082 int64_t tot = syn123_resample_total64(inrate, outrate, (int64_t)ins);
2083 return (tot >= 0 && tot <= SIZE_MAX) ? (size_t)tot : 0;
2084 }
2085
2086 size_t attribute_align_arg
2087 syn123_resample_incount(long inrate, long outrate, size_t outs)
2088 {
2089 // I had logic here about abusing syn123_resample_intotal(), but that is wrong.
2090 // Do not assume you know what interval to pick to get the maximum value.
2091 // Work the algorithm for the theoretical maximum at each point, even if that
2092 // may not be hit with your particular rate ratio and playback time.
2093 #if SIZE_MAX > INT64_MAX
2094 if(outs > INT64_MAX-1)
2095 return 0;
2096 #endif
2097 int oversample;
2098 unsigned int decim_stages;
2099 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
2100 return 0;
2101 if(oversample && decim_stages)
2102 return 0;
2103 uint64_t voutrate = outrate;
2104 voutrate <<= decim_stages;
2105 uint64_t vinrate = inrate;
2106 if(oversample)
2107 vinrate *= 2;
2108 // This works for outs > 0:
2109 // ins = (outs*inrate+offset)/outrate+1
2110 // First offset is -inrate, maximum offset is -1. I think.
2111 // You may want to work it out for yourself. Or trust me;-)
2112 int err;
2113 uint64_t vtot = muloffdiv64(outs, vinrate, -1, voutrate, &err, NULL);
2114 if(err)
2115 return SYN123_OVERFLOW;
2116 if(vtot == UINT64_MAX)
2117 return SYN123_OVERFLOW;
2118 ++vtot; // Must be at least 1 now!
2119 uint64_t tot = vtot;
2120 // Un-do oversampling, taking care not to hit overflow.
2121 if(oversample)
2122 {
2123 tot /= 2;
2124 if(tot*2 < vtot)
2125 ++tot;
2126 }
2127 // Now tot is the minimum needed input sample count before interpolation.
2128 // Decimation still needs to be accounted for. Worst case: We got no
2129 // sample buffered (dropped) in any stage: So just multiply.
2130 if(tot > UINT64_MAX>>decim_stages)
2131 return SYN123_OVERFLOW;
2132 tot<<=decim_stages;
2133 return (tot <= SIZE_MAX) ? (size_t)tot : 0;
2134 }
2135
2136 // A bit of convenience. Just rely on the other functions.
2137 size_t attribute_align_arg
2138 syn123_resample_fillcount(long input_rate, long output_rate, size_t outs)
2139 {
2140 size_t block = syn123_resample_incount(input_rate, output_rate, outs);
2141 // Reduce that to ensure that we never get more than a buffer's fill.
2142 while( block &&
2143 syn123_resample_count(input_rate, output_rate, block) > outs )
2144 --block;
2145 return block;
2146 }
2147
2148 // Store given error value to storage, if non-NULL, return
2149 // given return value for pass-through usage.
2150 static size_t size_err(int *err, int value, size_t ret)
2151 {
2152 if(err)
2153 *err = value;
2154 return ret;
2155 }
2156
2157 // The exact predictor: How many output samples will I get _now_
2158 // after feeding the indicated amount? This is concerned with
2159 // Buffer sizes, so let's drop the 32/64 bit distinction.
2160 // Since overflow can occur, we need a sign bit to signal errors.
2161 size_t attribute_align_arg
2162 syn123_resample_out(syn123_handle *sh, size_t ins, int *err)
2163 {
2164 size_err(err, SYN123_OK, 0);
2165 if(!sh || !sh->rd)
2166 return size_err(err, SYN123_BAD_HANDLE, 0);
2167 if(ins < 1)
2168 return 0;
2169 struct resample_data *rd = sh->rd;
2170 // Need to account for the current resampler state. That is:
2171 // 1. decimation stages having swallowed a sample or not,
2172 // 2. current interpolator offset.
2173 // The first sample of a block gets used, the following are dropped.
2174 // So I get one sample out of the first input sample, regardless of
2175 // decimation stage count. But the next sample comes only after
2176 // 2^decim_stages further input samples.
2177 // The formula for one stage: y = (x+1)/2
2178 // For n stages ... my guess is wrong. Heck, n is small, less than
2179 // 64 (see RATE_LIMIT). Let's just mimick the decimations in a loop.
2180 // Since vins < UINT64_MAX/2, adding 1 is no issue.
2181 for(unsigned int i=0; i<rd->decim_stages; ++i)
2182 {
2183 // If the stage is fresh or has explictly a past sample in memorian,
2184 // we can add one virtual input sample. Divide first to avoid overflow.
2185 size_t nins = ins / 2;
2186 // When there is one sample left over, a second one from storage
2187 // adds to the decimated count.
2188 if( nins*2 < ins && (!(rd->decim[i].sflags & lowpass_flow)
2189 || (rd->decim[i].sflags & decimate_store)) )
2190 ++nins;
2191 ins = nins;
2192 }
2193 #if SIZE_MAX > UINT64_MAX
2194 if(ins > UINT64_MAX) // Really?
2195 return size_err(err, SYN123_OVERFLOW, 0);
2196 #endif
2197 uint64_t vins = ins;
2198 if(rd->sflags & oversample_2x)
2199 {
2200 if(vins > UINT64_MAX/2)
2201 return size_err(err, SYN123_OVERFLOW, 0);
2202 vins *= 2;
2203 }
2204 int myerr = SYN123_OK;
2205 int64_t offset = rd->sflags & inter_flow ? rd->offset : -rd->vinrate;
2206 // Interpolation model:
2207 // outs = (vins*voutrate - offset - 1)/vinrate
2208 uint64_t tot = muloffdiv64( vins, (uint64_t)rd->voutrate
2209 , (int64_t)(-offset-1), (uint64_t)rd->vinrate, &myerr, NULL );
2210 // Any error is treated as overflow (div by zero -> infinity).
2211 if(myerr || tot > SIZE_MAX)
2212 return size_err(err, SYN123_OVERFLOW, 0);
2213 return (size_t)tot;
2214 }
2215
2216 syn123_ssize_t attribute_align_arg
2217 syn123_resample_expect(syn123_handle *sh, size_t ins)
2218 {
2219 int err = 0;
2220 size_t tot = syn123_resample_out(sh, ins, &err);
2221 if(tot >= SSIZE_MAX)
2222 err = SYN123_OVERFLOW;
2223 return (syn123_ssize_t)(err ? err : tot);
2224 }
2225
2226 // How many input samples are minimally needed to get at least the
2227 // minimally desired output right now?
2228 size_t attribute_align_arg
2229 syn123_resample_in(syn123_handle *sh, size_t outs, int *err)
2230 {
2231 size_err(err, SYN123_OK, 0);
2232 if(!sh || !sh->rd)
2233 return size_err(err, SYN123_BAD_HANDLE, 0);
2234 if(outs < 1)
2235 return 0;
2236 struct resample_data *rd = sh->rd;
2237 // This works for outs > 0:
2238 // ins = (outs*inrate+offset)/outrate+1
2239 int myerr;
2240 int64_t offset = rd->sflags & inter_flow ? rd->offset : -rd->vinrate;
2241 uint64_t vtot = muloffdiv64(outs, sh->rd->vinrate, offset, rd->voutrate
2242 , &myerr, NULL);
2243 if(myerr || vtot == UINT64_MAX)
2244 return size_err(err, SYN123_OVERFLOW, 0);
2245 ++vtot;
2246 uint64_t tot = vtot;
2247 // Un-do oversampling, taking care not to hit overflow.
2248 if(rd->sflags & oversample_2x)
2249 {
2250 tot /= 2;
2251 if(tot*2 < vtot)
2252 ++tot;
2253 }
2254 // Still: tot > 0
2255 // Got needed output of deciation, for each stage
2256 // a) need twice that amount if there is no sample buffered
2257 // b) need one less than twice the amount if there is a sample buffered
2258 for(unsigned int j=0; j<rd->decim_stages; ++j)
2259 {
2260 unsigned int i = rd->decim_stages - 1 - j;
2261 if(tot > UINT64_MAX/2+1)
2262 return size_err(err, SYN123_OVERFLOW, 0);
2263 tot = (tot - 1) + tot;
2264 if( (rd->decim[i].sflags & lowpass_flow)
2265 && !(rd->decim[i].sflags & decimate_store) )
2266 {
2267 if(tot == UINT64_MAX)
2268 return size_err(err, SYN123_OVERFLOW, 0);
2269 ++tot;
2270 }
2271 }
2272 return (size_t)(tot <= SIZE_MAX ? tot : size_err(err, SYN123_OVERFLOW, 0));
2273 }
2274
2275 syn123_ssize_t attribute_align_arg
2276 syn123_resample_inexpect(syn123_handle *sh, size_t outs)
2277 {
2278 int err = 0;
2279 size_t tot = syn123_resample_in(sh, outs, &err);
2280 if(tot >= SSIZE_MAX)
2281 err = SYN123_OVERFLOW;
2282 return (syn123_ssize_t)(err ? err : tot);
2283 }
2284
2285 void resample_free(struct resample_data *rd)
2286 {
2287 if(!rd)
2288 return;
2289 if(rd->stage_history)
2290 free(rd->stage_history);
2291 if(rd->decim_hist)
2292 free(rd->decim_hist);
2293 if(rd->decim)
2294 free(rd->decim);
2295 if(rd->upbuf)
2296 free(rd->upbuf);
2297 if(rd->prebuf)
2298 free(rd->prebuf);
2299 if(rd->ch)
2300 free(rd->ch);
2301 if(rd->frame)
2302 free(rd->frame);
2303 free(rd);
2304 }
2305
2306 // Want to support smooth rate changes.
2307 // If there is a handle present, try to keep as much as possible.
2308 // If you change the dirty flag, things get refreshed totally.
2309 int attribute_align_arg
2310 syn123_setup_resample( syn123_handle *sh, long inrate, long outrate
2311 , int channels, int dirty, int smooth )
2312 {
2313 int err = 0;
2314 struct resample_data *rd = NULL;
2315 int fresh = 0;
2316
2317 if(inrate == 0 && outrate == 0 && channels == 0)
2318 {
2319 err = SYN123_BAD_FMT;
2320 goto setup_resample_cleanup;
2321 }
2322
2323 if(channels < 1)
2324 {
2325 err = SYN123_BAD_FMT;
2326 goto setup_resample_cleanup;
2327 }
2328
2329 // If dirty setyp changed, start anew.
2330 // TODO: implement proper smooth rate change.
2331 if( sh->rd && ( (sh->rd->channels != channels)
2332 || (!(sh->rd->sflags & dirty_method) != !dirty)
2333 || (!(sh->rd->sflags & smooth_change) != !smooth) ) )
2334 {
2335 resample_free(sh->rd);
2336 sh->rd = NULL;
2337 }
2338
2339 int oversample;
2340 unsigned int decim_stages;
2341 if(rate_setup(inrate, outrate, &oversample, &decim_stages))
2342 return SYN123_BAD_FMT;
2343 if(oversample && decim_stages)
2344 return SYN123_WEIRD;
2345 long voutrate = outrate<<decim_stages;
2346 long vinrate = oversample ? inrate*2 : inrate;
2347 if(!sh->rd)
2348 {
2349 fresh = 1;
2350 sh->rd = malloc(sizeof(*(sh->rd)));
2351 if(!sh->rd)
2352 return SYN123_DOOM;
2353 memset(sh->rd, 0, sizeof(*(sh->rd)));
2354 rd = sh->rd;
2355 rd->inrate = rd->vinrate = rd->voutrate = rd->outrate = -1;
2356 rd->resample_func = NULL;
2357 rd->lowpass_func = dirty ? DIRTY_LOWPASS_PREEMP : LOWPASS_PREEMP;
2358 rd->lowpass_2x_func = dirty ? DIRTY_LOWPASS_PREEMP_2X : LOWPASS_PREEMP_2X;
2359 rd->decim = NULL;
2360 rd->decim_hist = NULL;
2361 rd->channels = channels;
2362 rd->stage_history = NULL;
2363 DE_DENORM_INIT(rd->dede, +1)
2364 rd->frame = NULL;
2365 #ifndef SYN123_NO_CASES
2366 if(channels > 2)
2367 #endif
2368 {
2369 rd->frame = malloc(sizeof(float)*channels);
2370 if(!rd->frame)
2371 {
2372 free(rd);
2373 return SYN123_DOOM;
2374 }
2375 }
2376 rd->ch = malloc(sizeof(struct channel_history)*channels);
2377 rd->prebuf = malloc(sizeof(float)*channels*BATCH);
2378 rd->upbuf = malloc(sizeof(float)*channels*2*BATCH);
2379 if(!rd->ch || !rd->prebuf || !rd->upbuf)
2380 {
2381 if(rd->upbuf)
2382 free(rd->upbuf);
2383 if(rd->prebuf)
2384 free(rd->prebuf);
2385 if(rd->ch)
2386 free(rd->ch);
2387 if(rd->stage_history)
2388 free(rd->stage_history);
2389 if(rd->frame)
2390 free(rd->frame);
2391 free(rd);
2392 return SYN123_DOOM;
2393 }
2394 memset(rd->ch, 0, sizeof(struct channel_history)*channels);
2395 }
2396 else
2397 rd = sh->rd;
2398
2399 mdebug("init in %ld out %ld", inrate, outrate);
2400 // To support on-the-fly rate changes, superfluous decimator stages are
2401 // forgotten, new ones added as needed.
2402 // If smoothing is desired and the stream already flowing, new stages will
2403 // get started on the previously utmost stage's history.
2404 if(fresh || decim_stages != rd->decim_stages)
2405 {
2406 if(smooth)
2407 {
2408 float *sth = INT123_safe_realloc( rd->stage_history
2409 , sizeof(float)*STAGE_HISTORY*channels*(decim_stages+1) );
2410 if(!sth)
2411 {
2412 err = SYN123_DOOM;
2413 goto setup_resample_cleanup;
2414 }
2415 rd->stage_history = sth;
2416 }
2417 if(decim_stages)
2418 {
2419 struct decimator_state *nd = INT123_safe_realloc( rd->decim
2420 , sizeof(*rd->decim)*decim_stages );
2421 struct lpf4_hist *ndh = INT123_safe_realloc( rd->decim_hist
2422 , sizeof(*rd->decim_hist)*decim_stages*channels );
2423 if(nd)
2424 rd->decim = nd;
2425 if(ndh)
2426 rd->decim_hist = ndh;
2427 if(!nd || !ndh)
2428 {
2429 perror("cannot allocate decimator state");
2430 err = SYN123_DOOM;
2431 goto setup_resample_cleanup;
2432 }
2433 } else
2434 {
2435 free(rd->decim); rd->decim = NULL;
2436 free(rd->decim_hist); rd->decim_hist = NULL;
2437 }
2438 // Link up the common memory blocks after each realloc.
2439 for(unsigned int dc=0; dc<decim_stages; ++dc)
2440 {
2441 rd->decim[dc].ch = rd->decim_hist+dc*channels;
2442 rd->decim[dc].out_hist = rd->stage_history
2443 ? rd->stage_history+(dc+1)*STAGE_HISTORY*channels
2444 : NULL;
2445 }
2446 // Smoothness does matter if the interpolator produced something
2447 // previously, so inter_flow is a good flag to test.
2448 int init_stages = rd->stage_history && rd->sflags & inter_flow;
2449 // The situation: Got stage rd->decim_stages (possibly zero).
2450 // with proper output history. Need to start up the additional
2451 // stages with the output of the preceeding stages. I'll pretend
2452 // each stage history being complete, even if I know that I am
2453 // halving the count in each step.
2454 for(unsigned int dc=rd->decim_stages; dc<decim_stages; ++dc)
2455 {
2456 rd->decim[dc].sflags = 0;
2457 if(init_stages)
2458 {
2459 // Take previous stage output dc, process through
2460 // the new stage and have it make up its output history.
2461 // As decimate() overwrites its input, work on a copy.
2462 memcpy( rd->prebuf
2463 , rd->stage_history+dc*STAGE_HISTORY*rd->channels
2464 , STAGE_HISTORY*rd->channels*sizeof(float) );
2465 decimate(rd, dc, rd->prebuf, STAGE_HISTORY);
2466 }
2467 }
2468 rd->decim_stages = decim_stages;
2469 }
2470
2471 if(rd->sflags & inter_flow)
2472 {
2473 // If the virtual input rate changes (esp. increases), the interpolation
2474 // offset needs adjustment to stay at/below predicted sample counts.
2475 // Could always re-initialize the interpolation, but want to keep things
2476 // smooth.
2477 long sign = rd->offset < 0 ? -1 : +1;
2478 // Zero on overflow, suits me just fine.
2479 uint64_t noff = muloffdiv64( (uint64_t)(sign*rd->offset)
2480 , (uint64_t)vinrate, 0, (uint64_t)rd->vinrate, NULL, NULL );
2481 // Magnitude must not be too large. Too small is OK.
2482 if(noff > LONG_MAX)
2483 noff = LONG_MAX;
2484 rd->offset = sign*(long)noff;
2485 // Total paranoia. This is the crucial condition.
2486 if(rd->offset < -vinrate)
2487 rd->offset = -vinrate;
2488 }
2489 rd->inrate = inrate;
2490 rd->outrate = outrate;
2491 rd->vinrate = vinrate;
2492 rd->voutrate = voutrate;
2493 if(oversample)
2494 rd->sflags |= oversample_2x;
2495 else
2496 rd->sflags &= ~oversample_2x;
2497
2498 // TODO: channels!
2499 if(rd->sflags & oversample_2x)
2500 rd->resample_func = dirty ? resample_2x_dirty : resample_2x_fine;
2501 else if(rd->decim_stages)
2502 rd->resample_func = dirty ? resample_0x_dirty : resample_0x_fine;
2503 else
2504 rd->resample_func = dirty ? resample_1x_dirty : resample_1x_fine;
2505
2506 if(dirty)
2507 rd->sflags |= dirty_method;
2508 if(smooth)
2509 rd->sflags |= smooth_change;
2510
2511 mdebug( "%u times decimation by 2"
2512 ", virtual output rate %ld, %ldx oversampling (%i)"
2513 , rd->decim_stages, rd->voutrate, rd->vinrate / rd->inrate
2514 , rd->sflags && oversample_2x ? 1 : 0 );
2515
2516 // Round result to single precision, but compute with double to be able
2517 // to correctly represent 32 bit longs first.
2518 rd->lpf_cutoff = (float)((double)(rd->inrate < rd->outrate ? rd->inrate : rd->voutrate)/rd->vinrate);
2519 mdebug( "lowpass cutoff: %.8f of virtual %.0f Hz = %.0f Hz"
2520 , rd->lpf_cutoff, 0.5*rd->vinrate, rd->lpf_cutoff*0.5*rd->vinrate );
2521 mdebug("rates final: %ld|%ld -> %ld|%ld"
2522 , rd->inrate, rd->vinrate, rd->voutrate, rd->outrate );
2523 preemp_init(rd);
2524 if(rd->sflags & lowpass_flow)
2525 {
2526 // First cheap stage of rate change smoothing:
2527 // Attempt to dampen spikes from the filter history magnitudes
2528 // not matching changed filter setup by simple rescaling.
2529 // This catches the massive spikes on adding decimation stages.
2530 float scale = lpf_history_scale(rd);
2531 lpf_init(rd);
2532 mdebug( "smooth old scale: %g, new scale: %g"
2533 , scale, lpf_history_scale(rd) );
2534 scale = scale > 1e-10f ? lpf_history_scale(rd) / scale : 1.f;
2535 for(unsigned int c=0; c<rd->channels; ++c)
2536 for(unsigned int j=0; j<LPF_MAX_TIMES; ++j)
2537 for(unsigned int i=0; i<LPF_ORDER; ++i)
2538 rd->ch[c].lpf_w[j][i] *= scale;
2539 // Full smoothness with present stage history: Let the lowpass roll.
2540 // Preemp needs to be re-configured beforehand!
2541 // This is the icing on the cake with much more work,
2542 // relaxing the filters to get rid of the trouble.
2543 if(rd->stage_history)
2544 {
2545 // Feed the final stage output through the filters,
2546 // manipulate the interpolator history to match.
2547 float *in = rd->stage_history
2548 + rd->decim_stages*STAGE_HISTORY*rd->channels;
2549 // Pointer to the last 5 filtered samples
2550 float *last5 = NULL;
2551 if(rd->sflags & oversample_2x)
2552 {
2553 rd->lowpass_2x_func(rd,in, STAGE_HISTORY, rd->upbuf);
2554 last5 = rd->upbuf+(2*STAGE_HISTORY-5)*rd->channels;
2555 } else
2556 {
2557 memcpy(rd->prebuf, in, STAGE_HISTORY*sizeof(*in)*rd->channels);
2558 rd->lowpass_func(rd, rd->prebuf, STAGE_HISTORY);
2559 last5 = rd->prebuf+(STAGE_HISTORY-5)*rd->channels;
2560 }
2561 // The above wrote preemp and lowpass channel histories.
2562 // Now store the last samples for the interpolator history.
2563 for(unsigned int c=0; c<rd->channels; ++c)
2564 for(unsigned int i=0; i<5; ++i)
2565 rd->ch[c].x[i] = last5[c+i*rd->channels];
2566 }
2567 }
2568 else
2569 lpf_init(rd);
2570
2571 rd->input_limit = syn123_resample_maxincount(inrate, outrate);
2572 return 0;
2573 setup_resample_cleanup:
2574 resample_free(sh->rd);
2575 sh->rd = NULL;
2576 return err;
2577 }
2578
2579 size_t attribute_align_arg
2580 syn123_resample( syn123_handle *sh,
2581 float * MPG123_RESTRICT dst, float * MPG123_RESTRICT src, size_t samples )
2582 {
2583 if(!sh)
2584 return 0;
2585 struct resample_data *rd = sh->rd;
2586 if(!rd)
2587 return 0;
2588 // Input limit is zero if no resampler configured.
2589 if(!samples || samples > sh->rd->input_limit)
2590 return 0;
2591 mdebug( "calling actual resample function from %p to %p with %zu samples"
2592 , (void*)src, (void*)dst, samples );
2593 return rd->resample_func(rd, src, samples, dst);
2594 }