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
speex_stereo_state_init()75 EXPORT SpeexStereoState *speex_stereo_state_init()
76 {
77 SpeexStereoState *stereo = speex_alloc(sizeof(SpeexStereoState));
78 speex_stereo_state_reset(stereo);
79 return stereo;
80 }
81
speex_stereo_state_reset(SpeexStereoState * _stereo)82 EXPORT 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
speex_stereo_state_destroy(SpeexStereoState * stereo)102 EXPORT void speex_stereo_state_destroy(SpeexStereoState *stereo)
103 {
104 speex_free(stereo);
105 }
106
107 #ifndef DISABLE_FLOAT_API
speex_encode_stereo(float * data,int frame_size,SpeexBits * bits)108 EXPORT 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
speex_encode_stereo_int(spx_int16_t * data,int frame_size,SpeexBits * bits)146 EXPORT 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
speex_decode_stereo(float * data,int frame_size,SpeexStereoState * _stereo)221 EXPORT 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
speex_decode_stereo_int(spx_int16_t * data,int frame_size,SpeexStereoState * _stereo)248 EXPORT 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
speex_std_stereo_request_handler(SpeexBits * bits,void * state,void * data)274 EXPORT 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 }
297