Making sure not to use the C library calls directly
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 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 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44
45 void exp_rotation(celt_norm_t *X, int len, int dir, int stride, int iter)
46 {
47    int i, k;
48    celt_word16_t c, s;
49    /* Equivalent to cos(.3) and sin(.3) */
50    c = QCONST16(0.95534,15);
51    s = dir*QCONST16(0.29552,15);
52    for (k=0;k<iter;k++)
53    {
54       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
55          but at this point, I really don't think it's necessary */
56       for (i=0;i<len-stride;i++)
57       {
58          celt_norm_t x1, x2;
59          x1 = X[i];
60          x2 = X[i+stride];
61          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
62          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
63       }
64       for (i=len-2*stride-1;i>=0;i--)
65       {
66          celt_norm_t x1, x2;
67          x1 = X[i];
68          x2 = X[i+stride];
69          X[i] = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
70          X[i+stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
71       }
72    }
73 }
74
75
76 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
77
78 #ifdef FIXED_POINT
79 /* Compute the amplitude (sqrt energy) in each of the bands */
80 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
81 {
82    int i, c, B, C;
83    const celt_int16_t *eBands = m->eBands;
84    B = m->nbMdctBlocks;
85    C = m->nbChannels;
86    for (c=0;c<C;c++)
87    {
88       for (i=0;i<m->nbEBands;i++)
89       {
90          int j;
91          celt_word32_t maxval=0;
92          celt_word32_t sum = 0;
93          for (j=B*eBands[i];j<B*eBands[i+1];j++)
94             maxval = MAX32(maxval, ABS32(X[j*C+c]));
95          if (maxval > 0)
96          {
97             int shift = celt_zlog2(maxval)-10;
98             for (j=B*eBands[i];j<B*eBands[i+1];j++)
99                sum += VSHR32(X[j*C+c],shift)*VSHR32(X[j*C+c],shift);
100             /* We're adding one here to make damn sure we never end up with a pitch vector that's
101                larger than unity norm */
102             bank[i*C+c] = 1+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
103          } else {
104             bank[i*C+c] = 0;
105          }
106          /*printf ("%f ", bank[i*C+c]);*/
107       }
108    }
109    /*printf ("\n");*/
110 }
111
112 /* Normalise each band such that the energy is one. */
113 void normalise_bands(const CELTMode *m, const celt_sig_t *freq, celt_norm_t *X, const celt_ener_t *bank)
114 {
115    int i, c, B, C;
116    const celt_int16_t *eBands = m->eBands;
117    B = m->nbMdctBlocks;
118    C = m->nbChannels;
119    for (c=0;c<C;c++)
120    {
121       for (i=0;i<m->nbEBands;i++)
122       {
123          celt_word16_t g;
124          int j,shift;
125          celt_word16_t E;
126          shift = celt_zlog2(bank[i*C+c])-13;
127          E = VSHR32(bank[i*C+c], shift);
128          if (E>0)
129             g = DIV32_16(QCONST32(1.f,28),MULT16_16_Q14(E,sqrtC_1[C-1]));
130          else
131             g = 0;
132          for (j=B*eBands[i];j<B*eBands[i+1];j++)
133             X[j*C+c] = MULT16_16_Q14(VSHR32(freq[j*C+c],shift),g);
134       }
135    }
136    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
137       X[i] = 0;
138 }
139
140 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
141 {
142    int i;
143    VARDECL(celt_ener_t, tmpE);
144    VARDECL(celt_sig_t, freq);
145    SAVE_STACK;
146    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
147    ALLOC(freq, m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
148    for (i=0;i<m->nbMdctBlocks*m->nbChannels*m->eBands[m->nbEBands+1];i++)
149       freq[i] = SHL32(EXTEND32(X[i]), 10);
150    compute_band_energies(m, freq, tmpE);
151    normalise_bands(m, freq, X, tmpE);
152    RESTORE_STACK;
153 }
154 #else
155 /* Compute the amplitude (sqrt energy) in each of the bands */
156 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
157 {
158    int i, c, B, C;
159    const celt_int16_t *eBands = m->eBands;
160    B = m->nbMdctBlocks;
161    C = m->nbChannels;
162    for (c=0;c<C;c++)
163    {
164       for (i=0;i<m->nbEBands;i++)
165       {
166          int j;
167          celt_word32_t sum = 1e-10;
168          for (j=B*eBands[i];j<B*eBands[i+1];j++)
169             sum += X[j*C+c]*X[j*C+c];
170          bank[i*C+c] = sqrt(sum);
171          /*printf ("%f ", bank[i*C+c]);*/
172       }
173    }
174    /*printf ("\n");*/
175 }
176
177 /* Normalise each band such that the energy is one. */
178 void normalise_bands(const CELTMode *m, const celt_sig_t *freq, celt_norm_t *X, const celt_ener_t *bank)
179 {
180    int i, c, B, C;
181    const celt_int16_t *eBands = m->eBands;
182    B = m->nbMdctBlocks;
183    C = m->nbChannels;
184    for (c=0;c<C;c++)
185    {
186       for (i=0;i<m->nbEBands;i++)
187       {
188          int j;
189          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
190          for (j=B*eBands[i];j<B*eBands[i+1];j++)
191             X[j*C+c] = freq[j*C+c]*g;
192       }
193    }
194    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
195       X[i] = 0;
196 }
197
198 void renormalise_bands(const CELTMode *m, celt_norm_t *X)
199 {
200    VARDECL(celt_ener_t, tmpE);
201    SAVE_STACK;
202    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
203    compute_band_energies(m, X, tmpE);
204    normalise_bands(m, X, X, tmpE);
205    RESTORE_STACK;
206 }
207 #endif
208
209 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
210 void denormalise_bands(const CELTMode *m, const celt_norm_t *X, celt_sig_t *freq, const celt_ener_t *bank)
211 {
212    int i, c, B, C;
213    const celt_int16_t *eBands = m->eBands;
214    B = m->nbMdctBlocks;
215    C = m->nbChannels;
216    if (C>2)
217       celt_fatal("denormalise_bands() not implemented for >2 channels");
218    for (c=0;c<C;c++)
219    {
220       for (i=0;i<m->nbEBands;i++)
221       {
222          int j;
223          celt_word32_t g = MULT16_32_Q14(sqrtC_1[C-1],bank[i*C+c]);
224          for (j=B*eBands[i];j<B*eBands[i+1];j++)
225             freq[j*C+c] = MULT16_32_Q14(X[j*C+c], g);
226       }
227    }
228    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
229       freq[i] = 0;
230 }
231
232
233 /* Compute the best gain for each "pitch band" */
234 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
235 {
236    int i, B;
237    const celt_int16_t *pBands = m->pBands;
238    B = m->nbMdctBlocks*m->nbChannels;
239    
240    for (i=0;i<m->nbPBands;i++)
241    {
242       celt_word32_t Sxy=0, Sxx=0;
243       int j;
244       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
245       for (j=B*pBands[i];j<B*pBands[i+1];j++)
246       {
247          Sxy = MAC16_16(Sxy, X[j], P[j]);
248          Sxx = MAC16_16(Sxx, X[j], X[j]);
249       }
250       /* No negative gain allowed */
251       if (Sxy < 0)
252          Sxy = 0;
253       /* Not sure how that would happen, just making sure */
254       if (Sxy > Sxx)
255          Sxy = Sxx;
256       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
257          residual doesn't quantise well */
258       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
259       /* gain = Sxy/Sxx */
260       gains[i] = DIV32_16(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON));
261       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
262    }
263    /*if(rand()%10==0)
264    {
265       for (i=0;i<m->nbPBands;i++)
266          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
267       printf ("\n");
268    }*/
269 }
270
271 /* Apply the (quantised) gain to each "pitch band" */
272 void pitch_quant_bands(const CELTMode *m, celt_norm_t *P, const celt_pgain_t *gains)
273 {
274    int i, B;
275    const celt_int16_t *pBands = m->pBands;
276    B = m->nbMdctBlocks*m->nbChannels;
277    for (i=0;i<m->nbPBands;i++)
278    {
279       int j;
280       for (j=B*pBands[i];j<B*pBands[i+1];j++)
281          P[j] = MULT16_16_Q15(gains[i], P[j]);
282       /*printf ("%f ", gain);*/
283    }
284    for (i=B*pBands[m->nbPBands];i<B*pBands[m->nbPBands+1];i++)
285       P[i] = 0;
286 }
287
288
289 /* Quantisation of the residual */
290 void quant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, celt_mask_t *W, int total_bits, ec_enc *enc)
291 {
292    int i, j, B, bits;
293    const celt_int16_t *eBands = m->eBands;
294    celt_word16_t alpha;
295    VARDECL(celt_norm_t, norm);
296    VARDECL(int, pulses);
297    VARDECL(int, offsets);
298    SAVE_STACK;
299
300    B = m->nbMdctBlocks*m->nbChannels;
301    
302    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
303    ALLOC(pulses, m->nbEBands, int);
304    ALLOC(offsets, m->nbEBands, int);
305
306    for (i=0;i<m->nbEBands;i++)
307       offsets[i] = 0;
308    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
309    bits = total_bits - ec_enc_tell(enc, 0) - 1;
310    compute_allocation(m, offsets, bits, pulses);
311    
312    /*printf("bits left: %d\n", bits);
313    for (i=0;i<m->nbEBands;i++)
314       printf ("%d ", pulses[i]);
315    printf ("\n");*/
316    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
317    for (i=0;i<m->nbEBands;i++)
318    {
319       int q;
320       celt_word16_t n;
321       q = pulses[i];
322       n = SHL16(celt_sqrt(B*(eBands[i+1]-eBands[i])),11);
323
324       /* If pitch isn't available, use intra-frame prediction */
325       if (eBands[i] >= m->pitchEnd || q<=0)
326       {
327          q -= 1;
328          alpha = 0;
329          if (q<0)
330             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
331          else
332             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);
333       } else {
334          alpha = QCONST16(.7f,15);
335       }
336       
337       if (q > 0)
338       {
339          int nb_rotations = (B*(eBands[i+1]-eBands[i])+4*q)/(8*q);
340          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), -1, B, nb_rotations);
341          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), -1, B, nb_rotations);
342          alg_quant(X+B*eBands[i], W+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, enc);
343          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), 1, B, nb_rotations);
344       }
345       for (j=B*eBands[i];j<B*eBands[i+1];j++)
346          norm[j] = MULT16_16_Q15(n,X[j]);
347    }
348    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
349       X[i] = 0;
350    RESTORE_STACK;
351 }
352
353 /* Decoding of the residual */
354 void unquant_bands(const CELTMode *m, celt_norm_t *X, celt_norm_t *P, int total_bits, ec_dec *dec)
355 {
356    int i, j, B, bits;
357    const celt_int16_t *eBands = m->eBands;
358    celt_word16_t alpha;
359    VARDECL(celt_norm_t, norm);
360    VARDECL(int, pulses);
361    VARDECL(int, offsets);
362    SAVE_STACK;
363
364    B = m->nbMdctBlocks*m->nbChannels;
365    
366    ALLOC(norm, B*eBands[m->nbEBands+1], celt_norm_t);
367    ALLOC(pulses, m->nbEBands, int);
368    ALLOC(offsets, m->nbEBands, int);
369
370    for (i=0;i<m->nbEBands;i++)
371       offsets[i] = 0;
372    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
373    bits = total_bits - ec_dec_tell(dec, 0) - 1;
374    compute_allocation(m, offsets, bits, pulses);
375
376    for (i=0;i<m->nbEBands;i++)
377    {
378       int q;
379       celt_word16_t n;
380       q = pulses[i];
381       n = SHL16(celt_sqrt(B*(eBands[i+1]-eBands[i])),11);
382
383       /* If pitch isn't available, use intra-frame prediction */
384       if (eBands[i] >= m->pitchEnd || q<=0)
385       {
386          q -= 1;
387          alpha = 0;
388          if (q<0)
389             intra_fold(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), norm, P+B*eBands[i], B, eBands[i], eBands[m->nbEBands+1]);
390          else
391             intra_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, norm, P+B*eBands[i], B, eBands[i], dec);
392       } else {
393          alpha = QCONST16(.7f,15);
394       }
395       
396       if (q > 0)
397       {
398          int nb_rotations = (B*(eBands[i+1]-eBands[i])+4*q)/(8*q);
399          exp_rotation(P+B*eBands[i], B*(eBands[i+1]-eBands[i]), -1, B, nb_rotations);
400          alg_unquant(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), q, P+B*eBands[i], alpha, dec);
401          exp_rotation(X+B*eBands[i], B*(eBands[i+1]-eBands[i]), 1, B, nb_rotations);
402       }
403       for (j=B*eBands[i];j<B*eBands[i+1];j++)
404          norm[j] = MULT16_16_Q15(n,X[j]);
405    }
406    for (i=B*eBands[m->nbEBands];i<B*eBands[m->nbEBands+1];i++)
407       X[i] = 0;
408    RESTORE_STACK;
409 }
410
411 void stereo_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int dir)
412 {
413    int i, B, C;
414    const celt_int16_t *eBands = m->eBands;
415    B = m->nbMdctBlocks;
416    C = m->nbChannels;
417    for (i=0;i<m->nbEBands;i++)
418    {
419       int j;
420       celt_word16_t left, right;
421       celt_word16_t a1, a2;
422       celt_word16_t norm;
423 #ifdef FIXED_POINT
424       int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
425 #endif
426       left = VSHR32(bank[i*C],shift);
427       right = VSHR32(bank[i*C+1],shift);
428       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
429       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
430       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
431       for (j=B*eBands[i];j<B*eBands[i+1];j++)
432       {
433          celt_norm_t r, l;
434          l = X[j*C];
435          r = X[j*C+1];
436          X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
437          X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
438       }
439    }
440    for (i=B*C*eBands[m->nbEBands];i<B*C*eBands[m->nbEBands+1];i++)
441       X[i] = 0;
442
443 }