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
00035
00036 #if !defined(_SPANDSP_FIR_H_)
00037 #define _SPANDSP_FIR_H_
00038
00039 #if defined(USE_MMX) || defined(USE_SSE2)
00040 #include "mmx.h"
00041 #endif
00042
00043
00044
00045
00046
00047 typedef struct
00048 {
00049 int taps;
00050 int curr_pos;
00051 const int16_t *coeffs;
00052 int16_t *history;
00053 } fir16_state_t;
00054
00055
00056
00057
00058
00059
00060 typedef struct
00061 {
00062 int taps;
00063 int curr_pos;
00064 const int32_t *coeffs;
00065 int16_t *history;
00066 } fir32_state_t;
00067
00068
00069
00070
00071
00072 typedef struct
00073 {
00074 int taps;
00075 int curr_pos;
00076 const float *coeffs;
00077 float *history;
00078 } fir_float_state_t;
00079
00080 #if defined(__cplusplus)
00081 extern "C"
00082 {
00083 #endif
00084
00085 static __inline__ const int16_t *fir16_create(fir16_state_t *fir,
00086 const int16_t *coeffs,
00087 int taps)
00088 {
00089 fir->taps = taps;
00090 fir->curr_pos = taps - 1;
00091 fir->coeffs = coeffs;
00092 #if defined(USE_MMX) || defined(USE_SSE2)
00093 if ((fir->history = malloc(2*taps*sizeof(int16_t))))
00094 memset(fir->history, 0, 2*taps*sizeof(int16_t));
00095 #else
00096 if ((fir->history = (int16_t *) malloc(taps*sizeof(int16_t))))
00097 memset(fir->history, 0, taps*sizeof(int16_t));
00098 #endif
00099 return fir->history;
00100 }
00101
00102
00103 static __inline__ void fir16_flush(fir16_state_t *fir)
00104 {
00105 #if defined(USE_MMX) || defined(USE_SSE2)
00106 memset(fir->history, 0, 2*fir->taps*sizeof(int16_t));
00107 #else
00108 memset(fir->history, 0, fir->taps*sizeof(int16_t));
00109 #endif
00110 }
00111
00112
00113 static __inline__ void fir16_free(fir16_state_t *fir)
00114 {
00115 free(fir->history);
00116 }
00117
00118
00119 static __inline__ int16_t fir16(fir16_state_t *fir, int16_t sample)
00120 {
00121 int i;
00122 int32_t y;
00123 #if defined(USE_MMX)
00124 mmx_t *mmx_coeffs;
00125 mmx_t *mmx_hist;
00126
00127 fir->history[fir->curr_pos] = sample;
00128 fir->history[fir->curr_pos + fir->taps] = sample;
00129
00130 mmx_coeffs = (mmx_t *) fir->coeffs;
00131 mmx_hist = (mmx_t *) &fir->history[fir->curr_pos];
00132 i = fir->taps;
00133 pxor_r2r(mm4, mm4);
00134
00135 while (i > 0)
00136 {
00137 movq_m2r(mmx_coeffs[0], mm0);
00138 movq_m2r(mmx_coeffs[1], mm2);
00139 movq_m2r(mmx_hist[0], mm1);
00140 movq_m2r(mmx_hist[1], mm3);
00141 mmx_coeffs += 2;
00142 mmx_hist += 2;
00143 pmaddwd_r2r(mm1, mm0);
00144 pmaddwd_r2r(mm3, mm2);
00145 paddd_r2r(mm0, mm4);
00146 paddd_r2r(mm2, mm4);
00147 i -= 8;
00148 }
00149 movq_r2r(mm4, mm0);
00150 psrlq_i2r(32, mm0);
00151 paddd_r2r(mm0, mm4);
00152 movd_r2m(mm4, y);
00153 emms();
00154 #elif defined(USE_SSE2)
00155 xmm_t *xmm_coeffs;
00156 xmm_t *xmm_hist;
00157
00158 fir->history[fir->curr_pos] = sample;
00159 fir->history[fir->curr_pos + fir->taps] = sample;
00160
00161 xmm_coeffs = (xmm_t *) fir->coeffs;
00162 xmm_hist = (xmm_t *) &fir->history[fir->curr_pos];
00163 i = fir->taps;
00164 pxor_r2r(xmm4, xmm4);
00165
00166 while (i > 0)
00167 {
00168 movdqu_m2r(xmm_coeffs[0], xmm0);
00169 movdqu_m2r(xmm_coeffs[1], xmm2);
00170 movdqu_m2r(xmm_hist[0], xmm1);
00171 movdqu_m2r(xmm_hist[1], xmm3);
00172 xmm_coeffs += 2;
00173 xmm_hist += 2;
00174 pmaddwd_r2r(xmm1, xmm0);
00175 pmaddwd_r2r(xmm3, xmm2);
00176 paddd_r2r(xmm0, xmm4);
00177 paddd_r2r(xmm2, xmm4);
00178 i -= 16;
00179 }
00180 movdqa_r2r(xmm4, xmm0);
00181 psrldq_i2r(8, xmm0);
00182 paddd_r2r(xmm0, xmm4);
00183 movdqa_r2r(xmm4, xmm0);
00184 psrldq_i2r(4, xmm0);
00185 paddd_r2r(xmm0, xmm4);
00186 movd_r2m(xmm4, y);
00187 #else
00188 int offset1;
00189 int offset2;
00190
00191 fir->history[fir->curr_pos] = sample;
00192
00193 offset2 = fir->curr_pos;
00194 offset1 = fir->taps - offset2;
00195 y = 0;
00196 for (i = fir->taps - 1; i >= offset1; i--)
00197 y += fir->coeffs[i]*fir->history[i - offset1];
00198 for ( ; i >= 0; i--)
00199 y += fir->coeffs[i]*fir->history[i + offset2];
00200 #endif
00201 if (fir->curr_pos <= 0)
00202 fir->curr_pos = fir->taps;
00203 fir->curr_pos--;
00204 return (int16_t) (y >> 15);
00205 }
00206
00207
00208 static __inline__ const int16_t *fir32_create(fir32_state_t *fir,
00209 const int32_t *coeffs,
00210 int taps)
00211 {
00212 fir->taps = taps;
00213 fir->curr_pos = taps - 1;
00214 fir->coeffs = coeffs;
00215 fir->history = (int16_t *) malloc(taps*sizeof(int16_t));
00216 if (fir->history)
00217 memset(fir->history, '\0', taps*sizeof(int16_t));
00218 return fir->history;
00219 }
00220
00221
00222 static __inline__ void fir32_flush(fir32_state_t *fir)
00223 {
00224 memset(fir->history, 0, fir->taps*sizeof(int16_t));
00225 }
00226
00227
00228 static __inline__ void fir32_free(fir32_state_t *fir)
00229 {
00230 free(fir->history);
00231 }
00232
00233
00234 static __inline__ int16_t fir32(fir32_state_t *fir, int16_t sample)
00235 {
00236 int i;
00237 int32_t y;
00238 int offset1;
00239 int offset2;
00240
00241 fir->history[fir->curr_pos] = sample;
00242 offset2 = fir->curr_pos;
00243 offset1 = fir->taps - offset2;
00244 y = 0;
00245 for (i = fir->taps - 1; i >= offset1; i--)
00246 y += fir->coeffs[i]*fir->history[i - offset1];
00247 for ( ; i >= 0; i--)
00248 y += fir->coeffs[i]*fir->history[i + offset2];
00249 if (fir->curr_pos <= 0)
00250 fir->curr_pos = fir->taps;
00251 fir->curr_pos--;
00252 return (int16_t) (y >> 15);
00253 }
00254
00255
00256 static __inline__ const float *fir_float_create(fir_float_state_t *fir,
00257 const float *coeffs,
00258 int taps)
00259 {
00260 fir->taps = taps;
00261 fir->curr_pos = taps - 1;
00262 fir->coeffs = coeffs;
00263 fir->history = (float *) malloc(taps*sizeof(float));
00264 if (fir->history)
00265 memset(fir->history, '\0', taps*sizeof(float));
00266 return fir->history;
00267 }
00268
00269
00270 static __inline__ void fir_float_free(fir_float_state_t *fir)
00271 {
00272 free(fir->history);
00273 }
00274
00275
00276 static __inline__ int16_t fir_float(fir_float_state_t *fir, int16_t sample)
00277 {
00278 int i;
00279 float y;
00280 int offset1;
00281 int offset2;
00282
00283 fir->history[fir->curr_pos] = sample;
00284
00285 offset2 = fir->curr_pos;
00286 offset1 = fir->taps - offset2;
00287 y = 0;
00288 for (i = fir->taps - 1; i >= offset1; i--)
00289 y += fir->coeffs[i]*fir->history[i - offset1];
00290 for ( ; i >= 0; i--)
00291 y += fir->coeffs[i]*fir->history[i + offset2];
00292 if (fir->curr_pos <= 0)
00293 fir->curr_pos = fir->taps;
00294 fir->curr_pos--;
00295 return (int16_t) y;
00296 }
00297
00298
00299 #if defined(__cplusplus)
00300 }
00301 #endif
00302
00303 #endif
00304