spandsp: fir.h Source File

spandsp 0.0.6

fir.h
1/*
2 * SpanDSP - a series of DSP components for telephony
3 *
4 * fir.h - General telephony FIR routines
5 *
6 * Written by Steve Underwood <steveu@coppice.org>
7 *
8 * Copyright (C) 2002 Steve Underwood
9 *
10 * All rights reserved.
11 *
12 * This program is free software; you can redistribute it and/or modify
13 * it under the terms of the GNU Lesser General Public License version 2.1,
14 * as published by the Free Software Foundation.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this program; if not, write to the Free Software
23 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 */
25
26/*! \page fir_page FIR filtering
27\section fir_page_sec_1 What does it do?
28???.
29
30\section fir_page_sec_2 How does it work?
31???.
32*/
33
34#if !defined(_SPANDSP_FIR_H_)
35#define _SPANDSP_FIR_H_
36
37#if defined(USE_MMX) || defined(USE_SSE2)
38#include "mmx.h"
39#endif
40
41/*!
42 16 bit integer FIR descriptor. This defines the working state for a single
43 instance of an FIR filter using 16 bit integer coefficients.
44*/
45 typedef struct
46{
47 int taps;
48 int curr_pos;
49 const int16_t *coeffs;
50 int16_t *history;
51} fir16_state_t;
52
53/*!
54 32 bit integer FIR descriptor. This defines the working state for a single
55 instance of an FIR filter using 32 bit integer coefficients, and filtering
56 16 bit integer data.
57*/
58 typedef struct
59{
60 int taps;
61 int curr_pos;
62 const int32_t *coeffs;
63 int16_t *history;
64} fir32_state_t;
65
66/*!
67 Floating point FIR descriptor. This defines the working state for a single
68 instance of an FIR filter using floating point coefficients and data.
69*/
70 typedef struct
71{
72 int taps;
73 int curr_pos;
74 const float *coeffs;
75 float *history;
76} fir_float_state_t;
77
78#if defined(__cplusplus)
79extern "C"
80{
81#endif
82
83static __inline__ const int16_t *fir16_create(fir16_state_t *fir,
84 const int16_t *coeffs,
85 int taps)
86{
87 fir->taps = taps;
88 fir->curr_pos = taps - 1;
89 fir->coeffs = coeffs;
90#if defined(USE_MMX) || defined(USE_SSE2)
91 if ((fir->history = malloc(2*taps*sizeof(int16_t))))
92 memset(fir->history, 0, 2*taps*sizeof(int16_t));
93#else
94 if ((fir->history = (int16_t *) malloc(taps*sizeof(int16_t))))
95 memset(fir->history, 0, taps*sizeof(int16_t));
96#endif
97 return fir->history;
98}
99/*- End of function --------------------------------------------------------*/
100
101static __inline__ void fir16_flush(fir16_state_t *fir)
102{
103#if defined(USE_MMX) || defined(USE_SSE2)
104 memset(fir->history, 0, 2*fir->taps*sizeof(int16_t));
105#else
106 memset(fir->history, 0, fir->taps*sizeof(int16_t));
107#endif
108}
109/*- End of function --------------------------------------------------------*/
110
111static __inline__ void fir16_free(fir16_state_t *fir)
112{
113 free(fir->history);
114}
115/*- End of function --------------------------------------------------------*/
116
117static __inline__ int16_t fir16(fir16_state_t *fir, int16_t sample)
118{
119 int i;
120 int32_t y;
121#if defined(USE_MMX)
122 mmx_t *mmx_coeffs;
123 mmx_t *mmx_hist;
124
125 fir->history[fir->curr_pos] = sample;
126 fir->history[fir->curr_pos + fir->taps] = sample;
127
128 mmx_coeffs = (mmx_t *) fir->coeffs;
129 mmx_hist = (mmx_t *) &fir->history[fir->curr_pos];
130 i = fir->taps;
131 pxor_r2r(mm4, mm4);
132 /* 8 samples per iteration, so the filter must be a multiple of 8 long. */
133 while (i > 0)
134 {
135 movq_m2r(mmx_coeffs[0], mm0);
136 movq_m2r(mmx_coeffs[1], mm2);
137 movq_m2r(mmx_hist[0], mm1);
138 movq_m2r(mmx_hist[1], mm3);
139 mmx_coeffs += 2;
140 mmx_hist += 2;
141 pmaddwd_r2r(mm1, mm0);
142 pmaddwd_r2r(mm3, mm2);
143 paddd_r2r(mm0, mm4);
144 paddd_r2r(mm2, mm4);
145 i -= 8;
146 }
147 movq_r2r(mm4, mm0);
148 psrlq_i2r(32, mm0);
149 paddd_r2r(mm0, mm4);
150 movd_r2m(mm4, y);
151 emms();
152#elif defined(USE_SSE2)
153 xmm_t *xmm_coeffs;
154 xmm_t *xmm_hist;
155
156 fir->history[fir->curr_pos] = sample;
157 fir->history[fir->curr_pos + fir->taps] = sample;
158
159 xmm_coeffs = (xmm_t *) fir->coeffs;
160 xmm_hist = (xmm_t *) &fir->history[fir->curr_pos];
161 i = fir->taps;
162 pxor_r2r(xmm4, xmm4);
163 /* 16 samples per iteration, so the filter must be a multiple of 16 long. */
164 while (i > 0)
165 {
166 movdqu_m2r(xmm_coeffs[0], xmm0);
167 movdqu_m2r(xmm_coeffs[1], xmm2);
168 movdqu_m2r(xmm_hist[0], xmm1);
169 movdqu_m2r(xmm_hist[1], xmm3);
170 xmm_coeffs += 2;
171 xmm_hist += 2;
172 pmaddwd_r2r(xmm1, xmm0);
173 pmaddwd_r2r(xmm3, xmm2);
174 paddd_r2r(xmm0, xmm4);
175 paddd_r2r(xmm2, xmm4);
176 i -= 16;
177 }
178 movdqa_r2r(xmm4, xmm0);
179 psrldq_i2r(8, xmm0);
180 paddd_r2r(xmm0, xmm4);
181 movdqa_r2r(xmm4, xmm0);
182 psrldq_i2r(4, xmm0);
183 paddd_r2r(xmm0, xmm4);
184 movd_r2m(xmm4, y);
185#else
186 int offset1;
187 int offset2;
188
189 fir->history[fir->curr_pos] = sample;
190
191 offset2 = fir->curr_pos;
192 offset1 = fir->taps - offset2;
193 y = 0;
194 for (i = fir->taps - 1; i >= offset1; i--)
195 y += fir->coeffs[i]*fir->history[i - offset1];
196 for ( ; i >= 0; i--)
197 y += fir->coeffs[i]*fir->history[i + offset2];
198#endif
199 if (fir->curr_pos <= 0)
200 fir->curr_pos = fir->taps;
201 fir->curr_pos--;
202 return (int16_t) (y >> 15);
203}
204/*- End of function --------------------------------------------------------*/
205
206static __inline__ const int16_t *fir32_create(fir32_state_t *fir,
207 const int32_t *coeffs,
208 int taps)
209{
210 fir->taps = taps;
211 fir->curr_pos = taps - 1;
212 fir->coeffs = coeffs;
213 fir->history = (int16_t *) malloc(taps*sizeof(int16_t));
214 if (fir->history)
215 memset(fir->history, '\0', taps*sizeof(int16_t));
216 return fir->history;
217}
218/*- End of function --------------------------------------------------------*/
219
220static __inline__ void fir32_flush(fir32_state_t *fir)
221{
222 memset(fir->history, 0, fir->taps*sizeof(int16_t));
223}
224/*- End of function --------------------------------------------------------*/
225
226static __inline__ void fir32_free(fir32_state_t *fir)
227{
228 free(fir->history);
229}
230/*- End of function --------------------------------------------------------*/
231
232static __inline__ int16_t fir32(fir32_state_t *fir, int16_t sample)
233{
234 int i;
235 int32_t y;
236 int offset1;
237 int offset2;
238
239 fir->history[fir->curr_pos] = sample;
240 offset2 = fir->curr_pos;
241 offset1 = fir->taps - offset2;
242 y = 0;
243 for (i = fir->taps - 1; i >= offset1; i--)
244 y += fir->coeffs[i]*fir->history[i - offset1];
245 for ( ; i >= 0; i--)
246 y += fir->coeffs[i]*fir->history[i + offset2];
247 if (fir->curr_pos <= 0)
248 fir->curr_pos = fir->taps;
249 fir->curr_pos--;
250 return (int16_t) (y >> 15);
251}
252/*- End of function --------------------------------------------------------*/
253
254static __inline__ const float *fir_float_create(fir_float_state_t *fir,
255 const float *coeffs,
256 int taps)
257{
258 fir->taps = taps;
259 fir->curr_pos = taps - 1;
260 fir->coeffs = coeffs;
261 fir->history = (float *) malloc(taps*sizeof(float));
262 if (fir->history)
263 memset(fir->history, '\0', taps*sizeof(float));
264 return fir->history;
265}
266/*- End of function --------------------------------------------------------*/
267
268static __inline__ void fir_float_free(fir_float_state_t *fir)
269{
270 free(fir->history);
271}
272/*- End of function --------------------------------------------------------*/
273
274static __inline__ int16_t fir_float(fir_float_state_t *fir, int16_t sample)
275{
276 int i;
277 float y;
278 int offset1;
279 int offset2;
280
281 fir->history[fir->curr_pos] = sample;
282
283 offset2 = fir->curr_pos;
284 offset1 = fir->taps - offset2;
285 y = 0;
286 for (i = fir->taps - 1; i >= offset1; i--)
287 y += fir->coeffs[i]*fir->history[i - offset1];
288 for ( ; i >= 0; i--)
289 y += fir->coeffs[i]*fir->history[i + offset2];
290 if (fir->curr_pos <= 0)
291 fir->curr_pos = fir->taps;
292 fir->curr_pos--;
293 return (int16_t) y;
294}
295/*- End of function --------------------------------------------------------*/
296
297#if defined(__cplusplus)
298}
299#endif
300
301#endif
302/*- End of file ------------------------------------------------------------*/
fir16_state_t
Definition fir.h:46
fir32_state_t
Definition fir.h:59
fir_float_state_t
Definition fir.h:71