7947fc9d7ba9f0ac676a30cc59aab5281324b88e
[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    OPUS_CLEAR(lpc, p);
53    if (ac[0] != 0)
54    {
55       for (i = 0; i < p; i++) {
56          /* Sum up this iteration's reflection coefficient */
57          opus_val32 rr = 0;
58          for (j = 0; j < i; j++)
59             rr += MULT32_32_Q31(lpc[j],ac[i - j]);
60          rr += SHR32(ac[i + 1],3);
61          r = -frac_div32(SHL32(rr,3), error);
62          /*  Update LPC coefficients and total error */
63          lpc[i] = SHR32(r,3);
64          for (j = 0; j < (i+1)>>1; j++)
65          {
66             opus_val32 tmp1, tmp2;
67             tmp1 = lpc[j];
68             tmp2 = lpc[i-1-j];
69             lpc[j]     = tmp1 + MULT32_32_Q31(r,tmp2);
70             lpc[i-1-j] = tmp2 + MULT32_32_Q31(r,tmp1);
71          }
72
73          error = error - MULT32_32_Q31(MULT32_32_Q31(r,r),error);
74          /* Bail out once we get 30 dB gain */
75 #ifdef FIXED_POINT
76          if (error<SHR32(ac[0],10))
77             break;
78 #else
79          if (error<.001f*ac[0])
80             break;
81 #endif
82       }
83    }
84 #ifdef FIXED_POINT
85    for (i=0;i<p;i++)
86       _lpc[i] = ROUND16(lpc[i],16);
87 #endif
88 }
89
90
91 void celt_fir_c(
92          const opus_val16 *x,
93          const opus_val16 *num,
94          opus_val16 *y,
95          int N,
96          int ord,
97          int arch)
98 {
99    int i,j;
100    VARDECL(opus_val16, rnum);
101    SAVE_STACK;
102
103    ALLOC(rnum, ord, opus_val16);
104    for(i=0;i<ord;i++)
105       rnum[i] = num[ord-i-1];
106    for (i=0;i<N-3;i+=4)
107    {
108       opus_val32 sum[4]={SHL32(EXTEND32(x[i  ]), SIG_SHIFT),
109                          SHL32(EXTEND32(x[i+1]), SIG_SHIFT),
110                          SHL32(EXTEND32(x[i+2]), SIG_SHIFT),
111                          SHL32(EXTEND32(x[i+3]), SIG_SHIFT)};
112       xcorr_kernel(rnum, x+i-ord, sum, ord, arch);
113       y[i  ] = ROUND16(sum[0], SIG_SHIFT);
114       y[i+1] = ROUND16(sum[1], SIG_SHIFT);
115       y[i+2] = ROUND16(sum[2], SIG_SHIFT);
116       y[i+3] = ROUND16(sum[3], SIG_SHIFT);
117    }
118    for (;i<N;i++)
119    {
120       opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
121       for (j=0;j<ord;j++)
122          sum = MAC16_16(sum,rnum[j],x[i+j-ord]);
123       y[i] = ROUND16(sum, SIG_SHIFT);
124    }
125    RESTORE_STACK;
126 }
127
128 void celt_iir(const opus_val32 *_x,
129          const opus_val16 *den,
130          opus_val32 *_y,
131          int N,
132          int ord,
133          opus_val16 *mem,
134          int arch)
135 {
136 #ifdef SMALL_FOOTPRINT
137    int i,j;
138    (void)arch;
139    for (i=0;i<N;i++)
140    {
141       opus_val32 sum = _x[i];
142       for (j=0;j<ord;j++)
143       {
144          sum -= MULT16_16(den[j],mem[j]);
145       }
146       for (j=ord-1;j>=1;j--)
147       {
148          mem[j]=mem[j-1];
149       }
150       mem[0] = SROUND16(sum, SIG_SHIFT);
151       _y[i] = sum;
152    }
153 #else
154    int i,j;
155    VARDECL(opus_val16, rden);
156    VARDECL(opus_val16, y);
157    SAVE_STACK;
158
159    celt_assert((ord&3)==0);
160    ALLOC(rden, ord, opus_val16);
161    ALLOC(y, N+ord, opus_val16);
162    for(i=0;i<ord;i++)
163       rden[i] = den[ord-i-1];
164    for(i=0;i<ord;i++)
165       y[i] = -mem[ord-i-1];
166    for(;i<N+ord;i++)
167       y[i]=0;
168    for (i=0;i<N-3;i+=4)
169    {
170       /* Unroll by 4 as if it were an FIR filter */
171       opus_val32 sum[4];
172       sum[0]=_x[i];
173       sum[1]=_x[i+1];
174       sum[2]=_x[i+2];
175       sum[3]=_x[i+3];
176       xcorr_kernel(rden, y+i, sum, ord, arch);
177
178       /* Patch up the result to compensate for the fact that this is an IIR */
179       y[i+ord  ] = -SROUND16(sum[0],SIG_SHIFT);
180       _y[i  ] = sum[0];
181       sum[1] = MAC16_16(sum[1], y[i+ord  ], den[0]);
182       y[i+ord+1] = -SROUND16(sum[1],SIG_SHIFT);
183       _y[i+1] = sum[1];
184       sum[2] = MAC16_16(sum[2], y[i+ord+1], den[0]);
185       sum[2] = MAC16_16(sum[2], y[i+ord  ], den[1]);
186       y[i+ord+2] = -SROUND16(sum[2],SIG_SHIFT);
187       _y[i+2] = sum[2];
188
189       sum[3] = MAC16_16(sum[3], y[i+ord+2], den[0]);
190       sum[3] = MAC16_16(sum[3], y[i+ord+1], den[1]);
191       sum[3] = MAC16_16(sum[3], y[i+ord  ], den[2]);
192       y[i+ord+3] = -SROUND16(sum[3],SIG_SHIFT);
193       _y[i+3] = sum[3];
194    }
195    for (;i<N;i++)
196    {
197       opus_val32 sum = _x[i];
198       for (j=0;j<ord;j++)
199          sum -= MULT16_16(rden[j],y[i+j]);
200       y[i+ord] = SROUND16(sum,SIG_SHIFT);
201       _y[i] = sum;
202    }
203    for(i=0;i<ord;i++)
204       mem[i] = _y[N-i-1];
205    RESTORE_STACK;
206 #endif
207 }
208
209 int _celt_autocorr(
210                    const opus_val16 *x,   /*  in: [0...n-1] samples x   */
211                    opus_val32       *ac,  /* out: [0...lag-1] ac values */
212                    const opus_val16       *window,
213                    int          overlap,
214                    int          lag,
215                    int          n,
216                    int          arch
217                   )
218 {
219    opus_val32 d;
220    int i, k;
221    int fastN=n-lag;
222    int shift;
223    const opus_val16 *xptr;
224    VARDECL(opus_val16, xx);
225    SAVE_STACK;
226    ALLOC(xx, n, opus_val16);
227    celt_assert(n>0);
228    celt_assert(overlap>=0);
229    if (overlap == 0)
230    {
231       xptr = x;
232    } else {
233       for (i=0;i<n;i++)
234          xx[i] = x[i];
235       for (i=0;i<overlap;i++)
236       {
237          xx[i] = MULT16_16_Q15(x[i],window[i]);
238          xx[n-i-1] = MULT16_16_Q15(x[n-i-1],window[i]);
239       }
240       xptr = xx;
241    }
242    shift=0;
243 #ifdef FIXED_POINT
244    {
245       opus_val32 ac0;
246       ac0 = 1+(n<<7);
247       if (n&1) ac0 += SHR32(MULT16_16(xptr[0],xptr[0]),9);
248       for(i=(n&1);i<n;i+=2)
249       {
250          ac0 += SHR32(MULT16_16(xptr[i],xptr[i]),9);
251          ac0 += SHR32(MULT16_16(xptr[i+1],xptr[i+1]),9);
252       }
253
254       shift = celt_ilog2(ac0)-30+10;
255       shift = (shift)/2;
256       if (shift>0)
257       {
258          for(i=0;i<n;i++)
259             xx[i] = PSHR32(xptr[i], shift);
260          xptr = xx;
261       } else
262          shift = 0;
263    }
264 #endif
265    celt_pitch_xcorr(xptr, xptr, ac, fastN, lag+1, arch);
266    for (k=0;k<=lag;k++)
267    {
268       for (i = k+fastN, d = 0; i < n; i++)
269          d = MAC16_16(d, xptr[i], xptr[i-k]);
270       ac[k] += d;
271    }
272 #ifdef FIXED_POINT
273    shift = 2*shift;
274    if (shift<=0)
275       ac[0] += SHL32((opus_int32)1, -shift);
276    if (ac[0] < 268435456)
277    {
278       int shift2 = 29 - EC_ILOG(ac[0]);
279       for (i=0;i<=lag;i++)
280          ac[i] = SHL32(ac[i], shift2);
281       shift -= shift2;
282    } else if (ac[0] >= 536870912)
283    {
284       int shift2=1;
285       if (ac[0] >= 1073741824)
286          shift2++;
287       for (i=0;i<=lag;i++)
288          ac[i] = SHR32(ac[i], shift2);
289       shift += shift2;
290    }
291 #endif
292
293    RESTORE_STACK;
294    return shift;
295 }