pitch_fr.c

来自「君正早期ucos系统(只有早期的才不没有打包成库),MPLAYER,文件系统,图」· C语言 代码 · 共 701 行 · 第 1/2 页

/*
********************************************************************************
*
*      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             : pitch_fr.c
*      Purpose          : Find the pitch period with 1/3 or 1/6 subsample
*                       : resolution (closed loop).
*
********************************************************************************
*/
/*
********************************************************************************
*                         MODULE INCLUDE FILE AND VERSION ID
********************************************************************************
*/
#include "pitch_fr.h"
const char pitch_fr_id[] = "@(#)$Id $" pitch_fr_h;
/*
********************************************************************************
*                         INCLUDE FILES
********************************************************************************
*/
#include <uclib.h>
#include <uclib.h>
#include "typedef.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "count.h"
#include "cnst.h"
#include "enc_lag3.h"
#include "enc_lag6.h"
#include "inter_36.h"
#include "inv_sqrt.h"
#include "convolve.h"
//add for jzmedia accerative instruction sets
#ifdef JZ4740_MXU_OPT
#include "jzmedia.h"
#endif
/*
********************************************************************************
*                         LOCAL VARIABLES AND TABLES
********************************************************************************
*/

/*
 * mode dependent parameters used in Pitch_fr()
 * Note: order of MRxx in 'enum Mode' is important!
 */
static const struct {
    Word16 max_frac_lag;     /* lag up to which fractional lags are used    */
    Word16 flag3;            /* enable 1/3 instead of 1/6 fract. resolution */
    Word16 first_frac;       /* first fractional to check                   */
    Word16 last_frac;        /* last fractional to check                    */
    Word16 delta_int_low;    /* integer lag below TO to start search from   */
    Word16 delta_int_range;  /* integer range around T0                     */
    Word16 delta_frc_low;    /* fractional below T0                         */
    Word16 delta_frc_range;  /* fractional range around T0                  */
    Word16 pit_min;          /* minimum pitch                               */
} mode_dep_parm[N_MODES] = {
    /* MR475 */  { 84,  1, -2,  2,  5, 10,  5,  9, PIT_MIN },
    /* MR515 */  { 84,  1, -2,  2,  5, 10,  5,  9, PIT_MIN },                 
    /* MR59  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR67  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR74  */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },
    /* MR795 */  { 84,  1, -2,  2,  3,  6, 10, 19, PIT_MIN },
    /* MR102 */  { 84,  1, -2,  2,  3,  6,  5,  9, PIT_MIN },                 
    /* MR122 */  { 94,  0, -3,  3,  3,  6,  5,  9, PIT_MIN_MR122 }
};

static int RangeT=0, normT = 0,searchFracT=0,Enc_lag6T=0 ;
/*
********************************************************************************
*                         LOCAL PROGRAM CODE
********************************************************************************
*/
/*************************************************************************
 *
 *  FUNCTION:   Norm_Corr()
 *
 *  PURPOSE: Find the normalized correlation between the target vector
 *           and the filtered past excitation.
 *
 *  DESCRIPTION:
 *     The normalized correlation is given by the correlation between the
 *     target and filtered past excitation divided by the square root of
 *     the energy of filtered excitation.
 *                   corr[k] = <x[], y_k[]>/sqrt(y_k[],y_k[])
 *     where x[] is the target vector and y_k[] is the filtered past
 *     excitation at delay k.
 *
 *************************************************************************/
static void Norm_Corr (Word16 exc[], Word16 xn[], Word16 h[], Word16 L_subfr,
                       Word16 t_min, Word16 t_max, Word16 corr_norm[])
{
    Word16 i, j, k;
    Word16 corr_h, corr_l, norm_h, norm_l;
    Word32 s;

    /* Usally dynamic allocation of (L_subfr) */
    Word16 excf[L_SUBFR];
    Word16 scaling, h_fac, *s_excf, scaled_excf[L_SUBFR];

    k = -t_min;//                                //move16 ();

    /* compute the filtered excitation for the first delay t_min */

    Convolve (&exc[k], h, excf, L_subfr);

    /* scale "excf[]" to avoid overflow */
    for (j = 0; j < L_subfr; j++) {
        scaled_excf[j] = shr (excf[j], 2); //    //move16 ();
    }

    /* Compute 1/sqrt(energy of excf[]) */
//add for jzmedia accerative instruction sets
#ifdef JZ4740_MXU_OPT
   Word16 *ptr1,*ptr2;

   ptr1 = excf - 2;
   Q16ADD_AA_WW(xr3,xr0,xr0,xr4);  //let xr3, xr4 to zero , like s = 0;
   for(j = 0; j < L_subfr / 2; j++){
       S32LDI(xr1, ptr1, 0x04);    //load excf[i];
       D16MAC_AA_WW(xr3, xr1, xr1, xr4); // excf[i] and excf[i+1] multiply and add.
   }
   D32ADD_AA(xr5, xr3, xr4, xr6);  // excf[i] and excf[i+1] sum result is added.
   D32SLL(xr3, xr5, xr0, xr0, 1);  // shift result of excf[i] square.
   s = S32M2I(xr3);                //let s = mul and add.
#else
    s = 0;                                  //   move32 ();
    for (j = 0; j < L_subfr; j++) {
        s = L_mac (s, excf[j], excf[j]);
    }
#endif
//    test ();
    if (L_sub (s, 67108864L) <= 0) {            /* if (s <= 2^26) */
        s_excf = excf;                         //move16 ();
        h_fac = 15 - 12;                       //move16 ();
        scaling = 0;                           //move16 ();
    }
    else {
        /* "excf[]" is divided by 2 */
        s_excf = scaled_excf;                  //move16 ();
        h_fac = 15 - 12 - 2;                   //move16 ();
        scaling = 2;                           //move16 ();
    }

    /* loop for every possible period */

    for (i = t_min; i <= t_max; i++) {
        /* Compute 1/sqrt(energy of excf[]) */
#ifdef JZ4740_MXU_OPT

   ptr1 = s_excf - 2;
   Q16ADD_AA_WW(xr3,xr0,xr0,xr4);  //let xr3, xr4 to zero , like s = 0;
   for(j = 0; j < L_subfr / 2; j++){
       S32LDI(xr1, ptr1, 0x04);    //load excf[i];
       D16MAC_AA_WW(xr3, xr1, xr1, xr4); // excf[i] and excf[i+1] multiply and add.
   }
   D32ADD_AA(xr5, xr3, xr4, xr6);  // excf[i] and excf[i+1] sum result is added.
   D32SLL(xr3, xr5, xr0, xr0, 1);  // shift result of excf[i] square.
   s = S32M2I(xr3);                //let s = mul and add.
#else
        
        s = 0;                                 //move32 ();
        for (j = 0; j < L_subfr; j++) {
            s = L_mac (s, s_excf[j], s_excf[j]);
        }
#endif        
        s = Inv_sqrt (s);
        L_Extract (s, &norm_h, &norm_l);
        
        /* Compute correlation between xn[] and excf[] */
#ifdef JZ4740_MXU_OPT

   ptr1 = s_excf - 2;
   ptr2 = xn - 2;
   Q16ADD_AA_WW(xr3,xr0,xr0,xr4);  //let xr3, xr4 to zero , like s = 0;
   for(j = 0; j < L_subfr / 2; j++){
       S32LDI(xr1, ptr1, 0x04);    //load excf[i];
       S32LDI(xr2, ptr2, 0x04);    //load excf[i];
       D16MAC_AA_WW(xr3, xr1, xr2, xr4); // excf[i] and excf[i+1] multiply and add.
   }
   D32ADD_AA(xr5, xr3, xr4, xr6);  // excf[i] and excf[i+1] sum result is added.
   D32SLL(xr3, xr5, xr0, xr0, 1);  // shift result of excf[i] square.
   s = S32M2I(xr3);                //let s = mul and add.
#else
          
        s = 0;                                  //move32 ();
        for (j = 0; j < L_subfr; j++) {
            s = L_mac (s, xn[j], s_excf[j]);
        }
#endif        
        L_Extract (s, &corr_h, &corr_l);

        /* Normalize correlation = correlation * (1/sqrt(energy)) */
        
        s = Mpy_32 (corr_h, corr_l, norm_h, norm_l);
        
        corr_norm[i] = extract_h (L_shl (s, 16));
//        move16 ();

            /* modify the filtered excitation excf[] for the next iteration */
        
 //       test ();

#ifdef JZ4740_MXU_OPT
        if (sub (i, t_max) != 0) {
            k--;
            ptr1 = h + L_subfr ;
            ptr2 = s_excf + L_subfr ;
            s = exc[k];
            S32I2M(xr10, s);
            S32LDI(xr11, ptr2, -0x04);
            h_fac += 1;
        for(j = L_subfr / 2 - 1; j > 0; j--){
            S32LDI(xr2, ptr1, -0x04);
            D16MUL_LW(xr3, xr10, xr2, xr4);
            if(h_fac == 2)
                D32SLL(xr5,xr3,xr4,xr6,2);
            else
                D32SLL(xr5,xr3,xr4,xr6,4);
            S32SFL(xr7, xr5,xr6,xr8,3);
            S32LDD(xr4, ptr2, -0x04);
            S32ALN(xr5, xr11, xr4, 2);
            Q16ADD_AA_WW(xr11, xr4,xr0,xr0);
            Q16ADD_AA_WW(xr2, xr7, xr5, xr0);
            S32STD(xr2, ptr2, 0x00);
            ptr2 -= 2;
        }
            s = L_mult(exc[k], h[1]);
            s = L_shl(s, h_fac-1);
            s_excf[1] = add(extract_h(s), s_excf[0]);
            s_excf[0] = shr (exc[k], scaling);  //move16 ();
        }
#else
        if (sub (i, t_max) != 0) {
            k--;
            for (j = L_subfr - 1; j > 0; j--) {
                s = L_mult (exc[k], h[j]);
                s = L_shl (s, h_fac);
                s_excf[j] = add (extract_h (s), s_excf[j - 1]); //move16 ();
            }
            s_excf[0] = shr (exc[k], scaling);  //move16 ();
        }
#endif        
    }
    return;
}

/*************************************************************************
 *
 *  FUNCTION:   searchFrac()
 *
 *  PURPOSE: Find fractional pitch
 *
 *  DESCRIPTION:
 *     The function interpolates the normalized correlation at the
 *     fractional positions around lag T0. The position at which the
 *     interpolation function reaches its maximum is the fractional pitch.
 *     Starting point of the search is frac, end point is last_frac.
 *     frac is overwritten with the fractional pitch.
 *
 *************************************************************************/
static void searchFrac (
    Word16 *lag,       /* i/o : integer pitch           */
    Word16 *frac,      /* i/o : start point of search -
                                fractional pitch        */
    Word16 last_frac,  /* i   : endpoint of search      */
    Word16 corr[],     /* i   : normalized correlation  */
    Word16 flag3       /* i   : subsample resolution
                                (3: =1 / 6: =0)         */
)
{
    Word16 i;
    Word16 max;
    Word16 corr_int;

    /* Test the fractions around T0 and choose the one which maximizes   */
    /* the interpolated normalized correlation.                          */

    max = Interpol_3or6 (&corr[*lag], *frac, flag3); //move16 (); /* function result */

    for (i = add (*frac, 1); i <= last_frac; i++) {
        corr_int = Interpol_3or6 (&corr[*lag], i, flag3);
//        move16 ();
//        test ();
        if (sub (corr_int, max) > 0) {
            max = corr_int;                       //move16 ();
            *frac = i;                            //move16 ();
        }
    }

//    test();
    if (flag3 == 0) {
        /* Limit the fraction value in the interval [-2,-1,0,1,2,3] */

//        test ();
        if (sub (*frac, -3) == 0) {
            *frac = 3;                            //move16 ();
            *lag = sub (*lag, 1);
        }
    }
    else {
        /* limit the fraction value between -1 and 1 */

//        test ();
        if (sub (*frac, -2) == 0) {
            *frac = 1;                            //move16 ();
            *lag = sub (*lag, 1);
        }
//        test ();
        if (sub (*frac, 2) == 0) {
            *frac = -1;                           //move16 ();
            *lag = add (*lag, 1);
        }
    }
}

/*************************************************************************
 *
 *  FUNCTION:   getRange()
 *
 *  PURPOSE: Sets range around open-loop pitch or integer pitch of last subframe
 *
 *  DESCRIPTION:
 *     Takes integer pitch T0 and calculates a range around it with
 *       t0_min = T0-delta_low  and t0_max = (T0-delta_low) + delta_range
 *     t0_min and t0_max are bounded by pitmin and pitmax
 *
 *************************************************************************/
static void getRange (
    Word16 T0,           /* i : integer pitch          */
    Word16 delta_low,    /* i : search start offset    */
    Word16 delta_range,  /* i : search range           */
    Word16 pitmin,       /* i : minimum pitch          */
    Word16 pitmax,       /* i : maximum pitch          */
    Word16 *t0_min,      /* o : search range minimum   */
    Word16 *t0_max)      /* o : search range maximum   */
{
    *t0_min = sub(T0, delta_low);
    test ();
    if (sub(*t0_min, pitmin) < 0) {
        *t0_min = pitmin;                                  //move16();
    }
    *t0_max = add(*t0_min, delta_range);
    test ();