vq_lib.c

音频编解码 2400 bps MELP语音编解码器 定点算法

/*

2.4 kbps MELP Proposed Federal Standard speech coder

Fixed-point C code, version 1.0

Copyright (c) 1998, Texas Instruments, Inc.  

Texas Instruments has intellectual property rights on the MELP
algorithm.  The Texas Instruments contact for licensing issues for
commercial and non-government use is William Gordon, Director,
Government Contracts, Texas Instruments Incorporated, Semiconductor
Group (phone [phone]).

The fixed-point version of the voice codec Mixed Excitation Linear
Prediction (MELP) is based on specifications on the C-language software
simulation contained in GSM 06.06 which is protected by copyright and
is the property of the European Telecommunications Standards Institute
(ETSI). This standard is available from the ETSI publication office
tel. [phone]. ETSI has granted a license to United States
Department of Defense to use the C-language software simulation contained
in GSM 06.06 for the purposes of the development of a fixed-point
version of the voice codec Mixed Excitation Linear Prediction (MELP).
Requests for authorization to make other use of the GSM 06.06 or
otherwise distribute or modify them need to be addressed to the ETSI
Secretariat fax: [phone].

*/
#include <stdio.h>
#include "spbstd.h"
#include "mathhalf.h"
#include "wmops.h"
#include "mat.h"
#include "math_lib.h"
#include "lpc.h"
#include "vq.h"
#include "constant.h"

/* VQ_LSPW- compute LSP weighting vector-

    Atal's method:
        From Atal and Paliwal (ICASSP 1991)
        (Note: Paliwal and Atal used w(k)^2(u(k)-u_hat(k))^2,
         and we use w(k)(u(k)-u_hat(k))^2 so our weights are different
         but the method (i.e. the weighted MSE) is the same.

                */
/* Q values:
   w is Q11
   lsp is Q15
   a is Q12 */

Shortword *vq_lspw(Shortword *w, Shortword *lsp, Shortword *a, 
		   Shortword p)
{
    Shortword j;
    Longword L_temp;
    
    for(j = 0; j < p; j++)
      {
	L_temp = lpc_aejw(a,lsp[j],p);    /* L_temp in Q19 */
	w[j] = L_pow_fxp(L_temp,(Shortword)XN03_Q15,19,11);

	if (j == 8)
	  w[j] = mult(w[j], (Shortword)X064_Q15);
	else if (j == 9)
	  w[j] = mult(w[j], (Shortword)X016_Q15);
      }

    return(w);
} /* VQ_LSPW */



/*
    VQ_MS4-
        Tree search multi-stage VQ encoder (optimized for speed
        should be compatible with VQ_MS)

    Synopsis: vq_ms4(cb,u,u_est,levels,ma,stages,p,w,u_hat,a_indices)
        Input:
            cb- one dimensional linear codebook array (codebook is structured 
                as [stages][levels for each stage][p])
            u- dimension p, the parameters to be encoded (u[0..p-1])
            u_est- dimension p, the estimated parameters or mean (if NULL, assume
               estimate is the all zero vector) (u_est[0..p-1])
            levels- the number of levels at each stage (levels[0..stages-1])
            ma- the tree search size (keep ma candidates from one stage to the
               next)
            stages- the number of stages of msvq
            p- the predictor order
            w- the weighting vector (w[0..p-1])
            max_inner- the maximum number of times the swapping procedure
                       in the inner loop can be executed
        Output:
            u_hat-   the reconstruction vector (if non null)
            a_indices- the codebook indices (for each stage) a_indices[0..stages-1]
        Parameters:

*/

#define P_SWAP(x,y,type) do{type u__p;u__p = x;x = y;y = u__p;}while(0)

/* cb is Q17
   u is Q15
   u_est is Q15
   w is Q11
   u_hat is Q15 */

Shortword vq_ms4(Shortword *cb, Shortword *u, Shortword *u_est, 
		 Shortword *levels, Shortword ma, Shortword stages, 
		 Shortword p, Shortword *w, Shortword *u_hat, 
		 Shortword *a_indices, Shortword max_inner)
{
    Shortword tmp,*u_tmp,*uhatw,uhatw_sq;
    Shortword d_cj,d_opt;
    Shortword *d,*p_d,*n_d,*p_distortion,*cb_currentstage,*cbp, *cb_table;
    Shortword *errors,*p_errors,*n_errors,*p_e;
    Shortword i,j,m,s,c,p_max,inner_counter;
    Shortword *indices,*p_indices,*n_indices;
    Shortword *parents,*p_parents,*n_parents;
    Shortword *tmp_p_e;

    Shortword temp;
    Longword L_temp,L_temp1;
    
    /* make sure weights don't get too big */
#define MAXWT  4096           /* w[i] < 2.0 to avoid saturation */
#define MAXWT2 (MAXWT*2)
#define MAXWT4 (MAXWT*4)
    j = 0;    data_move();
    for (i = 0; i < p; i++) {
      if (w[i] > MAXWT4) {
        /* increment complexity for if statement */
        compare_nonzero();
	j = 3;    data_move();
	break;
      }
      else if (w[i] > MAXWT2) {
        /* increment complexity for if statement */
        compare_nonzero();
	j = 2;    data_move();
      }
      else if (w[i] > MAXWT) {
        /* increment complexity for if statement */
        compare_nonzero();
	/* increment complexity for if statement */
        compare_zero();
	if (j == 0) {
	  j = 1;    data_move();
	}
      }
    }
    for (i = 0; i < p; i++) {
      w[i] = shr(w[i],j);    data_move();
    }


    /* allocate memory for the current node and
       parent node (thus, the factors of two everywhere)
       The parents and current nodes are allocated contiguously */
    MEM_ALLOC(MALLOC,indices,2*ma*stages,Shortword);
    MEM_ALLOC(MALLOC,errors,2*ma*p,Shortword);
    MEM_ALLOC(MALLOC,uhatw,p,Shortword);
    MEM_ALLOC(MALLOC,d,2*ma,Shortword);
    MEM_ALLOC(MALLOC,parents,2*ma,Shortword);
    MEM_ALLOC(MALLOC,tmp_p_e,ma*p,Shortword);
    
    /* initialize memory */
    v_zap(indices,(Shortword)(2*stages*ma));
    v_zap(parents,(Shortword)(2*ma));

    
    /* initialize inner loop counter */
    inner_counter = 0;  data_move();
    
    /* set up memory */
    p_indices = &indices[0];  data_move();
    n_indices = &indices[ma*stages];  data_move();
    p_errors = &errors[0];  data_move();
    n_errors = &errors[ma*p];  data_move();
    p_d = &d[0];  data_move();
    n_d = &d[ma];  data_move();
    p_parents = &parents[0];  data_move();
    n_parents = &parents[ma];  data_move();
    
    /* u_tmp is the input vector (i.e. if u_est is non-null, it
       is subtracted off) */
    MEM_ALLOC(MALLOC,u_tmp,p+1,Shortword);
    /* u_tmp is Q15 */
    (void)v_equ(u_tmp,u,p);
    if (u_est)
      (void)v_sub(u_tmp,u_est,p);

    /* change u_tmp from Q15 to Q17 */
    for (j=0; j<p; j++)
      u_tmp[j] = shl(u_tmp[j], 2);  data_move();

    /* L_temp is Q31 */
    L_temp = 0;  data_move();
    for(j = 0 ; j < p; j++) {
      temp = mult(u_tmp[j],w[j]);    /* temp = Q13 */
      L_temp = L_mac(L_temp,temp,u_tmp[j]);
    }

    /* tmp in Q15 */
    tmp = extract_h(L_temp);

    /* set up inital error vectors (i.e. error vectors = u_tmp) */
    for(c=0; c < ma; c++)
    {
	/* n_errors is Q17
	   n_d is Q15 */
	(void)v_equ(&n_errors[c*p],u_tmp,p);
	n_d[c] = tmp;	  data_move();
    }

    /* no longer need memory so free it here */
    MEM_FREE(FREE,u_tmp);

    /* codebook pointer is set to point to first stage */
    cbp = cb;

    /* set m to 1 for the first stage
       and loop over all stages */

    for(m=1,s=0; s < stages; s++)
    {
        /* Save the pointer to the beginning of the
           current stage.  Note: cbp is only incremented in
           one spot, and it is incremented all the way through
           all the stages. */
        cb_currentstage = cbp;

        /* set up pointers to the parent and current nodes */
        P_SWAP(p_indices,n_indices,Shortword*);
        P_SWAP(p_parents,n_parents,Shortword*);
	P_SWAP(p_errors,n_errors,Shortword*);
	P_SWAP(p_d,n_d,Shortword*);
	
        /* p_max is the pointer to the maximum distortion
           node over all candidates.  The so-called worst
           of the best. */
        p_max = 0;  data_move();

	/* store errors in Q15 in tmp_p_e */
	for (i = 0; i < m*p; i++) {
	  tmp_p_e[i] = shr(p_errors[i],2);  data_move();
	}

        /* set the distortions to a large value */
        for(c=0; c < ma; c++) {
	    n_d[c] = SW_MAX;  data_move();
	}
        for(j=0; j < levels[s]; j++)
        {
            /* compute weighted codebook element, increment codebook pointer */
	    /* L_temp is Q31 */
	    L_temp = 0;    data_move();
	    for(i=0 ; i < p; i++,cbp++)
            {
	        /* Q17*Q11<<1 = Q29 */
	        L_temp1 = L_mult(*cbp,w[i]);

		/* uhatw[i] = -2*tmp */
		/* uhatw is Q15 (shift 3 to take care of *2)*/
		uhatw[i] = negate(extract_h(L_shl(L_temp1,3)));

		/* tmp is now Q13 */
		tmp = extract_h(L_temp1);
		L_temp = L_mac(L_temp,*cbp,tmp);
            }
	    /* uhatw_sq is Q15 */
	    uhatw_sq = extract_h(L_temp);


            /* p_e points to the error vectors and p_distortion
               points to the node distortions.  Note: the error
               vectors are contiguous in memory, as are the distortions.
               Thus, the error vector for the 2nd node comes immediately
               after the error for the first node.  (This saves on
               repeated initializations) */
	    /* p_e is Q15 */
	    p_e = tmp_p_e;
            p_distortion = p_d;
	    
            /* iterate over all parent nodes */
            for(c=0; c < m; c++)
            {
		/* L_temp is Q31
		   p_distortion is same Q as n_d 
		   p_e is Q15 */
		L_temp = L_deposit_h(add(*p_distortion++,uhatw_sq));
                for(i=0; i < p; i++) {
		  L_temp = L_mac(L_temp,*p_e++,uhatw[i]);
		}

		/* d_cj is Q15 */