new function list_deep_free
[swftools.git] / lib / lame / util.c
1 /*
2  *      lame utility library source file
3  *
4  *      Copyright (c) 1999 Albert L Faber
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Library General Public
8  * License as published by the Free Software Foundation; either
9  * version 2 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14  * Library General Public License for more details.
15  *
16  * You should have received a copy of the GNU Library General Public
17  * License along with this library; if not, write to the
18  * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21
22 /* $Id: util.c,v 1.2 2006/02/09 16:56:23 kramm Exp $ */
23
24 #include <stdlib.h>
25 #include "config_static.h"
26
27 #define PRECOMPUTE
28
29 #include "util.h"
30 #include "tools.h"
31 #include <ctype.h>
32 #include <assert.h>
33 #include <stdarg.h>
34
35 #if defined(__FreeBSD__) && !defined(__alpha__)
36 # include <machine/floatingpoint.h>
37 #endif
38
39 #ifdef WITH_DMALLOC
40 #include <dmalloc.h>
41 #endif
42
43 /***********************************************************************
44 *
45 *  Global Function Definitions
46 *
47 ***********************************************************************/
48 /*empty and close mallocs in gfc */
49
50 void  freegfc ( lame_internal_flags* const gfc )   /* bit stream structure */
51 {
52     int  i;
53
54 #ifdef KLEMM_44
55     if (gfc->resample_in != NULL) {
56         resample_close(gfc->resample_in);
57         gfc->resample_in = NULL;
58     }
59     free(gfc->mfbuf[0]);
60     free(gfc->mfbuf[1]);
61 #endif
62
63     for ( i = 0 ; i <= 2*BPC; i++ )
64         if ( gfc->blackfilt[i] != NULL ) {
65             free ( gfc->blackfilt[i] );
66             gfc->blackfilt[i] = NULL;
67         }
68     if ( gfc->inbuf_old[0] ) { 
69         free ( gfc->inbuf_old[0] );
70         gfc->inbuf_old[0] = NULL;
71     }
72     if ( gfc->inbuf_old[1] ) { 
73         free ( gfc->inbuf_old[1] );
74         gfc->inbuf_old[1] = NULL;
75     }
76
77     if ( gfc->bs.buf != NULL ) {
78         free ( gfc->bs.buf );
79         gfc->bs.buf = NULL;
80     }
81
82     if ( gfc->VBR_seek_table.bag ) {
83         free ( gfc->VBR_seek_table.bag );
84     }
85     if ( gfc->ATH ) {
86         free ( gfc->ATH );
87     }
88     if ( gfc->VBR ) {
89         free ( gfc->VBR );
90     }
91     if ( gfc->PSY ) {
92         free ( gfc->PSY );
93     }
94     if ( gfc->s3_ll ) {
95         /* XXX allocated in psymodel_init() */
96         free ( gfc->s3_ll );
97     }
98     if ( gfc->s3_ss ) {
99         /* XXX allocated in psymodel_init() */
100         free ( gfc->s3_ss );
101     }
102     free ( gfc );
103 }
104
105
106
107 /*those ATH formulas are returning
108 their minimum value for input = -1*/
109
110 FLOAT8 ATHformula_GB(FLOAT8 f, FLOAT8 value)
111 {
112   /* from Painter & Spanias
113     modified by Gabriel Bouvigne to better fit the reality
114   ath =    3.640 * pow(f,-0.8)
115          - 6.800 * exp(-0.6*pow(f-3.4,2.0))
116          + 6.000 * exp(-0.15*pow(f-8.7,2.0))
117          + 0.6* 0.001 * pow(f,4.0);
118
119
120   In the past LAME was using the Painter &Spanias formula.
121   But we had some recurrent problems with HF content.
122   We measured real ATH values, and found the older formula
123   to be inacurate in the higher part. So we made this new
124   formula and this solved most of HF problematic testcases.
125   The tradeoff is that in VBR mode it increases a lot the
126   bitrate.*/
127
128
129 /*this curve can be udjusted according to the VBR scale:
130 it adjusts from something close to Painter & Spanias
131 on V9 up to Bouvigne's formula for V0. This way the VBR
132 bitrate is more balanced according to the -V value.*/
133
134   FLOAT8 ath;
135
136   if (f < -.3)
137       f=3410;
138
139   f /= 1000;  // convert to khz
140   f  = Max(0.01, f);
141   f  = Min(18.0, f);
142
143   ath =    3.640 * pow(f,-0.8)
144          - 6.800 * exp(-0.6*pow(f-3.4,2.0))
145          + 6.000 * exp(-0.15*pow(f-8.7,2.0))
146          + (0.6+0.04*value)* 0.001 * pow(f,4.0);
147   return ath;
148 }
149
150
151 /* 
152  *  Klemm 1994 and 1997. Experimental data. Sorry, data looks a little bit
153  *  dodderly. Data below 30 Hz is extrapolated from other material, above 18
154  *  kHz the ATH is limited due to the original purpose (too much noise at
155  *  ATH is not good even if it's theoretically inaudible).
156  */
157
158 FLOAT8  ATHformula_Frank( FLOAT8 freq )
159 {
160     /*
161      * one value per 100 cent = 1
162      * semitone = 1/4
163      * third = 1/12
164      * octave = 1/40 decade
165      * rest is linear interpolated, values are currently in decibel rel. 20 µPa
166      */
167     static FLOAT tab [] = {
168         /*    10.0 */  96.69, 96.69, 96.26, 95.12,
169         /*    12.6 */  93.53, 91.13, 88.82, 86.76,
170         /*    15.8 */  84.69, 82.43, 79.97, 77.48,
171         /*    20.0 */  74.92, 72.39, 70.00, 67.62,
172         /*    25.1 */  65.29, 63.02, 60.84, 59.00,
173         /*    31.6 */  57.17, 55.34, 53.51, 51.67,
174         /*    39.8 */  50.04, 48.12, 46.38, 44.66,
175         /*    50.1 */  43.10, 41.73, 40.50, 39.22,
176         /*    63.1 */  37.23, 35.77, 34.51, 32.81,
177         /*    79.4 */  31.32, 30.36, 29.02, 27.60,
178         /*   100.0 */  26.58, 25.91, 24.41, 23.01,
179         /*   125.9 */  22.12, 21.25, 20.18, 19.00,
180         /*   158.5 */  17.70, 16.82, 15.94, 15.12,
181         /*   199.5 */  14.30, 13.41, 12.60, 11.98,
182         /*   251.2 */  11.36, 10.57,  9.98,  9.43,
183         /*   316.2 */   8.87,  8.46,  7.44,  7.12,
184         /*   398.1 */   6.93,  6.68,  6.37,  6.06,
185         /*   501.2 */   5.80,  5.55,  5.29,  5.02,
186         /*   631.0 */   4.75,  4.48,  4.22,  3.98,
187         /*   794.3 */   3.75,  3.51,  3.27,  3.22,
188         /*  1000.0 */   3.12,  3.01,  2.91,  2.68,
189         /*  1258.9 */   2.46,  2.15,  1.82,  1.46,
190         /*  1584.9 */   1.07,  0.61,  0.13, -0.35,
191         /*  1995.3 */  -0.96, -1.56, -1.79, -2.35,
192         /*  2511.9 */  -2.95, -3.50, -4.01, -4.21,
193         /*  3162.3 */  -4.46, -4.99, -5.32, -5.35,
194         /*  3981.1 */  -5.13, -4.76, -4.31, -3.13,
195         /*  5011.9 */  -1.79,  0.08,  2.03,  4.03,
196         /*  6309.6 */   5.80,  7.36,  8.81, 10.22,
197         /*  7943.3 */  11.54, 12.51, 13.48, 14.21,
198         /* 10000.0 */  14.79, 13.99, 12.85, 11.93,
199         /* 12589.3 */  12.87, 15.19, 19.14, 23.69,
200         /* 15848.9 */  33.52, 48.65, 59.42, 61.77,
201         /* 19952.6 */  63.85, 66.04, 68.33, 70.09,
202         /* 25118.9 */  70.66, 71.27, 71.91, 72.60,
203     };
204     FLOAT8    freq_log;
205     unsigned  index;
206     
207     if (freq < -.3)
208         freq=3758;
209
210     if ( freq <    10. ) freq =    10.;
211     if ( freq > 29853. ) freq = 29853.;
212     
213     freq_log = 40. * log10 (0.1 * freq);   /* 4 steps per third, starting at 10 Hz */
214     index    = (unsigned) freq_log;
215     assert ( index < sizeof(tab)/sizeof(*tab) );
216     return tab [index] * (1 + index - freq_log) + tab [index+1] * (freq_log - index);
217 }
218
219
220
221 /* ATHformula_jd - Compute ATH at a given frequency from experimental data.
222                    Below 15000 Hz, this ATH curve is based on data merged from
223                    various existing sources.  New experimental data covers
224                    frequencies above 15000 Hz.  -jd
225      in: freq    (Hz)
226 returns: ATH value at freq in dB, or minimum ATH value if input freq is -1
227 design notes:
228    Above 15000 Hz, my data indicates roughly 10 dB between the edge of
229    ready detection, and statistical indistinguishability.  To provide a
230    balance between my data, and ATH data from other sources, roughly 5 dB
231    is added above 15000 Hz, except at frequencies above 20500 Hz.  The ATH
232    of 21000+ Hz frequencies is decreased by 5 dB, to reduce the possibility
233    of extra distortion that some output systems exhibit when given a contrived
234    sample with an intense, but hardly audible frequency.
235 */
236 FLOAT8
237 ATHformula_jd( FLOAT8 freq )
238 {
239   int i;
240   int ifreq;
241   int at_i;
242   int tstep;
243   int xtrans;
244   FLOAT coeff[3];
245   FLOAT8 rval;
246
247   const FLOAT ath_lt100[]       /* 20 - 120 Hz (for computing 0 - 100 Hz) */
248     = { 74.0, 47.9, 35.2, 28.1, 23.5,    20.4 };
249
250   const FLOAT ath_lt500[]       /* 100 - 600 Hz (for 100 - 500 Hz) */
251     = { 23.5, 13.4, 9.4, 6.9, 5.9,    5.0 };
252
253   const FLOAT ath_gt500[]       /* 500 Hz and above */
254     = { /*   500 */        5.9,  3.2,  1.6, -0.7,
255         /*  2500 */ -2.7, -4.5, -5.2, -4.8, -3.7,
256         /*  5000 */ -1.6,  1.5,  3.8,  5.3,  6.8,
257         /*  7500 */  8.2,  9.5, 10.5, 11.3, 11.8,
258         /* 10000 */ 12.1, 12.2, 12.4, 12.4, 12.4,
259         /* 12500 */ 12.8, 13.5, 14.9, 16.8, 19.0,
260         /* 15000 */ 23.0, 27.0, 33.0, 36.5, 39.5,
261         /* 17500 */ 43.5, 51.5, 58.5, 65.0, 71.5,
262         /* 20000 */ 78.0, 79.5, 80.0, 80.5, 80.5,    80.5 };
263
264   const FLOAT *ath_table[4];
265   const int ath_table_step[4] =     {   20,  100,   500,   500 };
266   const FLOAT ath_table_xratio[4] = { 0.05, 0.01, 0.002, 0.002 };
267
268   ath_table[0] = ath_lt100;
269   ath_table[1] = ath_lt500;
270   ath_table[3] = ath_gt500;
271
272   if( freq >= -0.5 && freq < 22000 ) {
273       ifreq = (int) freq;
274       at_i = ( (((99 - ifreq) >> (sizeof(int) * 8 - 1)) & 0x1)
275                | (((499 - ifreq) >> (sizeof(int) * 8 - 2)) & 0x3) );
276       tstep = ath_table_step[at_i];
277       
278       i = (ifreq / tstep);
279       i -= 2;
280       if( i >= 0 ) {
281           qinterp_cf_42( ath_table[at_i] + i, coeff );
282           xtrans = (i + 2) * tstep;
283       } else {                  /* leading edge */
284           qinterp_cf_3( ath_table[at_i], coeff );
285           xtrans = tstep;
286       }
287       rval = qinterp_eval( coeff, freq, (FLOAT)xtrans, ath_table_xratio[at_i]);
288       return(rval);
289   } else if( freq < 0 ) {
290       return(-5.2);             /* minimum value from table */
291   } else {
292       return( ath_gt500[ 22000 / 500 - 1 ] ); /* 22kHz ATH used for 22kHz+ */
293   }
294 }
295
296
297
298 FLOAT8 ATHformula(FLOAT8 f,lame_global_flags *gfp)
299 {
300   switch(gfp->ATHtype)
301     {
302     case 0:
303       return ATHformula_GB(f, 9);
304     case 1:
305       return ATHformula_Frank(f);
306     case 2:
307       return ATHformula_GB(f, 0);
308     case 3:
309       return ATHformula_GB(f, 1) +6;     /*modification of GB formula by Roel*/
310     case 4:
311       if (!(gfp->VBR == vbr_off || gfp->VBR == vbr_abr)) /*this case should be used with true vbr only*/
312         return ATHformula_GB(f,gfp->VBR_q);
313     case 5:
314       return ATHformula_jd(f);
315     }
316
317   return ATHformula_GB(f, 0);
318 }
319
320 /* see for example "Zwicker: Psychoakustik, 1982; ISBN 3-540-11401-7 */
321 FLOAT8 freq2bark(FLOAT8 freq)
322 {
323   /* input: freq in hz  output: barks */
324     if (freq<0) freq=0;
325     freq = freq * 0.001;
326     return 13.0*atan(.76*freq) + 3.5*atan(freq*freq/(7.5*7.5));
327 }
328
329 /* see for example "Zwicker: Psychoakustik, 1982; ISBN 3-540-11401-7 */
330 FLOAT8 freq2cbw(FLOAT8 freq)
331 {
332   /* input: freq in hz  output: critical band width */
333     freq = freq * 0.001;
334     return 25+75*pow(1+1.4*(freq*freq),0.69);
335 }
336
337
338
339
340
341
342 /***********************************************************************
343  * compute bitsperframe and mean_bits for a layer III frame 
344  **********************************************************************/
345 void getframebits(const lame_global_flags * gfp, int *bitsPerFrame, int *mean_bits) 
346 {
347   lame_internal_flags *gfc=gfp->internal_flags;
348   int  whole_SpF;  /* integral number of Slots per Frame without padding */
349   int  bit_rate;
350   
351   /* get bitrate in kbps [?] */
352   if (gfc->bitrate_index) 
353     bit_rate = bitrate_table[gfp->version][gfc->bitrate_index];
354   else
355     bit_rate = gfp->brate;
356   assert ( bit_rate <= 550 );
357   
358   // bytes_per_frame = bitrate * 1000 / ( gfp->out_samplerate / (gfp->version == 1  ?  1152  :  576 )) / 8;
359   // bytes_per_frame = bitrate * 1000 / gfp->out_samplerate * (gfp->version == 1  ?  1152  :  576 ) / 8;
360   // bytes_per_frame = bitrate * ( gfp->version == 1  ?  1152/8*1000  :  576/8*1000 ) / gfp->out_samplerate;
361   
362   whole_SpF = (gfp->version+1)*72000*bit_rate / gfp->out_samplerate;
363   
364   /* main encoding routine toggles padding on and off */
365   /* one Layer3 Slot consists of 8 bits */
366   *bitsPerFrame = 8 * (whole_SpF + gfc->padding);
367   
368   // sideinfo_len
369   *mean_bits = (*bitsPerFrame - 8*gfc->sideinfo_len) / gfc->mode_gr;
370 }
371
372
373
374
375 #define ABS(A) (((A)>0) ? (A) : -(A))
376
377 int FindNearestBitrate(
378 int bRate,        /* legal rates from 32 to 448 */
379 int version,      /* MPEG-1 or MPEG-2 LSF */
380 int samplerate)   /* convert bitrate in kbps to index */
381 {
382     int  bitrate = 0;
383     int  i;
384   
385     for ( i = 1; i <= 14; i++ )
386         if ( ABS (bitrate_table[version][i] - bRate) < ABS (bitrate - bRate) )
387             bitrate = bitrate_table [version] [i];
388             
389     return bitrate;
390 }
391
392
393 /* map frequency to a valid MP3 sample frequency
394  *
395  * Robert.Hegemann@gmx.de 2000-07-01
396  */
397 int map2MP3Frequency(int freq)
398 {
399     if (freq <=  8000) return  8000;
400     if (freq <= 11025) return 11025;
401     if (freq <= 12000) return 12000;
402     if (freq <= 16000) return 16000;
403     if (freq <= 22050) return 22050;
404     if (freq <= 24000) return 24000;
405     if (freq <= 32000) return 32000;
406     if (freq <= 44100) return 44100;
407     
408     return 48000;
409 }
410
411 int BitrateIndex(
412 int bRate,        /* legal rates from 32 to 448 kbps */
413 int version,      /* MPEG-1 or MPEG-2/2.5 LSF */
414 int samplerate)   /* convert bitrate in kbps to index */
415 {
416     int  i;
417
418     for ( i = 0; i <= 14; i++)
419         if ( bitrate_table [version] [i] == bRate )
420             return i;
421             
422     return -1;
423 }
424
425 /* convert samp freq in Hz to index */
426
427 int SmpFrqIndex ( int sample_freq, int* const version )
428 {
429     switch ( sample_freq ) {
430     case 44100: *version = 1; return  0;
431     case 48000: *version = 1; return  1;
432     case 32000: *version = 1; return  2;
433     case 22050: *version = 0; return  0;
434     case 24000: *version = 0; return  1;
435     case 16000: *version = 0; return  2;
436     case 11025: *version = 0; return  0;
437     case 12000: *version = 0; return  1;
438     case  8000: *version = 0; return  2;
439     default:    *version = 0; return -1;
440     }
441 }
442
443
444 /*****************************************************************************
445 *
446 *  End of bit_stream.c package
447 *
448 *****************************************************************************/
449
450 /* reorder the three short blocks By Takehiro TOMINAGA */
451 /*
452   Within each scalefactor band, data is given for successive
453   time windows, beginning with window 0 and ending with window 2.
454   Within each window, the quantized values are then arranged in
455   order of increasing frequency...
456 */
457 void freorder(int scalefac_band[],FLOAT8 ix_orig[576]) {
458   int i,sfb, window, j=0;
459   FLOAT8 ix[576];
460   for (sfb = 0; sfb < SBMAX_s; sfb++) {
461     int start = scalefac_band[sfb];
462     int end   = scalefac_band[sfb + 1];
463     for (window = 0; window < 3; window++) {
464       for (i = start; i < end; ++i) {
465         ix[j++] = ix_orig[3*i+window];
466       }
467     }
468   }
469   memcpy(ix_orig,ix,576*sizeof(FLOAT8));
470 }
471
472
473
474
475
476
477
478 #ifndef KLEMM_44
479
480
481 /* resampling via FIR filter, blackman window */
482 inline static FLOAT8 blackman(FLOAT8 x,FLOAT8 fcn,int l)
483 {
484   /* This algorithm from:
485 SIGNAL PROCESSING ALGORITHMS IN FORTRAN AND C
486 S.D. Stearns and R.A. David, Prentice-Hall, 1992
487   */
488   FLOAT8 bkwn,x2;
489   FLOAT8 wcn = (PI * fcn);
490   
491   x /= l;
492   if (x<0) x=0;
493   if (x>1) x=1;
494   x2 = x - .5;
495
496   bkwn = 0.42 - 0.5*cos(2*x*PI)  + 0.08*cos(4*x*PI);
497   if (fabs(x2)<1e-9) return wcn/PI;
498   else 
499     return  (  bkwn*sin(l*wcn*x2)  / (PI*l*x2)  );
500
501
502 }
503
504 /* gcd - greatest common divisor */
505 /* Joint work of Euclid and M. Hendry */
506
507 int gcd ( int i, int j )
508 {
509 //    assert ( i > 0  &&  j > 0 );
510     return j ? gcd(j, i % j) : i;
511 }
512
513
514
515 /* copy in new samples from in_buffer into mfbuf, with resampling & scaling 
516    if necessary.  n_in = number of samples from the input buffer that
517    were used.  n_out = number of samples copied into mfbuf  */
518
519 void fill_buffer(lame_global_flags *gfp,
520                  sample_t *mfbuf[2],
521                  sample_t *in_buffer[2],
522                  int nsamples, int *n_in, int *n_out)
523 {
524     lame_internal_flags *gfc = gfp->internal_flags;
525     int ch,i;
526
527     /* copy in new samples into mfbuf, with resampling if necessary */
528     if (gfc->resample_ratio != 1.0) {
529         for (ch = 0; ch < gfc->channels_out; ch++) {
530             *n_out =
531                 fill_buffer_resample(gfp, &mfbuf[ch][gfc->mf_size],
532                                      gfp->framesize, in_buffer[ch],
533                                      nsamples, n_in, ch);
534         }
535     }
536     else {
537         *n_out = Min(gfp->framesize, nsamples);
538         *n_in = *n_out;
539         for (i = 0; i < *n_out; ++i) {
540             mfbuf[0][gfc->mf_size + i] = in_buffer[0][i];
541             if (gfc->channels_out == 2)
542                 mfbuf[1][gfc->mf_size + i] = in_buffer[1][i];
543         }
544     }
545
546     /* user selected scaling of the samples */
547     if (gfp->scale != 0 && gfp->scale != 1.0) {
548         for (i=0 ; i<*n_out; ++i) {
549             mfbuf[0][gfc->mf_size+i] *= gfp->scale;
550             if (gfc->channels_out == 2)
551                 mfbuf[1][gfc->mf_size + i] *= gfp->scale;
552         }
553     }
554
555     /* user selected scaling of the channel 0 (left) samples */
556     if (gfp->scale_left != 0 && gfp->scale_left != 1.0) {
557         for (i=0 ; i<*n_out; ++i) {
558             mfbuf[0][gfc->mf_size+i] *= gfp->scale_left;
559         }
560     }
561
562     /* user selected scaling of the channel 1 (right) samples */
563     if (gfc->channels_out == 2) {
564         if (gfp->scale_right != 0 && gfp->scale_right != 1.0) {
565             for (i=0 ; i<*n_out; ++i) {
566                 mfbuf[1][gfc->mf_size + i] *= gfp->scale_right;
567             }
568         }
569     }
570 }
571     
572
573
574
575 int fill_buffer_resample(
576        lame_global_flags *gfp,
577        sample_t *outbuf,
578        int desired_len,
579        sample_t *inbuf,
580        int len,
581        int *num_used,
582        int ch) 
583 {
584
585   
586   lame_internal_flags *gfc=gfp->internal_flags;
587   int BLACKSIZE;
588   FLOAT8 offset,xvalue;
589   int i,j=0,k;
590   int filter_l;
591   FLOAT8 fcn,intratio;
592   FLOAT *inbuf_old;
593   int bpc;   /* number of convolution functions to pre-compute */
594   bpc = gfp->out_samplerate/gcd(gfp->out_samplerate,gfp->in_samplerate);
595   if (bpc>BPC) bpc = BPC;
596
597   intratio=( fabs(gfc->resample_ratio - floor(.5+gfc->resample_ratio)) < .0001 );
598   fcn = 1.00/gfc->resample_ratio;
599   if (fcn>1.00) fcn=1.00;
600   filter_l = gfp->quality < 7 ? 31 : 7;
601   filter_l = 31;
602   if (0==filter_l % 2 ) --filter_l;/* must be odd */
603   filter_l += intratio;            /* unless resample_ratio=int, it must be even */
604
605
606   BLACKSIZE = filter_l+1;  /* size of data needed for FIR */
607   
608   if ( gfc->fill_buffer_resample_init == 0 ) {
609     gfc->inbuf_old[0]=calloc(BLACKSIZE,sizeof(gfc->inbuf_old[0][0]));
610     gfc->inbuf_old[1]=calloc(BLACKSIZE,sizeof(gfc->inbuf_old[0][0]));
611     for (i=0; i<=2*bpc; ++i)
612       gfc->blackfilt[i]=calloc(BLACKSIZE,sizeof(gfc->blackfilt[0][0]));
613
614     gfc->itime[0]=0;
615     gfc->itime[1]=0;
616
617     /* precompute blackman filter coefficients */
618     for ( j = 0; j <= 2*bpc; j++ ) {
619         FLOAT8 sum = 0.; 
620         offset = (j-bpc) / (2.*bpc);
621         for ( i = 0; i <= filter_l; i++ ) 
622             sum += 
623             gfc->blackfilt[j][i]  = blackman(i-offset,fcn,filter_l);
624         for ( i = 0; i <= filter_l; i++ ) 
625           gfc->blackfilt[j][i] /= sum;
626     }
627     gfc->fill_buffer_resample_init = 1;
628   }
629   
630   inbuf_old=gfc->inbuf_old[ch];
631
632   /* time of j'th element in inbuf = itime + j/ifreq; */
633   /* time of k'th element in outbuf   =  j/ofreq */
634   for (k=0;k<desired_len;k++) {
635     FLOAT8 time0;
636     int joff;
637
638     time0 = k*gfc->resample_ratio;       /* time of k'th output sample */
639     j = floor( time0 -gfc->itime[ch]  );
640
641     /* check if we need more input data */
642     if ((filter_l + j - filter_l/2) >= len) break;
643
644     /* blackman filter.  by default, window centered at j+.5(filter_l%2) */
645     /* but we want a window centered at time0.   */
646     offset = ( time0 -gfc->itime[ch] - (j + .5*(filter_l%2)));
647     assert(fabs(offset)<=.501);
648
649     /* find the closest precomputed window for this offset: */
650     joff = floor((offset*2*bpc) + bpc +.5);
651
652     xvalue = 0.;
653     for (i=0 ; i<=filter_l ; ++i) {
654       int j2 = i+j-filter_l/2;
655       int y;
656       assert(j2<len);
657       assert(j2+BLACKSIZE >= 0);
658       y = (j2<0) ? inbuf_old[BLACKSIZE+j2] : inbuf[j2];
659 #define PRECOMPUTE
660 #ifdef PRECOMPUTE
661       xvalue += y*gfc->blackfilt[joff][i];
662 #else
663       xvalue += y*blackman(i-offset,fcn,filter_l);  /* very slow! */
664 #endif
665     }
666     outbuf[k]=xvalue;
667   }
668
669   
670   /* k = number of samples added to outbuf */
671   /* last k sample used data from [j-filter_l/2,j+filter_l-filter_l/2]  */
672
673   /* how many samples of input data were used:  */
674   *num_used = Min(len,filter_l+j-filter_l/2);
675
676   /* adjust our input time counter.  Incriment by the number of samples used,
677    * then normalize so that next output sample is at time 0, next
678    * input buffer is at time itime[ch] */
679   gfc->itime[ch] += *num_used - k*gfc->resample_ratio;
680
681   /* save the last BLACKSIZE samples into the inbuf_old buffer */
682   if (*num_used >= BLACKSIZE) {
683       for (i=0;i<BLACKSIZE;i++)
684           inbuf_old[i]=inbuf[*num_used + i -BLACKSIZE];
685   }else{
686       /* shift in *num_used samples into inbuf_old  */
687        int n_shift = BLACKSIZE-*num_used;  /* number of samples to shift */
688
689        /* shift n_shift samples by *num_used, to make room for the
690         * num_used new samples */
691        for (i=0; i<n_shift; ++i ) 
692            inbuf_old[i] = inbuf_old[i+ *num_used];
693
694        /* shift in the *num_used samples */
695        for (j=0; i<BLACKSIZE; ++i, ++j ) 
696            inbuf_old[i] = inbuf[j];
697
698        assert(j==*num_used);
699   }
700   return k;  /* return the number samples created at the new samplerate */
701 }
702
703
704 #endif /* ndef KLEMM_44 */
705
706
707
708 /***********************************************************************
709 *
710 *  Message Output
711 *
712 ***********************************************************************/
713 void  lame_debugf (const lame_internal_flags *gfc, const char* format, ... )
714 {
715     va_list  args;
716
717     va_start ( args, format );
718
719     if ( gfc->report.debugf != NULL ) {
720         gfc->report.debugf( format, args );
721     } else {
722         (void) vfprintf ( stderr, format, args );
723         fflush ( stderr );      /* an debug function should flush immediately */
724     }
725
726     va_end   ( args );
727 }
728
729
730 void  lame_msgf (const lame_internal_flags *gfc, const char* format, ... )
731 {
732     va_list  args;
733
734     va_start ( args, format );
735    
736     if ( gfc->report.msgf != NULL ) {
737         gfc->report.msgf( format, args );
738     } else {
739         (void) vfprintf ( stderr, format, args );
740         fflush ( stderr );     /* we print to stderr, so me may want to flush */
741     }
742
743     va_end   ( args );
744 }
745
746
747 void  lame_errorf (const lame_internal_flags *gfc, const char* format, ... )
748 {
749     va_list  args;
750
751     va_start ( args, format );
752     
753     if ( gfc->report.errorf != NULL ) {
754         gfc->report.errorf( format, args );
755     } else {
756         (void) vfprintf ( stderr, format, args );
757         fflush   ( stderr );    /* an error function should flush immediately */
758     }
759
760     va_end   ( args );
761 }
762
763
764
765 /***********************************************************************
766  *
767  *      routines to detect CPU specific features like 3DNow, MMX, SIMD
768  *
769  *  donated by Frank Klemm
770  *  added Robert Hegemann 2000-10-10
771  *
772  ***********************************************************************/
773
774 int  has_i387 ( void )
775 {
776 #ifdef HAVE_NASM 
777     return 1;
778 #else
779     return 0;   /* don't know, assume not */
780 #endif
781 }    
782
783 int  has_MMX ( void )
784 {
785 #ifdef HAVE_NASM 
786     extern int has_MMX_nasm ( void );
787     return has_MMX_nasm ();
788 #else
789     return 0;   /* don't know, assume not */
790 #endif
791 }    
792
793 int  has_3DNow ( void )
794 {
795 #ifdef HAVE_NASM 
796     extern int has_3DNow_nasm ( void );
797     return has_3DNow_nasm ();
798 #else
799     return 0;   /* don't know, assume not */
800 #endif
801 }    
802
803 int  has_SIMD ( void )
804 {
805 #ifdef HAVE_NASM 
806     extern int has_SIMD_nasm ( void );
807     return has_SIMD_nasm ();
808 #else
809     return 0;   /* don't know, assume not */
810 #endif
811 }    
812
813 int  has_SIMD2 ( void )
814 {
815 #ifdef HAVE_NASM 
816     extern int has_SIMD2_nasm ( void );
817     return has_SIMD2_nasm ();
818 #else
819     return 0;   /* don't know, assume not */
820 #endif
821 }    
822
823 /***********************************************************************
824  *
825  *  some simple statistics
826  *
827  *  bitrate index 0: free bitrate -> not allowed in VBR mode
828  *  : bitrates, kbps depending on MPEG version
829  *  bitrate index 15: forbidden
830  *
831  *  mode_ext:
832  *  0:  LR
833  *  1:  LR-i
834  *  2:  MS
835  *  3:  MS-i
836  *
837  ***********************************************************************/
838  
839 void updateStats( lame_internal_flags * const gfc )
840 {
841     assert ( gfc->bitrate_index < 16u );
842     assert ( gfc->mode_ext      <  4u );
843     
844     /* count bitrate indices */
845     gfc->bitrate_stereoMode_Hist [gfc->bitrate_index] [4] ++;
846     
847     /* count 'em for every mode extension in case of 2 channel encoding */
848     if (gfc->channels_out == 2)
849         gfc->bitrate_stereoMode_Hist [gfc->bitrate_index] [gfc->mode_ext]++;
850 }
851
852
853
854 /*  caution: a[] will be resorted!!
855  */
856 int select_kth_int(int a[], int N, int k)
857 {
858     int i, j, l, r, v, w;
859     
860     l = 0;
861     r = N-1;
862     while (r > l) {
863         v = a[r];
864         i = l-1;
865         j = r;
866         for (;;) {
867             while (a[++i] < v) /*empty*/;
868             while (a[--j] > v) /*empty*/;
869             if (i >= j) 
870                 break;
871             /* swap i and j */
872             w = a[i];
873             a[i] = a[j];
874             a[j] = w;
875         }
876         /* swap i and r */
877         w = a[i];
878         a[i] = a[r];
879         a[r] = w;
880         if (i >= k) 
881             r = i-1;
882         if (i <= k) 
883             l = i+1;
884     }
885     return a[k];
886 }
887
888
889
890 void disable_FPE(void) {
891 /* extremly system dependent stuff, move to a lib to make the code readable */
892 /*==========================================================================*/
893
894
895
896     /*
897      *  Disable floating point exceptions
898      */
899
900
901
902
903 #if defined(__FreeBSD__) && !defined(__alpha__)
904     {
905         /* seet floating point mask to the Linux default */
906         fp_except_t mask;
907         mask = fpgetmask();
908         /* if bit is set, we get SIGFPE on that error! */
909         fpsetmask(mask & ~(FP_X_INV | FP_X_DZ));
910         /*  DEBUGF("FreeBSD mask is 0x%x\n",mask); */
911     }
912 #endif
913
914 #if defined(__riscos__) && !defined(ABORTFP)
915     /* Disable FPE's under RISC OS */
916     /* if bit is set, we disable trapping that error! */
917     /*   _FPE_IVO : invalid operation */
918     /*   _FPE_DVZ : divide by zero */
919     /*   _FPE_OFL : overflow */
920     /*   _FPE_UFL : underflow */
921     /*   _FPE_INX : inexact */
922     DisableFPETraps(_FPE_IVO | _FPE_DVZ | _FPE_OFL);
923 #endif
924
925     /*
926      *  Debugging stuff
927      *  The default is to ignore FPE's, unless compiled with -DABORTFP
928      *  so add code below to ENABLE FPE's.
929      */
930
931 #if defined(ABORTFP)
932 #if defined(_MSC_VER)
933     {
934
935    /* set affinity to a single CPU.  Fix for EAC/lame on SMP systems from
936      "Todd Richmond" <todd.richmond@openwave.com> */
937     SYSTEM_INFO si;
938     GetSystemInfo(&si);
939     SetProcessAffinityMask(GetCurrentProcess(), si.dwActiveProcessorMask);
940
941 #include <float.h>
942         unsigned int mask;
943         mask = _controlfp(0, 0);
944         mask &= ~(_EM_OVERFLOW | _EM_UNDERFLOW | _EM_ZERODIVIDE | _EM_INVALID);
945         mask = _controlfp(mask, _MCW_EM);
946     }
947 #elif defined(__CYGWIN__)
948 #  define _FPU_GETCW(cw) __asm__ ("fnstcw %0" : "=m" (*&cw))
949 #  define _FPU_SETCW(cw) __asm__ ("fldcw %0" : : "m" (*&cw))
950
951 #  define _EM_INEXACT     0x00000020 /* inexact (precision) */
952 #  define _EM_UNDERFLOW   0x00000010 /* underflow */
953 #  define _EM_OVERFLOW    0x00000008 /* overflow */
954 #  define _EM_ZERODIVIDE  0x00000004 /* zero divide */
955 #  define _EM_INVALID     0x00000001 /* invalid */
956     {
957         unsigned int mask;
958         _FPU_GETCW(mask);
959         /* Set the FPU control word to abort on most FPEs */
960         mask &= ~(_EM_OVERFLOW | _EM_ZERODIVIDE | _EM_INVALID);
961         _FPU_SETCW(mask);
962     }
963 # elif defined(__linux__)
964     {
965
966 #  include <fpu_control.h>
967 #  ifndef _FPU_GETCW
968 #  define _FPU_GETCW(cw) __asm__ ("fnstcw %0" : "=m" (*&cw))
969 #  endif
970 #  ifndef _FPU_SETCW
971 #  define _FPU_SETCW(cw) __asm__ ("fldcw %0" : : "m" (*&cw))
972 #  endif
973
974         /* 
975          * Set the Linux mask to abort on most FPE's
976          * if bit is set, we _mask_ SIGFPE on that error!
977          *  mask &= ~( _FPU_MASK_IM | _FPU_MASK_ZM | _FPU_MASK_OM | _FPU_MASK_UM );
978          */
979
980         unsigned int mask;
981         _FPU_GETCW(mask);
982         mask &= ~(_FPU_MASK_IM | _FPU_MASK_ZM | _FPU_MASK_OM);
983         _FPU_SETCW(mask);
984     }
985 #endif
986 #endif /* ABORTFP */
987 }
988
989
990 /* end of util.c */