00001
00006 #include "bpm/bpm_wf.h"
00007 #include "bpm/bpm_dsp.h"
00008
00009
00010
00011 void cdft(int, int, double *, int *, double *);
00012 void rdft(int, int, double *, int *, double *);
00013
00014
00015
00016 static int *_fft_work_area = NULL;
00017 static int _fft_work_area_length = 0;
00018 static double *_fft_sc_table = NULL;
00019 static int _fft_sc_table_length = 0;
00020 static double *_fft_data = NULL;
00021 static int _fft_data_length = 0;
00022
00023
00024
00025 int _is_pow2( int n ) {
00026 int p = 0, r = 0;
00027 do {
00028 r = n % 2; n /= 2; p++;
00029 } while ( r == 0 && n > 1 );
00030 if( r != 0 ) return FALSE;
00031 return p;
00032 }
00033
00034
00035
00036 int _check_fft_buffers( int ns ) {
00037
00038
00039 int nw = 2+(1<<(int)(log(ns/2+0.5)/log(2))/2);
00040 int nt = ns/2;
00041
00042
00043 if ( _fft_work_area ) {
00044 if ( _fft_work_area_length < nw ) {
00045 bpm_warning( "FFT work buffer to small, increasing size...", __FILE__, __LINE__ );
00046 free( _fft_work_area );
00047 _fft_work_area = (int*) calloc( nw, sizeof(int) );
00048 if ( ! _fft_work_area ) {
00049 bpm_error( "Cannot allocate memory for FFT work buffer", __FILE__, __LINE__ );
00050 return BPM_FAILURE;
00051 }
00052 _fft_work_area_length = nw;
00053 }
00054 } else {
00055 bpm_warning( "Allocating FFT work buffer, no fft_initialise() found", __FILE__, __LINE__ );
00056 _fft_work_area = (int*) calloc( nw, sizeof(int) );
00057 if ( ! _fft_work_area ) {
00058 bpm_error( "Cannot allocate memory for FFT work buffer", __FILE__, __LINE__ );
00059 return BPM_FAILURE;
00060 }
00061 _fft_work_area_length = nw;
00062 }
00063
00064
00065 if ( _fft_sc_table ) {
00066 if ( _fft_sc_table_length < nt ) {
00067 bpm_warning( "FFT sin/cos table too small, increasing size...", __FILE__, __LINE__ );
00068 free( _fft_sc_table );
00069 _fft_sc_table = (double*) calloc( nt, sizeof(double) );
00070 if ( ! _fft_sc_table ) {
00071 bpm_error( "Cannot allocate memory for FFT sin/cos table", __FILE__, __LINE__ );
00072 return BPM_FAILURE;
00073 }
00074 _fft_sc_table_length = nt;
00075 }
00076 } else {
00077 bpm_warning( "Allocating FFT sin/cos table buffer, no fft_initialise() found",
00078 __FILE__, __LINE__ );
00079 _fft_sc_table = (double*) calloc( nt, sizeof(double) );
00080 if ( ! _fft_sc_table ) {
00081 bpm_error( "Cannot allocate memory for FFT sin/cos table", __FILE__, __LINE__ );
00082 return BPM_FAILURE;
00083 }
00084 _fft_sc_table_length = nt;
00085 }
00086
00087
00088 if ( _fft_data ) {
00089 if ( _fft_data_length < 2*ns ) {
00090 bpm_warning( "FFT data buffer length too small, increasing size...", __FILE__, __LINE__ );
00091 free( _fft_data );
00092 _fft_data = (double*) calloc( 2*ns, sizeof(double) );
00093 if ( ! _fft_data ) {
00094 bpm_error( "Cannot allocate memory for FFT data buffer", __FILE__, __LINE__ );
00095 return BPM_FAILURE;
00096 }
00097 _fft_data_length = 2*ns;
00098 }
00099 } else {
00100 bpm_warning( "Allocating FFT data buffer, no fft_initialise() found",
00101 __FILE__, __LINE__ );
00102 _fft_data = (double*) calloc( 2*ns, sizeof(double) );
00103 if ( ! _fft_data ) {
00104 bpm_error( "Cannot allocate memory for FFT data buffer", __FILE__, __LINE__ );
00105 return BPM_FAILURE;
00106 }
00107 _fft_data_length = 2*ns;
00108 }
00109
00110 return BPM_SUCCESS;
00111 }
00112
00113
00114
00115
00116 int fft_gen_tables( void ) {
00117
00118 if ( _fft_work_area ) {
00119 _fft_work_area[0] = 0;
00120 } else {
00121 bpm_error( "FFT work buffer not allocated, cannot regenerate tables", __FILE__, __LINE__ );
00122 return BPM_FAILURE;
00123 }
00124
00125 return BPM_SUCCESS;
00126 }
00127
00128
00129
00130 int fft_initialise( int ns ) {
00131
00132
00133 int nw = 2+(1<<(int)(log(ns/2+0.5)/log(2))/2);
00134 int nt = ns/2;
00135
00136 if ( _fft_work_area || _fft_sc_table || _fft_data ) {
00137 bpm_error( "FFT buffers alread initialised, please cleanup first with fft_cleanup()",
00138 __FILE__, __LINE__ );
00139 return BPM_FAILURE;
00140 }
00141
00142 _fft_work_area = (int*) calloc( nw, sizeof(int) );
00143 _fft_sc_table = (double*) calloc( nt, sizeof(double) );
00144 _fft_data = (double*) calloc( 2*ns, sizeof(double) );
00145
00146 if ( ! _fft_work_area || ! _fft_sc_table || ! _fft_data ) {
00147 bpm_error( "Failed to allocate memory for the FFT buffers in fft_initialise",
00148 __FILE__, __LINE__ );
00149 return BPM_FAILURE;
00150 }
00151
00152 _fft_work_area_length = nw;
00153 _fft_sc_table_length = nt;
00154 _fft_data_length = 2*ns;
00155
00156
00157
00158 return fft_gen_tables();
00159 }
00160
00161
00162
00163 void fft_cleanup( void ) {
00164
00165 if( _fft_work_area ) free( _fft_work_area );
00166 if( _fft_sc_table ) free( _fft_sc_table );
00167 if( _fft_data ) free( _fft_data );
00168
00169 _fft_work_area_length = 0;
00170 _fft_sc_table_length = 0;
00171 _fft_data_length = 0;
00172
00173 return;
00174 }
00175
00176
00177
00178 int complexfft( complexwf_t *z, int fft_mode ) {
00179
00180
00181
00182 int i = 0;
00183
00184 if ( ! z ) {
00185 bpm_error( "Invalid pointers in complexfft(...)", __FILE__, __LINE__ );
00186 return BPM_FAILURE;
00187 }
00188
00189 if ( ! _is_pow2(z->ns) ) {
00190 bpm_warning( "Number of samples is not of the form 2^n, may run into trouble with FFT !",
00191 __FILE__, __LINE__ );
00192 }
00193
00194 if ( _check_fft_buffers( z->ns ) == BPM_FAILURE ) {
00195 bpm_error( "Error checking FFT buffers in complexfft()", __FILE__, __LINE__ );
00196 return BPM_FAILURE;
00197 }
00198
00199
00200 for ( i=0; i<z->ns; i++ ) {
00201 _fft_data[2*i] = z->wf[i].re;
00202 _fft_data[2*i+1] = z->wf[i].im;
00203 }
00204
00205
00206 switch ( fft_mode ) {
00207 case FFT_FORWARD:
00208 cdft( 2*z->ns, 1, _fft_data, _fft_work_area, _fft_sc_table );
00209 break;
00210 case FFT_BACKWARD:
00211 cdft( 2*z->ns, -1, _fft_data, _fft_work_area, _fft_sc_table );
00212 break;
00213 default:
00214 bpm_error( "Unknown FFT mode in complexfft()", __FILE__, __LINE__ );
00215 return BPM_FAILURE;
00216 break;
00217 }
00218
00219
00220 for ( i=0; i<z->ns; i++ ) {
00221 z->wf[i].re = _fft_data[2*i];
00222 z->wf[i].im = _fft_data[2*i+1];
00223 }
00224
00225 return BPM_SUCCESS;
00226 }
00227
00228
00229
00230 int realfft( doublewf_t *y, int fft_mode, complexwf_t *z ) {
00231
00232
00233
00234
00235 int i = 0;
00236
00237 if ( ! y || ! z ) {
00238 bpm_error( "Invalid pointers in realfft(...)", __FILE__, __LINE__ );
00239 return BPM_FAILURE;
00240 }
00241
00242 if ( ! _is_pow2(z->ns) ) {
00243 bpm_warning( "Number of samples is not of the form 2^n, may run into trouble with FFT !",
00244 __FILE__, __LINE__ );
00245 }
00246
00247 if ( _check_fft_buffers( z->ns ) == BPM_FAILURE ) {
00248 bpm_error( "Error checking FFT buffers in complexfft()", __FILE__, __LINE__ );
00249 return BPM_FAILURE;
00250 }
00251
00252
00253
00254 switch ( fft_mode ) {
00255 case FFT_FORWARD:
00256
00257 for ( i=0; i<z->ns; i++ ) _fft_data[i] = y->wf[i];
00258
00259 rdft( z->ns, 1, _fft_data, _fft_work_area, _fft_sc_table );
00260
00261
00262 for ( i=0; i<z->ns/2; i++ ) {
00263 z->wf[i].re = z->wf[z->ns-1-i].re = _fft_data[2*i];
00264 z->wf[i].im = z->wf[z->ns-1-i].im = _fft_data[2*i+1];
00265 }
00266
00267 break;
00268 case FFT_BACKWARD:
00269
00270 for ( i=0; i<z->ns/2; i++ ) {
00271 _fft_data[2*i] = z->wf[i].re;
00272 _fft_data[2*i+1] = z->wf[i].im;
00273 }
00274
00275 rdft( z->ns, -1, _fft_data, _fft_work_area, _fft_sc_table );
00276
00277
00278 for ( i=0; i<z->ns; i++ ) y->wf[i] = _fft_data[i];
00279 break;
00280
00281 default:
00282 bpm_error( "Unknown FFT mode in complexfft()", __FILE__, __LINE__ );
00283 return BPM_FAILURE;
00284 break;
00285 }
00286
00287
00288 return BPM_SUCCESS;
00289 }