1 /* SoX Resampler Library Copyright (c) 2007-16 robs@users.sourceforge.net
2 * Licence for this file: LGPL v2.1 See LICENCE for details. */
3
4 /* Variable-rate resampling. */
5
6 #include <assert.h>
7 #include "math-wrap.h"
8 #include <string.h>
9 #include <stdlib.h>
10 #include "internal.h"
11 #define FIFO_SIZE_T int
12 #define FIFO_MIN 0x8000
13 #include "fifo.h"
14 #include "vr-coefs.h"
15
16 #define FADE_LEN_BITS 9
17 #define PHASE_BITS_D 10
18 #define PHASE_BITS_U 9
19
20 #define PHASES0_D 12
21 #define POLY_FIR_LEN_D 20
22 #define PHASES0_U 6
23 #define POLY_FIR_LEN_U 12
24
25 #define MULT32 (65536. * 65536.)
26 #define PHASES_D (1 << PHASE_BITS_D)
27 #define PHASES_U (1 << PHASE_BITS_U)
28
29 #define CONVOLVE \
30 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ \
31 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ \
32 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
33
34 #define HALF_FIR_LEN_2 (iAL(half_fir_coefs) - 1)
35 #define HALF_FIR_LEN_4 (HALF_FIR_LEN_2 / 2)
36
37 #define _ sum += (input[-i] + input[i]) * half_fir_coefs[i], ++i;
half_fir(float const * input)38 static float half_fir(float const * input)
39 {
40 long i = 1;
41 float sum = input[0] * half_fir_coefs[0];
42 CONVOLVE CONVOLVE
43 assert(i == HALF_FIR_LEN_2 + 1);
44 return (float)sum;
45 }
46 #undef _
47
48 #define _ sum += (input[-i] + input[i]) * half_fir_coefs[2*i], ++i;
double_fir0(float const * input)49 static float double_fir0(float const * input)
50 {
51 int i = 1;
52 float sum = input[0] * half_fir_coefs[0];
53 CONVOLVE
54 assert(i == HALF_FIR_LEN_4 + 1);
55 return (float)(sum * 2);
56 }
57 #undef _
58
59 #define _ sum += (input[-i] + input[1+i]) * half_fir_coefs[2*i+1], ++i;
double_fir1(float const * input)60 static float double_fir1(float const * input)
61 {
62 int i = 0;
63 float sum = 0;
64 CONVOLVE
65 assert(i == HALF_FIR_LEN_4 + 0);
66 return (float)(sum * 2);
67 }
68 #undef _
69
fast_half_fir(float const * input)70 static float fast_half_fir(float const * input)
71 {
72 int i = 0;
73 float sum = input[0] * .5f;
74 #define _ sum += (input[-(2*i+1)] + input[2*i+1]) * fast_half_fir_coefs[i], ++i;
75 _ _ _ _ _ _
76 #undef _
77 return (float)sum;
78 }
79
80 #define IIR_FILTER _ _ _ _ _ _ _
81 #define _ in1=(in1-p->y[i])*iir_coefs[i]+tmp1;tmp1=p->y[i],p->y[i]=in1;++i;\
82 in0=(in0-p->y[i])*iir_coefs[i]+tmp0;tmp0=p->y[i],p->y[i]=in0;++i;
83
84 typedef struct {float x[2], y[AL(iir_coefs)];} half_iir_t;
85
half_iir1(half_iir_t * p,float in0,float in1)86 static float half_iir1(half_iir_t * p, float in0, float in1)
87 {
88 int i = 0;
89 float tmp0, tmp1;
90 tmp0 = p->x[0], p->x[0] = in0;
91 tmp1 = p->x[1], p->x[1] = in1;
92 IIR_FILTER
93 p->y[i] = in1 = (in1 - p->y[i]) * iir_coefs[i] + tmp1;
94 return in1 + in0;
95 }
96 #undef _
97
half_iir(half_iir_t * p,float * obuf,float const * ibuf,int olen)98 static void half_iir(half_iir_t * p, float * obuf, float const * ibuf, int olen)
99 {
100 int i;
101 for (i=0; i < olen; obuf[i] = (float)half_iir1(p, ibuf[i*2], ibuf[i*2+1]),++i);
102 }
103
half_phase(half_iir_t * p,float * buf,int len)104 static void half_phase(half_iir_t * p, float * buf, int len)
105 {
106 float const small_normal = 1/MULT32/MULT32; /* To quash denormals on path 0.*/
107 int i;
108 for (i = 0; i < len; buf[i] = (float)half_iir1(p, buf[i], 0), ++i);
109 #define _ p->y[i] += small_normal, i += 2;
110 i = 0, _ IIR_FILTER
111 #undef _
112 #define _ p->y[i] -= small_normal, i += 2;
113 i = 0, _ IIR_FILTER
114 #undef _
115 }
116
117 #define coef(coef_p, interp_order, fir_len, phase_num, coef_interp_num, \
118 fir_coef_num) coef_p[(fir_len) * ((interp_order) + 1) * (phase_num) + \
119 ((interp_order) + 1) * (fir_coef_num) + (interp_order - coef_interp_num)]
120
121 #define COEF(h,l,i) ((i)<0||(i)>=(l)?0:(h)[(i)>(l)/2?(l)-(i):(i)])
prepare_coefs(float * coefs,int n,int phases0,int phases,float const * coefs0,double multiplier)122 static void prepare_coefs(float * coefs, int n, int phases0, int phases,
123 float const * coefs0, double multiplier)
124 {
125 double k[6];
126 int length0 = n * phases0, length = n * phases, K0 = iAL(k)/2 - 1, i, j, pos;
127 float * coefs1 = malloc(((size_t)length / 2 + 1) * sizeof(*coefs1));
128 float * p = coefs1, f0, f1 = 0;
129
130 for (j = 0; j < iAL(k); k[j] = COEF(coefs0, length0, j - K0), ++j);
131 for (pos = i = 0; i < length0 / 2; ++i) {
132 double b=(1/24.)*(k[0]+k[4]+6*k[2]-4*(k[1]+k[3])),d=.5*(k[1]+k[3])-k[2]-b;
133 double a=(1/120.)*(k[5]-k[2]-9*(9*b+d)+2.5*(k[3]-k[1])-2*(k[4]-k[0]));
134 double c=(1/12.)*(k[4]-k[0]-2*(k[3]-k[1])-60*a),e=.5*(k[3]-k[1])-a-c;
135 for (; pos / phases == i; pos += phases0) {
136 double x = (double)(pos % phases) / phases;
137 *p++ = (float)(k[K0] + ((((a*x + b)*x + c)*x + d)*x + e)*x);
138 }
139 for (j = 0; j < iAL(k) - 1; k[j] = k[j + 1], ++j);
140 k[j] = COEF(coefs0, length0, i + iAL(k) / 2 + 1);
141 }
142 if (!(length & 1))
143 *p++ = (float)k[K0];
144 assert(p - coefs1 == length / 2 + 1);
145
146 for (i = 0; i < n; ++i) for (j = phases - 1; j >= 0; --j, f1 = f0) {
147 pos = (n - 1 - i) * phases + j;
148 f0 = COEF(coefs1, length, pos) * (float)multiplier;
149 coef(coefs, 1, n, j, 0, i) = (float)f0;
150 coef(coefs, 1, n, j, 1, i) = (float)(f1 - f0);
151 }
152 free(coefs1);
153 }
154
155 #define _ sum += (b *x + a)*input[i], ++i;
156 #define a (coef(poly_fir_coefs_d, 1, POLY_FIR_LEN_D, phase, 0,i))
157 #define b (coef(poly_fir_coefs_d, 1, POLY_FIR_LEN_D, phase, 1,i))
158 static float poly_fir_coefs_d[POLY_FIR_LEN_D * PHASES_D * 2];
159
poly_fir1_d(float const * input,uint32_t frac)160 static float poly_fir1_d(float const * input, uint32_t frac)
161 {
162 int i = 0, phase = (int)(frac >> (32 - PHASE_BITS_D));
163 float sum = 0, x = (float)(frac << PHASE_BITS_D) * (float)(1 / MULT32);
164 _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _
165 assert(i == POLY_FIR_LEN_D);
166 return (float)sum;
167 }
168 #undef a
169 #undef b
170 #define a (coef(poly_fir_coefs_u, 1, POLY_FIR_LEN_U, phase, 0,i))
171 #define b (coef(poly_fir_coefs_u, 1, POLY_FIR_LEN_U, phase, 1,i))
172 static float poly_fir_coefs_u[POLY_FIR_LEN_U * PHASES_U * 2];
173
poly_fir1_u(float const * input,uint32_t frac)174 static float poly_fir1_u(float const * input, uint32_t frac)
175 {
176 int i = 0, phase = (int)(frac >> (32 - PHASE_BITS_U));
177 float sum = 0, x = (float)(frac << PHASE_BITS_U) * (float)(1 / MULT32);
178 _ _ _ _ _ _ _ _ _ _ _ _
179 assert(i == POLY_FIR_LEN_U);
180 return (float)sum;
181 }
182 #undef a
183 #undef b
184 #undef _
185
186 #define ADD_TO(x,y) x.all += y.all
187 #define SUBTRACT_FROM(x,y) x.all -= y.all
188 #define FRAC(x) x.part.frac
189 #define INT(x) x.part.integer
190
191 typedef struct {
192 union {
193 int64_t all;
194 #if HAVE_BIGENDIAN
195 struct {int32_t integer; uint32_t frac;} part;
196 #else
197 struct {uint32_t frac; int32_t integer;} part;
198 #endif
199 } at, step, step_step;
200 float const * input;
201 int len, stage_num;
202 bool is_d; /* true: downsampling at x2 rate; false: upsampling at 1x rate. */
203 double step_mult;
204 } stream_t;
205
poly_fir_d(stream_t * s,float * output,int olen)206 static int poly_fir_d(stream_t * s, float * output, int olen)
207 {
208 int i;
209 float const * input = s->input - POLY_FIR_LEN_D / 2 + 1;
210 for (i = 0; i < olen && INT(s->at) < s->len; ++i) {
211 output[i] = poly_fir1_d(input + INT(s->at), FRAC(s->at));
212 ADD_TO(s->at, s->step);
213 if (!(INT(s->at) < s->len)) {
214 SUBTRACT_FROM(s->at, s->step);
215 break;
216 }
217 output[++i] = poly_fir1_d(input + INT(s->at), FRAC(s->at));
218 ADD_TO(s->at, s->step);
219 ADD_TO(s->step, s->step_step);
220 }
221 return i;
222 }
223
poly_fir_fade_d(stream_t * s,float const * vol,int step,float * output,int olen)224 static int poly_fir_fade_d(
225 stream_t * s, float const * vol, int step, float * output, int olen)
226 {
227 int i;
228 float const * input = s->input - POLY_FIR_LEN_D / 2 + 1;
229 for (i = 0; i < olen && INT(s->at) < s->len; ++i, vol += step) {
230 output[i] += *vol * poly_fir1_d(input + INT(s->at), FRAC(s->at));
231 ADD_TO(s->at, s->step);
232 if (!(INT(s->at) < s->len)) {
233 SUBTRACT_FROM(s->at, s->step);
234 break;
235 }
236 output[++i] += *(vol += step) * poly_fir1_d(input + INT(s->at),FRAC(s->at));
237 ADD_TO(s->at, s->step);
238 ADD_TO(s->step, s->step_step);
239 }
240 return i;
241 }
242
poly_fir_u(stream_t * s,float * output,int olen)243 static int poly_fir_u(stream_t * s, float * output, int olen)
244 {
245 int i;
246 float const * input = s->input - POLY_FIR_LEN_U / 2 + 1;
247 for (i = 0; i < olen && INT(s->at) < s->len; ++i) {
248 output[i] = poly_fir1_u(input + INT(s->at), FRAC(s->at));
249 ADD_TO(s->at, s->step);
250 ADD_TO(s->step, s->step_step);
251 }
252 return i;
253 }
254
poly_fir_fade_u(stream_t * s,float const * vol,int step,float * output,int olen)255 static int poly_fir_fade_u(
256 stream_t * s, float const * vol, int step, float * output, int olen)
257 {
258 int i;
259 float const * input = s->input - POLY_FIR_LEN_U / 2 + 1;
260 for (i = 0; i < olen && INT(s->at) < s->len; i += 2, vol += step) {
261 output[i] += *vol * poly_fir1_u(input + INT(s->at), FRAC(s->at));
262 ADD_TO(s->at, s->step);
263 ADD_TO(s->step, s->step_step);
264 }
265 return i;
266 }
267
268 #define shiftr(x,by) ((by) < 0? (x) << (-(by)) : (x) >> (by))
269 #define shiftl(x,by) shiftr(x,-(by))
270 #define stage_occupancy(s) (fifo_occupancy(&(s)->fifo) - 4*HALF_FIR_LEN_2)
271 #define stage_read_p(s) ((float *)fifo_read_ptr(&(s)->fifo) + 2*HALF_FIR_LEN_2)
272 #define stage_preload(s) memset(fifo_reserve(&(s)->fifo, (s)->preload), \
273 0, sizeof(float) * (size_t)(s)->preload);
274
275 typedef struct {
276 fifo_t fifo;
277 double step_mult;
278 int is_fast, x_fade_len, preload;
279 } stage_t;
280
281 typedef struct {
282 int num_stages0, num_stages, flushing;
283 int fade_len, slew_len, xfade, stage_inc, switch_stage_num;
284 double new_io_ratio, default_io_ratio;
285 stage_t * stages;
286 fifo_t output_fifo;
287 half_iir_t halfer;
288 stream_t current, fadeout; /* Current/fade-in, fadeout streams. */
289 } rate_t;
290
291 static float fade_coefs[(2 << FADE_LEN_BITS) + 1];
292
vr_init(rate_t * p,double default_io_ratio,int num_stages,double mult)293 static void vr_init(rate_t * p, double default_io_ratio, int num_stages, double mult)
294 {
295 int i;
296 assert(num_stages >= 0);
297 memset(p, 0, sizeof(*p));
298
299 p->num_stages0 = num_stages;
300 p->num_stages = num_stages = max(num_stages, 1);
301 p->stages = (stage_t *)calloc((unsigned)num_stages + 1, sizeof(*p->stages)) + 1;
302 for (i = -1; i < p->num_stages; ++i) {
303 stage_t * s = &p->stages[i];
304 fifo_create(&s->fifo, sizeof(float));
305 s->step_mult = 2 * MULT32 / shiftl(2, i);
306 s->preload = i < 0? 0 : i == 0? 2 * HALF_FIR_LEN_2 : 3 * HALF_FIR_LEN_2 / 2;
307 stage_preload(s);
308 s->is_fast = true;
309 lsx_debug("%-3i preload=%i", i, s->preload);
310 }
311 fifo_create(&p->output_fifo, sizeof(float));
312 p->default_io_ratio = default_io_ratio;
313 if (fade_coefs[0]==0) {
314 for (i = 0; i < iAL(fade_coefs); ++i)
315 fade_coefs[i] = (float)(.5 * (1 + cos(M_PI * i / (AL(fade_coefs) - 1))));
316 prepare_coefs(poly_fir_coefs_u, POLY_FIR_LEN_U, PHASES0_U, PHASES_U, coefs0_u, mult);
317 prepare_coefs(poly_fir_coefs_d, POLY_FIR_LEN_D, PHASES0_D, PHASES_D, coefs0_d, mult *.5);
318 }
319 assert(fade_coefs[0]);
320 }
321
enter_new_stage(rate_t * p,int occupancy0)322 static void enter_new_stage(rate_t * p, int occupancy0)
323 {
324 p->current.len = shiftr(occupancy0, p->current.stage_num);
325 p->current.input = stage_read_p(&p->stages[p->current.stage_num]);
326
327 p->current.step_mult = p->stages[p->current.stage_num].step_mult;
328 p->current.is_d = p->current.stage_num >= 0;
329 if (p->current.is_d)
330 p->current.step_mult *= .5;
331 }
332
set_step(stream_t * p,double io_ratio)333 static void set_step(stream_t * p, double io_ratio)
334 {
335 p->step.all = (int64_t)(io_ratio * p->step_mult + .5);
336 }
337
set_step_step(stream_t * p,double io_ratio,int slew_len)338 static bool set_step_step(stream_t * p, double io_ratio, int slew_len)
339 {
340 int64_t dif;
341 int difi;
342 stream_t tmp = *p;
343 set_step(&tmp, io_ratio);
344 dif = tmp.step.all - p->step.all;
345 dif = dif < 0? dif - (slew_len >> 1) : dif + (slew_len >> 1);
346 difi = (int)dif; /* Try to avoid int64_t div. */
347 p->step_step.all = difi == dif? difi / slew_len : dif / slew_len;
348 return p->step_step.all != 0;
349 }
350
vr_set_io_ratio(rate_t * p,double io_ratio,size_t slew_len)351 static void vr_set_io_ratio(rate_t * p, double io_ratio, size_t slew_len)
352 {
353 assert(io_ratio > 0);
354 if (slew_len) {
355 if (!set_step_step(&p->current, io_ratio, p->slew_len = (int)slew_len))
356 p->slew_len = 0, p->new_io_ratio = 0, p->fadeout.step_step.all = 0;
357 else {
358 p->new_io_ratio = io_ratio;
359 if (p->fade_len)
360 set_step_step(&p->fadeout, io_ratio, p->slew_len);
361 }
362 }
363 else {
364 if (p->default_io_ratio!=0) { /* Then this is the first call to this fn. */
365 int octave = (int)floor(log(io_ratio) / M_LN2);
366 p->current.stage_num = octave < 0? -1 : min(octave, p->num_stages0-1);
367 enter_new_stage(p, 0);
368 }
369 else if (p->fade_len)
370 set_step(&p->fadeout, io_ratio);
371 set_step(&p->current, io_ratio);
372 if (p->default_io_ratio!=0) FRAC(p->current.at) = FRAC(p->current.step) >> 1;
373 p->default_io_ratio = 0;
374 }
375 }
376
do_input_stage(rate_t * p,int stage_num,int sign,int min_stage_num)377 static bool do_input_stage(rate_t * p, int stage_num, int sign, int min_stage_num)
378 {
379 int i = 0;
380 float * dest;
381 stage_t * s = &p->stages[stage_num];
382 stage_t * s1 = &p->stages[stage_num - sign];
383 float const * src = (float *)fifo_read_ptr(&s1->fifo) + HALF_FIR_LEN_2;
384 int len = shiftr(fifo_occupancy(&s1->fifo) - HALF_FIR_LEN_2 * 2, sign);
385 int already_done = fifo_occupancy(&s->fifo) - s->preload;
386 if ((len -= already_done) <= 0)
387 return false;
388 src += shiftl(already_done, sign);
389
390 dest = fifo_reserve(&s->fifo, len);
391 if (stage_num < 0) for (; i < len; ++src)
392 dest[i++] = double_fir0(src), dest[i++] = double_fir1(src);
393 else {
394 bool should_be_fast = p->stage_inc;
395 if (!s->x_fade_len && stage_num == p->switch_stage_num) {
396 p->switch_stage_num = 0;
397 if (s->is_fast != should_be_fast) {
398 s->x_fade_len = 1 << FADE_LEN_BITS, s->is_fast = should_be_fast, ++p->xfade;
399 lsx_debug("xfade level %i, inc?=%i", stage_num, p->stage_inc);
400 }
401 }
402 if (s->x_fade_len) {
403 float const * vol1 = fade_coefs + (s->x_fade_len << 1);
404 float const * vol2 = fade_coefs + (((1 << FADE_LEN_BITS) - s->x_fade_len) << 1);
405 int n = min(len, s->x_fade_len);
406 /*lsx_debug("xfade level %i, inc?=%i len=%i n=%i", stage_num, p->stage_inc, s->x_fade_len, n);*/
407 if (should_be_fast)
408 for (; i < n; vol2 += 2, vol1 -= 2, src += 2)
409 dest[i++] = *vol1 * fast_half_fir(src) + *vol2 * half_fir(src);
410 else for (; i < n; vol2 += 2, vol1 -= 2, src += 2)
411 dest[i++] = *vol2 * fast_half_fir(src) + *vol1 * half_fir(src);
412 s->x_fade_len -= n;
413 p->xfade -= !s->x_fade_len;
414 }
415 if (stage_num < min_stage_num)
416 for (; i < len; dest[i++] = fast_half_fir(src), src += 2);
417 else for (; i < len; dest[i++] = half_fir(src), src += 2);
418 }
419 if (p->flushing > 0)
420 stage_preload(s);
421 return true;
422 }
423
vr_process(rate_t * p,int olen0)424 static int vr_process(rate_t * p, int olen0)
425 {
426 assert(p->num_stages > 0);
427 if (p->default_io_ratio!=0)
428 vr_set_io_ratio(p, p->default_io_ratio, 0);
429 {
430 float * output = fifo_reserve(&p->output_fifo, olen0);
431 int j, odone0 = 0, min_stage_num = p->current.stage_num;
432 int occupancy0, max_stage_num = min_stage_num;
433 if (p->fade_len) {
434 min_stage_num = min(min_stage_num, p->fadeout.stage_num);
435 max_stage_num = max(max_stage_num, p->fadeout.stage_num);
436 }
437
438 for (j = min(min_stage_num, 0); j <= max_stage_num; ++j)
439 if (j && !do_input_stage(p, j, j < 0? -1 : 1, min_stage_num))
440 break;
441 if (p->flushing > 0)
442 p->flushing = -1;
443
444 occupancy0 = shiftl(max(0,stage_occupancy(&p->stages[max_stage_num])), max_stage_num);
445 p->current.len = shiftr(occupancy0, p->current.stage_num);
446 p->current.input = stage_read_p(&p->stages[p->current.stage_num]);
447 if (p->fade_len) {
448 p->fadeout.len = shiftr(occupancy0, p->fadeout.stage_num);
449 p->fadeout.input = stage_read_p(&p->stages[p->fadeout.stage_num]);
450 }
451
452 while (odone0 < olen0) {
453 int odone, odone2, olen = olen0 - odone0, stage_dif = 0, shift;
454 float buf[64 << 1];
455
456 olen = min(olen, (int)(AL(buf) >> 1));
457 if (p->slew_len)
458 olen = min(olen, p->slew_len);
459 else if (p->new_io_ratio!=0) {
460 set_step(&p->current, p->new_io_ratio);
461 set_step(&p->fadeout, p->new_io_ratio);
462 p->fadeout.step_step.all = p->current.step_step.all = 0;
463 p->new_io_ratio = 0;
464 }
465 if (!p->flushing && !p->fade_len && !p->xfade) {
466 if (p->current.is_d) {
467 if (INT(p->current.step) && FRAC(p->current.step))
468 stage_dif = 1, ++max_stage_num;
469 else if (!INT(p->current.step) && FRAC(p->current.step) < (1u << 31))
470 stage_dif = -1, --min_stage_num;
471 } else if (INT(p->current.step) > 1 && FRAC(p->current.step))
472 stage_dif = 1, ++max_stage_num;
473 }
474 if (stage_dif) {
475 int n = p->current.stage_num + stage_dif;
476 if (n >= p->num_stages)
477 --max_stage_num;
478 else {
479 p->stage_inc = stage_dif > 0;
480 p->fadeout = p->current;
481 p->current.stage_num += stage_dif;
482 if (!p->stage_inc)
483 p->switch_stage_num = p->current.stage_num;
484 if ((p->current.stage_num < 0 && stage_dif < 0) ||
485 (p->current.stage_num > 0 && stage_dif > 0)) {
486 stage_t * s = &p->stages[p->current.stage_num];
487 fifo_clear(&s->fifo);
488 stage_preload(s);
489 s->is_fast = false;
490 do_input_stage(p, p->current.stage_num, stage_dif, p->current.stage_num);
491 }
492 if (p->current.stage_num > 0 && stage_dif < 0) {
493 int idone = INT(p->current.at);
494 stage_t * s = &p->stages[p->current.stage_num];
495 fifo_trim_to(&s->fifo, 2 * HALF_FIR_LEN_2 + idone + (POLY_FIR_LEN_D >> 1));
496 do_input_stage(p, p->current.stage_num, 1, p->current.stage_num);
497 }
498 enter_new_stage(p, occupancy0);
499 shift = -stage_dif;
500 #define lshift(x,by) (x)=(by)>0?(x)<<(by):(x)>>-(by)
501 lshift(p->current.at.all, shift);
502 shift += p->fadeout.is_d - p->current.is_d;
503 lshift(p->current.step.all, shift);
504 lshift(p->current.step_step.all, shift);
505 p->fade_len = AL(fade_coefs) - 1;
506 lsx_debug("switch from stage %i to %i, x2 from %i to %i", p->fadeout.stage_num, p->current.stage_num, p->fadeout.is_d, p->current.is_d);
507 }
508 }
509
510 if (p->fade_len) {
511 float const * vol1 = fade_coefs + p->fade_len;
512 float const * vol2 = fade_coefs + (iAL(fade_coefs) - 1 - p->fade_len);
513 int olen2 = (olen = min(olen, p->fade_len >> 1)) << 1;
514
515 /* x2 is more fine-grained so may fail to produce a pair of samples
516 * where x1 would not (the x1 second sample is a zero so is always
517 * available). So do x2 first, then feed odone to the second one. */
518 memset(buf, 0, sizeof(*buf) * (size_t)olen2);
519 if (p->current.is_d && p->fadeout.is_d) {
520 odone = poly_fir_fade_d(&p->current, vol1,-1, buf, olen2);
521 odone2 = poly_fir_fade_d(&p->fadeout, vol2, 1, buf, odone);
522 } else if (p->current.is_d) {
523 odone = poly_fir_fade_d(&p->current, vol1,-1, buf, olen2);
524 odone2 = poly_fir_fade_u(&p->fadeout, vol2, 2, buf, odone);
525 } else {
526 assert(p->fadeout.is_d);
527 odone = poly_fir_fade_d(&p->fadeout, vol2, 1, buf, olen2);
528 odone2 = poly_fir_fade_u(&p->current, vol1,-2, buf, odone);
529 }
530 assert(odone == odone2);
531 (void)odone2;
532 p->fade_len -= odone;
533 if (!p->fade_len) {
534 if (p->stage_inc)
535 p->switch_stage_num = min_stage_num++;
536 else
537 --max_stage_num;
538 }
539 half_iir(&p->halfer, &output[odone0], buf, odone >>= 1);
540 }
541 else if (p->current.is_d) {
542 odone = poly_fir_d(&p->current, buf, olen << 1) >> 1;
543 half_iir(&p->halfer, &output[odone0], buf, odone);
544 }
545 else {
546 odone = poly_fir_u(&p->current, &output[odone0], olen);
547 if (p->num_stages0)
548 half_phase(&p->halfer, &output[odone0], odone);
549 }
550 odone0 += odone;
551 if (p->slew_len)
552 p->slew_len -= odone;
553 if (odone != olen)
554 break; /* Need more input. */
555 } {
556 int from = max(0, max_stage_num), to = min(0, min_stage_num);
557 int i, idone = shiftr(INT(p->current.at), from - p->current.stage_num);
558 INT(p->current.at) -= shiftl(idone, from - p->current.stage_num);
559 if (p->fade_len)
560 INT(p->fadeout.at) -= shiftl(idone, from - p->fadeout.stage_num);
561 for (i = from; i >= to; --i, idone <<= 1)
562 fifo_read(&p->stages[i].fifo, idone, NULL);
563 }
564 fifo_trim_by(&p->output_fifo, olen0 - odone0);
565 return odone0;
566 }
567 }
568
vr_input(rate_t * p,float const * input,size_t n)569 static float * vr_input(rate_t * p, float const * input, size_t n)
570 {
571 return fifo_write(&p->stages[0].fifo, (int)n, input);
572 }
573
vr_output(rate_t * p,float * output,size_t * n)574 static float const * vr_output(rate_t * p, float * output, size_t * n)
575 {
576 fifo_t * fifo = &p->output_fifo;
577 if (1 || !p->num_stages0)
578 return fifo_read(fifo, (int)(*n = min(*n, (size_t)fifo_occupancy(fifo))), output);
579 else { /* Ignore this complication for now. */
580 int const IIR_DELAY = 2;
581 float * ptr = fifo_read_ptr(fifo);
582 int olen = min((int)*n, max(0, fifo_occupancy(fifo) - IIR_DELAY));
583 *n = (size_t)olen;
584 if (output)
585 memcpy(output, ptr + IIR_DELAY, *n * sizeof(*output));
586 fifo_read(fifo, olen, NULL);
587 return ptr + IIR_DELAY;
588 }
589 }
590
vr_flush(rate_t * p)591 static void vr_flush(rate_t * p)
592 {
593 if (!p->flushing) {
594 stage_preload(&p->stages[0]);
595 ++p->flushing;
596 }
597 }
598
vr_close(rate_t * p)599 static void vr_close(rate_t * p)
600 {
601 int i;
602
603 fifo_delete(&p->output_fifo);
604 for (i = -1; i < p->num_stages; ++i) {
605 stage_t * s = &p->stages[i];
606 fifo_delete(&s->fifo);
607 }
608 free(p->stages - 1);
609 }
610
vr_delay(rate_t * p)611 static double vr_delay(rate_t * p)
612 {
613 return 100; /* TODO */
614 (void)p;
615 }
616
vr_sizes(size_t * shared,size_t * channel)617 static void vr_sizes(size_t * shared, size_t * channel)
618 {
619 *shared = 0;
620 *channel = sizeof(rate_t);
621 }
622
vr_create(void * channel,void * shared,double max_io_ratio,void * q_spec,void * r_spec,double scale)623 static char const * vr_create(void * channel, void * shared,double max_io_ratio,
624 void * q_spec, void * r_spec, double scale)
625 {
626 double x = max_io_ratio;
627 int n;
628 for (n = 0; x > 1; x *= .5, ++n);
629 vr_init(channel, max_io_ratio, n, scale);
630 return 0;
631 (void)shared, (void)q_spec, (void)r_spec;
632 }
633
vr_id(void)634 static char const * vr_id(void)
635 {
636 return "vr32";
637 }
638
639 typedef void (* fn_t)(void);
640 fn_t _soxr_vr32_cb[] = {
641 (fn_t)vr_input,
642 (fn_t)vr_process,
643 (fn_t)vr_output,
644 (fn_t)vr_flush,
645 (fn_t)vr_close,
646 (fn_t)vr_delay,
647 (fn_t)vr_sizes,
648 (fn_t)vr_create,
649 (fn_t)vr_set_io_ratio,
650 (fn_t)vr_id,
651 };
652