Adds missing RESTORE_STACK calls
[opus.git] / celt / celt_lpc.c
1 /* Copyright (c) 2009-2010 Xiph.Org Foundation
2    Written by Jean-Marc Valin */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14
15    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
19    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
20    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
21    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "celt_lpc.h"
33 #include "stack_alloc.h"
34 #include "mathops.h"
35 #include "pitch.h"
36
37 void _celt_lpc(
38       opus_val16       *_lpc, /* out: [0...p-1] LPC coefficients      */
39 const opus_val32 *ac,  /* in:  [0...p] autocorrelation values  */
40 int          p
41 )
42 {
43    int i, j;
44    opus_val32 r;
45    opus_val32 error = ac[0];
46 #ifdef FIXED_POINT
47    opus_val32 lpc[LPC_ORDER];
48 #else
49    float *lpc = _lpc;
50 #endif
51
52    for (i = 0; i < p; i++)
53       lpc[i] = 0;
54    if (ac[0] != 0)
55    {
56       for (i = 0; i < p; i++) {
57          /* Sum up this iteration's reflection coefficient */
58          opus_val32 rr = 0;
59          for (j = 0; j < i; j++)
60             rr += MULT32_32_Q31(lpc[j],ac[i - j]);
61          rr += SHR32(ac[i + 1],3);
62          r = -frac_div32(SHL32(rr,3), error);
63          /*  Update LPC coefficients and total error */
64          lpc[i] = SHR32(r,3);
65          for (j = 0; j < (i+1)>>1; j++)
66          {
67             opus_val32 tmp1, tmp2;
68             tmp1 = lpc[j];
69             tmp2 = lpc[i-1-j];
70             lpc[j]     = tmp1 + MULT32_32_Q31(r,tmp2);
71             lpc[i-1-j] = tmp2 + MULT32_32_Q31(r,tmp1);
72          }
73
74          error = error - MULT32_32_Q31(MULT32_32_Q31(r,r),error);
75          /* Bail out once we get 30 dB gain */
76 #ifdef FIXED_POINT
77          if (error<SHR32(ac[0],10))
78             break;
79 #else
80          if (error<.001f*ac[0])
81             break;
82 #endif
83       }
84    }
85 #ifdef FIXED_POINT
86    for (i=0;i<p;i++)
87       _lpc[i] = ROUND16(lpc[i],16);
88 #endif
89 }
90
91 void celt_fir(const opus_val16 *_x,
92          const opus_val16 *num,
93          opus_val16 *_y,
94          int N,
95          int ord,
96          opus_val16 *mem)
97 {
98    int i,j;
99    VARDECL(opus_val16, rnum);
100    VARDECL(opus_val16, x);
101    SAVE_STACK;
102
103    ALLOC(rnum, ord, opus_val16);
104    ALLOC(x, N+ord, opus_val16);
105    for(i=0;i<ord;i++)
106       rnum[i] = num[ord-i-1];
107    for(i=0;i<ord;i++)
108       x[i] = mem[ord-i-1];
109    for (i=0;i<N;i++)
110       x[i+ord]=_x[i];
111    for(i=0;i<ord;i++)
112       mem[i] = _x[N-i-1];
113 #ifdef SMALL_FOOTPRINT
114    for (i=0;i<N;i++)
115    {
116       opus_val32 sum = SHL32(EXTEND32(_x[i]), SIG_SHIFT);
117       for (j=0;j<ord;j++)
118       {
119          sum = MAC16_16(sum,rnum[j],x[i+j]);
120       }
121       _y[i] = ROUND16(sum, SIG_SHIFT);
122    }
123 #else
124    celt_assert((ord&3)==0);
125    for (i=0;i<N-3;i+=4)
126    {
127       opus_val32 sum1=0;
128       opus_val32 sum2=0;
129       opus_val32 sum3=0;
130       opus_val32 sum4=0;
131       const opus_val16 *xx = x+i;
132       const opus_val16 *z = rnum;
133       opus_val16 y_0, y_1, y_2, y_3;
134       y_3=0; /* gcc doesn't realize that y_3 can't be used uninitialized */
135       y_0=*xx++;
136       y_1=*xx++;
137       y_2=*xx++;
138       for (j=0;j<ord-3;j+=4)
139       {
140          opus_val16 tmp;
141          tmp = *z++;
142          y_3=*xx++;
143          sum1 = MAC16_16(sum1,tmp,y_0);
144          sum2 = MAC16_16(sum2,tmp,y_1);
145          sum3 = MAC16_16(sum3,tmp,y_2);
146          sum4 = MAC16_16(sum4,tmp,y_3);
147          tmp=*z++;
148          y_0=*xx++;
149          sum1 = MAC16_16(sum1,tmp,y_1);
150          sum2 = MAC16_16(sum2,tmp,y_2);
151          sum3 = MAC16_16(sum3,tmp,y_3);
152          sum4 = MAC16_16(sum4,tmp,y_0);
153          tmp=*z++;
154          y_1=*xx++;
155          sum1 = MAC16_16(sum1,tmp,y_2);
156          sum2 = MAC16_16(sum2,tmp,y_3);
157          sum3 = MAC16_16(sum3,tmp,y_0);
158          sum4 = MAC16_16(sum4,tmp,y_1);
159          tmp=*z++;
160          y_2=*xx++;
161          sum1 = MAC16_16(sum1,tmp,y_3);
162          sum2 = MAC16_16(sum2,tmp,y_0);
163          sum3 = MAC16_16(sum3,tmp,y_1);
164          sum4 = MAC16_16(sum4,tmp,y_2);
165       }
166       _y[i  ] = ADD16(_x[i  ], ROUND16(sum1, SIG_SHIFT));
167       _y[i+1] = ADD16(_x[i+1], ROUND16(sum2, SIG_SHIFT));
168       _y[i+2] = ADD16(_x[i+2], ROUND16(sum3, SIG_SHIFT));
169       _y[i+3] = ADD16(_x[i+3], ROUND16(sum4, SIG_SHIFT));
170    }
171    for (;i<N;i++)
172    {
173       opus_val32 sum = 0;
174       for (j=0;j<ord;j++)
175          sum = MAC16_16(sum,rnum[j],x[i+j]);
176       _y[i] = ADD16(_x[i  ], ROUND16(sum, SIG_SHIFT));
177    }
178 #endif
179    RESTORE_STACK;
180 }
181
182 void celt_iir(const opus_val32 *_x,
183          const opus_val16 *den,
184          opus_val32 *_y,
185          int N,
186          int ord,
187          opus_val16 *mem)
188 {
189 #ifdef SMALL_FOOTPRINT
190    int i,j;
191    for (i=0;i<N;i++)
192    {
193       opus_val32 sum = _x[i];
194       for (j=0;j<ord;j++)
195       {
196          sum -= MULT16_16(den[j],mem[j]);
197       }
198       for (j=ord-1;j>=1;j--)
199       {
200          mem[j]=mem[j-1];
201       }
202       mem[0] = ROUND16(sum,SIG_SHIFT);
203       _y[i] = sum;
204    }
205 #else
206    int i,j;
207    VARDECL(opus_val16, rden);
208    VARDECL(opus_val16, y);
209    SAVE_STACK;
210
211    celt_assert((ord&3)==0);
212    ALLOC(rden, ord, opus_val16);
213    ALLOC(y, N+ord, opus_val16);
214    for(i=0;i<ord;i++)
215       rden[i] = den[ord-i-1];
216    for(i=0;i<ord;i++)
217       y[i] = -mem[ord-i-1];
218    for(;i<N+ord;i++)
219       y[i]=0;
220    for (i=0;i<N-3;i+=4)
221    {
222       opus_val32 sum1=0;
223       opus_val32 sum2=0;
224       opus_val32 sum3=0;
225       opus_val32 sum4=0;
226       const opus_val16 *yy = y+i;
227       const opus_val16 *z = rden;
228       opus_val16 y_0, y_1, y_2, y_3;
229       sum1 = _x[i  ];
230       sum2 = _x[i+1];
231       sum3 = _x[i+2];
232       sum4 = _x[i+3];
233       y_3=0; /* gcc doesn't realize that y_3 can't be used uninitialized */
234       y_0=*yy++;
235       y_1=*yy++;
236       y_2=*yy++;
237       for (j=0;j<ord-3;j+=4)
238       {
239          opus_val16 tmp;
240          tmp = *z++;
241          y_3=*yy++;
242          sum1 = MAC16_16(sum1,tmp,y_0);
243          sum2 = MAC16_16(sum2,tmp,y_1);
244          sum3 = MAC16_16(sum3,tmp,y_2);
245          sum4 = MAC16_16(sum4,tmp,y_3);
246          tmp=*z++;
247          y_0=*yy++;
248          sum1 = MAC16_16(sum1,tmp,y_1);
249          sum2 = MAC16_16(sum2,tmp,y_2);
250          sum3 = MAC16_16(sum3,tmp,y_3);
251          sum4 = MAC16_16(sum4,tmp,y_0);
252          tmp=*z++;
253          y_1=*yy++;
254          sum1 = MAC16_16(sum1,tmp,y_2);
255          sum2 = MAC16_16(sum2,tmp,y_3);
256          sum3 = MAC16_16(sum3,tmp,y_0);
257          sum4 = MAC16_16(sum4,tmp,y_1);
258          tmp=*z++;
259          y_2=*yy++;
260          sum1 = MAC16_16(sum1,tmp,y_3);
261          sum2 = MAC16_16(sum2,tmp,y_0);
262          sum3 = MAC16_16(sum3,tmp,y_1);
263          sum4 = MAC16_16(sum4,tmp,y_2);
264       }
265       y[i+ord  ] = -ROUND16(sum1,SIG_SHIFT);
266       _y[i  ] = sum1;
267       sum2 = MAC16_16(sum2, y[i+ord  ], den[0]);
268       y[i+ord+1] = -ROUND16(sum2,SIG_SHIFT);
269       _y[i+1] = sum2;
270       sum3 = MAC16_16(sum3, y[i+ord+1], den[0]);
271       sum3 = MAC16_16(sum3, y[i+ord  ], den[1]);
272       y[i+ord+2] = -ROUND16(sum3,SIG_SHIFT);
273       _y[i+2] = sum3;
274
275       sum4 = MAC16_16(sum4, y[i+ord+2], den[0]);
276       sum4 = MAC16_16(sum4, y[i+ord+1], den[1]);
277       sum4 = MAC16_16(sum4, y[i+ord  ], den[2]);
278       y[i+ord+3] = -ROUND16(sum4,SIG_SHIFT);
279       _y[i+3] = sum4;
280    }
281    for (;i<N;i++)
282    {
283       opus_val32 sum = _x[i];
284       for (j=0;j<ord;j++)
285          sum -= MULT16_16(rden[j],y[i+j]);
286       y[i+ord] = ROUND16(sum,SIG_SHIFT);
287       _y[i] = sum;
288    }
289    for(i=0;i<ord;i++)
290       mem[i] = _y[N-i-1];
291 #endif
292    RESTORE_STACK;
293 }
294
295 void _celt_autocorr(
296                    const opus_val16 *x,   /*  in: [0...n-1] samples x   */
297                    opus_val32       *ac,  /* out: [0...lag-1] ac values */
298                    const opus_val16       *window,
299                    int          overlap,
300                    int          lag,
301                    int          n
302                   )
303 {
304    opus_val32 d;
305    int i;
306    int fastN=n-lag;
307    VARDECL(opus_val16, xx);
308    SAVE_STACK;
309    ALLOC(xx, n, opus_val16);
310    celt_assert(n>0);
311    celt_assert(overlap>=0);
312    for (i=0;i<n;i++)
313       xx[i] = x[i];
314    for (i=0;i<overlap;i++)
315    {
316       xx[i] = MULT16_16_Q15(x[i],window[i]);
317       xx[n-i-1] = MULT16_16_Q15(x[n-i-1],window[i]);
318    }
319 #ifdef FIXED_POINT
320    {
321       opus_val32 ac0;
322       int shift;
323       ac0 = 1+n;
324       if (n&1) ac0 += SHR32(MULT16_16(xx[0],xx[0]),9);
325       for(i=(n&1);i<n;i+=2)
326       {
327          ac0 += SHR32(MULT16_16(xx[i],xx[i]),9);
328          ac0 += SHR32(MULT16_16(xx[i+1],xx[i+1]),9);
329       }
330
331       shift = celt_ilog2(ac0)-30+10;
332       shift = (shift+1)/2;
333       for(i=0;i<n;i++)
334          xx[i] = VSHR32(xx[i], shift);
335    }
336 #endif
337    pitch_xcorr(xx, xx, ac, fastN, lag+1);
338    while (lag>=0)
339    {
340       for (i = lag+fastN, d = 0; i < n; i++)
341          d = MAC16_16(d, xx[i], xx[i-lag]);
342       ac[lag] += d;
343       /*printf ("%f ", ac[lag]);*/
344       lag--;
345    }
346    /*printf ("\n");*/
347    ac[0] += 10;
348
349    RESTORE_STACK;
350 }