opencore/codecs_v2/audio/gsm_amr/amr_wb/dec/src/pvamrwb_math_op.cpp

   1 /* ------------------------------------------------------------------
   2  * Copyright (C) 1998-2009 PacketVideo
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
  13  * express or implied.
  14  * See the License for the specific language governing permissions
  15  * and limitations under the License.
  16  * -------------------------------------------------------------------
  17  */
  18 /****************************************************************************************
  19 Portions of this file are derived from the following 3GPP standard:
  20
  21     3GPP TS 26.173
  22     ANSI-C code for the Adaptive Multi-Rate - Wideband (AMR-WB) speech codec
  23     Available from http://www.3gpp.org
  24
  25 (C) 2007, 3GPP Organizational Partners (ARIB, ATIS, CCSA, ETSI, TTA, TTC)
  26 Permission to distribute, modify and use this file under the standard license
  27 terms listed above has been obtained from the copyright holder.
  28 ****************************************************************************************/
  29 /*___________________________________________________________________________
  30
  31     This file contains mathematic operations in fixed point.
  32
  33     mult_int16_r()     : Same as mult_int16 with rounding
  34     shr_rnd()          : Same as shr(var1,var2) but with rounding
  35     div_16by16()       : fractional integer division
  36     one_ov_sqrt()      : Compute 1/sqrt(L_x)
  37     one_ov_sqrt_norm() : Compute 1/sqrt(x)
  38     power_of_2()       : power of 2
  39     Dot_product12()    : Compute scalar product of <x[],y[]> using accumulator
  40     Isqrt()            : inverse square root (16 bits precision).
  41     amrwb_log_2()      : log2 (16 bits precision).
  42
  43     These operations are not standard double precision operations.
  44     They are used where low complexity is important and the full 32 bits
  45     precision is not necessary. For example, the function Div_32() has a
  46     24 bits precision which is enough for our purposes.
  47
  48     In this file, the values use theses representations:
  49
  50     int32 L_32     : standard signed 32 bits format
  51     int16 hi, lo   : L_32 = hi<<16 + lo<<1  (DPF - Double Precision Format)
  52     int32 frac, int16 exp : L_32 = frac << exp-31  (normalised format)
  53     int16 int, frac        : L_32 = int.frac        (fractional format)
  54  ----------------------------------------------------------------------------*/
  55
  56 #include "pv_amr_wb_type_defs.h"
  57 #include "pvamrwbdecoder_basic_op.h"
  58 #include "pvamrwb_math_op.h"
  59
  60
  61 /*----------------------------------------------------------------------------
  62
  63      Function Name : mult_int16_r
  64
  65      Purpose :
  66
  67      Same as mult_int16 with rounding, i.e.:
  68        mult_int16_r(var1,var2) = extract_l(L_shr(((var1 * var2) + 16384),15)) and
  69        mult_int16_r(-32768,-32768) = 32767.
  70
  71      Complexity weight : 2
  72
  73      Inputs :
  74
  75       var1
  76                16 bit short signed integer (int16) whose value falls in the
  77                range : 0xffff 8000 <= var1 <= 0x0000 7fff.
  78
  79       var2
  80                16 bit short signed integer (int16) whose value falls in the
  81                range : 0xffff 8000 <= var1 <= 0x0000 7fff.
  82
  83      Outputs :
  84
  85       none
  86
  87      Return Value :
  88
  89       var_out
  90                16 bit short signed integer (int16) whose value falls in the
  91                range : 0xffff 8000 <= var_out <= 0x0000 7fff.
  92  ----------------------------------------------------------------------------*/
  93
  94 int16 mult_int16_r(int16 var1, int16 var2)
  95 {
  96     int32 L_product_arr;
  97
  98     L_product_arr = (int32) var1 * (int32) var2;      /* product */
  99     L_product_arr += (int32) 0x00004000L;      /* round */
 100     L_product_arr >>= 15;       /* shift */
 101     if ((L_product_arr >> 15) != (L_product_arr >> 31))
 102     {
 103         L_product_arr = (L_product_arr >> 31) ^ MAX_16;
 104     }
 105
 106     return ((int16)L_product_arr);
 107 }
 108
 109
 110
 111 /*----------------------------------------------------------------------------
 112
 113      Function Name : shr_rnd
 114
 115      Purpose :
 116
 117      Same as shr(var1,var2) but with rounding. Saturate the result in case of|
 118      underflows or overflows :
 119       - If var2 is greater than zero :
 120             if (sub(shl_int16(shr(var1,var2),1),shr(var1,sub(var2,1))))
 121             is equal to zero
 122                        then
 123                        shr_rnd(var1,var2) = shr(var1,var2)
 124                        else
 125                        shr_rnd(var1,var2) = add_int16(shr(var1,var2),1)
 126       - If var2 is less than or equal to zero :
 127                        shr_rnd(var1,var2) = shr(var1,var2).
 128
 129      Complexity weight : 2
 130
 131      Inputs :
 132
 133       var1
 134                16 bit short signed integer (int16) whose value falls in the
 135                range : 0xffff 8000 <= var1 <= 0x0000 7fff.
 136
 137       var2
 138                16 bit short signed integer (int16) whose value falls in the
 139                range : 0x0000 0000 <= var2 <= 0x0000 7fff.
 140
 141      Outputs :
 142
 143       none
 144
 145      Return Value :
 146
 147       var_out
 148                16 bit short signed integer (int16) whose value falls in the
 149                range : 0xffff 8000 <= var_out <= 0x0000 7fff.
 150  ----------------------------------------------------------------------------*/
 151
 152 int16 shr_rnd(int16 var1, int16 var2)
 153 {
 154     int16 var_out;
 155
 156     var_out = (int16)(var1 >> (var2 & 0xf));
 157     if (var2)
 158     {
 159         if ((var1 & ((int16) 1 << (var2 - 1))) != 0)
 160         {
 161             var_out++;
 162         }
 163     }
 164     return (var_out);
 165 }
 166
 167
 168 /*----------------------------------------------------------------------------
 169
 170      Function Name : div_16by16
 171
 172      Purpose :
 173
 174      Produces a result which is the fractional integer division of var1  by
 175      var2; var1 and var2 must be positive and var2 must be greater or equal
 176      to var1; the result is positive (leading bit equal to 0) and truncated
 177      to 16 bits.
 178      If var1 = var2 then div(var1,var2) = 32767.
 179
 180      Complexity weight : 18
 181
 182      Inputs :
 183
 184       var1
 185                16 bit short signed integer (int16) whose value falls in the
 186                range : 0x0000 0000 <= var1 <= var2 and var2 != 0.
 187
 188       var2
 189                16 bit short signed integer (int16) whose value falls in the
 190                range : var1 <= var2 <= 0x0000 7fff and var2 != 0.
 191
 192      Outputs :
 193
 194       none
 195
 196      Return Value :
 197
 198       var_out
 199                16 bit short signed integer (int16) whose value falls in the
 200                range : 0x0000 0000 <= var_out <= 0x0000 7fff.
 201                It's a Q15 value (point between b15 and b14).
 202  ----------------------------------------------------------------------------*/
 203
 204 int16 div_16by16(int16 var1, int16 var2)
 205 {
 206
 207     int16 var_out = 0;
 208     register int16 iteration;
 209     int32 L_num;
 210     int32 L_denom;
 211     int32 L_denom_by_2;
 212     int32 L_denom_by_4;
 213
 214     if ((var1 > var2) || (var1 < 0))
 215     {
 216         return 0; // used to exit(0);
 217     }
 218     if (var1)
 219     {
 220         if (var1 != var2)
 221         {
 222
 223             L_num = (int32) var1;
 224             L_denom = (int32) var2;
 225             L_denom_by_2 = (L_denom << 1);
 226             L_denom_by_4 = (L_denom << 2);
 227             for (iteration = 5; iteration > 0; iteration--)
 228             {
 229                 var_out <<= 3;
 230                 L_num   <<= 3;
 231
 232                 if (L_num >= L_denom_by_4)
 233                 {
 234                     L_num -= L_denom_by_4;
 235                     var_out |= 4;
 236                 }
 237
 238                 if (L_num >= L_denom_by_2)
 239                 {
 240                     L_num -= L_denom_by_2;
 241                     var_out |=  2;
 242                 }
 243
 244                 if (L_num >= (L_denom))
 245                 {
 246                     L_num -= (L_denom);
 247                     var_out |=  1;
 248                 }
 249
 250             }
 251         }
 252         else
 253         {
 254             var_out = MAX_16;
 255         }
 256     }
 257
 258     return (var_out);
 259
 260 }
 261
 262
 263
 264 /*----------------------------------------------------------------------------
 265
 266      Function Name : one_ov_sqrt
 267
 268          Compute 1/sqrt(L_x).
 269          if L_x is negative or zero, result is 1 (7fffffff).
 270
 271   Algorithm:
 272
 273      1- Normalization of L_x.
 274      2- call Isqrt_n(L_x, exponant)
 275      3- L_y = L_x << exponant
 276  ----------------------------------------------------------------------------*/
 277 int32 one_ov_sqrt(     /* (o) Q31 : output value (range: 0<=val<1)         */
 278     int32 L_x         /* (i) Q0  : input value  (range: 0<=val<=7fffffff) */
 279 )
 280 {
 281     int16 exp;
 282     int32 L_y;
 283
 284     exp = normalize_amr_wb(L_x);
 285     L_x <<= exp;                 /* L_x is normalized */
 286     exp = 31 - exp;
 287
 288     one_ov_sqrt_norm(&L_x, &exp);
 289
 290     L_y = shl_int32(L_x, exp);                 /* denormalization   */
 291
 292     return (L_y);
 293 }
 294
 295 /*----------------------------------------------------------------------------
 296
 297      Function Name : one_ov_sqrt_norm
 298
 299          Compute 1/sqrt(value).
 300          if value is negative or zero, result is 1 (frac=7fffffff, exp=0).
 301
 302   Algorithm:
 303
 304      The function 1/sqrt(value) is approximated by a table and linear
 305      interpolation.
 306
 307      1- If exponant is odd then shift fraction right once.
 308      2- exponant = -((exponant-1)>>1)
 309      3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization.
 310      4- a = bit10-b24
 311      5- i -=16
 312      6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2
 313  ----------------------------------------------------------------------------*/
 314 static const int16 table_isqrt[49] =
 315 {
 316     32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214,
 317     25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155,
 318     21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539,
 319     19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674,
 320     17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384
 321 };
 322
 323 void one_ov_sqrt_norm(
 324     int32 * frac,                        /* (i/o) Q31: normalized value (1.0 < frac <= 0.5) */
 325     int16 * exp                          /* (i/o)    : exponent (value = frac x 2^exponent) */
 326 )
 327 {
 328     int16 i, a, tmp;
 329
 330
 331     if (*frac <= (int32) 0)
 332     {
 333         *exp = 0;
 334         *frac = 0x7fffffffL;
 335         return;
 336     }
 337
 338     if ((*exp & 1) == 1)  /* If exponant odd -> shift right */
 339         *frac >>= 1;
 340
 341     *exp = negate_int16((*exp -  1) >> 1);
 342
 343     *frac >>= 9;
 344     i = extract_h(*frac);                  /* Extract b25-b31 */
 345     *frac >>= 1;
 346     a = (int16)(*frac);                  /* Extract b10-b24 */
 347     a = (int16)(a & (int16) 0x7fff);
 348
 349     i -= 16;
 350
 351     *frac = L_deposit_h(table_isqrt[i]);   /* table[i] << 16         */
 352     tmp = table_isqrt[i] - table_isqrt[i + 1];      /* table[i] - table[i+1]) */
 353
 354     *frac = msu_16by16_from_int32(*frac, tmp, a);          /* frac -=  tmp*a*2       */
 355
 356     return;
 357 }
 358
 359 /*----------------------------------------------------------------------------
 360
 361      Function Name : power_2()
 362
 363        L_x = pow(2.0, exponant.fraction)         (exponant = interger part)
 364            = pow(2.0, 0.fraction) << exponant
 365
 366   Algorithm:
 367
 368      The function power_2(L_x) is approximated by a table and linear
 369      interpolation.
 370
 371      1- i = bit10-b15 of fraction,   0 <= i <= 31
 372      2- a = bit0-b9   of fraction
 373      3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2
 374      4- L_x = L_x >> (30-exponant)     (with rounding)
 375  ----------------------------------------------------------------------------*/
 376 const int16 table_pow2[33] =
 377 {
 378     16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911,
 379     20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726,
 380     25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706,
 381     31379, 32066, 32767
 382 };
 383
 384 int32 power_of_2(                         /* (o) Q0  : result       (range: 0<=val<=0x7fffffff) */
 385     int16 exponant,                      /* (i) Q0  : Integer part.      (range: 0<=val<=30)   */
 386     int16 fraction                       /* (i) Q15 : Fractionnal part.  (range: 0.0<=val<1.0) */
 387 )
 388 {
 389     int16 exp, i, a, tmp;
 390     int32 L_x;
 391
 392     L_x = fraction << 5;          /* L_x = fraction<<6           */
 393     i = (fraction >> 10);                  /* Extract b10-b16 of fraction */
 394     a = (int16)(L_x);                    /* Extract b0-b9   of fraction */
 395     a = (int16)(a & (int16) 0x7fff);
 396
 397     L_x = ((int32)table_pow2[i]) << 15;    /* table[i] << 16        */
 398     tmp = table_pow2[i] - table_pow2[i + 1];        /* table[i] - table[i+1] */
 399     L_x -= ((int32)tmp * a);             /* L_x -= tmp*a*2        */
 400
 401     exp = 29 - exponant ;
 402
 403     if (exp)
 404     {
 405         L_x = ((L_x >> exp) + ((L_x >> (exp - 1)) & 1));
 406     }
 407
 408     return (L_x);
 409 }
 410
 411 /*----------------------------------------------------------------------------
 412  *
 413  *   Function Name : Dot_product12()
 414  *
 415  *       Compute scalar product of <x[],y[]> using accumulator.
 416  *
 417  *       The result is normalized (in Q31) with exponent (0..30).
 418  *
 419  *  Algorithm:
 420  *
 421  *       dot_product = sum(x[i]*y[i])     i=0..N-1
 422  ----------------------------------------------------------------------------*/
 423
 424 int32 Dot_product12(   /* (o) Q31: normalized result (1 < val <= -1) */
 425     int16 x[],        /* (i) 12bits: x vector                       */
 426     int16 y[],        /* (i) 12bits: y vector                       */
 427     int16 lg,         /* (i)    : vector length                     */
 428     int16 * exp       /* (o)    : exponent of result (0..+30)       */
 429 )
 430 {
 431     int16 i, sft;
 432     int32 L_sum;
 433     int16 *pt_x = x;
 434     int16 *pt_y = y;
 435
 436     L_sum = 1L;
 437
 438
 439     for (i = lg >> 3; i != 0; i--)
 440     {
 441         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 442         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 443         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 444         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 445         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 446         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 447         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 448         L_sum = mac_16by16_to_int32(L_sum, *(pt_x++), *(pt_y++));
 449     }
 450
 451     /* Normalize acc in Q31 */
 452
 453     sft = normalize_amr_wb(L_sum);
 454     L_sum <<= sft;
 455
 456     *exp = 30 - sft;                    /* exponent = 0..30 */
 457
 458     return (L_sum);
 459 }
 460
 461 /* Table for Log2() */
 462 const int16 Log2_norm_table[33] =
 463 {
 464     0, 1455, 2866, 4236, 5568, 6863, 8124, 9352, 10549, 11716,
 465     12855, 13967, 15054, 16117, 17156, 18172, 19167, 20142, 21097, 22033,
 466     22951, 23852, 24735, 25603, 26455, 27291, 28113, 28922, 29716, 30497,
 467     31266, 32023, 32767
 468 };
 469
 470 /*----------------------------------------------------------------------------
 471  *
 472  *   FUNCTION:   Lg2_normalized()
 473  *
 474  *   PURPOSE:   Computes log2(L_x, exp),  where   L_x is positive and
 475  *              normalized, and exp is the normalisation exponent
 476  *              If L_x is negative or zero, the result is 0.
 477  *
 478  *   DESCRIPTION:
 479  *        The function Log2(L_x) is approximated by a table and linear
 480  *        interpolation. The following steps are used to compute Log2(L_x)
 481  *
 482  *           1- exponent = 30-norm_exponent
 483  *           2- i = bit25-b31 of L_x;  32<=i<=63  (because of normalization).
 484  *           3- a = bit10-b24
 485  *           4- i -=32
 486  *           5- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2
 487  *
 488 ----------------------------------------------------------------------------*/
 489 void Lg2_normalized(
 490     int32 L_x,         /* (i) : input value (normalized)                    */
 491     int16 exp,         /* (i) : norm_l (L_x)                                */
 492     int16 *exponent,   /* (o) : Integer part of Log2.   (range: 0<=val<=30) */
 493     int16 *fraction    /* (o) : Fractional part of Log2. (range: 0<=val<1)  */
 494 )
 495 {
 496     int16 i, a, tmp;
 497     int32 L_y;
 498
 499     if (L_x <= (int32) 0)
 500     {
 501         *exponent = 0;
 502         *fraction = 0;;
 503         return;
 504     }
 505
 506     *exponent = 30 - exp;
 507
 508     L_x >>= 9;
 509     i = extract_h(L_x);                 /* Extract b25-b31 */
 510     L_x >>= 1;
 511     a = (int16)(L_x);                 /* Extract b10-b24 of fraction */
 512     a &= 0x7fff;
 513
 514     i -= 32;
 515
 516     L_y = L_deposit_h(Log2_norm_table[i]);             /* table[i] << 16        */
 517     tmp = Log2_norm_table[i] - Log2_norm_table[i + 1]; /* table[i] - table[i+1] */
 518     L_y = msu_16by16_from_int32(L_y, tmp, a);           /* L_y -= tmp*a*2        */
 519
 520     *fraction = extract_h(L_y);
 521
 522     return;
 523 }
 524
 525
 526
 527 /*----------------------------------------------------------------------------
 528  *
 529  *   FUNCTION:   amrwb_log_2()
 530  *
 531  *   PURPOSE:   Computes log2(L_x),  where   L_x is positive.
 532  *              If L_x is negative or zero, the result is 0.
 533  *
 534  *   DESCRIPTION:
 535  *        normalizes L_x and then calls Lg2_normalized().
 536  *
 537  ----------------------------------------------------------------------------*/
 538 void amrwb_log_2(
 539     int32 L_x,         /* (i) : input value                                 */
 540     int16 *exponent,   /* (o) : Integer part of Log2.   (range: 0<=val<=30) */
 541     int16 *fraction    /* (o) : Fractional part of Log2. (range: 0<=val<1) */
 542 )
 543 {
 544     int16 exp;
 545
 546     exp = normalize_amr_wb(L_x);
 547     Lg2_normalized(shl_int32(L_x, exp), exp, exponent, fraction);
 548 }
 549
 550
 551 /*****************************************************************************
 552  *
 553  *  These operations are not standard double precision operations.           *
 554  *  They are used where single precision is not enough but the full 32 bits  *
 555  *  precision is not necessary. For example, the function Div_32() has a     *
 556  *  24 bits precision which is enough for our purposes.                      *
 557  *                                                                           *
 558  *  The double precision numbers use a special representation:               *
 559  *                                                                           *
 560  *     L_32 = hi<<16 + lo<<1                                                 *
 561  *                                                                           *
 562  *  L_32 is a 32 bit integer.                                                *
 563  *  hi and lo are 16 bit signed integers.                                    *
 564  *  As the low part also contains the sign, this allows fast multiplication. *
 565  *                                                                           *
 566  *      0x8000 0000 <= L_32 <= 0x7fff fffe.                                  *
 567  *                                                                           *
 568  *  We will use DPF (Double Precision Format )in this file to specify        *
 569  *  this special format.                                                     *
 570  *****************************************************************************
 571 */
 572
 573
 574 /*----------------------------------------------------------------------------
 575  *
 576  *  Function int32_to_dpf()
 577  *
 578  *  Extract from a 32 bit integer two 16 bit DPF.
 579  *
 580  *  Arguments:
 581  *
 582  *   L_32      : 32 bit integer.
 583  *               0x8000 0000 <= L_32 <= 0x7fff ffff.
 584  *   hi        : b16 to b31 of L_32
 585  *   lo        : (L_32 - hi<<16)>>1
 586  *
 587  ----------------------------------------------------------------------------*/
 588
 589 void int32_to_dpf(int32 L_32, int16 *hi, int16 *lo)
 590 {
 591     *hi = (int16)(L_32 >> 16);
 592     *lo = (int16)((L_32 - (*hi << 16)) >> 1);
 593     return;
 594 }
 595
 596
 597 /*----------------------------------------------------------------------------
 598  * Function mpy_dpf_32()
 599  *
 600  *   Multiply two 32 bit integers (DPF). The result is divided by 2**31
 601  *
 602  *   L_32 = (hi1*hi2)<<1 + ( (hi1*lo2)>>15 + (lo1*hi2)>>15 )<<1
 603  *
 604  *   This operation can also be viewed as the multiplication of two Q31
 605  *   number and the result is also in Q31.
 606  *
 607  * Arguments:
 608  *
 609  *  hi1         hi part of first number
 610  *  lo1         lo part of first number
 611  *  hi2         hi part of second number
 612  *  lo2         lo part of second number
 613  *
 614  ----------------------------------------------------------------------------*/
 615
 616 int32 mpy_dpf_32(int16 hi1, int16 lo1, int16 hi2, int16 lo2)
 617 {
 618     int32 L_32;
 619
 620     L_32 = mul_16by16_to_int32(hi1, hi2);
 621     L_32 = mac_16by16_to_int32(L_32, mult_int16(hi1, lo2), 1);
 622     L_32 = mac_16by16_to_int32(L_32, mult_int16(lo1, hi2), 1);
 623
 624     return (L_32);
 625 }
 626
 627