fir.h

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 ------------------------------------------------------------*/

Generated on Thu Oct 18 15:28:01 2012 for spandsp by  doxygen 1.4.7