Fixed-point version of frac_div32()
[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_word32 rcp, result, rem;
10    while (b<(1<<30))
11    {
12       a = SHL32(a,1);
13       b = SHL32(b,1);
14    }
15    rcp = PSHR32(celt_rcp(ROUND16(b,16)),2);
16    result = SHL32(MULT16_32_Q15(rcp, a),1);
17    rem = a-MULT32_32_Q31(result, b);
18    result += SHL32(MULT16_32_Q15(rcp, rem),1);
19    return result;
20 }
21 #else
22 #define frac_div32(a,b) ((float)(a)/(b))
23 #endif
24
25
26 float _celt_lpc(
27       celt_word16       *_lpc, /* out: [0...p-1] LPC coefficients      */
28 const celt_word32 *ac,  /* in:  [0...p] autocorrelation values  */
29 int          p
30 )
31 {
32    int i, j;  
33    celt_word32 r;
34    celt_word32 error = ac[0];
35 #ifdef FIXED_POINT
36    celt_word32 lpc[LPC_ORDER];
37 #else
38    float *lpc = _lpc;
39 #endif
40
41    for (i = 0; i < p; i++)
42       lpc[i] = 0;
43    if (ac[0] != 0)
44    {
45       for (i = 0; i < p; i++) {
46          /* Sum up this iteration's reflection coefficient */
47          celt_word32 rr = 0;
48          for (j = 0; j < i; j++)
49             rr += MULT32_32_Q31(lpc[j],ac[i - j]);
50          rr += SHR32(ac[i + 1],3);
51          //r = -RC_SCALING*1.*SHL32(rr,3)/(error+1e-15);
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    return error;
80 }
81
82 void fir(const celt_word16 *x,
83          const celt_word16 *num,
84          celt_word16 *y,
85          int N,
86          int ord,
87          celt_word16 *mem)
88 {
89    int i,j;
90
91    for (i=0;i<N;i++)
92    {
93       celt_word32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
94       for (j=0;j<ord;j++)
95       {
96          sum += MULT16_16(num[j],mem[j]);
97       }
98       for (j=ord-1;j>=1;j--)
99       {
100          mem[j]=mem[j-1];
101       }
102       mem[0] = x[i];
103       y[i] = ROUND16(sum, SIG_SHIFT);
104    }
105 }
106
107 void iir(const celt_word32 *x,
108          const celt_word16 *den,
109          celt_word32 *y,
110          int N,
111          int ord,
112          celt_word16 *mem)
113 {
114    int i,j;
115    for (i=0;i<N;i++)
116    {
117       celt_word32 sum = x[i];
118       for (j=0;j<ord;j++)
119       {
120          sum -= MULT16_16(den[j],mem[j]);
121       }
122       for (j=ord-1;j>=1;j--)
123       {
124          mem[j]=mem[j-1];
125       }
126       mem[0] = ROUND16(sum,SIG_SHIFT);
127       y[i] = sum;
128    }
129 }
130
131 void _celt_autocorr(
132                    const celt_word16 *x,   /*  in: [0...n-1] samples x   */
133                    celt_word32       *ac,  /* out: [0...lag-1] ac values */
134                    const celt_word16       *window,
135                    int          overlap,
136                    int          lag, 
137                    int          n
138                   )
139 {
140    float d;
141    int i;
142    float scale=1;
143    VARDECL(float, xx);
144    SAVE_STACK;
145    ALLOC(xx, n, float);
146    for (i=0;i<n;i++)
147       xx[i] = x[i];
148    for (i=0;i<overlap;i++)
149    {
150       xx[i] *= (1./Q15ONE)*window[i];
151       xx[n-i-1] *= (1./Q15ONE)*window[i];
152    }
153 #ifdef FIXED_POINT
154    {
155       float ac0=0;
156       for(i=0;i<n;i++)
157          ac0 += x[i]*x[i];
158       ac0+=10;
159       scale = 2000000000/ac0;
160    }
161 #endif
162    while (lag>=0)
163    {
164       for (i = lag, d = 0; i < n; i++) 
165          d += x[i] * x[i-lag];
166       ac[lag] = d*scale;
167       /*printf ("%f ", ac[lag]);*/
168       lag--;
169    }
170    /*printf ("\n");*/
171    ac[0] += 10;
172
173    RESTORE_STACK;
174 }