diff options
Diffstat (limited to 'lib/rbcodec/codecs/libspeex/stereo.c')
-rw-r--r-- | lib/rbcodec/codecs/libspeex/stereo.c | 302 |
1 files changed, 302 insertions, 0 deletions
diff --git a/lib/rbcodec/codecs/libspeex/stereo.c b/lib/rbcodec/codecs/libspeex/stereo.c new file mode 100644 index 0000000000..652d2a6e90 --- /dev/null +++ b/lib/rbcodec/codecs/libspeex/stereo.c | |||
@@ -0,0 +1,302 @@ | |||
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-speex.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 | static SpeexStereoState global_stereo_state; | ||
76 | SpeexStereoState *speex_stereo_state_init() | ||
77 | { | ||
78 | /* SpeexStereoState *stereo = speex_alloc(sizeof(SpeexStereoState)); */ | ||
79 | SpeexStereoState *stereo = &global_stereo_state; | ||
80 | speex_stereo_state_reset(stereo); | ||
81 | return stereo; | ||
82 | } | ||
83 | |||
84 | void speex_stereo_state_reset(SpeexStereoState *_stereo) | ||
85 | { | ||
86 | RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; | ||
87 | #ifdef FIXED_POINT | ||
88 | stereo->balance = 65536; | ||
89 | stereo->e_ratio = 16384; | ||
90 | stereo->smooth_left = 16384; | ||
91 | stereo->smooth_right = 16384; | ||
92 | stereo->reserved1 = 0xdeadbeef; | ||
93 | stereo->reserved2 = 0; | ||
94 | #else | ||
95 | stereo->balance = 1.0f; | ||
96 | stereo->e_ratio = .5f; | ||
97 | stereo->smooth_left = 1.f; | ||
98 | stereo->smooth_right = 1.f; | ||
99 | stereo->reserved1 = 0; | ||
100 | stereo->reserved2 = 0; | ||
101 | #endif | ||
102 | } | ||
103 | |||
104 | void speex_stereo_state_destroy(SpeexStereoState *stereo) | ||
105 | { | ||
106 | (void)stereo; | ||
107 | /* speex_free(stereo); */ | ||
108 | } | ||
109 | |||
110 | #ifndef SPEEX_DISABLE_ENCODER | ||
111 | #ifndef DISABLE_FLOAT_API | ||
112 | void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits) | ||
113 | { | ||
114 | int i, tmp; | ||
115 | float e_left=0, e_right=0, e_tot=0; | ||
116 | float balance, e_ratio; | ||
117 | for (i=0;i<frame_size;i++) | ||
118 | { | ||
119 | e_left += ((float)data[2*i])*data[2*i]; | ||
120 | e_right += ((float)data[2*i+1])*data[2*i+1]; | ||
121 | data[i] = .5*(((float)data[2*i])+data[2*i+1]); | ||
122 | e_tot += ((float)data[i])*data[i]; | ||
123 | } | ||
124 | balance=(e_left+1)/(e_right+1); | ||
125 | e_ratio = e_tot/(1+e_left+e_right); | ||
126 | |||
127 | /*Quantization*/ | ||
128 | speex_bits_pack(bits, 14, 5); | ||
129 | speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4); | ||
130 | |||
131 | balance=4*log(balance); | ||
132 | |||
133 | /*Pack sign*/ | ||
134 | if (balance>0) | ||
135 | speex_bits_pack(bits, 0, 1); | ||
136 | else | ||
137 | speex_bits_pack(bits, 1, 1); | ||
138 | balance=floor(.5+fabs(balance)); | ||
139 | if (balance>30) | ||
140 | balance=31; | ||
141 | |||
142 | speex_bits_pack(bits, (int)balance, 5); | ||
143 | |||
144 | /* FIXME: this is a hack */ | ||
145 | tmp=scal_quant(e_ratio*Q15_ONE, e_ratio_quant_bounds, 4); | ||
146 | speex_bits_pack(bits, tmp, 2); | ||
147 | } | ||
148 | #endif /* #ifndef DISABLE_FLOAT_API */ | ||
149 | |||
150 | void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits) | ||
151 | { | ||
152 | int i, tmp; | ||
153 | spx_word32_t e_left=0, e_right=0, e_tot=0; | ||
154 | spx_word32_t balance, e_ratio; | ||
155 | spx_word32_t largest, smallest; | ||
156 | int balance_id; | ||
157 | #ifdef FIXED_POINT | ||
158 | int shift; | ||
159 | #endif | ||
160 | |||
161 | /* In band marker */ | ||
162 | speex_bits_pack(bits, 14, 5); | ||
163 | /* Stereo marker */ | ||
164 | speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4); | ||
165 | |||
166 | for (i=0;i<frame_size;i++) | ||
167 | { | ||
168 | e_left += SHR32(MULT16_16(data[2*i],data[2*i]),8); | ||
169 | e_right += SHR32(MULT16_16(data[2*i+1],data[2*i+1]),8); | ||
170 | #ifdef FIXED_POINT | ||
171 | /* I think this is actually unbiased */ | ||
172 | data[i] = SHR16(data[2*i],1)+PSHR16(data[2*i+1],1); | ||
173 | #else | ||
174 | data[i] = .5*(((float)data[2*i])+data[2*i+1]); | ||
175 | #endif | ||
176 | e_tot += SHR32(MULT16_16(data[i],data[i]),8); | ||
177 | } | ||
178 | if (e_left > e_right) | ||
179 | { | ||
180 | speex_bits_pack(bits, 0, 1); | ||
181 | largest = e_left; | ||
182 | smallest = e_right; | ||
183 | } else { | ||
184 | speex_bits_pack(bits, 1, 1); | ||
185 | largest = e_right; | ||
186 | smallest = e_left; | ||
187 | } | ||
188 | |||
189 | /* Balance quantization */ | ||
190 | #ifdef FIXED_POINT | ||
191 | shift = spx_ilog2(largest)-15; | ||
192 | largest = VSHR32(largest, shift-4); | ||
193 | smallest = VSHR32(smallest, shift); | ||
194 | balance = DIV32(largest, ADD32(smallest, 1)); | ||
195 | if (balance > 32767) | ||
196 | balance = 32767; | ||
197 | balance_id = scal_quant(EXTRACT16(balance), balance_bounds, 32); | ||
198 | #else | ||
199 | balance=(largest+1.)/(smallest+1.); | ||
200 | balance=4*log(balance); | ||
201 | balance_id=floor(.5+fabs(balance)); | ||
202 | if (balance_id>30) | ||
203 | balance_id=31; | ||
204 | #endif | ||
205 | |||
206 | speex_bits_pack(bits, balance_id, 5); | ||
207 | |||
208 | /* "coherence" quantisation */ | ||
209 | #ifdef FIXED_POINT | ||
210 | shift = spx_ilog2(e_tot); | ||
211 | e_tot = VSHR32(e_tot, shift-25); | ||
212 | e_left = VSHR32(e_left, shift-10); | ||
213 | e_right = VSHR32(e_right, shift-10); | ||
214 | e_ratio = DIV32(e_tot, e_left+e_right+1); | ||
215 | #else | ||
216 | e_ratio = e_tot/(1.+e_left+e_right); | ||
217 | #endif | ||
218 | |||
219 | tmp=scal_quant(EXTRACT16(e_ratio), e_ratio_quant_bounds, 4); | ||
220 | /*fprintf (stderr, "%d %d %d %d\n", largest, smallest, balance_id, e_ratio);*/ | ||
221 | speex_bits_pack(bits, tmp, 2); | ||
222 | } | ||
223 | #endif /* SPEEX_DISABLE_ENCODER */ | ||
224 | |||
225 | #ifndef DISABLE_FLOAT_API | ||
226 | void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *_stereo) | ||
227 | { | ||
228 | int i; | ||
229 | spx_word32_t balance; | ||
230 | spx_word16_t e_left, e_right, e_ratio; | ||
231 | RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; | ||
232 | |||
233 | COMPATIBILITY_HACK(stereo); | ||
234 | |||
235 | balance=stereo->balance; | ||
236 | e_ratio=stereo->e_ratio; | ||
237 | |||
238 | /* These two are Q14, with max value just below 2. */ | ||
239 | e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance)))); | ||
240 | e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8); | ||
241 | |||
242 | for (i=frame_size-1;i>=0;i--) | ||
243 | { | ||
244 | spx_word16_t tmp=data[i]; | ||
245 | stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15)); | ||
246 | stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15)); | ||
247 | data[2*i] = (float)MULT16_16_P14(stereo->smooth_left, tmp); | ||
248 | data[2*i+1] = (float)MULT16_16_P14(stereo->smooth_right, tmp); | ||
249 | } | ||
250 | } | ||
251 | #endif /* #ifndef DISABLE_FLOAT_API */ | ||
252 | |||
253 | void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo) | ||
254 | { | ||
255 | int i; | ||
256 | spx_word32_t balance; | ||
257 | spx_word16_t e_left, e_right, e_ratio; | ||
258 | RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; | ||
259 | |||
260 | /* COMPATIBILITY_HACK(stereo); */ | ||
261 | |||
262 | balance=stereo->balance; | ||
263 | e_ratio=stereo->e_ratio; | ||
264 | |||
265 | /* These two are Q14, with max value just below 2. */ | ||
266 | e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance)))); | ||
267 | e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8); | ||
268 | |||
269 | for (i=frame_size-1;i>=0;i--) | ||
270 | { | ||
271 | spx_int16_t tmp=data[i]; | ||
272 | stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15)); | ||
273 | stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15)); | ||
274 | data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp); | ||
275 | data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp); | ||
276 | } | ||
277 | } | ||
278 | |||
279 | int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data) | ||
280 | { | ||
281 | (void)state; | ||
282 | RealSpeexStereoState *stereo; | ||
283 | spx_word16_t sign=1, dexp; | ||
284 | int tmp; | ||
285 | |||
286 | stereo = (RealSpeexStereoState*)data; | ||
287 | |||
288 | /* COMPATIBILITY_HACK(stereo); */ | ||
289 | |||
290 | if (speex_bits_unpack_unsigned(bits, 1)) | ||
291 | sign=-1; | ||
292 | dexp = speex_bits_unpack_unsigned(bits, 5); | ||
293 | #ifndef FIXED_POINT | ||
294 | stereo->balance = exp(sign*.25*dexp); | ||
295 | #else | ||
296 | stereo->balance = spx_exp(MULT16_16(sign, SHL16(dexp, 9))); | ||
297 | #endif | ||
298 | tmp = speex_bits_unpack_unsigned(bits, 2); | ||
299 | stereo->e_ratio = e_ratio_quant[tmp]; | ||
300 | |||
301 | return 0; | ||
302 | } | ||