Speeds up celt_iir() by more than a factor of two.
[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 }
180
181 void celt_iir(const opus_val32 *_x,
182          const opus_val16 *den,
183          opus_val32 *_y,
184          int N,
185          int ord,
186          opus_val16 *mem)
187 {
188 #ifdef SMALL_FOOTPRINT
189    int i,j;
190    for (i=0;i<N;i++)
191    {
192       opus_val32 sum = _x[i];
193       for (j=0;j<ord;j++)
194       {
195          sum -= MULT16_16(den[j],mem[j]);
196       }
197       for (j=ord-1;j>=1;j--)
198       {
199          mem[j]=mem[j-1];
200       }
201       mem[0] = ROUND16(sum,SIG_SHIFT);
202       _y[i] = sum;
203    }
204 #else
205    int i,j;
206    VARDECL(opus_val16, rden);
207    VARDECL(opus_val16, y);
208    SAVE_STACK;
209
210    celt_assert((ord&3)==0);
211    ALLOC(rden, ord, opus_val16);
212    ALLOC(y, N+ord, opus_val16);
213    for(i=0;i<ord;i++)
214       rden[i] = den[ord-i-1];
215    for(i=0;i<ord;i++)
216       y[i] = -mem[ord-i-1];
217    for(;i<N+ord;i++)
218       y[i]=0;
219    for (i=0;i<N-3;i+=4)
220    {
221       opus_val32 sum1=0;
222       opus_val32 sum2=0;
223       opus_val32 sum3=0;
224       opus_val32 sum4=0;
225       const opus_val16 *yy = y+i;
226       const opus_val16 *z = rden;
227       opus_val16 y_0, y_1, y_2, y_3;
228       sum1 = _x[i  ];
229       sum2 = _x[i+1];
230       sum3 = _x[i+2];
231       sum4 = _x[i+3];
232       y_3=0; /* gcc doesn't realize that y_3 can't be used uninitialized */
233       y_0=*yy++;
234       y_1=*yy++;
235       y_2=*yy++;
236       for (j=0;j<ord-3;j+=4)
237       {
238          opus_val16 tmp;
239          tmp = *z++;
240          y_3=*yy++;
241          sum1 = MAC16_16(sum1,tmp,y_0);
242          sum2 = MAC16_16(sum2,tmp,y_1);
243          sum3 = MAC16_16(sum3,tmp,y_2);
244          sum4 = MAC16_16(sum4,tmp,y_3);
245          tmp=*z++;
246          y_0=*yy++;
247          sum1 = MAC16_16(sum1,tmp,y_1);
248          sum2 = MAC16_16(sum2,tmp,y_2);
249          sum3 = MAC16_16(sum3,tmp,y_3);
250          sum4 = MAC16_16(sum4,tmp,y_0);
251          tmp=*z++;
252          y_1=*yy++;
253          sum1 = MAC16_16(sum1,tmp,y_2);
254          sum2 = MAC16_16(sum2,tmp,y_3);
255          sum3 = MAC16_16(sum3,tmp,y_0);
256          sum4 = MAC16_16(sum4,tmp,y_1);
257          tmp=*z++;
258          y_2=*yy++;
259          sum1 = MAC16_16(sum1,tmp,y_3);
260          sum2 = MAC16_16(sum2,tmp,y_0);
261          sum3 = MAC16_16(sum3,tmp,y_1);
262          sum4 = MAC16_16(sum4,tmp,y_2);
263       }
264       y[i+ord  ] = -ROUND16(sum1,SIG_SHIFT);
265       _y[i  ] = sum1;
266       sum2 = MAC16_16(sum2, y[i+ord  ], den[0]);
267       y[i+ord+1] = -ROUND16(sum2,SIG_SHIFT);
268       _y[i+1] = sum2;
269       sum3 = MAC16_16(sum3, y[i+ord+1], den[0]);
270       sum3 = MAC16_16(sum3, y[i+ord  ], den[1]);
271       y[i+ord+2] = -ROUND16(sum3,SIG_SHIFT);
272       _y[i+2] = sum3;
273
274       sum4 = MAC16_16(sum4, y[i+ord+2], den[0]);
275       sum4 = MAC16_16(sum4, y[i+ord+1], den[1]);
276       sum4 = MAC16_16(sum4, y[i+ord  ], den[2]);
277       y[i+ord+3] = -ROUND16(sum4,SIG_SHIFT);
278       _y[i+3] = sum4;
279    }
280    for (;i<N;i++)
281    {
282       opus_val32 sum = _x[i];
283       for (j=0;j<ord;j++)
284          sum -= MULT16_16(rden[j],y[i+j]);
285       y[i+ord] = ROUND16(sum,SIG_SHIFT);
286       _y[i] = sum;
287    }
288    for(i=0;i<ord;i++)
289       mem[i] = _y[N-i-1];
290 #endif
291 }
292
293 void _celt_autocorr(
294                    const opus_val16 *x,   /*  in: [0...n-1] samples x   */
295                    opus_val32       *ac,  /* out: [0...lag-1] ac values */
296                    const opus_val16       *window,
297                    int          overlap,
298                    int          lag,
299                    int          n
300                   )
301 {
302    opus_val32 d;
303    int i;
304    int fastN=n-lag;
305    VARDECL(opus_val16, xx);
306    SAVE_STACK;
307    ALLOC(xx, n, opus_val16);
308    celt_assert(n>0);
309    celt_assert(overlap>=0);
310    for (i=0;i<n;i++)
311       xx[i] = x[i];
312    for (i=0;i<overlap;i++)
313    {
314       xx[i] = MULT16_16_Q15(x[i],window[i]);
315       xx[n-i-1] = MULT16_16_Q15(x[n-i-1],window[i]);
316    }
317 #ifdef FIXED_POINT
318    {
319       opus_val32 ac0;
320       int shift;
321       ac0 = 1+n;
322       if (n&1) ac0 += SHR32(MULT16_16(xx[0],xx[0]),9);
323       for(i=(n&1);i<n;i+=2)
324       {
325          ac0 += SHR32(MULT16_16(xx[i],xx[i]),9);
326          ac0 += SHR32(MULT16_16(xx[i+1],xx[i+1]),9);
327       }
328
329       shift = celt_ilog2(ac0)-30+10;
330       shift = (shift+1)/2;
331       for(i=0;i<n;i++)
332          xx[i] = VSHR32(xx[i], shift);
333    }
334 #endif
335    pitch_xcorr(xx, xx, ac, fastN, lag+1);
336    while (lag>=0)
337    {
338       for (i = lag+fastN, d = 0; i < n; i++)
339          d = MAC16_16(d, xx[i], xx[i-lag]);
340       ac[lag] += d;
341       /*printf ("%f ", ac[lag]);*/
342       lag--;
343    }
344    /*printf ("\n");*/
345    ac[0] += 10;
346
347    RESTORE_STACK;
348 }