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