Revise celt_fir_c() to not pass in argument "mem"
[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 #ifdef SMALL_FOOTPRINT
107    (void)arch;
108    for (i=0;i<N;i++)
109    {
110       opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
111       for (j=0;j<ord;j++)
112       {
113          sum = MAC16_16(sum,rnum[j],x[i+j-ord]);
114       }
115       y[i] = SATURATE16(PSHR32(sum, SIG_SHIFT));
116    }
117 #else
118    for (i=0;i<N-3;i+=4)
119    {
120       opus_val32 sum[4]={0,0,0,0};
121       xcorr_kernel(rnum, x+i-ord, sum, ord, arch);
122       y[i  ] = SATURATE16(ADD32(EXTEND32(x[i  ]), PSHR32(sum[0], SIG_SHIFT)));
123       y[i+1] = SATURATE16(ADD32(EXTEND32(x[i+1]), PSHR32(sum[1], SIG_SHIFT)));
124       y[i+2] = SATURATE16(ADD32(EXTEND32(x[i+2]), PSHR32(sum[2], SIG_SHIFT)));
125       y[i+3] = SATURATE16(ADD32(EXTEND32(x[i+3]), PSHR32(sum[3], SIG_SHIFT)));
126    }
127    for (;i<N;i++)
128    {
129       opus_val32 sum = 0;
130       for (j=0;j<ord;j++)
131          sum = MAC16_16(sum,rnum[j],x[i+j-ord]);
132       y[i] = SATURATE16(ADD32(EXTEND32(x[i]), PSHR32(sum, SIG_SHIFT)));
133    }
134 #endif
135    RESTORE_STACK;
136 }
137
138 void celt_iir(const opus_val32 *_x,
139          const opus_val16 *den,
140          opus_val32 *_y,
141          int N,
142          int ord,
143          opus_val16 *mem,
144          int arch)
145 {
146 #ifdef SMALL_FOOTPRINT
147    int i,j;
148    (void)arch;
149    for (i=0;i<N;i++)
150    {
151       opus_val32 sum = _x[i];
152       for (j=0;j<ord;j++)
153       {
154          sum -= MULT16_16(den[j],mem[j]);
155       }
156       for (j=ord-1;j>=1;j--)
157       {
158          mem[j]=mem[j-1];
159       }
160       mem[0] = SROUND16(sum, SIG_SHIFT);
161       _y[i] = sum;
162    }
163 #else
164    int i,j;
165    VARDECL(opus_val16, rden);
166    VARDECL(opus_val16, y);
167    SAVE_STACK;
168
169    celt_assert((ord&3)==0);
170    ALLOC(rden, ord, opus_val16);
171    ALLOC(y, N+ord, opus_val16);
172    for(i=0;i<ord;i++)
173       rden[i] = den[ord-i-1];
174    for(i=0;i<ord;i++)
175       y[i] = -mem[ord-i-1];
176    for(;i<N+ord;i++)
177       y[i]=0;
178    for (i=0;i<N-3;i+=4)
179    {
180       /* Unroll by 4 as if it were an FIR filter */
181       opus_val32 sum[4];
182       sum[0]=_x[i];
183       sum[1]=_x[i+1];
184       sum[2]=_x[i+2];
185       sum[3]=_x[i+3];
186       xcorr_kernel(rden, y+i, sum, ord, arch);
187
188       /* Patch up the result to compensate for the fact that this is an IIR */
189       y[i+ord  ] = -SROUND16(sum[0],SIG_SHIFT);
190       _y[i  ] = sum[0];
191       sum[1] = MAC16_16(sum[1], y[i+ord  ], den[0]);
192       y[i+ord+1] = -SROUND16(sum[1],SIG_SHIFT);
193       _y[i+1] = sum[1];
194       sum[2] = MAC16_16(sum[2], y[i+ord+1], den[0]);
195       sum[2] = MAC16_16(sum[2], y[i+ord  ], den[1]);
196       y[i+ord+2] = -SROUND16(sum[2],SIG_SHIFT);
197       _y[i+2] = sum[2];
198
199       sum[3] = MAC16_16(sum[3], y[i+ord+2], den[0]);
200       sum[3] = MAC16_16(sum[3], y[i+ord+1], den[1]);
201       sum[3] = MAC16_16(sum[3], y[i+ord  ], den[2]);
202       y[i+ord+3] = -SROUND16(sum[3],SIG_SHIFT);
203       _y[i+3] = sum[3];
204    }
205    for (;i<N;i++)
206    {
207       opus_val32 sum = _x[i];
208       for (j=0;j<ord;j++)
209          sum -= MULT16_16(rden[j],y[i+j]);
210       y[i+ord] = SROUND16(sum,SIG_SHIFT);
211       _y[i] = sum;
212    }
213    for(i=0;i<ord;i++)
214       mem[i] = _y[N-i-1];
215    RESTORE_STACK;
216 #endif
217 }
218
219 int _celt_autocorr(
220                    const opus_val16 *x,   /*  in: [0...n-1] samples x   */
221                    opus_val32       *ac,  /* out: [0...lag-1] ac values */
222                    const opus_val16       *window,
223                    int          overlap,
224                    int          lag,
225                    int          n,
226                    int          arch
227                   )
228 {
229    opus_val32 d;
230    int i, k;
231    int fastN=n-lag;
232    int shift;
233    const opus_val16 *xptr;
234    VARDECL(opus_val16, xx);
235    SAVE_STACK;
236    ALLOC(xx, n, opus_val16);
237    celt_assert(n>0);
238    celt_assert(overlap>=0);
239    if (overlap == 0)
240    {
241       xptr = x;
242    } else {
243       for (i=0;i<n;i++)
244          xx[i] = x[i];
245       for (i=0;i<overlap;i++)
246       {
247          xx[i] = MULT16_16_Q15(x[i],window[i]);
248          xx[n-i-1] = MULT16_16_Q15(x[n-i-1],window[i]);
249       }
250       xptr = xx;
251    }
252    shift=0;
253 #ifdef FIXED_POINT
254    {
255       opus_val32 ac0;
256       ac0 = 1+(n<<7);
257       if (n&1) ac0 += SHR32(MULT16_16(xptr[0],xptr[0]),9);
258       for(i=(n&1);i<n;i+=2)
259       {
260          ac0 += SHR32(MULT16_16(xptr[i],xptr[i]),9);
261          ac0 += SHR32(MULT16_16(xptr[i+1],xptr[i+1]),9);
262       }
263
264       shift = celt_ilog2(ac0)-30+10;
265       shift = (shift)/2;
266       if (shift>0)
267       {
268          for(i=0;i<n;i++)
269             xx[i] = PSHR32(xptr[i], shift);
270          xptr = xx;
271       } else
272          shift = 0;
273    }
274 #endif
275    celt_pitch_xcorr(xptr, xptr, ac, fastN, lag+1, arch);
276    for (k=0;k<=lag;k++)
277    {
278       for (i = k+fastN, d = 0; i < n; i++)
279          d = MAC16_16(d, xptr[i], xptr[i-k]);
280       ac[k] += d;
281    }
282 #ifdef FIXED_POINT
283    shift = 2*shift;
284    if (shift<=0)
285       ac[0] += SHL32((opus_int32)1, -shift);
286    if (ac[0] < 268435456)
287    {
288       int shift2 = 29 - EC_ILOG(ac[0]);
289       for (i=0;i<=lag;i++)
290          ac[i] = SHL32(ac[i], shift2);
291       shift -= shift2;
292    } else if (ac[0] >= 536870912)
293    {
294       int shift2=1;
295       if (ac[0] >= 1073741824)
296          shift2++;
297       for (i=0;i<=lag;i++)
298          ac[i] = SHR32(ac[i], shift2);
299       shift += shift2;
300    }
301 #endif
302
303    RESTORE_STACK;
304    return shift;
305 }