jitter buffer: check for buffer overflow only when zero-copy is off
[speexdsp.git] / libspeex / stereo.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: stereo.c
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 <speex/speex_stereo.h>
37 #include <speex/speex_callbacks.h>
38 #include "math_approx.h"
39 #include "vq.h"
40 #include <math.h>
41 #include "os_support.h"
42
43 typedef struct RealSpeexStereoState {
44    spx_word32_t balance;      /**< Left/right balance info */
45    spx_word32_t e_ratio;      /**< Ratio of energies: E(left+right)/[E(left)+E(right)]  */
46    spx_word32_t smooth_left;  /**< Smoothed left channel gain */
47    spx_word32_t smooth_right; /**< Smoothed right channel gain */
48    spx_uint32_t reserved1;     /**< Reserved for future use */
49    spx_int32_t reserved2;     /**< Reserved for future use */
50 } RealSpeexStereoState;
51
52
53 /*float e_ratio_quant[4] = {1, 1.26, 1.587, 2};*/
54 #ifndef FIXED_POINT
55 static const float e_ratio_quant[4] = {.25f, .315f, .397f, .5f};
56 static const float e_ratio_quant_bounds[3] = {0.2825f, 0.356f, 0.4485f};
57 #else
58 static const spx_word16_t e_ratio_quant[4] = {8192, 10332, 13009, 16384};
59 static const spx_word16_t e_ratio_quant_bounds[3] = {9257, 11665, 14696};
60 static const spx_word16_t balance_bounds[31] = {18, 23, 30, 38, 49, 63,  81, 104,
61    134, 172, 221,  284, 364, 468, 600, 771,
62    990, 1271, 1632, 2096, 2691, 3455, 4436, 5696,
63    7314, 9392, 12059, 15484, 19882, 25529, 32766};
64 #endif
65
66 /* This is an ugly compatibility hack that properly resets the stereo state
67    In case it it compiled in fixed-point, but initialised with the deprecated
68    floating point static initialiser */
69 #ifdef FIXED_POINT
70 #define COMPATIBILITY_HACK(s) do {if ((s)->reserved1 != 0xdeadbeef) speex_stereo_state_reset((SpeexStereoState*)s); } while (0);
71 #else
72 #define COMPATIBILITY_HACK(s) 
73 #endif
74
75 SpeexStereoState *speex_stereo_state_init()
76 {
77    SpeexStereoState *stereo = speex_alloc(sizeof(SpeexStereoState));
78    speex_stereo_state_reset(stereo);
79    return stereo;
80 }
81
82 void speex_stereo_state_reset(SpeexStereoState *_stereo)
83 {
84    RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
85 #ifdef FIXED_POINT
86    stereo->balance = 65536;
87    stereo->e_ratio = 16384;
88    stereo->smooth_left = 16384;
89    stereo->smooth_right = 16384;
90    stereo->reserved1 = 0xdeadbeef;
91    stereo->reserved2 = 0;
92 #else
93    stereo->balance = 1.0f;
94    stereo->e_ratio = .5f;
95    stereo->smooth_left = 1.f;
96    stereo->smooth_right = 1.f;
97    stereo->reserved1 = 0;
98    stereo->reserved2 = 0;
99 #endif   
100 }
101
102 void speex_stereo_state_destroy(SpeexStereoState *stereo)
103 {
104    speex_free(stereo);
105 }
106
107 #ifndef DISABLE_FLOAT_API
108 void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits)
109 {
110    int i, tmp;
111    float e_left=0, e_right=0, e_tot=0;
112    float balance, e_ratio;
113    for (i=0;i<frame_size;i++)
114    {
115       e_left  += ((float)data[2*i])*data[2*i];
116       e_right += ((float)data[2*i+1])*data[2*i+1];
117       data[i] =  .5*(((float)data[2*i])+data[2*i+1]);
118       e_tot   += ((float)data[i])*data[i];
119    }
120    balance=(e_left+1)/(e_right+1);
121    e_ratio = e_tot/(1+e_left+e_right);
122
123    /*Quantization*/
124    speex_bits_pack(bits, 14, 5);
125    speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4);
126    
127    balance=4*log(balance);
128
129    /*Pack sign*/
130    if (balance>0)
131       speex_bits_pack(bits, 0, 1);
132    else
133       speex_bits_pack(bits, 1, 1);
134    balance=floor(.5+fabs(balance));
135    if (balance>30)
136       balance=31;
137    
138    speex_bits_pack(bits, (int)balance, 5);
139    
140    /* FIXME: this is a hack */
141    tmp=scal_quant(e_ratio*Q15_ONE, e_ratio_quant_bounds, 4);
142    speex_bits_pack(bits, tmp, 2);
143 }
144 #endif /* #ifndef DISABLE_FLOAT_API */
145
146 void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits)
147 {
148    int i, tmp;
149    spx_word32_t e_left=0, e_right=0, e_tot=0;
150    spx_word32_t balance, e_ratio;
151    spx_word32_t largest, smallest;
152    int balance_id;
153 #ifdef FIXED_POINT
154    int shift;
155 #endif
156    
157    /* In band marker */
158    speex_bits_pack(bits, 14, 5);
159    /* Stereo marker */
160    speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4);
161
162    for (i=0;i<frame_size;i++)
163    {
164       e_left  += SHR32(MULT16_16(data[2*i],data[2*i]),8);
165       e_right += SHR32(MULT16_16(data[2*i+1],data[2*i+1]),8);
166 #ifdef FIXED_POINT
167       /* I think this is actually unbiased */
168       data[i] =  SHR16(data[2*i],1)+PSHR16(data[2*i+1],1);
169 #else
170       data[i] =  .5*(((float)data[2*i])+data[2*i+1]);
171 #endif
172       e_tot   += SHR32(MULT16_16(data[i],data[i]),8);
173    }
174    if (e_left > e_right)
175    {
176       speex_bits_pack(bits, 0, 1);
177       largest = e_left;
178       smallest = e_right;
179    } else {
180       speex_bits_pack(bits, 1, 1);
181       largest = e_right;
182       smallest = e_left;
183    }
184
185    /* Balance quantization */
186 #ifdef FIXED_POINT
187    shift = spx_ilog2(largest)-15;
188    largest = VSHR32(largest, shift-4);
189    smallest = VSHR32(smallest, shift);
190    balance = DIV32(largest, ADD32(smallest, 1));
191    if (balance > 32767)
192       balance = 32767;
193    balance_id = scal_quant(EXTRACT16(balance), balance_bounds, 32);
194 #else
195    balance=(largest+1.)/(smallest+1.);
196    balance=4*log(balance);
197    balance_id=floor(.5+fabs(balance));
198    if (balance_id>30)
199       balance_id=31;
200 #endif
201    
202    speex_bits_pack(bits, balance_id, 5);
203    
204    /* "coherence" quantisation */
205 #ifdef FIXED_POINT
206    shift = spx_ilog2(e_tot);
207    e_tot = VSHR32(e_tot, shift-25);
208    e_left = VSHR32(e_left, shift-10);
209    e_right = VSHR32(e_right, shift-10);
210    e_ratio = DIV32(e_tot, e_left+e_right+1);
211 #else
212    e_ratio = e_tot/(1.+e_left+e_right);
213 #endif
214    
215    tmp=scal_quant(EXTRACT16(e_ratio), e_ratio_quant_bounds, 4);
216    /*fprintf (stderr, "%d %d %d %d\n", largest, smallest, balance_id, e_ratio);*/
217    speex_bits_pack(bits, tmp, 2);
218 }
219
220 #ifndef DISABLE_FLOAT_API
221 void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *_stereo)
222 {
223    int i;
224    spx_word32_t balance;
225    spx_word16_t e_left, e_right, e_ratio;
226    RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
227    
228    COMPATIBILITY_HACK(stereo);
229    
230    balance=stereo->balance;
231    e_ratio=stereo->e_ratio;
232    
233    /* These two are Q14, with max value just below 2. */
234    e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance))));
235    e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8);
236
237    for (i=frame_size-1;i>=0;i--)
238    {
239       spx_word16_t tmp=data[i];
240       stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15));
241       stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15));
242       data[2*i] = (float)MULT16_16_P14(stereo->smooth_left, tmp);
243       data[2*i+1] = (float)MULT16_16_P14(stereo->smooth_right, tmp);
244    }
245 }
246 #endif /* #ifndef DISABLE_FLOAT_API */
247
248 void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo)
249 {
250    int i;
251    spx_word32_t balance;
252    spx_word16_t e_left, e_right, e_ratio;
253    RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;
254
255    COMPATIBILITY_HACK(stereo);
256    
257    balance=stereo->balance;
258    e_ratio=stereo->e_ratio;
259    
260    /* These two are Q14, with max value just below 2. */
261    e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance))));
262    e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8);
263
264    for (i=frame_size-1;i>=0;i--)
265    {
266       spx_int16_t tmp=data[i];
267       stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15));
268       stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15));
269       data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp);
270       data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp);
271    }
272 }
273
274 int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data)
275 {
276    RealSpeexStereoState *stereo;
277    spx_word16_t sign=1, dexp;
278    int tmp;
279
280    stereo = (RealSpeexStereoState*)data;
281    
282    COMPATIBILITY_HACK(stereo);
283
284    if (speex_bits_unpack_unsigned(bits, 1))
285       sign=-1;
286    dexp = speex_bits_unpack_unsigned(bits, 5);
287 #ifndef FIXED_POINT
288    stereo->balance = exp(sign*.25*dexp);
289 #else
290    stereo->balance = spx_exp(MULT16_16(sign, SHL16(dexp, 9)));
291 #endif
292    tmp = speex_bits_unpack_unsigned(bits, 2);
293    stereo->e_ratio = e_ratio_quant[tmp];
294
295    return 0;
296 }