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