spandsp 0.0.6
|
00001 /* 00002 * SpanDSP - a series of DSP components for telephony 00003 * 00004 * fir.h - General telephony FIR routines 00005 * 00006 * Written by Steve Underwood <steveu@coppice.org> 00007 * 00008 * Copyright (C) 2002 Steve Underwood 00009 * 00010 * All rights reserved. 00011 * 00012 * This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU Lesser General Public License version 2.1, 00014 * as published by the Free Software Foundation. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Lesser General Public License for more details. 00020 * 00021 * You should have received a copy of the GNU Lesser General Public 00022 * License along with this program; if not, write to the Free Software 00023 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 00024 */ 00025 00026 /*! \page fir_page FIR filtering 00027 \section fir_page_sec_1 What does it do? 00028 ???. 00029 00030 \section fir_page_sec_2 How does it work? 00031 ???. 00032 */ 00033 00034 #if !defined(_SPANDSP_FIR_H_) 00035 #define _SPANDSP_FIR_H_ 00036 00037 #if defined(USE_MMX) || defined(USE_SSE2) 00038 #include "mmx.h" 00039 #endif 00040 00041 /*! 00042 16 bit integer FIR descriptor. This defines the working state for a single 00043 instance of an FIR filter using 16 bit integer coefficients. 00044 */ 00045 typedef struct 00046 { 00047 int taps; 00048 int curr_pos; 00049 const int16_t *coeffs; 00050 int16_t *history; 00051 } fir16_state_t; 00052 00053 /*! 00054 32 bit integer FIR descriptor. This defines the working state for a single 00055 instance of an FIR filter using 32 bit integer coefficients, and filtering 00056 16 bit integer data. 00057 */ 00058 typedef struct 00059 { 00060 int taps; 00061 int curr_pos; 00062 const int32_t *coeffs; 00063 int16_t *history; 00064 } fir32_state_t; 00065 00066 /*! 00067 Floating point FIR descriptor. This defines the working state for a single 00068 instance of an FIR filter using floating point coefficients and data. 00069 */ 00070 typedef struct 00071 { 00072 int taps; 00073 int curr_pos; 00074 const float *coeffs; 00075 float *history; 00076 } fir_float_state_t; 00077 00078 #if defined(__cplusplus) 00079 extern "C" 00080 { 00081 #endif 00082 00083 static __inline__ const int16_t *fir16_create(fir16_state_t *fir, 00084 const int16_t *coeffs, 00085 int taps) 00086 { 00087 fir->taps = taps; 00088 fir->curr_pos = taps - 1; 00089 fir->coeffs = coeffs; 00090 #if defined(USE_MMX) || defined(USE_SSE2) 00091 if ((fir->history = malloc(2*taps*sizeof(int16_t)))) 00092 memset(fir->history, 0, 2*taps*sizeof(int16_t)); 00093 #else 00094 if ((fir->history = (int16_t *) malloc(taps*sizeof(int16_t)))) 00095 memset(fir->history, 0, taps*sizeof(int16_t)); 00096 #endif 00097 return fir->history; 00098 } 00099 /*- End of function --------------------------------------------------------*/ 00100 00101 static __inline__ void fir16_flush(fir16_state_t *fir) 00102 { 00103 #if defined(USE_MMX) || defined(USE_SSE2) 00104 memset(fir->history, 0, 2*fir->taps*sizeof(int16_t)); 00105 #else 00106 memset(fir->history, 0, fir->taps*sizeof(int16_t)); 00107 #endif 00108 } 00109 /*- End of function --------------------------------------------------------*/ 00110 00111 static __inline__ void fir16_free(fir16_state_t *fir) 00112 { 00113 free(fir->history); 00114 } 00115 /*- End of function --------------------------------------------------------*/ 00116 00117 static __inline__ int16_t fir16(fir16_state_t *fir, int16_t sample) 00118 { 00119 int i; 00120 int32_t y; 00121 #if defined(USE_MMX) 00122 mmx_t *mmx_coeffs; 00123 mmx_t *mmx_hist; 00124 00125 fir->history[fir->curr_pos] = sample; 00126 fir->history[fir->curr_pos + fir->taps] = sample; 00127 00128 mmx_coeffs = (mmx_t *) fir->coeffs; 00129 mmx_hist = (mmx_t *) &fir->history[fir->curr_pos]; 00130 i = fir->taps; 00131 pxor_r2r(mm4, mm4); 00132 /* 8 samples per iteration, so the filter must be a multiple of 8 long. */ 00133 while (i > 0) 00134 { 00135 movq_m2r(mmx_coeffs[0], mm0); 00136 movq_m2r(mmx_coeffs[1], mm2); 00137 movq_m2r(mmx_hist[0], mm1); 00138 movq_m2r(mmx_hist[1], mm3); 00139 mmx_coeffs += 2; 00140 mmx_hist += 2; 00141 pmaddwd_r2r(mm1, mm0); 00142 pmaddwd_r2r(mm3, mm2); 00143 paddd_r2r(mm0, mm4); 00144 paddd_r2r(mm2, mm4); 00145 i -= 8; 00146 } 00147 movq_r2r(mm4, mm0); 00148 psrlq_i2r(32, mm0); 00149 paddd_r2r(mm0, mm4); 00150 movd_r2m(mm4, y); 00151 emms(); 00152 #elif defined(USE_SSE2) 00153 xmm_t *xmm_coeffs; 00154 xmm_t *xmm_hist; 00155 00156 fir->history[fir->curr_pos] = sample; 00157 fir->history[fir->curr_pos + fir->taps] = sample; 00158 00159 xmm_coeffs = (xmm_t *) fir->coeffs; 00160 xmm_hist = (xmm_t *) &fir->history[fir->curr_pos]; 00161 i = fir->taps; 00162 pxor_r2r(xmm4, xmm4); 00163 /* 16 samples per iteration, so the filter must be a multiple of 16 long. */ 00164 while (i > 0) 00165 { 00166 movdqu_m2r(xmm_coeffs[0], xmm0); 00167 movdqu_m2r(xmm_coeffs[1], xmm2); 00168 movdqu_m2r(xmm_hist[0], xmm1); 00169 movdqu_m2r(xmm_hist[1], xmm3); 00170 xmm_coeffs += 2; 00171 xmm_hist += 2; 00172 pmaddwd_r2r(xmm1, xmm0); 00173 pmaddwd_r2r(xmm3, xmm2); 00174 paddd_r2r(xmm0, xmm4); 00175 paddd_r2r(xmm2, xmm4); 00176 i -= 16; 00177 } 00178 movdqa_r2r(xmm4, xmm0); 00179 psrldq_i2r(8, xmm0); 00180 paddd_r2r(xmm0, xmm4); 00181 movdqa_r2r(xmm4, xmm0); 00182 psrldq_i2r(4, xmm0); 00183 paddd_r2r(xmm0, xmm4); 00184 movd_r2m(xmm4, y); 00185 #else 00186 int offset1; 00187 int offset2; 00188 00189 fir->history[fir->curr_pos] = sample; 00190 00191 offset2 = fir->curr_pos; 00192 offset1 = fir->taps - offset2; 00193 y = 0; 00194 for (i = fir->taps - 1; i >= offset1; i--) 00195 y += fir->coeffs[i]*fir->history[i - offset1]; 00196 for ( ; i >= 0; i--) 00197 y += fir->coeffs[i]*fir->history[i + offset2]; 00198 #endif 00199 if (fir->curr_pos <= 0) 00200 fir->curr_pos = fir->taps; 00201 fir->curr_pos--; 00202 return (int16_t) (y >> 15); 00203 } 00204 /*- End of function --------------------------------------------------------*/ 00205 00206 static __inline__ const int16_t *fir32_create(fir32_state_t *fir, 00207 const int32_t *coeffs, 00208 int taps) 00209 { 00210 fir->taps = taps; 00211 fir->curr_pos = taps - 1; 00212 fir->coeffs = coeffs; 00213 fir->history = (int16_t *) malloc(taps*sizeof(int16_t)); 00214 if (fir->history) 00215 memset(fir->history, '\0', taps*sizeof(int16_t)); 00216 return fir->history; 00217 } 00218 /*- End of function --------------------------------------------------------*/ 00219 00220 static __inline__ void fir32_flush(fir32_state_t *fir) 00221 { 00222 memset(fir->history, 0, fir->taps*sizeof(int16_t)); 00223 } 00224 /*- End of function --------------------------------------------------------*/ 00225 00226 static __inline__ void fir32_free(fir32_state_t *fir) 00227 { 00228 free(fir->history); 00229 } 00230 /*- End of function --------------------------------------------------------*/ 00231 00232 static __inline__ int16_t fir32(fir32_state_t *fir, int16_t sample) 00233 { 00234 int i; 00235 int32_t y; 00236 int offset1; 00237 int offset2; 00238 00239 fir->history[fir->curr_pos] = sample; 00240 offset2 = fir->curr_pos; 00241 offset1 = fir->taps - offset2; 00242 y = 0; 00243 for (i = fir->taps - 1; i >= offset1; i--) 00244 y += fir->coeffs[i]*fir->history[i - offset1]; 00245 for ( ; i >= 0; i--) 00246 y += fir->coeffs[i]*fir->history[i + offset2]; 00247 if (fir->curr_pos <= 0) 00248 fir->curr_pos = fir->taps; 00249 fir->curr_pos--; 00250 return (int16_t) (y >> 15); 00251 } 00252 /*- End of function --------------------------------------------------------*/ 00253 00254 static __inline__ const float *fir_float_create(fir_float_state_t *fir, 00255 const float *coeffs, 00256 int taps) 00257 { 00258 fir->taps = taps; 00259 fir->curr_pos = taps - 1; 00260 fir->coeffs = coeffs; 00261 fir->history = (float *) malloc(taps*sizeof(float)); 00262 if (fir->history) 00263 memset(fir->history, '\0', taps*sizeof(float)); 00264 return fir->history; 00265 } 00266 /*- End of function --------------------------------------------------------*/ 00267 00268 static __inline__ void fir_float_free(fir_float_state_t *fir) 00269 { 00270 free(fir->history); 00271 } 00272 /*- End of function --------------------------------------------------------*/ 00273 00274 static __inline__ int16_t fir_float(fir_float_state_t *fir, int16_t sample) 00275 { 00276 int i; 00277 float y; 00278 int offset1; 00279 int offset2; 00280 00281 fir->history[fir->curr_pos] = sample; 00282 00283 offset2 = fir->curr_pos; 00284 offset1 = fir->taps - offset2; 00285 y = 0; 00286 for (i = fir->taps - 1; i >= offset1; i--) 00287 y += fir->coeffs[i]*fir->history[i - offset1]; 00288 for ( ; i >= 0; i--) 00289 y += fir->coeffs[i]*fir->history[i + offset2]; 00290 if (fir->curr_pos <= 0) 00291 fir->curr_pos = fir->taps; 00292 fir->curr_pos--; 00293 return (int16_t) y; 00294 } 00295 /*- End of function --------------------------------------------------------*/ 00296 00297 #if defined(__cplusplus) 00298 } 00299 #endif 00300 00301 #endif 00302 /*- End of file ------------------------------------------------------------*/