Preparing for beta 3, cleaned up the mode/bit-rate code in speexdec,
[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 void filter_mem2(float *x, float *_num, float *_den, float *y, int N, int ord, float *_mem)
34 {
35    float __num[20], __den[20], __mem[20];
36    float *num, *den, *mem;
37    int i,j;
38    float xi,yi;
39
40    num = (float*)(((int)(__num+4))&0xfffffff0)-1;
41    den = (float*)(((int)(__den+4))&0xfffffff0)-1;
42    mem = (float*)(((int)(__mem+4))&0xfffffff0)-1;
43    for (i=0;i<ord+1;i++)
44    {
45       num[i]=_num[i];
46       den[i]=_den[i];
47    }
48    for (i=0;i<ord;i++)
49       mem[i]=_mem[i];
50    for (i=0;i<N;i+=4)
51    {
52
53       __asm__ __volatile__ 
54       ("
55        movss (%1), %%xmm0
56        movss (%0), %%xmm1
57        addss %%xmm0, %%xmm1
58        movss %%xmm1, (%2)
59        shufps $0x00, %%xmm0, %%xmm0
60        shufps $0x00, %%xmm1, %%xmm1
61
62        movaps 4(%3),  %%xmm2
63        movaps 4(%4),  %%xmm3
64        mulps  %%xmm0, %%xmm2
65        mulps  %%xmm1, %%xmm3
66        movaps 20(%3), %%xmm4
67        mulps  %%xmm0, %%xmm4
68        addps  4(%0),  %%xmm2
69        movaps 20(%4), %%xmm5
70        mulps  %%xmm1, %%xmm5
71        addps  20(%0), %%xmm4
72        subps  %%xmm3, %%xmm2
73        movups %%xmm2, (%0)
74        subps  %%xmm5, %%xmm4
75        movups %%xmm4, 16(%0)
76
77        movss  36(%3), %%xmm2
78        mulss  %%xmm0, %%xmm2
79        movss  36(%4), %%xmm3
80        mulss  %%xmm1, %%xmm3
81        addss  36(%0), %%xmm2
82        movss  40(%3), %%xmm4
83        mulss  %%xmm0, %%xmm4
84        movss  40(%4), %%xmm5
85        mulss  %%xmm1, %%xmm5
86        subss  %%xmm3, %%xmm2
87        movss  %%xmm2, 32(%0)       
88        subss  %%xmm5, %%xmm4
89        movss  %%xmm4, 36(%0)
90
91
92
93        movss 4(%1), %%xmm0
94        movss (%0), %%xmm1
95        addss %%xmm0, %%xmm1
96        movss %%xmm1, 4(%2)
97        shufps $0x00, %%xmm0, %%xmm0
98        shufps $0x00, %%xmm1, %%xmm1
99
100        movaps 4(%3),  %%xmm2
101        movaps 4(%4),  %%xmm3
102        mulps  %%xmm0, %%xmm2
103        mulps  %%xmm1, %%xmm3
104        movaps 20(%3), %%xmm4
105        mulps  %%xmm0, %%xmm4
106        addps  4(%0),  %%xmm2
107        movaps 20(%4), %%xmm5
108        mulps  %%xmm1, %%xmm5
109        addps  20(%0), %%xmm4
110        subps  %%xmm3, %%xmm2
111        movups %%xmm2, (%0)
112        subps  %%xmm5, %%xmm4
113        movups %%xmm4, 16(%0)
114
115        movss  36(%3), %%xmm2
116        mulss  %%xmm0, %%xmm2
117        movss  36(%4), %%xmm3
118        mulss  %%xmm1, %%xmm3
119        addss  36(%0), %%xmm2
120        movss  40(%3), %%xmm4
121        mulss  %%xmm0, %%xmm4
122        movss  40(%4), %%xmm5
123        mulss  %%xmm1, %%xmm5
124        subss  %%xmm3, %%xmm2
125        movss  %%xmm2, 32(%0)       
126        subss  %%xmm5, %%xmm4
127        movss  %%xmm4, 36(%0)
128
129
130
131        movss 8(%1), %%xmm0
132        movss (%0), %%xmm1
133        addss %%xmm0, %%xmm1
134        movss %%xmm1, 8(%2)
135        shufps $0x00, %%xmm0, %%xmm0
136        shufps $0x00, %%xmm1, %%xmm1
137
138        movaps 4(%3),  %%xmm2
139        movaps 4(%4),  %%xmm3
140        mulps  %%xmm0, %%xmm2
141        mulps  %%xmm1, %%xmm3
142        movaps 20(%3), %%xmm4
143        mulps  %%xmm0, %%xmm4
144        addps  4(%0),  %%xmm2
145        movaps 20(%4), %%xmm5
146        mulps  %%xmm1, %%xmm5
147        addps  20(%0), %%xmm4
148        subps  %%xmm3, %%xmm2
149        movups %%xmm2, (%0)
150        subps  %%xmm5, %%xmm4
151        movups %%xmm4, 16(%0)
152
153        movss  36(%3), %%xmm2
154        mulss  %%xmm0, %%xmm2
155        movss  36(%4), %%xmm3
156        mulss  %%xmm1, %%xmm3
157        addss  36(%0), %%xmm2
158        movss  40(%3), %%xmm4
159        mulss  %%xmm0, %%xmm4
160        movss  40(%4), %%xmm5
161        mulss  %%xmm1, %%xmm5
162        subss  %%xmm3, %%xmm2
163        movss  %%xmm2, 32(%0)       
164        subss  %%xmm5, %%xmm4
165        movss  %%xmm4, 36(%0)
166
167
168
169        movss 12(%1), %%xmm0
170        movss (%0), %%xmm1
171        addss %%xmm0, %%xmm1
172        movss %%xmm1, 12(%2)
173        shufps $0x00, %%xmm0, %%xmm0
174        shufps $0x00, %%xmm1, %%xmm1
175
176        movaps 4(%3),  %%xmm2
177        movaps 4(%4),  %%xmm3
178        mulps  %%xmm0, %%xmm2
179        mulps  %%xmm1, %%xmm3
180        movaps 20(%3), %%xmm4
181        mulps  %%xmm0, %%xmm4
182        addps  4(%0),  %%xmm2
183        movaps 20(%4), %%xmm5
184        mulps  %%xmm1, %%xmm5
185        addps  20(%0), %%xmm4
186        subps  %%xmm3, %%xmm2
187        movups %%xmm2, (%0)
188        subps  %%xmm5, %%xmm4
189        movups %%xmm4, 16(%0)
190
191        movss  36(%3), %%xmm2
192        mulss  %%xmm0, %%xmm2
193        movss  36(%4), %%xmm3
194        mulss  %%xmm1, %%xmm3
195        addss  36(%0), %%xmm2
196        movss  40(%3), %%xmm4
197        mulss  %%xmm0, %%xmm4
198        movss  40(%4), %%xmm5
199        mulss  %%xmm1, %%xmm5
200        subss  %%xmm3, %%xmm2
201        movss  %%xmm2, 32(%0)       
202        subss  %%xmm5, %%xmm4
203        movss  %%xmm4, 36(%0)
204
205        "
206        : : "r" (mem), "r" (x+i), "r" (y+i), "r" (num), "r" (den)
207        : "memory" );
208
209    }
210    for (i=0;i<ord;i++)
211       _mem[i]=mem[i];
212
213 }
214
215
216 void iir_mem2(float *x, float *_den, float *y, int N, int ord, float *_mem)
217 {
218    float  __den[20], __mem[20];
219    float *num, *den, *mem;
220    int i,j;
221
222    den = (float*)(((int)(__den+4))&0xfffffff0)-1;
223    mem = (float*)(((int)(__mem+4))&0xfffffff0)-1;
224    for (i=0;i<ord+1;i++)
225    {
226       den[i]=_den[i];
227    }
228    for (i=0;i<ord;i++)
229       mem[i]=_mem[i];
230
231    for (i=0;i<N;i++)
232    {
233 #if 0
234       y[i] = x[i] + mem[0];
235       for (j=0;j<ord-1;j++)
236       {
237          mem[j] = mem[j+1] - den[j+1]*y[i];
238       }
239       mem[ord-1] = - den[ord]*y[i];
240 #else
241       __asm__ __volatile__ 
242       ("
243        movss (%1), %%xmm0
244        movss (%0), %%xmm1
245        addss %%xmm0, %%xmm1
246        movss %%xmm1, (%2)
247        shufps $0x00, %%xmm0, %%xmm0
248        shufps $0x00, %%xmm1, %%xmm1
249
250        
251        movaps 4(%3),  %%xmm2
252        movaps 20(%3), %%xmm3
253        mulps  %%xmm1, %%xmm2
254        mulps  %%xmm1, %%xmm3
255        movss  36(%3), %%xmm4
256        movss  40(%3), %%xmm5
257        mulss  %%xmm1, %%xmm4
258        mulss  %%xmm1, %%xmm5
259        movaps 4(%0),  %%xmm6
260        subps  %%xmm2, %%xmm6
261        movups %%xmm6, (%0)
262        movaps 20(%0), %%xmm7
263        subps  %%xmm3, %%xmm7
264        movups %%xmm7, 16(%0)
265
266
267        movss  36(%0), %%xmm7
268        subss  %%xmm4, %%xmm7
269        movss  %%xmm7, 32(%0)       
270        xorps  %%xmm2, %%xmm2
271        subss  %%xmm5, %%xmm2
272        movss  %%xmm2, 36(%0)
273
274        "
275        : : "r" (mem), "r" (x+i), "r" (y+i), "r" (den)
276        : "memory" );
277 #endif
278    }
279    for (i=0;i<ord;i++)
280       _mem[i]=mem[i];
281
282 }
283