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