diff options
Diffstat (limited to 'libspeex/stereo.c')
-rw-r--r-- | libspeex/stereo.c | 240 |
1 files changed, 172 insertions, 68 deletions
diff --git a/libspeex/stereo.c b/libspeex/stereo.c index f18387e..bc6e1ba 100644 --- a/libspeex/stereo.c +++ b/libspeex/stereo.c @@ -35,12 +35,76 @@ #include <speex/speex_stereo.h> #include <speex/speex_callbacks.h> +#include "math_approx.h" #include "vq.h" #include <math.h> +#include "os_support.h" + +typedef struct RealSpeexStereoState { + spx_word32_t balance; /**< Left/right balance info */ + spx_word32_t e_ratio; /**< Ratio of energies: E(left+right)/[E(left)+E(right)] */ + spx_word32_t smooth_left; /**< Smoothed left channel gain */ + spx_word32_t smooth_right; /**< Smoothed right channel gain */ + spx_uint32_t reserved1; /**< Reserved for future use */ + spx_int32_t reserved2; /**< Reserved for future use */ +} RealSpeexStereoState; + /*float e_ratio_quant[4] = {1, 1.26, 1.587, 2};*/ +#ifndef FIXED_POINT static const float e_ratio_quant[4] = {.25f, .315f, .397f, .5f}; +static const float e_ratio_quant_bounds[3] = {0.2825f, 0.356f, 0.4485f}; +#else +static const spx_word16_t e_ratio_quant[4] = {8192, 10332, 13009, 16384}; +static const spx_word16_t e_ratio_quant_bounds[3] = {9257, 11665, 14696}; +static const spx_word16_t balance_bounds[31] = {18, 23, 30, 38, 49, 63, 81, 104, + 134, 172, 221, 284, 364, 468, 600, 771, + 990, 1271, 1632, 2096, 2691, 3455, 4436, 5696, + 7314, 9392, 12059, 15484, 19882, 25529, 32766}; +#endif + +/* This is an ugly compatibility hack that properly resets the stereo state + In case it it compiled in fixed-point, but initialised with the deprecated + floating point static initialiser */ +#ifdef FIXED_POINT +#define COMPATIBILITY_HACK(s) do {if ((s)->reserved1 != 0xdeadbeef) speex_stereo_state_reset((SpeexStereoState*)s); } while (0); +#else +#define COMPATIBILITY_HACK(s) +#endif +SpeexStereoState *speex_stereo_state_init() +{ + SpeexStereoState *stereo = speex_alloc(sizeof(SpeexStereoState)); + speex_stereo_state_reset(stereo); + return stereo; +} + +void speex_stereo_state_reset(SpeexStereoState *_stereo) +{ + RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; +#ifdef FIXED_POINT + stereo->balance = 65536; + stereo->e_ratio = 16384; + stereo->smooth_left = 16384; + stereo->smooth_right = 16384; + stereo->reserved1 = 0xdeadbeef; + stereo->reserved2 = 0; +#else + stereo->balance = 1.0f; + stereo->e_ratio = .5f; + stereo->smooth_left = 1.f; + stereo->smooth_right = 1.f; + stereo->reserved1 = 0; + stereo->reserved2 = 0; +#endif +} + +void speex_stereo_state_destroy(SpeexStereoState *stereo) +{ + speex_free(stereo); +} + +#ifndef DISABLE_FLOAT_API void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits) { int i, tmp; @@ -73,118 +137,158 @@ void speex_encode_stereo(float *data, int frame_size, SpeexBits *bits) speex_bits_pack(bits, (int)balance, 5); - /*Quantize energy ratio*/ - tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4); + /* FIXME: this is a hack */ + tmp=scal_quant(e_ratio*Q15_ONE, e_ratio_quant_bounds, 4); speex_bits_pack(bits, tmp, 2); } +#endif /* #ifndef DISABLE_FLOAT_API */ void speex_encode_stereo_int(spx_int16_t *data, int frame_size, SpeexBits *bits) { int i, tmp; - float e_left=0, e_right=0, e_tot=0; - float balance, e_ratio; + spx_word32_t e_left=0, e_right=0, e_tot=0; + spx_word32_t balance, e_ratio; + spx_word32_t largest, smallest; + int balance_id; +#ifdef FIXED_POINT + int shift; +#endif + + /* In band marker */ + speex_bits_pack(bits, 14, 5); + /* Stereo marker */ + speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4); + for (i=0;i<frame_size;i++) { - e_left += ((float)data[2*i])*data[2*i]; - e_right += ((float)data[2*i+1])*data[2*i+1]; + e_left += SHR32(MULT16_16(data[2*i],data[2*i]),8); + e_right += SHR32(MULT16_16(data[2*i+1],data[2*i+1]),8); +#ifdef FIXED_POINT + /* I think this is actually unbiased */ + data[i] = SHR16(data[2*i],1)+PSHR16(data[2*i+1],1); +#else data[i] = .5*(((float)data[2*i])+data[2*i+1]); - e_tot += ((float)data[i])*data[i]; +#endif + e_tot += SHR32(MULT16_16(data[i],data[i]),8); } - balance=(e_left+1)/(e_right+1); - e_ratio = e_tot/(1+e_left+e_right); - - /*Quantization*/ - speex_bits_pack(bits, 14, 5); - speex_bits_pack(bits, SPEEX_INBAND_STEREO, 4); - - balance=4*log(balance); - - /*Pack sign*/ - if (balance>0) + if (e_left > e_right) + { speex_bits_pack(bits, 0, 1); - else + largest = e_left; + smallest = e_right; + } else { speex_bits_pack(bits, 1, 1); - balance=floor(.5+fabs(balance)); - if (balance>30) - balance=31; + largest = e_right; + smallest = e_left; + } + + /* Balance quantization */ +#ifdef FIXED_POINT + shift = spx_ilog2(largest)-15; + largest = VSHR32(largest, shift-4); + smallest = VSHR32(smallest, shift); + balance = DIV32(largest, ADD32(smallest, 1)); + if (balance > 32767) + balance = 32767; + balance_id = scal_quant(EXTRACT16(balance), balance_bounds, 32); +#else + balance=(largest+1.)/(smallest+1.); + balance=4*log(balance); + balance_id=floor(.5+fabs(balance)); + if (balance_id>30) + balance_id=31; +#endif - speex_bits_pack(bits, (int)balance, 5); + speex_bits_pack(bits, balance_id, 5); + + /* "coherence" quantisation */ +#ifdef FIXED_POINT + shift = spx_ilog2(e_tot); + e_tot = VSHR32(e_tot, shift-25); + e_left = VSHR32(e_left, shift-10); + e_right = VSHR32(e_right, shift-10); + e_ratio = DIV32(e_tot, e_left+e_right+1); +#else + e_ratio = e_tot/(1.+e_left+e_right); +#endif - /*Quantize energy ratio*/ - tmp=vq_index(&e_ratio, e_ratio_quant, 1, 4); + tmp=scal_quant(EXTRACT16(e_ratio), e_ratio_quant_bounds, 4); + /*fprintf (stderr, "%d %d %d %d\n", largest, smallest, balance_id, e_ratio);*/ speex_bits_pack(bits, tmp, 2); } -void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *stereo) +#ifndef DISABLE_FLOAT_API +void speex_decode_stereo(float *data, int frame_size, SpeexStereoState *_stereo) { - float balance, e_ratio; int i; - float e_tot=0, e_left, e_right, e_sum; - + spx_word32_t balance; + spx_word16_t e_left, e_right, e_ratio; + RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; + + COMPATIBILITY_HACK(stereo); + balance=stereo->balance; e_ratio=stereo->e_ratio; - for (i=frame_size-1;i>=0;i--) - { - e_tot += ((float)data[i])*data[i]; - } - e_sum=e_tot/e_ratio; - e_left = e_sum*balance / (1+balance); - e_right = e_sum-e_left; - - e_left = sqrt(e_left/(e_tot+.01)); - e_right = sqrt(e_right/(e_tot+.01)); + + /* These two are Q14, with max value just below 2. */ + e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance)))); + e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8); for (i=frame_size-1;i>=0;i--) { - float ftmp=data[i]; - stereo->smooth_left = .98*stereo->smooth_left + .02*e_left; - stereo->smooth_right = .98*stereo->smooth_right + .02*e_right; - data[2*i] = stereo->smooth_left*ftmp; - data[2*i+1] = stereo->smooth_right*ftmp; + spx_word16_t tmp=data[i]; + stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15)); + stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15)); + data[2*i] = (float)MULT16_16_P14(stereo->smooth_left, tmp); + data[2*i+1] = (float)MULT16_16_P14(stereo->smooth_right, tmp); } } +#endif /* #ifndef DISABLE_FLOAT_API */ -void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *stereo) +void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo) { - float balance, e_ratio; int i; - float e_tot=0, e_left, e_right, e_sum; + spx_word32_t balance; + spx_word16_t e_left, e_right, e_ratio; + RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo; + COMPATIBILITY_HACK(stereo); + balance=stereo->balance; e_ratio=stereo->e_ratio; - for (i=frame_size-1;i>=0;i--) - { - e_tot += ((float)data[i])*data[i]; - } - e_sum=e_tot/e_ratio; - e_left = e_sum*balance / (1+balance); - e_right = e_sum-e_left; - - e_left = sqrt(e_left/(e_tot+.01)); - e_right = sqrt(e_right/(e_tot+.01)); + + /* These two are Q14, with max value just below 2. */ + e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance)))); + e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8); for (i=frame_size-1;i>=0;i--) { - float ftmp=data[i]; - stereo->smooth_left = .98*stereo->smooth_left + .02*e_left; - stereo->smooth_right = .98*stereo->smooth_right + .02*e_right; - data[2*i] = stereo->smooth_left*ftmp; - data[2*i+1] = stereo->smooth_right*ftmp; + spx_int16_t tmp=data[i]; + stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15)); + stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15)); + data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp); + data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp); } } int speex_std_stereo_request_handler(SpeexBits *bits, void *state, void *data) { - SpeexStereoState *stereo; - float sign=1; + RealSpeexStereoState *stereo; + spx_word16_t sign=1, dexp; int tmp; - stereo = (SpeexStereoState*)data; + stereo = (RealSpeexStereoState*)data; + + COMPATIBILITY_HACK(stereo); + if (speex_bits_unpack_unsigned(bits, 1)) sign=-1; - tmp = speex_bits_unpack_unsigned(bits, 5); - stereo->balance = exp(sign*.25*tmp); - + dexp = speex_bits_unpack_unsigned(bits, 5); +#ifndef FIXED_POINT + stereo->balance = exp(sign*.25*dexp); +#else + stereo->balance = spx_exp(MULT16_16(sign, SHL16(dexp, 9))); +#endif tmp = speex_bits_unpack_unsigned(bits, 2); stereo->e_ratio = e_ratio_quant[tmp]; |