decoder excitation now in 16-bit precision (was 32), which saves quite a bit
[speexdsp.git] / libspeex / filters_bfin.h
1 /* Copyright (C) 2005 Analog Devices */
2 /**
3    @file filters_bfin.h
4    @brief Various analysis/synthesis filters (Blackfin version)
5 */
6 /*
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions
9    are met:
10    
11    - Redistributions of source code must retain the above copyright
12    notice, this list of conditions and the following disclaimer.
13    
14    - Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17    
18    - Neither the name of the Xiph.org Foundation nor the names of its
19    contributors may be used to endorse or promote products derived from
20    this software without specific prior written permission.
21    
22    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
25    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
26    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
27    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
28    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
29    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
30    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
31    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 */
34
35 #include <stdio.h>
36
37 #define OVERRIDE_NORMALIZE16
38 int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
39 {
40    spx_sig_t max_val=1;
41    int sig_shift;
42
43    __asm__ 
44    (
45    "%0 = 0;\n\t"
46    "I0 = %1;\n\t"
47    "L0 = 0;\n\t"
48    "R1 = [I0++];\n\t"
49    "LOOP norm_max%= LC0 = %2;\n\t"
50    "LOOP_BEGIN norm_max%=;\n\t"
51       "R2 = ABS R1 || R1 = [I0++];\n\t"
52       "%0 = MAX(%0, R2);\n\t"
53    "LOOP_END norm_max%=;\n\t"
54    : "=&d" (max_val)
55    : "a" (x), "a" (len)
56    : "R1", "R2"
57    );
58
59    sig_shift=0;
60    while (max_val>max_scale)
61    {
62       sig_shift++;
63       max_val >>= 1;
64    }
65
66    __asm__ __volatile__ 
67    (
68    "I0 = %0;\n\t"
69    "L0 = 0;\n\t"
70    "I1 = %1;\n\t"
71    "L1 = 0;\n\t"
72    "R0 = [I0++];\n\t"
73    "LOOP norm_shift%= LC0 = %3 >> 1;\n\t"
74    "LOOP_BEGIN norm_shift%=;\n\t"
75       "R1 = ASHIFT R0 by %2.L || R2 = [I0++];\n\t"
76       "R3 = ASHIFT R2 by %2.L || R0 = [I0++];\n\t"
77       "R3 = PACK(R3.L, R1.L);\n\t"
78       "[I1++] = R3;\n\t"
79    "LOOP_END norm_shift%=;\n\t"
80    : : "a" (x), "a" (y), "d" (-sig_shift), "a" (len)
81    : "I0", "L0", "I1", "L1", "R0", "R1", "R2", "R3", "memory"
82    );
83    return sig_shift;
84 }
85
86 #define OVERRIDE_FILTER_MEM2
87 void filter_mem2(const spx_sig_t *_x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *_y, int N, int ord, spx_mem_t *mem)
88 {
89    spx_word32_t xy2[N+1];
90    spx_word32_t *xy = xy2+1;
91    spx_word32_t numden_a[2*ord+2];
92    spx_word16_t *numden = (spx_word16_t*) numden_a;
93    int i;
94    for (i=0;i<ord;i++)
95    {
96       numden[2*i] = num[i];
97       numden[2*i+1] = den[i];
98    }
99    __asm__ __volatile__
100    (
101    /* Register setup */
102    "R0 = %5;\n\t"      /*ord */
103    
104    "P0 = %3;\n\t"
105    "I0 = P0;\n\t"
106    "B0 = P0;\n\t" /* numden */
107    "L0 = 0;\n\t"
108       
109    "P2 = %0;\n\t" /* Fused xy */
110    "I2 = P2;\n\t"
111    "L2 = 0;\n\t"
112    
113    "P4 = %6;\n\t" /* mem */
114    "P0 = %1;\n\t" /* _x */
115    "P1 = %2;\n\t" /* _y */
116    
117    /* First sample */
118    "R1 = [P4++];\n\t"
119    "R1 <<= 1;\n\t" /* shift mem */
120    "R2 = [P0++];\n\t" /* load x[0] */
121    "R1 = R1 + R2;\n\t"
122    "[P1++] = R1;\n\t" /* store y[0] */
123    "R1 <<= 2;\n\t"
124    "R2 <<= 2;\n\t"
125    "R2 = PACK(R1.H, R2.H);\n\t" /* pack x16 and y16 */
126    "[P2] = R2;\n\t"
127                
128    /* Samples 1 to ord-1 (using memory) */
129    "R0 += -1;\n\t"
130    "R3 = 0;\n\t"
131    "LC0 = R0;\n\t"
132    "LOOP filter_start%= LC0;\n\t"
133    "LOOP_BEGIN filter_start%=;\n\t"
134       "R3 += 1;\n\t"
135       "LC1 = R3;\n\t"
136       
137       "R1 = [P4++];\n\t"
138       "A1 = R1;\n\t"
139       "A0 = 0;\n\t"
140       "I0 = B0;\n\t"
141       "I2 = P2;\n\t"
142       "P2 += 4;\n\t"
143       "R4 = [I0++] || R5 = [I2--];\n\t"
144       "LOOP filter_start_inner%= LC1;\n\t"
145       "LOOP_BEGIN filter_start_inner%=;\n\t"
146          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
147       "LOOP_END filter_start_inner%=;\n\t"
148       "A0 += A1;\n\t"
149       "R4 = A0;\n\t"
150       "R4 <<= 1;\n\t" /* shift mem */
151       "R2 = [P0++];\n\t" /* load x */
152       "R4 = R4 + R2;\n\t"
153       "[P1++] = R4;\n\t" /* store y */
154       "R4 <<= 2;\n\t"
155       "R2 <<= 2;\n\t"
156       "R2 = PACK(R4.H, R2.H);\n\t" /* pack x16 and y16 */
157       "[P2] = R2;\n\t"
158
159    "LOOP_END filter_start%=;\n\t"
160
161    /* Samples ord to N*/   
162    "R0 = %5;\n\t"
163    "R0 <<= 1;\n\t"
164    "I0 = B0;\n\t" /* numden */
165    "R0 <<= 1;\n\t"   
166    "L0 = R0;\n\t"
167    
168    "R0 = %5;\n\t" /* org */
169    "R2 = %4;\n\t" /* N */
170    "R2 = R2 - R0;\n\t"
171    "R4 = [I0++];\n\t" /* numden */
172    "LC0 = R2;\n\t"
173    "P3 = R0;\n\t"
174    "R0 <<= 2;\n\t"
175    "R0 += 8;\n\t"
176    "I2 = P2;\n\t"
177    "M0 = R0;\n\t"
178    "A1 = A0 = 0;\n\t"
179    "R5 = [I2--];\n\t" /* load xy */
180    "LOOP filter_mid%= LC0;\n\t"
181    "LOOP_BEGIN filter_mid%=;\n\t"
182       "LOOP filter_mid_inner%= LC1=P3;\n\t"
183       "LOOP_BEGIN filter_mid_inner%=;\n\t"
184          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
185       "LOOP_END filter_mid_inner%=;\n\t"
186       "R0 = (A0 += A1) || I2 += M0;\n\t"
187       "R0 = R0 << 1 || R5 = [P0++];\n\t" /* load x */
188       "R0 = R0 + R5;\n\t"
189       "R0 = R0 << 2 || [P1++] = R0;\n\t" /* shift y | store y */
190       "R5 = R5 << 2;\n\t"
191       "R5 = PACK(R0.H, R5.H);\n\t"
192       "A1 = A0 = 0 || [I2--] = R5\n\t"
193       "LOOP_END filter_mid%=;\n\t"
194    "I2 += 4;\n\t"
195    "P2 = I2;\n\t"
196    /* Update memory */
197    "P4 = %6;\n\t"
198    "R0 = %5;\n\t"
199    "LC0 = R0;\n\t"
200    "P0 = B0;\n\t"
201    "A1 = A0 = 0;\n\t"
202    "LOOP mem_update%= LC0;\n\t"
203    "LOOP_BEGIN mem_update%=;\n\t"
204       "I2 = P2;\n\t"
205       "I0 = P0;\n\t"
206       "P0 += 4;\n\t"
207       "R0 = LC0;\n\t"
208       "LC1 = R0;\n\t"
209       "R5 = [I2--] || R4 = [I0++];\n\t"
210       "LOOP mem_accum%= LC1;\n\t"
211       "LOOP_BEGIN mem_accum%=;\n\t"
212          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
213       "LOOP_END mem_accum%=;\n\t"
214       "R0 = (A0 += A1);\n\t"
215       "A1 = A0 = 0 || [P4++] = R0;\n\t"
216    "LOOP_END mem_update%=;\n\t"
217    "L0 = 0;\n\t"
218    : : "m" (xy), "m" (_x), "m" (_y), "m" (numden), "m" (N), "m" (ord), "m" (mem)
219    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B0", "I0", "I2", "L0", "L2", "M0", "memory"
220    );
221
222 }
223
224
225 #define OVERRIDE_FILTER_MEM16
226 void filter_mem16(const spx_word16_t *_x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *_y, int N, int ord, spx_mem_t *mem)
227 {
228    spx_word32_t xy2[N+1];
229    spx_word32_t *xy = xy2+1;
230    spx_word32_t numden_a[2*ord+2];
231    spx_word16_t *numden = (spx_word16_t*) numden_a;
232    int i;
233    for (i=0;i<ord;i++)
234    {
235       numden[2*i] = num[i];
236       numden[2*i+1] = den[i];
237    }
238    __asm__ __volatile__
239    (
240    /* Register setup */
241    "R0 = %5;\n\t"      /*ord */
242    
243    "P0 = %3;\n\t"
244    "I0 = P0;\n\t"
245    "B0 = P0;\n\t" /* numden */
246    "L0 = 0;\n\t"
247       
248    "P2 = %0;\n\t" /* Fused xy */
249    "I2 = P2;\n\t"
250    "L2 = 0;\n\t"
251    
252    "P4 = %6;\n\t" /* mem */
253    "P0 = %1;\n\t" /* _x */
254    "P1 = %2;\n\t" /* _y */
255    
256    /* First sample */
257    "R1 = [P4++];\n\t"
258    "R1 <<= 3;\n\t" /* shift mem */
259    "R1.L = R1 (RND);\n\t"
260    "R2 = W[P0++];\n\t" /* load x[0] */
261    "R1.L = R1.L + R2.L;\n\t"
262    "W[P1++] = R1;\n\t" /* store y[0] */
263    "R2 = PACK(R1.L, R2.L);\n\t" /* pack x16 and y16 */
264    "[P2] = R2;\n\t"
265                
266    /* Samples 1 to ord-1 (using memory) */
267    "R0 += -1;\n\t"
268    "R3 = 0;\n\t"
269    "LC0 = R0;\n\t"
270    "LOOP filter_start%= LC0;\n\t"
271    "LOOP_BEGIN filter_start%=;\n\t"
272       "R3 += 1;\n\t"
273       "LC1 = R3;\n\t"
274       
275       "R1 = [P4++];\n\t"
276       "A1 = R1;\n\t"
277       "A0 = 0;\n\t"
278       "I0 = B0;\n\t"
279       "I2 = P2;\n\t"
280       "P2 += 4;\n\t"
281       "R4 = [I0++] || R5 = [I2--];\n\t"
282       "LOOP filter_start_inner%= LC1;\n\t"
283       "LOOP_BEGIN filter_start_inner%=;\n\t"
284          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
285       "LOOP_END filter_start_inner%=;\n\t"
286       "A0 += A1;\n\t"
287       "R4 = A0;\n\t"
288       "R4 <<= 3;\n\t" /* shift mem */
289       "R4.L = R4 (RND);\n\t"
290       "R2 = W[P0++];\n\t" /* load x */
291       "R4.L = R4.L + R2.L;\n\t"
292       "W[P1++] = R4;\n\t" /* store y */
293       //"R4 <<= 2;\n\t"
294       //"R2 <<= 2;\n\t"
295       "R2 = PACK(R4.L, R2.L);\n\t" /* pack x16 and y16 */
296       "[P2] = R2;\n\t"
297
298    "LOOP_END filter_start%=;\n\t"
299
300    /* Samples ord to N*/   
301    "R0 = %5;\n\t"
302    "R0 <<= 1;\n\t"
303    "I0 = B0;\n\t" /* numden */
304    "R0 <<= 1;\n\t"   
305    "L0 = R0;\n\t"
306    
307    "R0 = %5;\n\t" /* org */
308    "R2 = %4;\n\t" /* N */
309    "R2 = R2 - R0;\n\t"
310    "R4 = [I0++];\n\t" /* numden */
311    "LC0 = R2;\n\t"
312    "P3 = R0;\n\t"
313    "R0 <<= 2;\n\t"
314    "R0 += 8;\n\t"
315    "I2 = P2;\n\t"
316    "M0 = R0;\n\t"
317    "A1 = A0 = 0;\n\t"
318    "R5 = [I2--];\n\t" /* load xy */
319    "LOOP filter_mid%= LC0;\n\t"
320    "LOOP_BEGIN filter_mid%=;\n\t"
321       "LOOP filter_mid_inner%= LC1=P3;\n\t"
322       "LOOP_BEGIN filter_mid_inner%=;\n\t"
323          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
324       "LOOP_END filter_mid_inner%=;\n\t"
325       "R0 = (A0 += A1) || I2 += M0;\n\t"
326       "R0 = R0 << 3 || R5 = W[P0++];\n\t" /* load x */
327       "R0.L = R0 (RND);\n\t"
328       "R0.L = R0.L + R5.L;\n\t"
329       "R5 = PACK(R0.L, R5.L) || W[P1++] = R0;\n\t" /* shift y | store y */
330       "A1 = A0 = 0 || [I2--] = R5\n\t"
331       "LOOP_END filter_mid%=;\n\t"
332    "I2 += 4;\n\t"
333    "P2 = I2;\n\t"
334    /* Update memory */
335    "P4 = %6;\n\t"
336    "R0 = %5;\n\t"
337    "LC0 = R0;\n\t"
338    "P0 = B0;\n\t"
339    "A1 = A0 = 0;\n\t"
340    "LOOP mem_update%= LC0;\n\t"
341    "LOOP_BEGIN mem_update%=;\n\t"
342       "I2 = P2;\n\t"
343       "I0 = P0;\n\t"
344       "P0 += 4;\n\t"
345       "R0 = LC0;\n\t"
346       "LC1 = R0;\n\t"
347       "R5 = [I2--] || R4 = [I0++];\n\t"
348       "LOOP mem_accum%= LC1;\n\t"
349       "LOOP_BEGIN mem_accum%=;\n\t"
350          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
351       "LOOP_END mem_accum%=;\n\t"
352       "R0 = (A0 += A1);\n\t"
353       "A1 = A0 = 0 || [P4++] = R0;\n\t"
354    "LOOP_END mem_update%=;\n\t"
355    "L0 = 0;\n\t"
356    : : "m" (xy), "m" (_x), "m" (_y), "m" (numden), "m" (N), "m" (ord), "m" (mem)
357    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B0", "I0", "I2", "L0", "L2", "M0", "memory"
358    );
359
360 }
361
362
363
364
365 #define OVERRIDE_IIR_MEM2
366 void iir_mem2(const spx_sig_t *_x, const spx_coef_t *den, spx_sig_t *_y, int N, int ord, spx_mem_t *mem)
367 {
368    spx_word16_t y[N+2];
369    spx_word16_t *yy;
370    yy = y+2;
371    __asm__ __volatile__
372    (
373    /* Register setup */
374    "R0 = %5;\n\t"      /*ord */
375    
376    "P1 = %3;\n\t"
377    "I1 = P1;\n\t"
378    "B1 = P1;\n\t"
379    "L1 = 0;\n\t"
380    
381    "P3 = %0;\n\t"
382    "I3 = P3;\n\t"
383    "L3 = 0;\n\t"
384    
385    "P4 = %6;\n\t"
386    "P0 = %1;\n\t"
387    "P1 = %2;\n\t"
388    
389    /* First sample */
390    "R1 = [P4++];\n\t"
391    "R1 <<= 1;\n\t"
392    "R2 = [P0++];\n\t"
393    "R1 = R1 + R2;\n\t"
394    "[P1++] = R1;\n\t"
395    "R1 <<= 2;\n\t"
396    "W[P3] = R1.H;\n\t"
397    "R2 <<= 2;\n\t"
398
399    /* Samples 1 to ord-1 (using memory) */
400    "R0 += -1;\n\t"
401    "R3 = 0;\n\t"
402    "LC0 = R0;\n\t"
403    "LOOP filter_start%= LC0;\n\t"
404    "LOOP_BEGIN filter_start%=;\n\t"
405       "R3 += 1;\n\t"
406       "LC1 = R3;\n\t"
407       
408       "R1 = [P4++];\n\t"
409       "A1 = R1;\n\t"
410       "I1 = B1;\n\t"
411       "I3 = P3;\n\t"
412       "P3 += 2;\n\t"
413       "LOOP filter_start_inner%= LC1;\n\t"
414       "LOOP_BEGIN filter_start_inner%=;\n\t"
415          "R4.L = W[I1++];\n\t"
416          "R5.L = W[I3--];\n\t"
417          "A1 -= R4.L*R5.L (IS);\n\t"
418       "LOOP_END filter_start_inner%=;\n\t"
419    
420       "R1 = A1;\n\t"
421       "R1 <<= 1;\n\t"
422       "R2 = [P0++];\n\t"
423       "R1 = R1 + R2;\n\t"
424       "[P1++] = R1;\n\t"
425       "R1 <<= 2;\n\t"
426       "W[P3] = R1.H;\n\t"
427       "R2 <<= 2;\n\t"
428    "LOOP_END filter_start%=;\n\t"
429
430    /* Samples ord to N*/   
431    "R0 = %5;\n\t"
432    "R0 <<= 1;\n\t"
433    "I1 = B1;\n\t"
434    "L1 = R0;\n\t"
435    
436    "R0 = %5;\n\t"
437    "R2 = %4;\n\t"
438    "R2 = R2 - R0;\n\t"
439    "R4.L = W[I1++];\n\t"
440    "LC0 = R2;\n\t"
441    "LOOP filter_mid%= LC0;\n\t"
442    "LOOP_BEGIN filter_mid%=;\n\t"
443       "LC1 = R0;\n\t"
444       "A1 = 0;\n\t"
445       "I3 = P3;\n\t"
446       "P3 += 2;\n\t"
447       "R5.L = W[I3--];\n\t"
448       "LOOP filter_mid_inner%= LC1;\n\t"
449       "LOOP_BEGIN filter_mid_inner%=;\n\t"
450          "A1 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
451       "LOOP_END filter_mid_inner%=;\n\t"
452       "R1 = A1;\n\t"
453       "R1 = R1 << 1 || R2 = [P0++];\n\t"
454       "R1 = R1 + R2;\n\t"
455       "R1 = R1 << 2 || [P1++] = R1;\n\t"
456       "W[P3] = R1.H;\n\t"
457    "LOOP_END filter_mid%=;\n\t"
458      
459    /* Update memory */
460    "P4 = %6;\n\t"
461    "R0 = %5;\n\t"
462    "LC0 = R0;\n\t"
463    "P1 = B1;\n\t"
464    "LOOP mem_update%= LC0;\n\t"
465    "LOOP_BEGIN mem_update%=;\n\t"
466       "A0 = 0;\n\t"
467       "I3 = P3;\n\t"
468       "I1 = P1;\n\t"
469       "P1 += 2;\n\t"
470       "R0 = LC0;\n\t"
471       "LC1=R0;\n\t"
472       "R5.L = W[I3--] || R4.L = W[I1++];\n\t"
473       "LOOP mem_accum%= LC1;\n\t"
474       "LOOP_BEGIN mem_accum%=;\n\t"
475          "A0 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
476       "LOOP_END mem_accum%=;\n\t"
477       "R0 = A0;\n\t"
478       "[P4++] = R0;\n\t"
479    "LOOP_END mem_update%=;\n\t"
480    "L1 = 0;\n\t"
481    : : "m" (yy), "m" (_x), "m" (_y), "m" (den), "m" (N), "m" (ord), "m" (mem)
482    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B1", "I1", "I3", "L1", "L3", "memory"
483    );
484
485 }
486
487
488 #define OVERRIDE_IIR_MEM16
489 void iir_mem16(const spx_word16_t *_x, const spx_coef_t *den, spx_word16_t *_y, int N, int ord, spx_mem_t *mem)
490 {
491    spx_word16_t y[N+2];
492    spx_word16_t *yy;
493    yy = y+2;
494    __asm__ __volatile__
495    (
496    /* Register setup */
497    "R0 = %5;\n\t"      /*ord */
498    
499    "P1 = %3;\n\t"
500    "I1 = P1;\n\t"
501    "B1 = P1;\n\t"
502    "L1 = 0;\n\t"
503    
504    "P3 = %0;\n\t"
505    "I3 = P3;\n\t"
506    "L3 = 0;\n\t"
507    
508    "P4 = %6;\n\t"
509    "P0 = %1;\n\t"
510    "P1 = %2;\n\t"
511    
512    /* First sample */
513    "R1 = [P4++];\n\t"
514    "R1 <<= 3;\n\t"
515    "R1.L = R1 (RND);\n\t"
516    "R2 = W[P0++];\n\t"
517    "R1 = R1 + R2;\n\t"
518    "W[P1++] = R1;\n\t"
519    "W[P3] = R1;\n\t"
520
521    /* Samples 1 to ord-1 (using memory) */
522    "R0 += -1;\n\t"
523    "R3 = 0;\n\t"
524    "LC0 = R0;\n\t"
525    "LOOP filter_start%= LC0;\n\t"
526    "LOOP_BEGIN filter_start%=;\n\t"
527       "R3 += 1;\n\t"
528       "LC1 = R3;\n\t"
529       
530       "R1 = [P4++];\n\t"
531       "A1 = R1;\n\t"
532       "I1 = B1;\n\t"
533       "I3 = P3;\n\t"
534       "P3 += 2;\n\t"
535       "LOOP filter_start_inner%= LC1;\n\t"
536       "LOOP_BEGIN filter_start_inner%=;\n\t"
537          "R4.L = W[I1++];\n\t"
538          "R5.L = W[I3--];\n\t"
539          "A1 -= R4.L*R5.L (IS);\n\t"
540       "LOOP_END filter_start_inner%=;\n\t"
541    
542       "R1 = A1;\n\t"
543       "R1 <<= 3;\n\t"
544       "R1.L = R1 (RND);\n\t"
545       "R2 = W[P0++];\n\t"
546       "R1 = R1 + R2;\n\t"
547       "W[P1++] = R1;\n\t"
548       "W[P3] = R1;\n\t"
549    "LOOP_END filter_start%=;\n\t"
550
551    /* Samples ord to N*/   
552    "R0 = %5;\n\t"
553    "R0 <<= 1;\n\t"
554    "I1 = B1;\n\t"
555    "L1 = R0;\n\t"
556    
557    "R0 = %5;\n\t"
558    "R2 = %4;\n\t"
559    "R2 = R2 - R0;\n\t"
560    "R4.L = W[I1++];\n\t"
561    "LC0 = R2;\n\t"
562    "LOOP filter_mid%= LC0;\n\t"
563    "LOOP_BEGIN filter_mid%=;\n\t"
564       "LC1 = R0;\n\t"
565       "A1 = 0;\n\t"
566       "I3 = P3;\n\t"
567       "P3 += 2;\n\t"
568       "R5.L = W[I3--];\n\t"
569       "LOOP filter_mid_inner%= LC1;\n\t"
570       "LOOP_BEGIN filter_mid_inner%=;\n\t"
571          "A1 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
572       "LOOP_END filter_mid_inner%=;\n\t"
573       "R1 = A1;\n\t"
574       "R1 = R1 << 3 || R2 = W[P0++];\n\t"
575       "R1.L = R1 (RND);\n\t"
576       "R1 = R1 + R2;\n\t"
577       "W[P1++] = R1;\n\t"
578       "W[P3] = R1;\n\t"
579    "LOOP_END filter_mid%=;\n\t"
580      
581    /* Update memory */
582    "P4 = %6;\n\t"
583    "R0 = %5;\n\t"
584    "LC0 = R0;\n\t"
585    "P1 = B1;\n\t"
586    "LOOP mem_update%= LC0;\n\t"
587    "LOOP_BEGIN mem_update%=;\n\t"
588       "A0 = 0;\n\t"
589       "I3 = P3;\n\t"
590       "I1 = P1;\n\t"
591       "P1 += 2;\n\t"
592       "R0 = LC0;\n\t"
593       "LC1=R0;\n\t"
594       "R5.L = W[I3--] || R4.L = W[I1++];\n\t"
595       "LOOP mem_accum%= LC1;\n\t"
596       "LOOP_BEGIN mem_accum%=;\n\t"
597          "A0 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
598       "LOOP_END mem_accum%=;\n\t"
599       "R0 = A0;\n\t"
600       "[P4++] = R0;\n\t"
601    "LOOP_END mem_update%=;\n\t"
602    "L1 = 0;\n\t"
603    : : "m" (yy), "m" (_x), "m" (_y), "m" (den), "m" (N), "m" (ord), "m" (mem)
604    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B1", "I1", "I3", "L1", "L3", "memory"
605    );
606
607 }
608
609
610 #define OVERRIDE_FIR_MEM2
611 void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem)
612 {
613    int i;
614    spx_coef_t den2[12];
615    spx_coef_t *den;
616    den = (spx_coef_t*)((((int)den2)+4)&0xfffffffc);
617    for (i=0;i<10;i++)
618       den[i] = 0;
619    filter_mem2(x, num, den, y, N, ord, mem);
620 }
621
622 #define OVERRIDE_FIR_MEM16
623 void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem)
624 {
625    int i;
626    spx_coef_t den2[12];
627    spx_coef_t *den;
628    den = (spx_coef_t*)((((int)den2)+4)&0xfffffffc);
629    for (i=0;i<10;i++)
630       den[i] = 0;
631    filter_mem16(x, num, den, y, N, ord, mem);
632 }
633
634
635 #define OVERRIDE_COMPUTE_IMPULSE_RESPONSE
636 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
637 {
638    int i;
639    VARDECL(spx_word16_t *ytmp);
640    ALLOC(ytmp, N, spx_word16_t);
641    spx_word16_t *ytmp2 = ytmp;
642    y[0] = LPC_SCALING;
643    for (i=0;i<ord;i++)
644       y[i+1] = awk1[i];
645    i++;
646    for (;i<N;i++)
647       y[i] = 0;
648
649    N-=1;
650    __asm__ __volatile__
651    (
652          "I0 = %0;\n\t"
653          "I1 = %1;\n\t"
654          "L0 = 0;\n\t"
655          "L1 = 0;\n\t"
656          "L2 = 0;\n\t"
657          "L3 = 0;\n\t"
658          "R0 = 1;\n\t"
659          "R0 <<= 13;\n\t"
660          "W[I0] = R0.L;\n\t"
661          "R0 <<= 1;\n\t"
662          "W[I1] = R0.L;\n\t"
663          "R0 = %5;\n\t"
664          "LC0 = R0;\n\t"
665          "R2 = 0;\n\t"
666          "LOOP samples%= LC0;\n\t"
667          "LOOP_BEGIN samples%=;\n\t"
668             "R2 += 1;\n\t"
669             "R2 = MIN(R2, %4);\n\t"
670             "I0 = %0;\n\t"
671             "I1 = %1;\n\t"
672             "I2 = %2;\n\t"
673             "I3 = %3;\n\t"
674             "%0 += 2;\n\t"
675             "%1 += 2;\n\t"
676             "A1 = A0 = 0;\n\t"
677             "R0.L = W[I0--] || R1.L = W[I2++];\n\t"
678             "LC1 = R2;\n\t"
679             "LOOP filter%= LC1;\n\t"
680             "LOOP_BEGIN filter%=;\n\t"
681                "A0 -= R0.L*R1.L (IS) || R0.L = W[I1--] || R1.L = W[I3++];\n\t"
682                "A1 -= R0.L*R1.L (IS) || R0.L = W[I0--] || R1.L = W[I2++];\n\t"
683             "LOOP_END filter%=;\n\t"
684             "R0 = A0, R1 = A1;\n\t"
685             "R3 = W[%1] (X);\n\t"
686             "R3 <<= 13;\n\t"
687             "R0 = R0 + R3;\n\t"
688             "R3 = R0 >>> 13;\n\t"
689             "W[%0] = R3.L;\n\t"
690             "R0 <<= 1;\n\t"
691             "R1 = R1 + R0;\n\t"
692             "R1 >>>= 13;\n\t"
693             "W[%1] = R1.L;\n\t"
694          "LOOP_END samples%=;\n\t"
695    : "=a" (ytmp2), "=a" (y)
696    : "a" (awk2), "a" (ak), "d" (ord), "m" (N), "0" (ytmp2), "1" (y)
697    : "A0", "A1", "R0", "R1", "R2", "R3", "I0", "I1", "I2", "I3", "L0", "L1", "L2", "L3", "A0", "A1"
698    );
699 }
700
701
702
703 #if 0 /* Equivalent C function for filter_mem2 and compute_impulse_response */
704 #define min(a,b) ((a)<(b) ? (a):(b))
705
706 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
707 {
708    int i,j;
709    VARDECL(spx_word16_t *ytmp);
710    ALLOC(ytmp, N, spx_word16_t);
711    
712    y[0] = LPC_SCALING;
713    for (i=0;i<ord;i++)
714       y[i+1] = awk1[i];
715    i++;
716    for (;i<N;i++)
717       y[i] = 0;
718
719    for (i=0;i<N;i++)
720    {
721       spx_word32_t yi = SHL32(EXTEND32(y[i]),LPC_SHIFT);
722       spx_word32_t yi2 = 0;
723       for (j=0;j<min(i,ord);j++)
724       {
725          yi = MAC16_16(yi, awk2[j], -ytmp[i-j-1]);
726          yi2 = MAC16_16(yi2, ak[j], -y[i-j-1]);
727       }
728       ytmp[i] = EXTRACT16(SHR32(yi,LPC_SHIFT));
729       yi2 = ADD32(yi2,SHL32(yi,1));
730       y[i] = EXTRACT16(SHR32(yi2,LPC_SHIFT));
731    }
732
733 }
734
735
736 void filter_mem2(const spx_sig_t *_x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *_y, int N, int ord, spx_mem_t *mem)
737 {
738    int i,j;
739    spx_word16_t xi,yi,nyi;
740    spx_word16_t x[N],y[N];
741    spx_word16_t *xx, *yy;
742    xx = x;
743    yy = y;
744    for (i=0;i<N;i++)
745    {
746       x[i] = EXTRACT16(SHR32(_x[i],SIG_SHIFT));
747    }
748    
749    for (i=0;i<ord;i++)
750    {
751       spx_word32_t yi = mem[i];
752       for (j=0;j<i;j++)
753       {
754          yi = MAC16_16(yi, num[j], x[i-j-1]);
755          yi = MAC16_16(yi, den[j], -y[i-j-1]);
756       }
757       _y[i] = ADD32(_x[i],SHL32(yi,1));
758       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
759    }
760    for (i=ord;i<N;i++)
761    {
762       spx_word32_t yi = 0;
763       for (j=0;j<ord;j++)
764       {
765          yi = MAC16_16(yi, num[j], x[i-j-1]);
766          yi = MAC16_16(yi, den[j], -y[i-j-1]);
767       }
768       _y[i] = ADD32(_x[i],SHL32(yi,1));
769       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
770    }
771
772    for (i=0;i<ord;i++)
773    {
774       spx_mem_t m = 0;
775       for (j=0;j<ord-i;j++)
776       {
777          m = MAC16_16(m, x[N-1-j], num[j+i]);
778          m = MAC16_16(m, -y[N-1-j], den[j+i]);
779       }
780       mem[i] = m;
781    }
782 }
783 #endif