--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libgsmefr/d_gains.c	Fri Nov 25 16:18:21 2022 +0000
@@ -0,0 +1,490 @@
+/*************************************************************************
+ *
+ *  FILE NAME:   D_GAINS.C
+ *
+ *  FUNCTIONS DEFINED IN THIS FILE:
+ *
+ *        d_gain_pitch(), d_gain_code() and gmed5()
+ *
+ * MA prediction is performed on the innovation energy
+ * ( in dB/(20*log10(2)) ) with mean removed.
+ *************************************************************************/
+
+#include "typedef.h"
+#include "basic_op.h"
+#include "oper_32b.h"
+#include "count.h"
+#include "sig_proc.h"
+
+#include "gains_tb.h"
+
+#include "cnst.h"
+#include "dtx.h"
+
+extern Word16 gain_code_old_rx[4 * DTX_HANGOVER];
+
+/* Static variables for bad frame handling */
+
+/* Variables used by d_gain_pitch: */
+Word16 pbuf[5], past_gain_pit, prev_gp;
+
+/* Variables used by d_gain_code: */
+Word16 gbuf[5], past_gain_code, prev_gc;
+
+/* Static variables for CNI (used by d_gain_code) */
+
+Word16 gcode0_CN, gain_code_old_CN, gain_code_new_CN, gain_code_muting_CN;
+
+/* Memories of gain dequantization: */
+
+/* past quantized energies.      */
+/* initialized to -14.0/constant, constant = 20*Log10(2) */
+
+Word16 past_qua_en[4];
+
+/* MA prediction coeff   */
+Word16 pred[4];
+
+/*************************************************************************
+ *
+ *  FUNCTION:   gmed5
+ *
+ *  PURPOSE:    calculates 5-point median.
+ *
+ *  DESCRIPTION:
+ *             
+ *************************************************************************/
+
+Word16 gmed5 (        /* out      : index of the median value (0...4) */
+    Word16 ind[]      /* in       : Past gain values                  */
+)
+{
+    Word16 i, j, ix = 0, tmp[5];
+    Word16 max, tmp2[5];
+
+    for (i = 0; i < 5; i++)
+    {
+        tmp2[i] = ind[i];                                      move16 (); 
+    }
+
+    for (i = 0; i < 5; i++)
+    {
+        max = -8192;                                           move16 (); 
+        for (j = 0; j < 5; j++)
+        {
+            test (); 
+            if (sub (tmp2[j], max) >= 0)
+            {
+                max = tmp2[j];                                 move16 (); 
+                ix = j;                                        move16 (); 
+            }
+        }
+        tmp2[ix] = -16384;                                     move16 (); 
+        tmp[i] = ix;                                           move16 (); 
+    }
+
+    return (ind[tmp[2]]);
+}
+
+/*************************************************************************
+ *
+ *  FUNCTION:   d_gain_pitch
+ *
+ *  PURPOSE:  decodes the pitch gain using the received index.
+ *
+ *  DESCRIPTION:
+ *       In case of no frame erasure, the gain is obtained from the
+ *       quantization table at the given index; otherwise, a downscaled
+ *       past gain is used.
+ *
+ *************************************************************************/
+
+Word16 d_gain_pitch ( /* out      : quantized pitch gain           */
+    Word16 index,     /* in       : index of quantization          */
+    Word16 bfi,       /* in       : bad frame indicator (good = 0) */
+    Word16 state,
+    Word16 prev_bf,
+    Word16 rxdtx_ctrl
+)
+{
+    static const Word16 pdown[7] =
+    {
+        32767, 32112, 32112, 26214,
+        9830, 6553, 6553
+    };
+
+    Word16 gain, tmp, i;
+
+    test (); 
+    if (bfi == 0)
+    {
+        test (); logic16 (); 
+        if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
+        {
+            gain = shr (qua_gain_pitch[index], 2);             move16 (); 
+
+            test (); 
+            if (prev_bf != 0)
+            {
+                test (); 
+                if (sub (gain, prev_gp) > 0)
+                {
+                    gain = prev_gp;
+                }
+            }
+        }
+        else
+        {
+            gain = 0;                                          move16 (); 
+        }
+        prev_gp = gain;                                        move16 (); 
+    }
+    else
+    {
+        test (); logic16 (); 
+        if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
+        {
+            tmp = gmed5 (pbuf);                                move16 (); 
+
+            test (); 
+            if (sub (tmp, past_gain_pit) < 0)
+            {
+                past_gain_pit = tmp;                           move16 (); 
+            }
+            gain = mult (pdown[state], past_gain_pit);
+        }
+        else
+        {
+            gain = 0;                                          move16 (); 
+        }
+    }
+
+    past_gain_pit = gain;                                      move16 (); 
+
+    test (); 
+    if (sub (past_gain_pit, 4096) > 0)  /* if (past_gain_pit > 1.0) */
+    {
+        past_gain_pit = 4096;                                  move16 (); 
+    }
+    for (i = 1; i < 5; i++)
+    {
+        pbuf[i - 1] = pbuf[i];                                 move16 (); 
+    }
+
+    pbuf[4] = past_gain_pit;                                   move16 (); 
+
+    return gain;
+}
+
+/*************************************************************************
+ *
+ *  FUNCTION:  d_gain_code
+ *
+ *  PURPOSE:  decode the fixed codebook gain using the received index.
+ *
+ *  DESCRIPTION:
+ *      The received index gives the gain correction factor gamma.
+ *      The quantized gain is given by   g_q = g0 * gamma
+ *      where g0 is the predicted gain.
+ *      To find g0, 4th order MA prediction is applied to the mean-removed
+ *      innovation energy in dB.
+ *      In case of frame erasure, downscaled past gain is used.
+ *
+ *************************************************************************/
+
+/* average innovation energy.                             */
+/* MEAN_ENER = 36.0/constant, constant = 20*Log10(2)      */
+#define MEAN_ENER  783741L      /* 36/(20*log10(2))       */
+
+void d_gain_code (
+    Word16 index,      /* input : received quantization index */
+    Word16 code[],     /* input : innovation codevector       */
+    Word16 lcode,      /* input : codevector length           */
+    Word16 *gain_code, /* output: decoded innovation gain     */
+    Word16 bfi,        /* input : bad frame indicator         */
+    Word16 state,
+    Word16 prev_bf,
+    Word16 rxdtx_ctrl,
+    Word16 i_subfr,
+    Word16 rx_dtx_state
+)
+{
+    static const Word16 cdown[7] =
+    {
+        32767, 32112, 32112, 32112,
+        32112, 32112, 22937
+    };
+
+    Word16 i, tmp;
+    Word16 gcode0, exp, frac, av_pred_en;
+    Word32 ener, ener_code;
+
+    test (); test (); logic16 (); 
+    if (((rxdtx_ctrl & RX_UPD_SID_QUANT_MEM) != 0) && (i_subfr == 0))
+    {
+        gcode0_CN = update_gcode0_CN (gain_code_old_rx);       move16 (); 
+        gcode0_CN = shl (gcode0_CN, 4);
+    }
+
+    /* Handle cases of comfort noise fixed codebook gain decoding in
+       which past valid SID frames are repeated */
+
+    test (); test (); test (); logic16 (); logic16 (); logic16 (); 
+    if (((rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
+        || ((rxdtx_ctrl & RX_INVALID_SID_FRAME) != 0)
+        || ((rxdtx_ctrl & RX_LOST_SID_FRAME) != 0))
+    {
+
+        test (); logic16 (); 
+        if ((rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
+        {
+            /* DTX active: no transmission. Interpolate gain values
+            in memory */
+            test (); 
+            if (i_subfr == 0)
+            {
+                *gain_code = interpolate_CN_param (gain_code_old_CN,
+                                            gain_code_new_CN, rx_dtx_state);
+                                                               move16 (); 
+            }
+            else
+            {
+                *gain_code = prev_gc;                          move16 (); 
+            }
+        }
+        else
+        {                       /* Invalid or lost SID frame:
+            use gain values from last good SID frame */
+            gain_code_old_CN = gain_code_new_CN;               move16 (); 
+            *gain_code = gain_code_new_CN;                     move16 (); 
+
+            /* reset table of past quantized energies */
+            for (i = 0; i < 4; i++)
+            {
+                past_qua_en[i] = -2381;                        move16 (); 
+            }
+        }
+
+        test (); logic16 (); 
+        if ((rxdtx_ctrl & RX_DTX_MUTING) != 0)
+        {
+            /* attenuate the gain value by 0.75 dB in each subframe */
+            /* (total of 3 dB per frame) */
+            gain_code_muting_CN = mult (gain_code_muting_CN, 30057);
+            *gain_code = gain_code_muting_CN;                  move16 (); 
+        }
+        else
+        {
+            /* Prepare for DTX muting by storing last good gain value */
+            gain_code_muting_CN = gain_code_new_CN;            move16 (); 
+        }
+
+        past_gain_code = *gain_code;                           move16 (); 
+
+        for (i = 1; i < 5; i++)
+        {
+            gbuf[i - 1] = gbuf[i];                             move16 (); 
+        }
+
+        gbuf[4] = past_gain_code;                              move16 (); 
+        prev_gc = past_gain_code;                              move16 (); 
+
+        return;
+    }
+
+    /*----------------- Test erasure ---------------*/
+
+    test (); 
+    if (bfi != 0)
+    {
+        tmp = gmed5 (gbuf);                                    move16 (); 
+        test (); 
+        if (sub (tmp, past_gain_code) < 0)
+        {
+            past_gain_code = tmp;                              move16 (); 
+        }
+        past_gain_code = mult (past_gain_code, cdown[state]);
+        *gain_code = past_gain_code;                           move16 (); 
+
+        av_pred_en = 0;                                        move16 (); 
+        for (i = 0; i < 4; i++)
+        {
+            av_pred_en = add (av_pred_en, past_qua_en[i]);
+        }
+
+        /* av_pred_en = 0.25*av_pred_en - 4/(20Log10(2)) */
+        av_pred_en = mult (av_pred_en, 8192);   /*  *= 0.25  */
+
+        /* if (av_pred_en < -14/(20Log10(2))) av_pred_en = .. */
+        test (); 
+        if (sub (av_pred_en, -2381) < 0)
+        {
+            av_pred_en = -2381;                                move16 (); 
+        }
+        for (i = 3; i > 0; i--)
+        {
+            past_qua_en[i] = past_qua_en[i - 1];               move16 (); 
+        }
+        past_qua_en[0] = av_pred_en;                           move16 (); 
+        for (i = 1; i < 5; i++)
+        {
+            gbuf[i - 1] = gbuf[i];                             move16 (); 
+        }
+        gbuf[4] = past_gain_code;                              move16 (); 
+
+        /* Use the most recent comfort noise fixed codebook gain value
+           for updating the fixed codebook gain history */
+        test ();
+        if (gain_code_new_CN == 0)
+        {
+            tmp = prev_gc;                                     move16 ();
+        }
+        else
+        {
+            tmp = gain_code_new_CN;
+        }
+
+        update_gain_code_history_rx (tmp, gain_code_old_rx);
+
+        test ();
+        if (sub (i_subfr, (3 * L_SUBFR)) == 0)
+        {
+            gain_code_old_CN = *gain_code;                     move16 (); 
+        }
+        return;
+    }
+
+    test (); logic16 (); 
+    if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
+    {
+
+        /*-------------- Decode codebook gain ---------------*/
+
+        /*-------------------------------------------------------------------*
+         *  energy of code:                                                   *
+         *  ~~~~~~~~~~~~~~~                                                   *
+         *  ener_code = 10 * Log10(energy/lcode) / constant                   *
+         *            = 1/2 * Log2(energy/lcode)                              *
+         *                                           constant = 20*Log10(2)   *
+         *-------------------------------------------------------------------*/
+
+        /* ener_code = log10(ener_code/lcode) / (20*log10(2)) */
+        ener_code = 0;                                         move32 (); 
+        for (i = 0; i < lcode; i++)
+        {
+            ener_code = L_mac (ener_code, code[i], code[i]);
+        }
+        /* ener_code = ener_code / lcode */
+        ener_code = L_mult (round (ener_code), 26214);
+
+        /* ener_code = 1/2 * Log2(ener_code) */
+        Log2 (ener_code, &exp, &frac);
+        ener_code = L_Comp (sub (exp, 30), frac);
+
+        /* predicted energy */
+
+        ener = MEAN_ENER;                                      move32 (); 
+        for (i = 0; i < 4; i++)
+        {
+            ener = L_mac (ener, past_qua_en[i], pred[i]);
+        }
+
+        /*-------------------------------------------------------------------*
+         *  predicted codebook gain                                           *
+         *  ~~~~~~~~~~~~~~~~~~~~~~~                                           *
+         *  gcode0     = Pow10( (ener*constant - ener_code*constant) / 20 )   *
+         *             = Pow2(ener-ener_code)                                 *
+         *                                           constant = 20*Log10(2)   *
+         *-------------------------------------------------------------------*/
+
+        ener = L_shr (L_sub (ener, ener_code), 1);
+        L_Extract (ener, &exp, &frac);
+
+        gcode0 = extract_l (Pow2 (exp, frac));  /* predicted gain */
+
+        gcode0 = shl (gcode0, 4);
+
+        *gain_code = mult (qua_gain_code[index], gcode0);      move16 (); 
+
+        test (); 
+        if (prev_bf != 0)
+        {
+            test (); 
+            if (sub (*gain_code, prev_gc) > 0)
+            {
+                *gain_code = prev_gc;     move16 (); 
+            }
+        }
+        /*-------------------------------------------------------------------*
+         *  update table of past quantized energies                           *
+         *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                           *
+         *  past_qua_en      = 20 * Log10(qua_gain_code) / constant           *
+         *                   = Log2(qua_gain_code)                            *
+         *                                           constant = 20*Log10(2)   *
+         *-------------------------------------------------------------------*/
+
+        for (i = 3; i > 0; i--)
+        {
+            past_qua_en[i] = past_qua_en[i - 1];               move16 (); 
+        }
+        Log2 (L_deposit_l (qua_gain_code[index]), &exp, &frac);
+
+        past_qua_en[0] = shr (frac, 5);                        move16 (); 
+        past_qua_en[0] = add (past_qua_en[0], shl (sub (exp, 11), 10));
+        move16 (); 
+
+        update_gain_code_history_rx (*gain_code, gain_code_old_rx);
+
+        if (sub (i_subfr, (3 * L_SUBFR)) == 0)
+        {
+            gain_code_old_CN = *gain_code;                     move16 (); 
+        }
+    }
+    else
+    {
+        test (); test (); logic16 (); 
+        if (((rxdtx_ctrl & RX_FIRST_SID_UPDATE) != 0) && (i_subfr == 0))
+        {
+            gain_code_new_CN = mult (gcode0_CN, qua_gain_code[index]);
+
+            /*---------------------------------------------------------------*
+             *  reset table of past quantized energies                        *
+             *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                        *
+             *---------------------------------------------------------------*/
+
+            for (i = 0; i < 4; i++)
+            {
+                past_qua_en[i] = -2381;                        move16 (); 
+            }
+        }
+        test (); test (); logic16 (); 
+        if (((rxdtx_ctrl & RX_CONT_SID_UPDATE) != 0) && (i_subfr == 0))
+        {
+            gain_code_old_CN = gain_code_new_CN;               move16 (); 
+            gain_code_new_CN = mult (gcode0_CN, qua_gain_code[index]);
+                                                               move16 (); 
+        }
+        test (); 
+        if (i_subfr == 0)
+        {
+            *gain_code = interpolate_CN_param (gain_code_old_CN,
+                                               gain_code_new_CN,
+                                               rx_dtx_state);  move16 (); 
+        }
+        else
+        {
+            *gain_code = prev_gc;                              move16 (); 
+        }
+    }
+
+    past_gain_code = *gain_code;                               move16 (); 
+
+    for (i = 1; i < 5; i++)
+    {
+        gbuf[i - 1] = gbuf[i];                                 move16 (); 
+    }
+    gbuf[4] = past_gain_code;                                  move16 (); 
+    prev_gc = past_gain_code;                                  move16 (); 
+
+    return;
+}