00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
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
00043
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
00055
00056
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
00068
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
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
00110
00111 static __inline__ void fir16_free(fir16_state_t *fir)
00112 {
00113 free(fir->history);
00114 }
00115
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
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
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
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
00219
00220 static __inline__ void fir32_flush(fir32_state_t *fir)
00221 {
00222 memset(fir->history, 0, fir->taps*sizeof(int16_t));
00223 }
00224
00225
00226 static __inline__ void fir32_free(fir32_state_t *fir)
00227 {
00228 free(fir->history);
00229 }
00230
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
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
00267
00268 static __inline__ void fir_float_free(fir_float_state_t *fir)
00269 {
00270 free(fir->history);
00271 }
00272
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
00296
00297 #if defined(__cplusplus)
00298 }
00299 #endif
00300
00301 #endif
00302