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