apps/codecs/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-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 }