8983121473e7a1d44dd973992939207f8dd1f23c
[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    int i,j;
189    for (i=0;i<N;i++)
190    {
191       opus_val32 sum = x[i];
192       for (j=0;j<ord;j++)
193       {
194          sum -= MULT16_16(den[j],mem[j]);
195       }
196       for (j=ord-1;j>=1;j--)
197       {
198          mem[j]=mem[j-1];
199       }
200       mem[0] = ROUND16(sum,SIG_SHIFT);
201       y[i] = sum;
202    }
203 }
204
205 void _celt_autocorr(
206                    const opus_val16 *x,   /*  in: [0...n-1] samples x   */
207                    opus_val32       *ac,  /* out: [0...lag-1] ac values */
208                    const opus_val16       *window,
209                    int          overlap,
210                    int          lag,
211                    int          n
212                   )
213 {
214    opus_val32 d;
215    int i;
216    int fastN=n-lag;
217    VARDECL(opus_val16, xx);
218    SAVE_STACK;
219    ALLOC(xx, n, opus_val16);
220    celt_assert(n>0);
221    celt_assert(overlap>=0);
222    for (i=0;i<n;i++)
223       xx[i] = x[i];
224    for (i=0;i<overlap;i++)
225    {
226       xx[i] = MULT16_16_Q15(x[i],window[i]);
227       xx[n-i-1] = MULT16_16_Q15(x[n-i-1],window[i]);
228    }
229 #ifdef FIXED_POINT
230    {
231       opus_val32 ac0;
232       int shift;
233       ac0 = 1+n;
234       if (n&1) ac0 += SHR32(MULT16_16(xx[0],xx[0]),9);
235       for(i=(n&1);i<n;i+=2)
236       {
237          ac0 += SHR32(MULT16_16(xx[i],xx[i]),9);
238          ac0 += SHR32(MULT16_16(xx[i+1],xx[i+1]),9);
239       }
240
241       shift = celt_ilog2(ac0)-30+10;
242       shift = (shift+1)/2;
243       for(i=0;i<n;i++)
244          xx[i] = VSHR32(xx[i], shift);
245    }
246 #endif
247    pitch_xcorr(xx, xx, ac, fastN, lag+1);
248    while (lag>=0)
249    {
250       for (i = lag+fastN, d = 0; i < n; i++)
251          d = MAC16_16(d, xx[i], xx[i-lag]);
252       ac[lag] += d;
253       /*printf ("%f ", ac[lag]);*/
254       lag--;
255    }
256    /*printf ("\n");*/
257    ac[0] += 10;
258
259    RESTORE_STACK;
260 }