This fixes a budget overrun and segfault for unreasonably low bitrates.
[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 #include "rate.h"
45
46 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
47
48 #ifdef FIXED_POINT
49 /* Compute the amplitude (sqrt energy) in each of the bands */
50 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
51 {
52    int i, c;
53    const celt_int16_t *eBands = m->eBands;
54    const int C = CHANNELS(m);
55    for (c=0;c<C;c++)
56    {
57       for (i=0;i<m->nbEBands;i++)
58       {
59          int j;
60          celt_word32_t maxval=0;
61          celt_word32_t sum = 0;
62          
63          j=eBands[i]; do {
64             maxval = MAX32(maxval, X[j*C+c]);
65             maxval = MAX32(maxval, -X[j*C+c]);
66          } while (++j<eBands[i+1]);
67          
68          if (maxval > 0)
69          {
70             int shift = celt_ilog2(maxval)-10;
71             j=eBands[i]; do {
72                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j*C+c],shift)),
73                                    EXTRACT16(VSHR32(X[j*C+c],shift)));
74             } while (++j<eBands[i+1]);
75             /* We're adding one here to make damn sure we never end up with a pitch vector that's
76                larger than unity norm */
77             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
78          } else {
79             bank[i*C+c] = EPSILON;
80          }
81          /*printf ("%f ", bank[i*C+c]);*/
82       }
83    }
84    /*printf ("\n");*/
85 }
86
87 /* Normalise each band such that the energy is one. */
88 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
89 {
90    int i, c;
91    const celt_int16_t *eBands = m->eBands;
92    const int C = CHANNELS(m);
93    for (c=0;c<C;c++)
94    {
95       i=0; do {
96          celt_word16_t g;
97          int j,shift;
98          celt_word16_t E;
99          shift = celt_zlog2(bank[i*C+c])-13;
100          E = VSHR32(bank[i*C+c], shift);
101          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
102          j=eBands[i]; do {
103             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
104          } while (++j<eBands[i+1]);
105       } while (++i<m->nbEBands);
106    }
107 }
108
109 #else /* FIXED_POINT */
110 /* Compute the amplitude (sqrt energy) in each of the bands */
111 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
112 {
113    int i, c;
114    const celt_int16_t *eBands = m->eBands;
115    const int C = CHANNELS(m);
116    for (c=0;c<C;c++)
117    {
118       for (i=0;i<m->nbEBands;i++)
119       {
120          int j;
121          celt_word32_t sum = 1e-10;
122          for (j=eBands[i];j<eBands[i+1];j++)
123             sum += X[j*C+c]*X[j*C+c];
124          bank[i*C+c] = sqrt(sum);
125          /*printf ("%f ", bank[i*C+c]);*/
126       }
127    }
128    /*printf ("\n");*/
129 }
130
131 #ifdef EXP_PSY
132 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank)
133 {
134    int i, c;
135    const celt_int16_t *eBands = m->eBands;
136    const int C = CHANNELS(m);
137    for (c=0;c<C;c++)
138    {
139       for (i=0;i<m->nbEBands;i++)
140       {
141          int j;
142          celt_word32_t sum = 1e-10;
143          for (j=eBands[i];j<eBands[i+1];j++)
144             sum += X[j*C+c]*X[j*C+c]*tonality[j];
145          bank[i*C+c] = sqrt(sum);
146          /*printf ("%f ", bank[i*C+c]);*/
147       }
148    }
149    /*printf ("\n");*/
150 }
151 #endif
152
153 /* Normalise each band such that the energy is one. */
154 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
155 {
156    int i, c;
157    const celt_int16_t *eBands = m->eBands;
158    const int C = CHANNELS(m);
159    for (c=0;c<C;c++)
160    {
161       for (i=0;i<m->nbEBands;i++)
162       {
163          int j;
164          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
165          for (j=eBands[i];j<eBands[i+1];j++)
166             X[j*C+c] = freq[j*C+c]*g;
167       }
168    }
169 }
170
171 #endif /* FIXED_POINT */
172
173 #ifndef DISABLE_STEREO
174 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
175 {
176    int i, c;
177    const celt_int16_t *eBands = m->eBands;
178    const int C = CHANNELS(m);
179    for (c=0;c<C;c++)
180    {
181       i=0; do {
182          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
183       } while (++i<m->nbEBands);
184    }
185 }
186 #endif /* DISABLE_STEREO */
187
188 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
189 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
190 {
191    int i, c;
192    const celt_int16_t *eBands = m->eBands;
193    const int C = CHANNELS(m);
194    if (C>2)
195       celt_fatal("denormalise_bands() not implemented for >2 channels");
196    for (c=0;c<C;c++)
197    {
198       for (i=0;i<m->nbEBands;i++)
199       {
200          int j;
201          celt_word32_t g = MULT16_32_Q15(sqrtC_1[C-1],bank[i*C+c]);
202          j=eBands[i]; do {
203             freq[j*C+c] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
204          } while (++j<eBands[i+1]);
205       }
206    }
207    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
208       freq[i] = 0;
209 }
210
211
212 /* Compute the best gain for each "pitch band" */
213 int compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
214 {
215    int i;
216    int gain_sum = 0;
217    const celt_int16_t *pBands = m->pBands;
218    const int C = CHANNELS(m);
219
220    for (i=0;i<m->nbPBands;i++)
221    {
222       celt_word32_t Sxy=0, Sxx=0;
223       int j;
224       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
225       for (j=C*pBands[i];j<C*pBands[i+1];j++)
226       {
227          Sxy = MAC16_16(Sxy, X[j], P[j]);
228          Sxx = MAC16_16(Sxx, X[j], X[j]);
229       }
230       /* No negative gain allowed */
231       if (Sxy < 0)
232          Sxy = 0;
233       /* Not sure how that would happen, just making sure */
234       if (Sxy > Sxx)
235          Sxy = Sxx;
236       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
237          residual doesn't quantise well */
238       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
239       /* gain = Sxy/Sxx */
240       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
241       if (gains[i]>QCONST16(.5,15))
242          gain_sum++;
243       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
244    }
245    /*if(rand()%10==0)
246    {
247       for (i=0;i<m->nbPBands;i++)
248          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
249       printf ("\n");
250    }*/
251    return gain_sum > 5;
252 }
253
254 static void intensity_band(celt_norm_t * restrict X, int len)
255 {
256    int j;
257    celt_word32_t E = 1e-15;
258    celt_word32_t E2 = 1e-15;
259    for (j=0;j<len;j++)
260    {
261       X[j] = X[2*j];
262       E = MAC16_16(E, X[j],X[j]);
263       E2 = MAC16_16(E2, X[2*j+1],X[2*j+1]);
264    }
265 #ifndef FIXED_POINT
266    E  = celt_sqrt(E+E2)/celt_sqrt(E);
267    for (j=0;j<len;j++)
268       X[j] *= E;
269 #endif
270    for (j=0;j<len;j++)
271       X[len+j] = 0;
272
273 }
274
275 static void dup_band(celt_norm_t * restrict X, int len)
276 {
277    int j;
278    for (j=len-1;j>=0;j--)
279    {
280       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
281       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
282    }
283 }
284
285 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, const int *stereo_mode, int bandID, int dir)
286 {
287    int i = bandID;
288    const celt_int16_t *eBands = m->eBands;
289    const int C = CHANNELS(m);
290    {
291       int j;
292       if (stereo_mode[i] && dir <0)
293       {
294          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
295       } else {
296          celt_word16_t a1, a2;
297          if (stereo_mode[i]==0)
298          {
299             /* Do mid-side when not doing intensity stereo */
300             a1 = QCONST16(.70711f,14);
301             a2 = dir*QCONST16(.70711f,14);
302          } else {
303             celt_word16_t left, right;
304             celt_word16_t norm;
305 #ifdef FIXED_POINT
306             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
307 #endif
308             left = VSHR32(bank[i*C],shift);
309             right = VSHR32(bank[i*C+1],shift);
310             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
311             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
312             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
313          }
314          for (j=eBands[i];j<eBands[i+1];j++)
315          {
316             celt_norm_t r, l;
317             l = X[j*C];
318             r = X[j*C+1];
319             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
320             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
321          }
322       }
323       if (stereo_mode[i] && dir>0)
324       {
325          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
326       }
327    }
328 }
329
330 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
331 {
332    int i;
333    for (i=0;i<len-5;i++)
334       stereo_mode[i] = 0;
335    for (;i<len;i++)
336       stereo_mode[i] = 0;
337 }
338
339
340
341 /* Quantisation of the residual */
342 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
343 {
344    int i, j, remaining_bits, balance;
345    const celt_int16_t * restrict eBands = m->eBands;
346    celt_norm_t * restrict norm;
347    VARDECL(celt_norm_t, _norm);
348    const int C = CHANNELS(m);
349    const celt_int16_t *pBands = m->pBands;
350    int pband=-1;
351    int B;
352    SAVE_STACK;
353
354    B = shortBlocks ? m->nbShortMdcts : 1;
355    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
356    norm = _norm;
357
358    balance = 0;
359    /*printf("bits left: %d\n", bits);
360    for (i=0;i<m->nbEBands;i++)
361       printf ("(%d %d) ", pulses[i], ebits[i]);
362    printf ("\n");*/
363    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
364    for (i=0;i<m->nbEBands;i++)
365    {
366       int tell;
367       int q;
368       celt_word16_t n;
369       const celt_int16_t * const *BPbits;
370       
371       int curr_balance, curr_bits;
372       
373       if (C>1 && stereo_mode[i]==0)
374          BPbits = m->bits_stereo;
375       else
376          BPbits = m->bits;
377
378       tell = ec_enc_tell(enc, 4);
379       if (i != 0)
380          balance -= tell;
381       remaining_bits = (total_bits<<BITRES)-tell-1;
382       curr_balance = (m->nbEBands-i);
383       if (curr_balance > 3)
384          curr_balance = 3;
385       curr_balance = balance / curr_balance;
386       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
387       curr_bits = BPbits[i][q];
388       remaining_bits -= curr_bits;
389       while (remaining_bits < 0 && q > 0)
390       {
391          remaining_bits += curr_bits;
392          q--;
393          curr_bits = BPbits[i][q];
394          remaining_bits -= curr_bits;
395       }
396       balance += pulses[i] + tell;
397       
398       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
399
400       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
401       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
402       {
403          int enabled = 1;
404          pband++;
405          if (remaining_bits >= 1<<BITRES) {
406            enabled = pgains[pband] > QCONST16(.5,15);
407            ec_enc_bits(enc, enabled, 1);
408            balance += 1<<BITRES;
409          }
410          if (enabled)
411             pgains[pband] = QCONST16(.9,15);
412          else
413             pgains[pband] = 0;
414       }
415
416       /* If pitch isn't available, use intra-frame prediction */
417       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
418       {
419          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
420       } else if (pitch_used && eBands[i] < m->pitchEnd) {
421          for (j=C*eBands[i];j<C*eBands[i+1];j++)
422             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
423       } else {
424          for (j=C*eBands[i];j<C*eBands[i+1];j++)
425             P[j] = 0;
426       }
427       
428       if (q > 0)
429       {
430          int ch=C;
431          if (C==2 && stereo_mode[i]==1)
432             ch = 1;
433          if (C==2)
434          {
435             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
436             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
437          }
438          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
439          if (C==2)
440             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
441       } else {
442          for (j=C*eBands[i];j<C*eBands[i+1];j++)
443             X[j] = P[j];
444       }
445       for (j=C*eBands[i];j<C*eBands[i+1];j++)
446          norm[j] = MULT16_16_Q15(n,X[j]);
447    }
448    RESTORE_STACK;
449 }
450
451 /* Decoding of the residual */
452 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, const int *stereo_mode, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
453 {
454    int i, j, remaining_bits, balance;
455    const celt_int16_t * restrict eBands = m->eBands;
456    celt_norm_t * restrict norm;
457    VARDECL(celt_norm_t, _norm);
458    const int C = CHANNELS(m);
459    const celt_int16_t *pBands = m->pBands;
460    int pband=-1;
461    int B;
462    SAVE_STACK;
463
464    B = shortBlocks ? m->nbShortMdcts : 1;
465    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
466    norm = _norm;
467
468    balance = 0;
469    for (i=0;i<m->nbEBands;i++)
470    {
471       int tell;
472       int q;
473       celt_word16_t n;
474       const celt_int16_t * const *BPbits;
475       
476       int curr_balance, curr_bits;
477       
478       if (C>1 && stereo_mode[i]==0)
479          BPbits = m->bits_stereo;
480       else
481          BPbits = m->bits;
482
483       tell = ec_dec_tell(dec, 4);
484       if (i != 0)
485          balance -= tell;
486       remaining_bits = (total_bits<<BITRES)-tell-1;
487       curr_balance = (m->nbEBands-i);
488       if (curr_balance > 3)
489          curr_balance = 3;
490       curr_balance = balance / curr_balance;
491       q = bits2pulses(m, BPbits[i], pulses[i]+curr_balance);
492       curr_bits = BPbits[i][q];
493       remaining_bits -= curr_bits;
494       while (remaining_bits < 0 && q > 0)
495       {
496          remaining_bits += curr_bits;
497          q--;
498          curr_bits = BPbits[i][q];
499          remaining_bits -= curr_bits;
500       }
501       balance += pulses[i] + tell;
502
503       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
504
505       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
506       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
507       {
508          int enabled = 1;
509          pband++;
510          if (remaining_bits >= 1<<BITRES) {
511            enabled = ec_dec_bits(dec, 1);
512            balance += 1<<BITRES;
513          }
514          if (enabled)
515             pgains[pband] = QCONST16(.9,15);
516          else
517             pgains[pband] = 0;
518       }
519
520       /* If pitch isn't available, use intra-frame prediction */
521       if ((eBands[i] >= m->pitchEnd && fold) || q<=0)
522       {
523          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B);
524       } else if (pitch_used && eBands[i] < m->pitchEnd) {
525          for (j=C*eBands[i];j<C*eBands[i+1];j++)
526             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
527       } else {
528          for (j=C*eBands[i];j<C*eBands[i+1];j++)
529             P[j] = 0;
530       }
531       
532       if (q > 0)
533       {
534          int ch=C;
535          if (C==2 && stereo_mode[i]==1)
536             ch = 1;
537          if (C==2)
538             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
539          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
540          if (C==2)
541             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
542       } else {
543          for (j=C*eBands[i];j<C*eBands[i+1];j++)
544             X[j] = P[j];
545       }
546       for (j=C*eBands[i];j<C*eBands[i+1];j++)
547          norm[j] = MULT16_16_Q15(n,X[j]);
548    }
549    RESTORE_STACK;
550 }
551