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