trying some ideas for soft-decision DTD based on residual-to-signal ratio
[speexdsp.git] / libspeex / filters_sse.h
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: filters.c
3    Various analysis/synthesis filters
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <xmmintrin.h>
34
35 void filter_mem2_10(const float *x, const float *_num, const float *_den, float *y, int N, int ord, float *_mem)
36 {
37    __m128 num[3], den[3], mem[3];
38
39    int i;
40
41    /* Copy numerator, denominator and memory to aligned xmm */
42    for (i=0;i<2;i++)
43    {
44       mem[i] = _mm_loadu_ps(_mem+4*i);
45       num[i] = _mm_loadu_ps(_num+4*i+1);
46       den[i] = _mm_loadu_ps(_den+4*i+1);
47    }
48    mem[2] = _mm_setr_ps(_mem[8], _mem[9], 0, 0);
49    num[2] = _mm_setr_ps(_num[9], _num[10], 0, 0);
50    den[2] = _mm_setr_ps(_den[9], _den[10], 0, 0);
51    
52    for (i=0;i<N;i++)
53    {
54       __m128 xx;
55       __m128 yy;
56       /* Compute next filter result */
57       xx = _mm_load_ps1(x+i);
58       yy = _mm_add_ss(xx, mem[0]);
59       _mm_store_ss(y+i, yy);
60       yy = _mm_shuffle_ps(yy, yy, 0);
61       
62       /* Update memory */
63       mem[0] = _mm_move_ss(mem[0], mem[1]);
64       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
65
66       mem[0] = _mm_add_ps(mem[0], _mm_mul_ps(xx, num[0]));
67       mem[0] = _mm_sub_ps(mem[0], _mm_mul_ps(yy, den[0]));
68
69       mem[1] = _mm_move_ss(mem[1], mem[2]);
70       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
71
72       mem[1] = _mm_add_ps(mem[1], _mm_mul_ps(xx, num[1]));
73       mem[1] = _mm_sub_ps(mem[1], _mm_mul_ps(yy, den[1]));
74
75       mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0xfd);
76
77       mem[2] = _mm_add_ps(mem[2], _mm_mul_ps(xx, num[2]));
78       mem[2] = _mm_sub_ps(mem[2], _mm_mul_ps(yy, den[2]));
79    }
80    /* Put memory back in its place */
81    _mm_storeu_ps(_mem, mem[0]);
82    _mm_storeu_ps(_mem+4, mem[1]);
83    _mm_store_ss(_mem+8, mem[2]);
84    mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0x55);
85    _mm_store_ss(_mem+9, mem[2]);
86 }
87
88 void filter_mem2_8(const float *x, const float *_num, const float *_den, float *y, int N, int ord, float *_mem)
89 {
90    __m128 num[2], den[2], mem[2];
91
92    int i;
93
94    /* Copy numerator, denominator and memory to aligned xmm */
95    for (i=0;i<2;i++)
96    {
97       mem[i] = _mm_loadu_ps(_mem+4*i);
98       num[i] = _mm_loadu_ps(_num+4*i+1);
99       den[i] = _mm_loadu_ps(_den+4*i+1);
100    }
101    
102    for (i=0;i<N;i++)
103    {
104       __m128 xx;
105       __m128 yy;
106       /* Compute next filter result */
107       xx = _mm_load_ps1(x+i);
108       yy = _mm_add_ss(xx, mem[0]);
109       _mm_store_ss(y+i, yy);
110       yy = _mm_shuffle_ps(yy, yy, 0);
111       
112       /* Update memory */
113       mem[0] = _mm_move_ss(mem[0], mem[1]);
114       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
115
116       mem[0] = _mm_add_ps(mem[0], _mm_mul_ps(xx, num[0]));
117       mem[0] = _mm_sub_ps(mem[0], _mm_mul_ps(yy, den[0]));
118
119       mem[1] = _mm_sub_ss(mem[1], mem[1]);
120       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
121
122       mem[1] = _mm_add_ps(mem[1], _mm_mul_ps(xx, num[1]));
123       mem[1] = _mm_sub_ps(mem[1], _mm_mul_ps(yy, den[1]));
124    }
125    /* Put memory back in its place */
126    _mm_storeu_ps(_mem, mem[0]);
127    _mm_storeu_ps(_mem+4, mem[1]);
128 }
129
130
131
132 void filter_mem2(const float *x, const float *_num, const float *_den, float *y, int N, int ord, float *_mem)
133 {
134    if(ord==10)
135       filter_mem2_10(x, _num, _den, y, N, ord, _mem);
136    else if (ord==8)
137       filter_mem2_8(x, _num, _den, y, N, ord, _mem);
138 }
139
140
141
142 void iir_mem2_10(const float *x, const float *_den, float *y, int N, int ord, float *_mem)
143 {
144    __m128 den[3], mem[3];
145
146    int i;
147
148    /* Copy numerator, denominator and memory to aligned xmm */
149    for (i=0;i<2;i++)
150    {
151       mem[i] = _mm_loadu_ps(_mem+4*i);
152       den[i] = _mm_loadu_ps(_den+4*i+1);
153    }
154    mem[2] = _mm_setr_ps(_mem[8], _mem[9], 0, 0);
155    den[2] = _mm_setr_ps(_den[9], _den[10], 0, 0);
156    
157    for (i=0;i<N;i++)
158    {
159       __m128 xx;
160       __m128 yy;
161       /* Compute next filter result */
162       xx = _mm_load_ps1(x+i);
163       yy = _mm_add_ss(xx, mem[0]);
164       _mm_store_ss(y+i, yy);
165       yy = _mm_shuffle_ps(yy, yy, 0);
166       
167       /* Update memory */
168       mem[0] = _mm_move_ss(mem[0], mem[1]);
169       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
170
171       mem[0] = _mm_sub_ps(mem[0], _mm_mul_ps(yy, den[0]));
172
173       mem[1] = _mm_move_ss(mem[1], mem[2]);
174       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
175
176       mem[1] = _mm_sub_ps(mem[1], _mm_mul_ps(yy, den[1]));
177
178       mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0xfd);
179
180       mem[2] = _mm_sub_ps(mem[2], _mm_mul_ps(yy, den[2]));
181    }
182    /* Put memory back in its place */
183    _mm_storeu_ps(_mem, mem[0]);
184    _mm_storeu_ps(_mem+4, mem[1]);
185    _mm_store_ss(_mem+8, mem[2]);
186    mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0x55);
187    _mm_store_ss(_mem+9, mem[2]);
188 }
189
190
191 void iir_mem2_8(const float *x, const float *_den, float *y, int N, int ord, float *_mem)
192 {
193    __m128 den[2], mem[2];
194
195    int i;
196
197    /* Copy numerator, denominator and memory to aligned xmm */
198    for (i=0;i<2;i++)
199    {
200       mem[i] = _mm_loadu_ps(_mem+4*i);
201       den[i] = _mm_loadu_ps(_den+4*i+1);
202    }
203    
204    for (i=0;i<N;i++)
205    {
206       __m128 xx;
207       __m128 yy;
208       /* Compute next filter result */
209       xx = _mm_load_ps1(x+i);
210       yy = _mm_add_ss(xx, mem[0]);
211       _mm_store_ss(y+i, yy);
212       yy = _mm_shuffle_ps(yy, yy, 0);
213       
214       /* Update memory */
215       mem[0] = _mm_move_ss(mem[0], mem[1]);
216       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
217
218       mem[0] = _mm_sub_ps(mem[0], _mm_mul_ps(yy, den[0]));
219
220       mem[1] = _mm_sub_ss(mem[1], mem[1]);
221       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
222
223       mem[1] = _mm_sub_ps(mem[1], _mm_mul_ps(yy, den[1]));
224    }
225    /* Put memory back in its place */
226    _mm_storeu_ps(_mem, mem[0]);
227    _mm_storeu_ps(_mem+4, mem[1]);
228 }
229
230 void iir_mem2(const float *x, const float *_den, float *y, int N, int ord, float *_mem)
231 {
232    if(ord==10)
233       iir_mem2_10(x, _den, y, N, ord, _mem);
234    else if (ord==8)
235       iir_mem2_8(x, _den, y, N, ord, _mem);
236 }
237
238
239 void fir_mem2_10(const float *x, const float *_num, float *y, int N, int ord, float *_mem)
240 {
241    __m128 num[3], mem[3];
242
243    int i;
244
245    /* Copy numerator, denominator and memory to aligned xmm */
246    for (i=0;i<2;i++)
247    {
248       mem[i] = _mm_loadu_ps(_mem+4*i);
249       num[i] = _mm_loadu_ps(_num+4*i+1);
250    }
251    mem[2] = _mm_setr_ps(_mem[8], _mem[9], 0, 0);
252    num[2] = _mm_setr_ps(_num[9], _num[10], 0, 0);
253    
254    for (i=0;i<N;i++)
255    {
256       __m128 xx;
257       __m128 yy;
258       /* Compute next filter result */
259       xx = _mm_load_ps1(x+i);
260       yy = _mm_add_ss(xx, mem[0]);
261       _mm_store_ss(y+i, yy);
262       yy = _mm_shuffle_ps(yy, yy, 0);
263       
264       /* Update memory */
265       mem[0] = _mm_move_ss(mem[0], mem[1]);
266       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
267
268       mem[0] = _mm_add_ps(mem[0], _mm_mul_ps(xx, num[0]));
269
270       mem[1] = _mm_move_ss(mem[1], mem[2]);
271       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
272
273       mem[1] = _mm_add_ps(mem[1], _mm_mul_ps(xx, num[1]));
274
275       mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0xfd);
276
277       mem[2] = _mm_add_ps(mem[2], _mm_mul_ps(xx, num[2]));
278    }
279    /* Put memory back in its place */
280    _mm_storeu_ps(_mem, mem[0]);
281    _mm_storeu_ps(_mem+4, mem[1]);
282    _mm_store_ss(_mem+8, mem[2]);
283    mem[2] = _mm_shuffle_ps(mem[2], mem[2], 0x55);
284    _mm_store_ss(_mem+9, mem[2]);
285 }
286
287 void fir_mem2_8(const float *x, const float *_num, float *y, int N, int ord, float *_mem)
288 {
289    __m128 num[2], mem[2];
290
291    int i;
292
293    /* Copy numerator, denominator and memory to aligned xmm */
294    for (i=0;i<2;i++)
295    {
296       mem[i] = _mm_loadu_ps(_mem+4*i);
297       num[i] = _mm_loadu_ps(_num+4*i+1);
298    }
299    
300    for (i=0;i<N;i++)
301    {
302       __m128 xx;
303       __m128 yy;
304       /* Compute next filter result */
305       xx = _mm_load_ps1(x+i);
306       yy = _mm_add_ss(xx, mem[0]);
307       _mm_store_ss(y+i, yy);
308       yy = _mm_shuffle_ps(yy, yy, 0);
309       
310       /* Update memory */
311       mem[0] = _mm_move_ss(mem[0], mem[1]);
312       mem[0] = _mm_shuffle_ps(mem[0], mem[0], 0x39);
313
314       mem[0] = _mm_add_ps(mem[0], _mm_mul_ps(xx, num[0]));
315
316       mem[1] = _mm_sub_ss(mem[1], mem[1]);
317       mem[1] = _mm_shuffle_ps(mem[1], mem[1], 0x39);
318
319       mem[1] = _mm_add_ps(mem[1], _mm_mul_ps(xx, num[1]));
320    }
321    /* Put memory back in its place */
322    _mm_storeu_ps(_mem, mem[0]);
323    _mm_storeu_ps(_mem+4, mem[1]);
324 }
325
326
327 void fir_mem2(const float *x, const float *_num, float *y, int N, int ord, float *_mem)
328 {
329    if(ord==10)
330       fir_mem2_10(x, _num, y, N, ord, _mem);
331    else if (ord==8)
332       fir_mem2_8(x, _num, y, N, ord, _mem);
333 }