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 General Public License as published by
00014  * the Free Software Foundation; either version 2 of the License, or
00015  * (at your option) any later version.
00016  *
00017  * This program is distributed in the hope that it will be useful,
00018  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  * GNU General Public License for more details.
00021  *
00022  * You should have received a copy of the GNU General Public License
00023  * along with this program; if not, write to the Free Software
00024  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00025  *
00026  * $Id: fir.h,v 1.4 2005/11/23 17:09:47 steveu Exp $
00027  */
00028 
00029 /*! \page fir_page FIR filtering
00030 \section fir_page_sec_1 What does it do?
00031 ???.
00032 
00033 \section fir_page_sec_2 How does it work?
00034 ???.
00035 */
00036 
00037 #if !defined(_FIR_H_)
00038 #define _FIR_H_
00039 
00040 #if defined(USE_MMX)  ||  defined(USE_SSE2)
00041 #include "mmx.h"
00042 #endif
00043 
00044 /*!
00045     16 bit integer FIR descriptor. This defines the working state for a single
00046     instance of an FIR filter using 16 bit integer coefficients.
00047 */
00048 typedef struct
00049 {
00050     int taps;
00051     int curr_pos;
00052     const int16_t *coeffs;
00053     int16_t *history;
00054 } fir16_state_t;
00055 
00056 /*!
00057     32 bit integer FIR descriptor. This defines the working state for a single
00058     instance of an FIR filter using 32 bit integer coefficients, and filtering
00059     16 bit integer data.
00060 */
00061 typedef struct
00062 {
00063     int taps;
00064     int curr_pos;
00065     const int32_t *coeffs;
00066     int16_t *history;
00067 } fir32_state_t;
00068 
00069 /*!
00070     Floating point FIR descriptor. This defines the working state for a single
00071     instance of an FIR filter using floating point coefficients and data.
00072 */
00073 typedef struct
00074 {
00075     int taps;
00076     int curr_pos;
00077     const float *coeffs;
00078     float *history;
00079 } fir_float_state_t;
00080 
00081 #ifdef __cplusplus
00082 extern "C" {
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 /*- End of function --------------------------------------------------------*/
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 /*- End of function --------------------------------------------------------*/
00112 
00113 static inline void fir16_free(fir16_state_t *fir)
00114 {
00115     free(fir->history);
00116 }
00117 /*- End of function --------------------------------------------------------*/
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     /* 8 samples per iteration, so the filter must be a multiple of 8 long. */
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     /* 16 samples per iteration, so the filter must be a multiple of 16 long. */
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  y >> 15;
00205 }
00206 /*- End of function --------------------------------------------------------*/
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 /*- End of function --------------------------------------------------------*/
00221 
00222 static inline void fir32_flush(fir32_state_t *fir)
00223 {
00224     memset(fir->history, 0, fir->taps*sizeof(int16_t));
00225 }
00226 /*- End of function --------------------------------------------------------*/
00227 
00228 static inline void fir32_free(fir32_state_t *fir)
00229 {
00230     free(fir->history);
00231 }
00232 /*- End of function --------------------------------------------------------*/
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  y >> 15;
00253 }
00254 /*- End of function --------------------------------------------------------*/
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 /*- End of function --------------------------------------------------------*/
00269     
00270 static inline void fir_float_free(fir_float_state_t *fir)
00271 {
00272     free(fir->history);
00273 }
00274 /*- End of function --------------------------------------------------------*/
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 /*- End of function --------------------------------------------------------*/
00298 
00299 #ifdef __cplusplus
00300 }
00301 #endif
00302 
00303 #endif
00304 /*- End of file ------------------------------------------------------------*/

Generated on Fri Nov 10 09:40:23 2006 for libspandsp by  doxygen 1.5.1