src/lib/dl/ext/speex/quant_lsp.c

   1 /* Copyright (C) 2002 Jean-Marc Valin
   2    File: quant_lsp.c
   3    LSP vector quantization
   4
   5    Redistribution and use in source and binary forms, with or without
   6    modification, are permitted provided that the following conditions
   7    are met:
   8
   9    - Redistributions of source code must retain the above copyright
  10    notice, this list of conditions and the following disclaimer.
  11
  12    - Redistributions in binary form must reproduce the above copyright
  13    notice, this list of conditions and the following disclaimer in the
  14    documentation and/or other materials provided with the distribution.
  15
  16    - Neither the name of the Xiph.org Foundation nor the names of its
  17    contributors may be used to endorse or promote products derived from
  18    this software without specific prior written permission.
  19
  20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  31 */
  32
  33 #ifdef HAVE_CONFIG_H
  34 #include "config.h"
  35 #endif
  36
  37 #include "quant_lsp.h"
  38 #include "os_support.h"
  39 #include <math.h>
  40 #ifndef M_PI
  41 #define M_PI 3.14159265358979323846
  42 #endif
  43
  44 #include "arch.h"
  45
  46 #ifdef BFIN_ASM
  47 #include "quant_lsp_bfin.h"
  48 #endif
  49
  50 #ifdef FIXED_POINT
  51
  52 #define LSP_LINEAR(i) (SHL16(i+1,11))
  53 #define LSP_LINEAR_HIGH(i) (ADD16(MULT16_16_16(i,2560),6144))
  54 #define LSP_DIV_256(x) (SHL16((spx_word16_t)x, 5))
  55 #define LSP_DIV_512(x) (SHL16((spx_word16_t)x, 4))
  56 #define LSP_DIV_1024(x) (SHL16((spx_word16_t)x, 3))
  57 #define LSP_PI 25736
  58
  59 #else
  60
  61 #define LSP_LINEAR(i) (.25*(i)+.25)
  62 #define LSP_LINEAR_HIGH(i) (.3125*(i)+.75)
  63 #define LSP_SCALE 256.
  64 #define LSP_DIV_256(x) (0.0039062*(x))
  65 #define LSP_DIV_512(x) (0.0019531*(x))
  66 #define LSP_DIV_1024(x) (0.00097656*(x))
  67 #define LSP_PI M_PI
  68
  69 #endif
  70
  71 static void compute_quant_weights(spx_lsp_t *qlsp, spx_word16_t *quant_weight, int order)
  72 {
  73    int i;
  74    spx_word16_t tmp1, tmp2;
  75    for (i=0;i<order;i++)
  76    {
  77       if (i==0)
  78          tmp1 = qlsp[i];
  79       else
  80          tmp1 = qlsp[i]-qlsp[i-1];
  81       if (i==order-1)
  82          tmp2 = LSP_PI-qlsp[i];
  83       else
  84          tmp2 = qlsp[i+1]-qlsp[i];
  85       if (tmp2<tmp1)
  86          tmp1 = tmp2;
  87 #ifdef FIXED_POINT
  88       quant_weight[i] = DIV32_16(81920,ADD16(300,tmp1));
  89 #else
  90       quant_weight[i] = 10/(.04+tmp1);
  91 #endif
  92    }
  93
  94 }
  95
  96 /* Note: x is modified*/
  97 #ifndef OVERRIDE_LSP_QUANT
  98 static int lsp_quant(spx_word16_t *x, const signed char *cdbk, int nbVec, int nbDim)
  99 {
 100    int i,j;
 101    spx_word32_t dist;
 102    spx_word16_t tmp;
 103    spx_word32_t best_dist=VERY_LARGE32;
 104    int best_id=0;
 105    const signed char *ptr=cdbk;
 106    for (i=0;i<nbVec;i++)
 107    {
 108       dist=0;
 109       for (j=0;j<nbDim;j++)
 110       {
 111          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
 112          dist=MAC16_16(dist,tmp,tmp);
 113       }
 114       if (dist<best_dist)
 115       {
 116          best_dist=dist;
 117          best_id=i;
 118       }
 119    }
 120
 121    for (j=0;j<nbDim;j++)
 122       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
 123
 124    return best_id;
 125 }
 126 #endif
 127
 128 /* Note: x is modified*/
 129 #ifndef OVERRIDE_LSP_WEIGHT_QUANT
 130 static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, const signed char *cdbk, int nbVec, int nbDim)
 131 {
 132    int i,j;
 133    spx_word32_t dist;
 134    spx_word16_t tmp;
 135    spx_word32_t best_dist=VERY_LARGE32;
 136    int best_id=0;
 137    const signed char *ptr=cdbk;
 138    for (i=0;i<nbVec;i++)
 139    {
 140       dist=0;
 141       for (j=0;j<nbDim;j++)
 142       {
 143          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
 144          dist=MAC16_32_Q15(dist,weight[j],MULT16_16(tmp,tmp));
 145       }
 146       if (dist<best_dist)
 147       {
 148          best_dist=dist;
 149          best_id=i;
 150       }
 151    }
 152
 153    for (j=0;j<nbDim;j++)
 154       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
 155    return best_id;
 156 }
 157 #endif
 158
 159 void lsp_quant_nb(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 160 {
 161    int i;
 162    int id;
 163    spx_word16_t quant_weight[10];
 164
 165    for (i=0;i<order;i++)
 166       qlsp[i]=lsp[i];
 167
 168    compute_quant_weights(qlsp, quant_weight, order);
 169
 170    for (i=0;i<order;i++)
 171       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
 172
 173 #ifndef FIXED_POINT
 174    for (i=0;i<order;i++)
 175       qlsp[i] = LSP_SCALE*qlsp[i];
 176 #endif
 177    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
 178    speex_bits_pack(bits, id, 6);
 179
 180    for (i=0;i<order;i++)
 181       qlsp[i]*=2;
 182
 183    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
 184    speex_bits_pack(bits, id, 6);
 185
 186    for (i=0;i<5;i++)
 187       qlsp[i]*=2;
 188
 189    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
 190    speex_bits_pack(bits, id, 6);
 191
 192    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
 193    speex_bits_pack(bits, id, 6);
 194
 195    for (i=5;i<10;i++)
 196       qlsp[i]*=2;
 197
 198    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
 199    speex_bits_pack(bits, id, 6);
 200
 201 #ifdef FIXED_POINT
 202    for (i=0;i<order;i++)
 203       qlsp[i]=PSHR16(qlsp[i],2);
 204 #else
 205    for (i=0;i<order;i++)
 206       qlsp[i]=qlsp[i] * .00097656;
 207 #endif
 208
 209    for (i=0;i<order;i++)
 210       qlsp[i]=lsp[i]-qlsp[i];
 211 }
 212
 213 void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
 214 {
 215    int i, id;
 216    for (i=0;i<order;i++)
 217       lsp[i]=LSP_LINEAR(i);
 218
 219
 220    id=speex_bits_unpack_unsigned(bits, 6);
 221    for (i=0;i<10;i++)
 222       lsp[i] = ADD32(lsp[i], LSP_DIV_256(cdbk_nb[id*10+i]));
 223
 224    id=speex_bits_unpack_unsigned(bits, 6);
 225    for (i=0;i<5;i++)
 226       lsp[i] = ADD16(lsp[i], LSP_DIV_512(cdbk_nb_low1[id*5+i]));
 227
 228    id=speex_bits_unpack_unsigned(bits, 6);
 229    for (i=0;i<5;i++)
 230       lsp[i] = ADD32(lsp[i], LSP_DIV_1024(cdbk_nb_low2[id*5+i]));
 231
 232    id=speex_bits_unpack_unsigned(bits, 6);
 233    for (i=0;i<5;i++)
 234       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_512(cdbk_nb_high1[id*5+i]));
 235
 236    id=speex_bits_unpack_unsigned(bits, 6);
 237    for (i=0;i<5;i++)
 238       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_1024(cdbk_nb_high2[id*5+i]));
 239 }
 240
 241
 242 void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 243 {
 244    int i;
 245    int id;
 246    spx_word16_t quant_weight[10];
 247
 248    for (i=0;i<order;i++)
 249       qlsp[i]=lsp[i];
 250
 251    compute_quant_weights(qlsp, quant_weight, order);
 252
 253    for (i=0;i<order;i++)
 254       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
 255 #ifndef FIXED_POINT
 256    for (i=0;i<order;i++)
 257       qlsp[i]=qlsp[i]*LSP_SCALE;
 258 #endif
 259    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
 260    speex_bits_pack(bits, id, 6);
 261
 262    for (i=0;i<order;i++)
 263       qlsp[i]*=2;
 264
 265    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
 266    speex_bits_pack(bits, id, 6);
 267
 268    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
 269    speex_bits_pack(bits, id, 6);
 270
 271 #ifdef FIXED_POINT
 272    for (i=0;i<order;i++)
 273       qlsp[i] = PSHR16(qlsp[i],1);
 274 #else
 275    for (i=0;i<order;i++)
 276       qlsp[i] = qlsp[i]*0.0019531;
 277 #endif
 278
 279    for (i=0;i<order;i++)
 280       qlsp[i]=lsp[i]-qlsp[i];
 281 }
 282
 283 void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
 284 {
 285    int i, id;
 286    for (i=0;i<order;i++)
 287       lsp[i]=LSP_LINEAR(i);
 288
 289
 290    id=speex_bits_unpack_unsigned(bits, 6);
 291    for (i=0;i<10;i++)
 292       lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
 293
 294    id=speex_bits_unpack_unsigned(bits, 6);
 295    for (i=0;i<5;i++)
 296       lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
 297
 298    id=speex_bits_unpack_unsigned(bits, 6);
 299    for (i=0;i<5;i++)
 300       lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
 301
 302 }
 303
 304
 305 #ifdef DISABLE_WIDEBAND
 306 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 307 {
 308    speex_fatal("Wideband and Ultra-wideband are disabled");
 309 }
 310 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
 311 {
 312    speex_fatal("Wideband and Ultra-wideband are disabled");
 313 }
 314 #else
 315 extern const signed char high_lsp_cdbk[];
 316 extern const signed char high_lsp_cdbk2[];
 317
 318
 319 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
 320 {
 321    int i;
 322    int id;
 323    spx_word16_t quant_weight[10];
 324
 325    for (i=0;i<order;i++)
 326       qlsp[i]=lsp[i];
 327
 328    compute_quant_weights(qlsp, quant_weight, order);
 329
 330    /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
 331    quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
 332    for (i=1;i<order-1;i++)
 333    {
 334       tmp1 = 10/(qlsp[i]-qlsp[i-1]);
 335       tmp2 = 10/(qlsp[i+1]-qlsp[i]);
 336       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
 337       }*/
 338
 339    for (i=0;i<order;i++)
 340       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR_HIGH(i));
 341 #ifndef FIXED_POINT
 342    for (i=0;i<order;i++)
 343       qlsp[i] = qlsp[i]*LSP_SCALE;
 344 #endif
 345    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
 346    speex_bits_pack(bits, id, 6);
 347
 348    for (i=0;i<order;i++)
 349       qlsp[i]*=2;
 350
 351    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
 352    speex_bits_pack(bits, id, 6);
 353
 354 #ifdef FIXED_POINT
 355    for (i=0;i<order;i++)
 356       qlsp[i] = PSHR16(qlsp[i],1);
 357 #else
 358    for (i=0;i<order;i++)
 359       qlsp[i] = qlsp[i]*0.0019531;
 360 #endif
 361
 362    for (i=0;i<order;i++)
 363       qlsp[i]=lsp[i]-qlsp[i];
 364 }
 365
 366 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
 367 {
 368
 369    int i, id;
 370    for (i=0;i<order;i++)
 371       lsp[i]=LSP_LINEAR_HIGH(i);
 372
 373
 374    id=speex_bits_unpack_unsigned(bits, 6);
 375    for (i=0;i<order;i++)
 376       lsp[i] += LSP_DIV_256(high_lsp_cdbk[id*order+i]);
 377
 378
 379    id=speex_bits_unpack_unsigned(bits, 6);
 380    for (i=0;i<order;i++)
 381       lsp[i] += LSP_DIV_512(high_lsp_cdbk2[id*order+i]);
 382 }
 383
 384 #endif
 385