--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/libtwamr/ph_disp.c	Mon May 06 18:20:22 2024 +0000
@@ -0,0 +1,311 @@
+/*
+********************************************************************************
+*
+*      GSM AMR-NB speech codec   R98   Version 7.6.0   December 12, 2001
+*                                R99   Version 3.3.0                
+*                                REL-4 Version 4.1.0                
+*
+********************************************************************************
+*
+*      File             : ph_disp.c
+*      Purpose          : Perform adaptive phase dispersion of the excitation
+*                         signal.
+*
+********************************************************************************
+*/
+/*
+********************************************************************************
+*                         MODULE INCLUDE FILE AND VERSION ID
+********************************************************************************
+*/
+#include "namespace.h"
+#include "ph_disp.h"
+
+/*
+********************************************************************************
+*                         INCLUDE FILES
+********************************************************************************
+*/
+#include "typedef.h"
+#include "basic_op.h"
+#include "no_count.h"
+#include "cnst.h"
+#include "memops.h"
+
+/*
+********************************************************************************
+*                         LOCAL VARIABLES AND TABLES
+********************************************************************************
+*/
+
+#include "ph_disp.tab"
+
+/*
+********************************************************************************
+*                         PUBLIC PROGRAM CODE
+********************************************************************************
+*/
+
+/*************************************************************************
+*
+*  Function:   ph_disp_reset
+*
+**************************************************************************
+*/
+void ph_disp_reset (ph_dispState *state)
+{
+   Word16 i;
+
+   for (i=0; i<PHDGAINMEMSIZE; i++)
+   {
+       state->gainMem[i] = 0;
+   }
+   state->prevState = 0;
+   state->prevCbGain = 0;
+   state->lockFull = 0;
+   state->onset = 0;          /* assume no onset in start */ 
+}
+
+/*************************************************************************
+*
+*  Function:   ph_disp_lock
+*
+**************************************************************************
+*/
+void ph_disp_lock (ph_dispState *state)
+{
+  state->lockFull = 1;
+  return;
+}
+
+/*************************************************************************
+*
+*  Function:   ph_disp_release
+*
+**************************************************************************
+*/
+void ph_disp_release (ph_dispState *state)
+{
+  state->lockFull = 0;
+  return;
+}
+
+
+/*************************************************************************
+*
+*  Function:   ph_disp
+*
+*              Adaptive phase dispersion; forming of total excitation
+*              (for synthesis part of decoder)
+*
+**************************************************************************
+*/
+void ph_disp (
+      ph_dispState *state, /* i/o     : State struct                     */
+      enum Mode mode,      /* i       : codec mode                       */
+      Word16 x[],          /* i/o Q0  : in:  LTP excitation signal       */
+                           /*           out: total excitation signal     */
+      Word16 cbGain,       /* i   Q1  : Codebook gain                    */
+      Word16 ltpGain,      /* i   Q14 : LTP gain                         */
+      Word16 inno[],       /* i/o Q13 : Innovation vector (Q12 for 12.2) */
+      Word16 pitch_fac,    /* i   Q14 : pitch factor used to scale the
+                                        LTP excitation (Q13 for 12.2)    */
+      Word16 tmp_shift     /* i   Q0  : shift factor applied to sum of   
+                                        scaled LTP ex & innov. before
+                                        rounding                         */
+)
+{
+   Word16 i, i1;
+   Word16 tmp1;
+   Word32 L_temp;
+   Word16 impNr;           /* indicator for amount of disp./filter used */
+
+   Word16 inno_sav[L_SUBFR];
+   Word16 ps_poss[L_SUBFR];
+   Word16 j, nze, nPulse, ppos;
+   const Word16 *ph_imp;   /* Pointer to phase dispersion filter */
+
+   /* Update LTP gain memory */
+   for (i = PHDGAINMEMSIZE-1; i > 0; i--)
+   {
+       state->gainMem[i] = state->gainMem[i-1];                    move16 ();
+   }
+   state->gainMem[0] = ltpGain;                                    move16 ();
+   
+   /* basic adaption of phase dispersion */
+   test ();
+   if (sub(ltpGain, PHDTHR2LTP) < 0) {    /* if (ltpGain < 0.9) */
+       test ();
+       if (sub(ltpGain, PHDTHR1LTP) > 0)
+       {  /* if (ltpGain > 0.6 */
+          impNr = 1; /* medium dispersion */                      move16 ();
+       }
+       else
+       {
+          impNr = 0; /* maximum dispersion */                     move16 ();
+       }
+   }
+   else
+   {
+      impNr = 2; /* no dispersion */                              move16 ();
+   }
+   
+   /* onset indicator */
+   /* onset = (cbGain  > onFact * cbGainMem[0]) */
+                                                                   move32 ();
+   tmp1 = round(L_shl(L_mult(state->prevCbGain, ONFACTPLUS1), 2));
+   test ();
+   if (sub(cbGain, tmp1) > 0)
+   {
+       state->onset = ONLENGTH;                                    move16 ();
+   }
+   else
+   {
+       test (); 
+       if (state->onset > 0)
+       {
+           state->onset = sub (state->onset, 1);                   move16 ();
+       }
+   }
+   
+   /* if not onset, check ltpGain buffer and use max phase dispersion if
+      half or more of the ltpGain-parameters say so */
+   test ();
+   if (state->onset == 0)
+   {
+       /* Check LTP gain memory and set filter accordingly */
+       i1 = 0;                                                     move16 ();
+       for (i = 0; i < PHDGAINMEMSIZE; i++)
+       {
+           test ();
+           if (sub(state->gainMem[i], PHDTHR1LTP) < 0)
+           {
+               i1 = add (i1, 1);
+           }
+       }
+       test ();
+       if (sub(i1, 2) > 0)
+       {
+           impNr = 0;                                              move16 ();
+       }
+       
+   }
+   /* Restrict decrease in phase dispersion to one step if not onset */
+   test (); test ();
+   if ((sub(impNr, add(state->prevState, 1)) > 0) && (state->onset == 0))
+   {
+       impNr = sub (impNr, 1);
+   }
+   /* if onset, use one step less phase dispersion */
+   test (); test ();
+   if((sub(impNr, 2) < 0) && (state->onset > 0))
+   {
+       impNr = add (impNr, 1);
+   }
+   
+   /* disable for very low levels */
+   test ();
+   if(sub(cbGain, 10) < 0)
+   {
+       impNr = 2;                                                  move16 ();
+   }
+   
+   test ();
+   if(sub(state->lockFull, 1) == 0)
+   {
+       impNr = 0;                                                  move16 ();
+   }
+
+   /* update static memory */
+   state->prevState = impNr;                                       move16 ();
+   state->prevCbGain = cbGain;                                     move16 ();
+  
+   /* do phase dispersion for all modes but 12.2 and 7.4;
+      don't modify the innovation if impNr >=2 (= no phase disp) */
+   test (); test (); test(); test();
+   if (sub(mode, MR122) != 0 && 
+       sub(mode, MR102) != 0 &&
+       sub(mode, MR74) != 0 &&
+       sub(impNr, 2) < 0)
+   {
+       /* track pulse positions, save innovation,
+          and initialize new innovation          */
+       nze = 0;                                                    move16 ();
+       for (i = 0; i < L_SUBFR; i++)
+       {
+           move16 (); test();
+           if (inno[i] != 0)
+           {
+               ps_poss[nze] = i;                                   move16 ();
+               nze = add (nze, 1);
+           }
+           inno_sav[i] = inno[i];                                  move16 ();
+           inno[i] = 0;                                            move16 ();
+       }
+       /* Choose filter corresponding to codec mode and dispersion criterium */
+       test ();
+       if (sub (mode, MR795) == 0)
+       {
+           test ();
+           if (impNr == 0)
+           {
+               ph_imp = ph_imp_low_MR795;                            move16 ();
+           }
+           else
+           {
+               ph_imp = ph_imp_mid_MR795;                            move16 ();
+           }
+       }
+       else
+       {
+           test ();
+           if (impNr == 0)
+           {
+               ph_imp = ph_imp_low;                                  move16 ();
+           }
+           else
+           {
+               ph_imp = ph_imp_mid;                                  move16 ();
+           }
+       }
+       
+       /* Do phase dispersion of innovation */
+       for (nPulse = 0; nPulse < nze; nPulse++)
+       {
+           ppos = ps_poss[nPulse];                                   move16 ();
+           
+           /* circular convolution with impulse response */
+           j = 0;                                                    move16 ();
+           for (i = ppos; i < L_SUBFR; i++)
+           {
+               /* inno[i1] += inno_sav[ppos] * ph_imp[i1-ppos] */
+               tmp1 = mult(inno_sav[ppos], ph_imp[j++]);
+               inno[i] = add(inno[i], tmp1);                         move16 ();
+           }    
+           
+           for (i = 0; i < ppos; i++)
+           {
+               /* inno[i] += inno_sav[ppos] * ph_imp[L_SUBFR-ppos+i] */
+               tmp1 = mult(inno_sav[ppos], ph_imp[j++]);
+               inno[i] = add(inno[i], tmp1);                         move16 ();
+           }
+       }
+   }
+       
+   /* compute total excitation for synthesis part of decoder
+      (using modified innovation if phase dispersion is active) */
+   for (i = 0; i < L_SUBFR; i++)
+   {
+       /* x[i] = gain_pit*x[i] + cbGain*code[i]; */
+       L_temp = L_mult (        x[i],    pitch_fac);
+                                                /* 12.2: Q0 * Q13 */
+                                                /*  7.4: Q0 * Q14 */
+       L_temp = L_mac  (L_temp, inno[i], cbGain);
+                                                /* 12.2: Q12 * Q1 */
+                                                /*  7.4: Q13 * Q1 */
+       L_temp = L_shl (L_temp, tmp_shift);                 /* Q16 */           
+       x[i] = round (L_temp);                                        move16 (); 
+   }
+
+   return;
+}