trying some ideas for soft-decision DTD based on residual-to-signal ratio
[speexdsp.git] / libspeex / filters_arm4.h
1 /* Copyright (C) 2004 Jean-Marc Valin 
2    File: filters_arm4.h
3    ARM4-optimized filtering routines
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 int normalize16(const spx_sig_t *x, spx_word16_t *y, int max_scale, int len)
34 {
35    int i;
36    spx_sig_t max_val=1;
37    int sig_shift;
38    int dead1, dead2, dead3, dead4, dead5, dead6;
39
40    __asm__ __volatile__ (
41          "\tmov %1, #1 \n"
42          "\tmov %3, #0 \n"
43
44          ".normalize16loop1%=: \n"
45
46          "\tldr %4, [%0], #4 \n"
47          "\tcmps %4, %1 \n"
48          "\tmovgt %1, %4 \n"
49          "\tcmps %4, %3 \n"
50          "\tmovlt %3, %4 \n"
51
52          "\tsubs %2, %2, #1 \n"
53          "\tbne .normalize16loop1%=\n"
54
55          "\trsb %3, %3, #0 \n"
56          "\tcmp %1, %3 \n"
57          "\tmovlt %1, %3 \n"
58    : "=r" (dead1), "=r" (max_val), "=r" (dead3), "=r" (dead4),
59    "=r" (dead5), "=r" (dead6)
60    : "0" (x), "2" (len)
61    : "cc");
62
63    sig_shift=0;
64    while (max_val>max_scale)
65    {
66       sig_shift++;
67       max_val >>= 1;
68    }
69    
70    __asm__ __volatile__ (
71          ".normalize16loop%=: \n"
72
73          "\tldr %4, [%0], #4 \n"
74          "\tldr %5, [%0], #4 \n"
75          "\tmov %4, %4, asr %3 \n"
76          "\tstrh %4, [%1], #2 \n"
77          "\tldr %4, [%0], #4 \n"
78          "\tmov %5, %5, asr %3 \n"
79          "\tstrh %5, [%1], #2 \n"
80          "\tldr %5, [%0], #4 \n"
81          "\tmov %4, %4, asr %3 \n"
82          "\tstrh %4, [%1], #2 \n"
83          "\tsubs %2, %2, #1 \n"
84          "\tmov %5, %5, asr %3 \n"
85          "\tstrh %5, [%1], #2 \n"
86
87          "\tbge .normalize16loop%=\n"
88    : "=r" (dead1), "=r" (dead2), "=r" (dead3), "=r" (dead4),
89    "=r" (dead5), "=r" (dead6)
90    : "0" (x), "1" (y), "2" (len>>2), "3" (sig_shift)
91    : "cc", "memory");
92    return sig_shift;
93 }
94
95
96 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)
97 {
98    int i,j;
99    spx_sig_t xi,yi,nyi;
100
101    for (i=0;i<N;i++)
102    {
103       int deadm, deadn, deadd, deadidx, x1, y1, dead1, dead2, dead3, dead4, dead5, dead6;
104       xi=SATURATE(x[i],805306368);
105       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
106       nyi = -yi;
107       y[i] = yi;
108       __asm__ __volatile__ (
109             "\tldrsh %6, [%1], #2\n"
110             "\tsmull %8, %9, %4, %6\n"
111 #ifdef SHORTCUTS
112             "\tldrsh %6, [%2], #2\n"
113             "\tldr %10, [%0, #4]\n"
114             "\tmov %8, %8, lsr #15\n"
115             "\tsmull %7, %11, %5, %6\n"
116             "\tldrsh %6, [%1], #2\n"
117             "\tadd %8, %8, %9, lsl #17\n"
118             "\tadd %10, %10, %8\n"
119             "\tsmull %8, %9, %4, %6\n"
120             "\tadd %10, %10, %7, lsr #15\n"
121             "\tadd %10, %10, %11, lsl #17\n"
122             "\tstr %10, [%0], #4 \n"
123
124             "\tldrsh %6, [%2], #2\n"
125             "\tldr %10, [%0, #4]\n"
126             "\tmov %8, %8, lsr #15\n"
127             "\tsmull %7, %11, %5, %6\n"
128             "\tldrsh %6, [%1], #2\n"
129             "\tadd %8, %8, %9, lsl #17\n"
130             "\tadd %10, %10, %8\n"
131             "\tsmull %8, %9, %4, %6\n"
132             "\tadd %10, %10, %7, lsr #15\n"
133             "\tadd %10, %10, %11, lsl #17\n"
134             "\tstr %10, [%0], #4 \n"
135
136             "\tldrsh %6, [%2], #2\n"
137             "\tldr %10, [%0, #4]\n"
138             "\tmov %8, %8, lsr #15\n"
139             "\tsmull %7, %11, %5, %6\n"
140             "\tldrsh %6, [%1], #2\n"
141             "\tadd %8, %8, %9, lsl #17\n"
142             "\tadd %10, %10, %8\n"
143             "\tsmull %8, %9, %4, %6\n"
144             "\tadd %10, %10, %7, lsr #15\n"
145             "\tadd %10, %10, %11, lsl #17\n"
146             "\tstr %10, [%0], #4 \n"
147
148             "\tldrsh %6, [%2], #2\n"
149             "\tldr %10, [%0, #4]\n"
150             "\tmov %8, %8, lsr #15\n"
151             "\tsmull %7, %11, %5, %6\n"
152             "\tldrsh %6, [%1], #2\n"
153             "\tadd %8, %8, %9, lsl #17\n"
154             "\tadd %10, %10, %8\n"
155             "\tsmull %8, %9, %4, %6\n"
156             "\tadd %10, %10, %7, lsr #15\n"
157             "\tadd %10, %10, %11, lsl #17\n"
158             "\tstr %10, [%0], #4 \n"
159
160             "\tldrsh %6, [%2], #2\n"
161             "\tldr %10, [%0, #4]\n"
162             "\tmov %8, %8, lsr #15\n"
163             "\tsmull %7, %11, %5, %6\n"
164             "\tldrsh %6, [%1], #2\n"
165             "\tadd %8, %8, %9, lsl #17\n"
166             "\tadd %10, %10, %8\n"
167             "\tsmull %8, %9, %4, %6\n"
168             "\tadd %10, %10, %7, lsr #15\n"
169             "\tadd %10, %10, %11, lsl #17\n"
170             "\tstr %10, [%0], #4 \n"
171
172             "\tldrsh %6, [%2], #2\n"
173             "\tldr %10, [%0, #4]\n"
174             "\tmov %8, %8, lsr #15\n"
175             "\tsmull %7, %11, %5, %6\n"
176             "\tldrsh %6, [%1], #2\n"
177             "\tadd %8, %8, %9, lsl #17\n"
178             "\tadd %10, %10, %8\n"
179             "\tsmull %8, %9, %4, %6\n"
180             "\tadd %10, %10, %7, lsr #15\n"
181             "\tadd %10, %10, %11, lsl #17\n"
182             "\tstr %10, [%0], #4 \n"
183
184             "\tldrsh %6, [%2], #2\n"
185             "\tldr %10, [%0, #4]\n"
186             "\tmov %8, %8, lsr #15\n"
187             "\tsmull %7, %11, %5, %6\n"
188             "\tldrsh %6, [%1], #2\n"
189             "\tadd %8, %8, %9, lsl #17\n"
190             "\tadd %10, %10, %8\n"
191             "\tsmull %8, %9, %4, %6\n"
192             "\tadd %10, %10, %7, lsr #15\n"
193             "\tadd %10, %10, %11, lsl #17\n"
194             "\tstr %10, [%0], #4 \n"
195
196             "\tldrsh %6, [%2], #2\n"
197             "\tldr %10, [%0, #4]\n"
198             "\tmov %8, %8, lsr #15\n"
199             "\tsmull %7, %11, %5, %6\n"
200             "\tldrsh %6, [%1], #2\n"
201             "\tadd %8, %8, %9, lsl #17\n"
202             "\tadd %10, %10, %8\n"
203             "\tsmull %8, %9, %4, %6\n"
204             "\tadd %10, %10, %7, lsr #15\n"
205             "\tadd %10, %10, %11, lsl #17\n"
206             "\tstr %10, [%0], #4 \n"
207
208             "\tldrsh %6, [%2], #2\n"
209             "\tldr %10, [%0, #4]\n"
210             "\tmov %8, %8, lsr #15\n"
211             "\tsmull %7, %11, %5, %6\n"
212             "\tldrsh %6, [%1], #2\n"
213             "\tadd %8, %8, %9, lsl #17\n"
214             "\tadd %10, %10, %8\n"
215             "\tsmull %8, %9, %4, %6\n"
216             "\tadd %10, %10, %7, lsr #15\n"
217             "\tadd %10, %10, %11, lsl #17\n"
218             "\tstr %10, [%0], #4 \n"
219
220
221 #else
222             ".filterloop%=: \n"
223             "\tldrsh %6, [%2], #2\n"
224             "\tldr %10, [%0, #4]\n"
225             "\tmov %8, %8, lsr #15\n"
226             "\tsmull %7, %11, %5, %6\n"
227             "\tadd %8, %8, %9, lsl #17\n"
228             "\tldrsh %6, [%1], #2\n"
229             "\tadd %10, %10, %8\n"
230             "\tsmull %8, %9, %4, %6\n"
231             "\tadd %10, %10, %7, lsr #15\n"
232             "\tsubs %3, %3, #1\n"
233             "\tadd %10, %10, %11, lsl #17\n"
234             "\tstr %10, [%0], #4 \n"
235             "\t bne .filterloop%=\n"
236 #endif
237             "\tmov %8, %8, lsr #15\n"
238             "\tadd %10, %8, %9, lsl #17\n"
239             "\tldrsh %6, [%2], #2\n"
240             "\tsmull %8, %9, %5, %6\n"
241             "\tadd %10, %10, %8, lsr #15\n"
242             "\tadd %10, %10, %9, lsl #17\n"
243             "\tstr %10, [%0], #4 \n"
244
245          : "=r" (deadm), "=r" (deadn), "=r" (deadd), "=r" (deadidx),
246       "=r" (xi), "=r" (nyi), "=r" (dead1), "=r" (dead2),
247       "=r" (dead3), "=r" (dead4), "=r" (dead5), "=r" (dead6)
248          : "0" (mem), "1" (num+1), "2" (den+1), "3" (ord-1), "4" (xi), "5" (nyi)
249          : "cc", "memory");
250    
251    }
252 }
253
254 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)
255 {
256    int i,j;
257    spx_sig_t xi,yi,nyi;
258
259    for (i=0;i<N;i++)
260    {
261       int deadm, deadd, deadidx, dead1, dead2, dead3, dead4, dead5, dead6;
262       xi=SATURATE(x[i],805306368);
263       yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368);
264       nyi = -yi;
265       y[i] = yi;
266       __asm__ __volatile__ (
267             "\tldrsh %4, [%1], #2\n"
268             "\tsmull %5, %6, %3, %4\n"
269
270 #ifdef SHORTCUTS
271                         
272             "\tldrsh %4, [%1], #2\n"
273             "\tmov %5, %5, lsr #15\n"
274             "\tldr %7, [%0, #4]\n"
275             "\tadd %8, %5, %6, lsl #17\n"
276             "\tsmull %5, %6, %3, %4\n"
277             "\tadd %7, %7, %8\n"
278             "\tstr %7, [%0], #4 \n"
279
280                  
281             "\tldrsh %4, [%1], #2\n"
282             "\tmov %5, %5, lsr #15\n"
283             "\tldr %9, [%0, #4]\n"
284             "\tadd %8, %5, %6, lsl #17\n"
285             "\tsmull %5, %6, %3, %4\n"
286             "\tadd %9, %9, %8\n"
287             "\tstr %9, [%0], #4 \n"
288
289             "\tldrsh %4, [%1], #2\n"
290             "\tmov %5, %5, lsr #15\n"
291             "\tldr %7, [%0, #4]\n"
292             "\tadd %8, %5, %6, lsl #17\n"
293             "\tsmull %5, %6, %3, %4\n"
294             "\tadd %7, %7, %8\n"
295             "\tstr %7, [%0], #4 \n"
296
297             
298             "\tldrsh %4, [%1], #2\n"
299             "\tmov %5, %5, lsr #15\n"
300             "\tldr %9, [%0, #4]\n"
301             "\tadd %8, %5, %6, lsl #17\n"
302             "\tsmull %5, %6, %3, %4\n"
303             "\tadd %9, %9, %8\n"
304             "\tstr %9, [%0], #4 \n"
305
306             "\tldrsh %4, [%1], #2\n"
307             "\tmov %5, %5, lsr #15\n"
308             "\tldr %7, [%0, #4]\n"
309             "\tadd %8, %5, %6, lsl #17\n"
310             "\tsmull %5, %6, %3, %4\n"
311             "\tadd %7, %7, %8\n"
312             "\tstr %7, [%0], #4 \n"
313
314             
315             "\tldrsh %4, [%1], #2\n"
316             "\tmov %5, %5, lsr #15\n"
317             "\tldr %9, [%0, #4]\n"
318             "\tadd %8, %5, %6, lsl #17\n"
319             "\tsmull %5, %6, %3, %4\n"
320             "\tadd %9, %9, %8\n"
321             "\tstr %9, [%0], #4 \n"
322
323             "\tldrsh %4, [%1], #2\n"
324             "\tmov %5, %5, lsr #15\n"
325             "\tldr %7, [%0, #4]\n"
326             "\tadd %8, %5, %6, lsl #17\n"
327             "\tsmull %5, %6, %3, %4\n"
328             "\tadd %7, %7, %8\n"
329             "\tstr %7, [%0], #4 \n"
330
331             
332             "\tldrsh %4, [%1], #2\n"
333             "\tmov %5, %5, lsr #15\n"
334             "\tldr %9, [%0, #4]\n"
335             "\tadd %8, %5, %6, lsl #17\n"
336             "\tsmull %5, %6, %3, %4\n"
337             "\tadd %9, %9, %8\n"
338             "\tstr %9, [%0], #4 \n"
339
340             "\tldrsh %4, [%1], #2\n"
341             "\tmov %5, %5, lsr #15\n"
342             "\tldr %7, [%0, #4]\n"
343             "\tadd %8, %5, %6, lsl #17\n"
344             "\tsmull %5, %6, %3, %4\n"
345             "\tadd %7, %7, %8\n"
346             "\tstr %7, [%0], #4 \n"
347
348             
349             
350 #else
351             ".iirloop%=: \n"
352             "\tldr %7, [%0, #4]\n"
353
354             "\tldrsh %4, [%1], #2\n"
355             "\tmov %5, %5, lsr #15\n"
356             "\tadd %8, %5, %6, lsl #17\n"
357             "\tsmull %5, %6, %3, %4\n"
358             "\tadd %7, %7, %8\n"
359             "\tstr %7, [%0], #4 \n"
360             "\tsubs %2, %2, #1\n"
361             "\t bne .iirloop%=\n"
362             
363 #endif
364             "\tmov %5, %5, lsr #15\n"
365             "\tadd %7, %5, %6, lsl #17\n"
366             "\tstr %7, [%0], #4 \n"
367
368          : "=r" (deadm), "=r" (deadd), "=r" (deadidx), "=r" (nyi),
369       "=r" (dead1), "=r" (dead2), "=r" (dead3), "=r" (dead4),
370       "=r" (dead5), "=r" (dead6)
371          : "0" (mem), "1" (den+1), "2" (ord-1), "3" (nyi)
372          : "cc", "memory");
373    
374    }
375 }