Initial support for a managed stack/scratchpad. Still needs some work.
[opus.git] / libcelt / bands.c
1 /* (C) 2007 Jean-Marc Valin, CSIRO
2 */
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    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41
42
43 void exp_rotation(celt_norm_t *X, int len, float theta, int dir, int stride, int iter)
44 {
45    int i, k;
46    celt_word16_t c, s;
47    c = Q15ONE*cos(theta);
48    s = dir*Q15ONE*sin(theta);
49    for (k=0;k<iter;k++)
50    {
51       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
52          but at this point, I really don't think it's necessary */
53       for (i=0;i<len-stride;i++)
54       {
55          celt_norm_t x1, x2;
56          x1 = X[i];
57          x2 = X[i+stride];
58          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
59          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
60       }
61       for (i=len-2*stride-1;i>=0;i--)
62       {
63          celt_norm_t x1, x2;
64          x1 = X[i];
65          x2 = X[i+stride];
66          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
67          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
68       }
69    }
70 }
71
72 /* Compute the amplitude (sqrt energy) in each of the bands */
73 void compute_band_energies(const CELTMode *m, celt_sig_t *X, celt_ener_t *bank)
74 {
75    int i, c, B, C;
76    const int *eBands = m->eBands;
77    B = m->nbMdctBlocks;
78    C = m->nbChannels;
79    for (c=0;c<C;c++)
80    {
81       for (i=0;i<m->nbEBands;i++)
82       {
83          int j;
84          float sum = 1e-10;
85          for (j=B*eBands[i];j<B*eBands[i+1];j++)
86             sum += SIG_SCALING_1*SIG_SCALING_1*X[j*C+c]*X[j*C+c];
87          bank[i*C+c] = ENER_SCALING*sqrt(sum);
88          /*printf ("%f ", bank[i*C+c]);*/
89       }
90    }
91    /*printf ("\n");*/
92 }
93
94 /* Normalise each band such that the energy is one. */
95 void normalise_bands(const CELTMode *m, celt_sig_t *freq, celt_norm_t *X, celt_ener_t *bank)
96 {
97    int i, c, B, C;
98    const int *eBands = m->eBands;
99    B = m->nbMdctBlocks;
100    C = m->nbChannels;
101    for (c=0;c<C;c++)
102    {
103       for (i=0;i<m->nbEBands;i++)
104       {
105          int j;
106          float g = 1.f/(1e-10+ENER_SCALING_1*bank[i*C+c]*sqrt(C));
107          for (j=B*eBands[i];j<B*eBands[i+1];j++)
108             X[j*C+c] = NORM_SCALING*SIG_SCALING_1*freq[j*C+c]*g;
109       }
110    }
111    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
112       X[i] = 0;
113 }
114
115 #ifdef FIXED_POINT
116 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
117 {
118    int i;
119    VARDECL(celt_ener_t *tmpE);
120    VARDECL(celt_sig_t *freq);
121    SAVE_STACK;
122    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
123    ALLOC(freq, m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
124    for (i=0;i<m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1];i++)
125       freq[i] = SHL32(EXTEND32(X[i]), 10);
126    compute_band_energies(m, freq, tmpE);
127    normalise_bands(m, freq, X, tmpE);
128    RESTORE_STACK;
129 }
130 #else
131 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
132 {
133    VARDECL(celt_ener_t *tmpE);
134    SAVE_STACK;
135    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
136    compute_band_energies(m, X, tmpE);
137    normalise_bands(m, X, X, tmpE);
138    RESTORE_STACK;
139 }
140 #endif
141
142 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
143 void denormalise_bands(const CELTMode *m, celt_norm_t *X, celt_sig_t *freq, celt_ener_t *bank)
144 {
145    int i, c, B, C;
146    const int *eBands = m->eBands;
147    B = m->nbMdctBlocks;
148    C = m->nbChannels;
149    for (c=0;c<C;c++)
150    {
151       for (i=0;i<m->nbEBands;i++)
152       {
153          int j;
154          float g = ENER_SCALING_1*sqrt(C)*bank[i*C+c];
155          for (j=B*eBands[i];j<B*eBands[i+1];j++)
156             freq[j*C+c] = NORM_SCALING_1*SIG_SCALING*X[j*C+c] * g;
157       }
158    }
159    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
160       freq[i] = 0;
161 }
162
163
164 /* Compute the best gain for each "pitch band" */
165 void compute_pitch_gain(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, celt_pgain_t *gains)
166 {
167    int i, B;
168    const int *pBands = m->pBands;
169    B = m->nbMdctBlocks*m->nbChannels;
170    
171    for (i=0;i<m->nbPBands;i++)
172    {
173       celt_word32_t Sxy=0, Sxx=0;
174       int j;
175       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
176       for (j=B*pBands[i];j<B*pBands[i+1];j++)
177       {
178          Sxy = MAC16_16(Sxy, X[j], P[j]);
179          Sxx = MAC16_16(Sxx, X[j], X[j]);
180       }
181       /* No negative gain allowed */
182       if (Sxy < 0)
183          Sxy = 0;
184       /* Not sure how that would happen, just making sure */
185       if (Sxy > Sxx)
186          Sxy = Sxx;
187       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
188          residual doesn't quantise well */
189       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
190       /* gain = Sxy/Sxx */
191       gains[i] = DIV32_16(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON));
192       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
193    }
194    /*if(rand()%10==0)
195    {
196       for (i=0;i<m->nbPBands;i++)
197          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
198       printf ("\n");
199    }*/
200    for (i=B*pBands[m->nbPBands];i<B*pBands[m->nbPBands+1];i++)
201       P[i] = 0;
202 }
203
204 /* Apply the (quantised) gain to each "pitch band" */
205 void pitch_quant_bands(const CELTMode *m, celt_norm_t *P, celt_pgain_t *gains)
206 {
207    int i, B;
208    const int *pBands = m->pBands;
209    B = m->nbMdctBlocks*m->nbChannels;
210    for (i=0;i<m->nbPBands;i++)
211    {
212       int j;
213       for (j=B*pBands[i];j<B*pBands[i+1];j++)
214          P[j] = MULT16_16_Q15(gains[i], P[j]);
215       /*printf ("%f ", gain);*/
216    }
217    for (i=B*pBands[m->nbPBands];i<B*pBands[m->nbPBands+1];i++)
218       P[i] = 0;
219 }
220
221
222 /* Quantisation of the residual */
223 void quant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, celt_mask_t *W, int total_bits, ec_enc *enc)
224 {
225    int i, j, B, bits;
226    const int *eBands = m->eBands;
227    celt_word16_t alpha;
228    VARDECL(celt_norm_t *norm);
229    VARDECL(int *pulses);
230    VARDECL(int *offsets);
231    SAVE_STACK;
232
233    B = m->nbMdctBlocks*m->nbChannels;
234    
235    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
236    ALLOC(pulses, m->nbEBands, int);
237    ALLOC(offsets, m->nbEBands, int);
238
239    for (i=0;i<m->nbEBands;i++)
240       offsets[i] = 0;
241    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
242    bits = total_bits - ec_enc_tell(enc, 0) - 1;
243    compute_allocation(m, offsets, bits, pulses);
244    
245    /*printf("bits left: %d\n", bits);
246    for (i=0;i<m->nbEBands;i++)
247       printf ("%d ", pulses[i]);
248    printf ("\n");*/
249    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
250    for (i=0;i<m->nbEBands;i++)
251    {
252       int q;
253       float theta, n;
254       q = pulses[i];
255       /*Scale factor of .0625f is just there to prevent overflows in fixed-point
256        (has no effect on float)*/
257       n = .0625f*sqrt(B*(eBands[i+1]-eBands[i]));
258       theta = .007*(B*(eBands[i+1]-eBands[i]))/(.1f+q);
259
260       /* If pitch isn't available, use intra-frame prediction */
261       if (eBands[i] >= m->pitchEnd || q<=0)
262       {
263          q -= 1;
264          alpha = 0;
265          if (q<0)
266             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
267          else
268             intra_prediction(X+B*eBands[i], W+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, norm, P+B*eBands[i], B, eBands[i], enc);
269       } else {
270          alpha = QCONST16(.7f,15);
271       }
272       
273       if (q > 0)
274       {
275          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
276          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
277          alg_quant(X+B*eBands[i], W+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, enc);
278          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, 1, B, 8);
279       }
280       for (j=B*eBands[i];j<B*eBands[i+1];j++)
281          norm[j] = X[j] * n;
282    }
283    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
284       X[i] = 0;
285    RESTORE_STACK;
286 }
287
288 /* Decoding of the residual */
289 void unquant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, int total_bits, ec_dec *dec)
290 {
291    int i, j, B, bits;
292    const int *eBands = m->eBands;
293    celt_word16_t alpha;
294    VARDECL(celt_norm_t *norm);
295    VARDECL(int *pulses);
296    VARDECL(int *offsets);
297    SAVE_STACK;
298
299    B = m->nbMdctBlocks*m->nbChannels;
300    
301    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
302    ALLOC(pulses, m->nbEBands, int);
303    ALLOC(offsets, m->nbEBands, int);
304
305    for (i=0;i<m->nbEBands;i++)
306       offsets[i] = 0;
307    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
308    bits = total_bits - ec_dec_tell(dec, 0) - 1;
309    compute_allocation(m, offsets, bits, pulses);
310
311    for (i=0;i<m->nbEBands;i++)
312    {
313       int q;
314       float theta, n;
315       q = pulses[i];
316       /*Scale factor of .0625f is just there to prevent overflows in fixed-point
317       (has no effect on float)*/
318       n = .0625f*sqrt(B*(eBands[i+1]-eBands[i]));
319       theta = .007*(B*(eBands[i+1]-eBands[i]))/(.1f+q);
320
321       /* If pitch isn't available, use intra-frame prediction */
322       if (eBands[i] >= m->pitchEnd || q<=0)
323       {
324          q -= 1;
325          alpha = 0;
326          if (q<0)
327             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
328          else
329             intra_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, norm, P+B*eBands[i], B, eBands[i], dec);
330       } else {
331          alpha = QCONST16(.7f,15);
332       }
333       
334       if (q > 0)
335       {
336          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, -1, B, 8);
337          alg_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, dec);
338          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), theta, 1, B, 8);
339       }
340       for (j=B*eBands[i];j<B*eBands[i+1];j++)
341          norm[j] = X[j] * n;
342    }
343    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
344       X[i] = 0;
345    RESTORE_STACK;
346 }
347
348 void stereo_mix(const CELTMode *m, celt_norm_t *X, celt_ener_t *bank, int dir)
349 {
350    int i, B, C;
351    const int *eBands = m->eBands;
352    B = m->nbMdctBlocks;
353    C = m->nbChannels;
354    for (i=0;i<m->nbEBands;i++)
355    {
356       int j;
357       float left, right;
358       float a1, a2;
359       left = bank[i*C];
360       right = bank[i*C+1];
361       a1 = left/sqrt(.01+left*left+right*right);
362       a2 = dir*right/sqrt(.01+left*left+right*right);
363       for (j=B*eBands[i];j<B*eBands[i+1];j++)
364       {
365          float r, l;
366          l = X[j*C];
367          r = X[j*C+1];         
368          X[j*C] = a1*l + a2*r;
369          X[j*C+1] = a1*r - a2*l;
370       }
371    }
372    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
373       X[i] = 0;
374
375 }