/***** File:    lsp.c**** Description: Functions that implement line spectral pair **      (LSP) operations.  **** Functions:****  Converting between linear predictive coding (LPC) coefficients**  and LSP frequencies:****      AtoLsp()**      LsptoA()****  Vector quantization (VQ) of LSP frequencies:****      Lsp_Qnt()**      Lsp_Svq()**      Lsp_Inq()****  Interpolation of LSP frequencies:****      Lsp_Int()*//*    ITU-T G.723 Speech Coder   ANSI-C Source Code     Version 5.00    copyright (c) 1995, AudioCodes, DSP Group, France Telecom,    Universite de Sherbrooke.  All rights reserved.*/#include <stdio.h>#include "basop.h"#include "cst_lbc.h"#include "tab_lbc.h"#include "lsp.h"/***** Function:            AtoLsp()**** Description:     Transforms 10 LPC coefficients to the 10**          corresponding LSP frequencies for a subframe.**          This transformation is done once per frame,**          for subframe 3 only.  The transform algorithm**          generates sum and difference polynomials from**          the LPC coefficients.  It then evaluates the**          sum and difference polynomials at uniform**          intervals of pi/256 along the unit circle.**          Intervals where a sign change occurs are**          interpolated to find the zeros of the**          polynomials, which are the LSP frequencies.**** Links to text:   Section 2.5**** Arguments:       ****  Word16 *LspVect     Empty Buffer**  Word16 Lpc[]        Unquantized LPC coefficients (10 words)**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)**** Outputs:****  Word16 LspVect[]    LSP frequencies for the current frame (10 words)**** Return value:        None****/void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp ){    int   i,j,k ;    Word32   Lpq[LpcOrder+2] ;    Word16   Spq[LpcOrder+2] ;    Word16   Exp   ;    Word16   LspCnt ;    Word32   PrevVal,CurrVal   ;    Word32   Acc0,Acc1   ; /*  * Perform a bandwidth expansion on the LPC coefficients.  This  * scales the poles of the LPC synthesis filter by a factor of  * 0.994.  */    for ( i = 0 ; i < LpcOrder ; i ++ )        LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ; /*  * Compute the sum and difference polynomials with the roots at z =  * -1 (sum) or z = +1 (difference) removed.  Let these polynomials  * be P(z) and Q(z) respectively, and let their coefficients be  * {p_i} amd {q_i}.  The coefficients are stored in the array Lpq[]  * as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5.  There is no need  * to store the other coefficients because of symmetry.  */ /*  * Set p_0 = q_0 = 1.  The LPC coefficients are already scaled by  *  1/4.  P(z) and Q(z) are scaled by an additional scaling factor of  *  1/16, for an overall factor of 1/64 = 0x02000000L.  */    Lpq[0] = Lpq[1] = (Word32) 0x02000000L ; /*  * This loop computes the coefficients of P(z) and Q(z).  The long  * division (to remove the real zeros) is done recursively.  */    for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {        /* P(z) */        Acc0 = L_negate( Lpq[2*i+0] ) ;        Acc1 = L_deposit_h( LspVect[i] ) ;        Acc1 = L_shr( Acc1, (Word16) 4 ) ;        Acc0 = L_sub( Acc0, Acc1 ) ;        Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;        Acc1 = L_shr( Acc1, (Word16) 4 ) ;        Acc0 = L_sub( Acc0, Acc1 ) ;        Lpq[2*i+2] = Acc0 ;        /* Q(z) */        Acc0 = Lpq[2*i+1] ;        Acc1 = L_deposit_h( LspVect[i] ) ;        Acc1 = L_shr( Acc1, (Word16) 4 ) ;        Acc0 = L_sub( Acc0, Acc1 ) ;        Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;        Acc1 = L_shr( Acc1, (Word16) 4 ) ;        Acc0 = L_add( Acc0, Acc1 ) ;        Lpq[2*i+3] = Acc0 ;    } /*  * Divide p_5 and q_5 by 2 for proper weighting during polynomial  * evaluation.  */    Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;    Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ; /*  * Normalize the polynomial coefficients and convert to shorts  */    /* Find the maximum */    Acc1 = L_abs( Lpq[0] ) ;    for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {        Acc0 = L_abs( Lpq[i] ) ;        if ( Acc0 > Acc1 )            Acc1 = Acc0 ;    }    /* Compute the normalization factor */    Exp = norm_l( Acc1 ) ;    /* Normalize and convert to shorts */    for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {        Acc0 = L_shl( Lpq[i], Exp ) ;        Spq[i] = round( Acc0 ) ;    } /*  * Initialize the search loop  */ /*  * The variable k is a flag that indicates which polynomial (sum or  * difference) the algorithm is currently evaluating.  Start with  * the sum.  */    k = 0 ;    /* Evaluate the sum polynomial at frequency zero */    PrevVal = (Word32) 0 ;    for ( j = 0 ; j <= LpcOrder/2 ; j ++ )        PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ; /*  * Search loop.  Evaluate P(z) and Q(z) at uniform intervals of  * pi/256 along the unit circle.  Check for zero crossings.  The  * zeros of P(w) and Q(w) alternate, so only one of them need by  * evaluated at any given step.  */    LspCnt = (Word16) 0 ;    for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {        /* Evaluate the selected polynomial */        CurrVal = (Word32) 0 ;        for ( j = 0 ; j <= LpcOrder/2 ; j ++ )            CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],                                    CosineTable[i*j%CosineTableSize] ) ;        /* Check for a sign change, indicating a zero crossing */        if ( (CurrVal ^ PrevVal) < (Word32) 0 ) { /*  * Interpolate to find the bottom 7 bits of the  * zero-crossing frequency  */            Acc0 = L_abs( CurrVal ) ;            Acc1 = L_abs( PrevVal ) ;            Acc0 = L_add( Acc0, Acc1 ) ;            /* Normalize the sum */            Exp = norm_l( Acc0 ) ;            Acc0 = L_shl( Acc0, Exp ) ;            Acc1 = L_shl( Acc1, Exp ) ;            Acc1 = L_shr( Acc1, (Word16) 8 ) ;            LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ; /*  * Add the upper part of the zero-crossing frequency,  * i.e. bits 7-15  */            Exp = shl( (Word16) (i-1), (Word16) 7 ) ;            LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;            LspCnt ++ ;            /* Check if all zeros have been found */            if ( LspCnt == (Word16) LpcOrder )                break ; /*  * Switch the pointer between sum and difference polynomials  */            k ^= 1 ; /*  * Evaluate the new polynomial at the current frequency  */            CurrVal = (Word32) 0 ;            for ( j = 0 ; j <= LpcOrder/2 ; j ++ )                CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],                                    CosineTable[i*j%CosineTableSize] ) ;        }        /* Update the previous value */        PrevVal = CurrVal ;    } /*  * Check if all 10 zeros were found.  If not, ignore the results of  * the search and use the previous frame's LSP frequencies instead.  */    if ( LspCnt != (Word16) LpcOrder ) {        for ( j = 0 ; j < LpcOrder ; j ++ )            LspVect[j] = PrevLsp[j] ;    }    return ;}/***** Function:            Lsp_Qnt()**** Description:     Vector quantizes the LSP frequencies.  The LSP**          vector is divided into 3 sub-vectors, or**          bands, of dimension 3, 3, and 4.  Each band is**          quantized separately using a different VQ**          table.  Each table has 256 entries, so the**          quantization generates three indices of 8 bits**          each.  (Only the LSP vector for subframe 3 is**          quantized per frame.)**** Links to text:   Section 2.5**** Arguments:       ****  Word16 CurrLsp[]    Unquantized LSP frequencies for the current frame (10 words)**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)**** Outputs:             Quantized LSP frequencies for the current frame (10 words)**** Return value:****  Word32      Long word packed with the 3 VQ indices.  Band 0**          corresponds to bits [23:16], band 1 corresponds**          to bits [15:8], and band 2 corresponds to bits [7:0].**          (Bit 0 is the least significant.)***/Word32   Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp ){    int   i ;    Word16   Wvect[LpcOrder] ;    Word16   Tmp0,Tmp1   ;    Word16   Exp   ; /*  * Compute the VQ weighting vector.  The weights assign greater  * precision to those frequencies that are closer together.  */    /* Compute the end differences */    Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ;    Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ;    /* Compute the rest of the differences */    for ( i = 1 ; i < LpcOrder-1 ; i ++ ) {        Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ;        Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ;        if ( Tmp0 > Tmp1 )            Wvect[i] = Tmp1 ;        else            Wvect[i] = Tmp0 ;    }    /* Invert the differences */    Tmp0 = (Word16) 0x0020 ;    for ( i = 0 ; i < LpcOrder ; i ++ ) {        if ( Wvect[i] > Tmp0 )            Wvect[i] = div_s( Tmp0, Wvect[i] ) ;        else            Wvect[i] = MAX_16 ;    }    /* Normalize the weight vector */    Tmp0 = (Word16) 0 ;    for ( i = 0 ; i < LpcOrder ; i ++ )        if ( Wvect[i] > Tmp0 )            Tmp0 = Wvect[i] ;    Exp = norm_s( Tmp0 ) ;    for ( i = 0 ; i < LpcOrder ; i ++ )        Wvect[i] = shl( Wvect[i], Exp ) ; /*  * Compute the VQ target vector.  This is the residual that remains  * after subtracting both the DC and predicted  * components.  */ /*  * Subtract the DC component from both the current and previous LSP  * vectors.  */    for ( i = 0 ; i < LpcOrder ; i ++ ) {        CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ;        PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ;    } /*  * Generate the prediction vector and subtract it.  Use a constant  * first-order predictor based on the previous (DC-free) LSP  * vector.  */    for ( i = 0 ; i < LpcOrder ; i ++ ) {        Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ;        CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ;    } /*  * Add the DC component back to the previous LSP vector.  This  * vector is needed in later routines.  */    for ( i = 0 ; i < LpcOrder ; i ++ )        PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ; /*  * Do the vector quantization for all three bands  */    return Lsp_Svq( CurrLsp, Wvect ) ;}/***** Function:            Lsp_Svq()**** Description:     Performs the search of the VQ tables to find**          the optimum LSP indices for all three bands.**          For each band, the search finds the index which **          minimizes the weighted squared error between **          the table entry and the target.**** Links to text:   Section 2.5**** Arguments:       ****  Word16 Tv[]     VQ target vector (10 words)**  Word16 Wvect[]      VQ weight vector (10 words)**** Outputs:         None**** Return value:    ****  Word32      Long word packed with the 3 VQ indices.  Band 0**          corresponds to bits [23:16], band 1 corresponds**          to bits [15:8], and band 2 corresponds to bits [7:0].**              */Word32   Lsp_Svq( Word16 *Tv, Word16 *Wvect ){    int   i,j,k ;    Word32   Rez,Indx    ;    Word32   Acc0,Acc1   ;    Word16   Tmp[LpcOrder] ;    Word16  *LspQntPnt  ; /*  * Initialize the return value  */    Rez = (Word32) 0 ; /*  * Quantize each band separately  */    for ( k = 0 ; k < LspQntBands ; k ++ ) { /*  * Search over the entire VQ table to find the index that  * minimizes the error.  */        /* Initialize the search */        Acc1 = (Word32) -1 ;        Indx = (Word32) 0 ;        LspQntPnt = BandQntTable[k] ;        for ( i = 0 ; i < LspCbSize ; i ++ ) { /*  * Generate the metric, which is the negative error with the  * constant component removed.  */            for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )