Home | History | Annotate | Download | only in src
      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 
    628