More work on fixed-point Levinson-Durbin
[opus.git] / libcelt / plc.c
1
2 #ifndef NEW_PLC
3 #define NEW_PLC
4 #endif
5
6 #ifdef FIXED_POINT
7 #define frac_div(a,b) ((celt_word32)((32768*65535+32767)*(float)(a)/(b)))
8 #else
9 #define frac_div(a,b) ((float)(a)/(b))
10 #endif
11
12
13 float _celt_lpc(
14       celt_word16       *_lpc, /* out: [0...p-1] LPC coefficients      */
15 const celt_word32 *ac,  /* in:  [0...p] autocorrelation values  */
16 int          p
17 )
18 {
19    int i, j;  
20    celt_word32 r;
21    celt_word32 error = ac[0];
22 #ifdef FIXED_POINT
23    celt_word32 lpc[LPC_ORDER];
24 #else
25    float *lpc = _lpc;
26 #endif
27
28    for (i = 0; i < p; i++)
29       lpc[i] = 0;
30    if (ac[0] != 0)
31    {
32       for (i = 0; i < p; i++) {
33          /* Sum up this iteration's reflection coefficient */
34          celt_word32 rr = 0;
35          for (j = 0; j < i; j++)
36             rr += MULT32_32_Q31(lpc[j],ac[i - j]);
37          rr += SHR32(ac[i + 1],3);
38          //r = -RC_SCALING*1.*SHL32(rr,3)/(error+1e-15);
39          r = -frac_div(SHL32(rr,3), error);
40          /*  Update LPC coefficients and total error */
41          lpc[i] = SHR32(r,3);
42          for (j = 0; j < (i+1)>>1; j++)
43          {
44             celt_word32 tmp1, tmp2;
45             tmp1 = lpc[j];
46             tmp2 = lpc[i-1-j];
47             lpc[j]     = tmp1 + MULT32_32_Q31(r,tmp2);
48             lpc[i-1-j] = tmp2 + MULT32_32_Q31(r,tmp1);
49          }
50
51          error = error - MULT32_32_Q31(MULT32_32_Q31(r,r),error);
52          if (error<.00001*ac[0])
53             break;
54       }
55    }
56 #ifdef FIXED_POINT
57    for (i=0;i<p;i++)
58       _lpc[i] = ROUND16(lpc[i],16);
59 #endif
60    return error;
61 }
62
63 void fir(const celt_word16 *x,
64          const celt_word16 *num,
65          celt_word16 *y,
66          int N,
67          int ord,
68          celt_word16 *mem)
69 {
70    int i,j;
71
72    for (i=0;i<N;i++)
73    {
74       celt_word32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
75       for (j=0;j<ord;j++)
76       {
77          sum += MULT16_16(num[j],mem[j]);
78       }
79       for (j=ord-1;j>=1;j--)
80       {
81          mem[j]=mem[j-1];
82       }
83       mem[0] = x[i];
84       y[i] = ROUND16(sum, SIG_SHIFT);
85    }
86 }
87
88 void iir(const celt_word32 *x,
89          const celt_word16 *den,
90          celt_word32 *y,
91          int N,
92          int ord,
93          celt_word16 *mem)
94 {
95    int i,j;
96    for (i=0;i<N;i++)
97    {
98       celt_word32 sum = x[i];
99       for (j=0;j<ord;j++)
100       {
101          sum -= MULT16_16(den[j],mem[j]);
102       }
103       for (j=ord-1;j>=1;j--)
104       {
105          mem[j]=mem[j-1];
106       }
107       mem[0] = ROUND16(sum,SIG_SHIFT);
108       y[i] = sum;
109    }
110 }
111
112 void _celt_autocorr(
113                    const celt_word16 *x,   /*  in: [0...n-1] samples x   */
114                    celt_word32       *ac,  /* out: [0...lag-1] ac values */
115                    const celt_word16       *window,
116                    int          overlap,
117                    int          lag, 
118                    int          n
119                   )
120 {
121    float d;
122    int i;
123    float scale=1;
124    VARDECL(float, xx);
125    SAVE_STACK;
126    ALLOC(xx, n, float);
127    for (i=0;i<n;i++)
128       xx[i] = x[i];
129    for (i=0;i<overlap;i++)
130    {
131       xx[i] *= (1./Q15ONE)*window[i];
132       xx[n-i-1] *= (1./Q15ONE)*window[i];
133    }
134 #ifdef FIXED_POINT
135    {
136       float ac0=0;
137       for(i=0;i<n;i++)
138          ac0 += x[i]*x[i];
139       ac0+=10;
140       scale = 2000000000/ac0;
141    }
142 #endif
143    while (lag>=0)
144    {
145       for (i = lag, d = 0; i < n; i++) 
146          d += x[i] * x[i-lag];
147       ac[lag] = d*scale;
148       /*printf ("%f ", ac[lag]);*/
149       lag--;
150    }
151    /*printf ("\n");*/
152    ac[0] += 10;
153
154    RESTORE_STACK;
155 }