vq_lib.c

MELPe 1200 bps, fixed point

/*

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 "sc1200.h"
#include "macro.h"
#include "mathhalf.h"
#include "mat_lib.h"
#include "math_lib.h"
#include "lpc_lib.h"
#include "constant.h"


#define MAXWT		4096                    /* w[i] < 2.0 to avoid saturation */
#define MAXWT2		(MAXWT*2)
#define MAXWT4		(MAXWT*4)
#define ONE_Q28		268435456L                                   /* (1 << 28) */
#define EIGHT_Q11   16384                                    /* 8 * (1 << 11) */
#define X016_Q15	5242                                  /* 0.16 * (1 << 15) */
#define X064_Q15	20971                                 /* 0.64 * (1 << 15) */
#define X069_Q15	22609                                 /* 0.69 * (1 << 15) */
#define X14_Q14		22937                                  /* 1.4 * (1 << 14) */
#define X25_Q6		1600                                     /* 25 * (1 << 6) */
#define X75_Q8		19200                                    /* 75 * (1 << 8) */
#define X117_Q5		3744                                    /* 117 * (1 << 5) */
#define XN03_Q15	-9830                                 /* -0.3 * (1 << 15) */

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


/* 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:                                                                  */
/*      weight[] is Q11, lsp[] is Q15, lpc[] is Q12                           */
/*                                                                            */
/* Note:                                                                      */
/*      The coder does not use the returned value from vq_lspw() at all.      */

Shortword *vq_lspw(Shortword weight[], Shortword lsp[], Shortword lpc[],
				   Shortword order)
{
	register Shortword	i;
	Longword	L_temp;


	for (i = 0; i < order; i++){
		L_temp = lpc_aejw(lpc, lsp[i], order);               /* L_temp in Q19 */
		weight[i] = L_pow_fxp(L_temp, XN03_Q15, 19, 11);
	}

	/* Modifying weight[8] and weight[9]. */

	weight[8] = mult(weight[8], X064_Q15);
	weight[9] = mult(weight[9], X016_Q15);
	return(weight);
}


/* 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, order, 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                              */
/*			order (p)- the predictor order                                    */
/*			weights (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]                                 */
/*                                                                            */
/* cb is Q17, u is Q15, u_est is Q15, weights is Q11, u_hat is Q15            */
/*                                                                            */
/*      Note:                                                                 */
/*          The coder does not use the returned value from vq_lspw() at all.  */

Shortword vq_ms4(const Shortword *cb, Shortword *u, const Shortword *u_est,
				 const Shortword levels[], Shortword ma, Shortword stages,
				 Shortword order, Shortword weights[], Shortword *u_hat,
				 Shortword *a_indices, Shortword max_inner)
{
	const Shortword		*cbp, *cb_currentstage, *cb_table;
	Shortword	tmp, *u_tmp, *uhatw, uhatw_sq;
	Shortword	d_cj, d_opt;
	Shortword	*d, *p_d, *n_d, *p_distortion;
	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 */
	j = 0;
	for (i = 0; i < order; i++){
		if (weights[i] > MAXWT4){
			j = 3;
			break;
		} else if (weights[i] > MAXWT2){
			j = 2;
		} else if (weights[i] > MAXWT){
			if (j == 0)
				j = 1;
		}
	}
	for (i = 0; i < order; i++){
		weights[i] = shr(weights[i], j);
	}

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

	/* initialize memory */
	v_zap(indices, (Shortword) (2*stages*ma));
	v_zap(parents, (Shortword) (2*ma));

	/* initialize inner loop counter */
	inner_counter = 0;

	/* set up memory */
	p_indices = &indices[0];
	n_indices = &indices[ma*stages];
	p_errors = &errors[0];
	n_errors = &errors[ma*order];
	p_d = &d[0];
	n_d = &d[ma];
	p_parents = &parents[0];
	n_parents = &parents[ma];

	/* u_tmp is the input vector (i.e. if u_est is non-null, it is subtracted */
	/* off) */
	u_tmp = v_get((Shortword) (order + 1));                     /* u_tmp is Q15 */
	(void) v_equ(u_tmp, u, order);
	if (u_est)
		(void) v_sub(u_tmp, u_est, order);

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

	/* L_temp is Q31 */
	L_temp = 0;
	for (j = 0; j < order; j++){
		temp = mult(u_tmp[j], weights[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*order], u_tmp, order);
		n_d[c] = tmp;
	}

	/* no longer need memory so free it here */
	v_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;

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

		/* set the distortions to a large value */
		for (c = 0; c < ma; c++){
			n_d[c] = SW_MAX;
		}
		for (j = 0; j < levels[s]; j++){
			/* compute weighted codebook element, increment codebook pointer */
			/* L_temp is Q31 */
			L_temp = 0;
			for (i = 0; i < order; i++, cbp++){
				/* Q17*Q11 << 1 = Q29 */
				L_temp1 = L_mult(*cbp, weights[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++){