pit_lib.c

C50_melp.tar.gz为美军2400bps语音压缩编码算法

/*

*2.4 kbps MELP Proposed Federal Standard speech coder
*
*TMS320C5x assembly 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 following code was hand optimized for the Texas Instuments
* TMS320C5x DSP by DSPCon, Inc.  For information, please contact DSPCon
* at:
* 
*                       DSPCon, Inc.
*                       380 Foothill Road
*                       Bridgewater, New Jersey 08807
*                       [phone]
*                       [e-mail]
*                       www.dspcon.com
*
*************************************************************************/



/*

  pit_lib.c: pitch analysis subroutines

*/


#include <math.h>
#include "spbstd.h"
#include "mathhalf.h"
#include "mathdp31.h"
#include "mat.h"
#include "math_lib.h"
#include "dsp_sub.h"
#include "pit.h"


extern int saturation;  /* saturation flag */

/*                                                                  */
/*  double_chk.c: check for pitch doubling                          */
/*  and also verify pitch multiple for short pitches.               */
/*                                                                  */
/* Q values
   --------
   sig_in - Q0
   pcorr - Q14
   pitch - Q7
   pdouble - Q7
*/

#define NUM_MULT 8

Shortword double_chk(Shortword sig_in[],Shortword *pcorr,Shortword pitch,
		     Shortword pdouble,Shortword pmin,Shortword pmax,
		     Shortword pmin_q7,Shortword pmax_q7,
		     Shortword lmin)
{
    Shortword mult;
    Shortword corr,thresh;
    Shortword temp_pit;
    Shortword temp1,temp2;
    Longword L_temp;

    pitch = frac_pch(sig_in,pcorr,pitch,0,pmin,pmax,pmin_q7,pmax_q7,lmin);

    /* compute threshold Q14*Q7>>8 */
    /* extra right shift to compensate left shift of L_mult */
    L_temp = L_mult(*pcorr,pdouble);
    L_temp = L_shr(L_temp,8);
    thresh = extract_l(L_temp);      /* Q14 */

    /* Check pitch submultiples from shortest to longest */
    for (mult = NUM_MULT; mult >= 2; mult--) {

	/* temp_pit = pitch / mult */
        temp1 = 0;      
        temp2 = shl(mult,11);     /* Q11 */      
        temp_pit = pitch;     
	while (temp_pit > temp2) {
	  /* increment complexity for while statement */
          
          temp_pit = shr(temp_pit,1);      
          temp1 = add(temp1,1);      
	}
	/* Q7*Q15/Q11 -> Q11 */
        temp2 = divide_s(temp_pit,temp2);     
        temp1 = sub(4,temp1);      
	/* temp_pit=pitch/mult in Q7 */
        temp_pit = shr(temp2,temp1);      

	/* increment complexity for if statement */
        
	if (temp_pit >= pmin_q7) {
	    temp_pit = frac_pch(sig_in,&corr,temp_pit,0,pmin,pmax,pmin_q7,
				    pmax_q7,lmin);
	    double_ver(sig_in,&corr,temp_pit,pmin,pmax,pmin_q7,
			   pmax_q7,lmin);

	    /* stop if submultiple greater than threshold */
	    /* increment complexity for if statement */
            
	    if (corr > thresh) {

		/* refine estimate one more time since previous window */
		/* may be off center slightly and temp_pit has moved */
		pitch = frac_pch(sig_in,pcorr,temp_pit,0,pmin,pmax,pmin_q7,
				     pmax_q7,lmin);
		break;
	    }
	}
    }

    /* Verify pitch multiples for short pitches */
    double_ver(sig_in,pcorr,pitch,pmin,pmax,pmin_q7,pmax_q7,lmin);

    /* Return full floating point pitch value and correlation*/
    return(pitch);

}
#undef NUM_MULT


/*                                                                  */
/*  double_ver.c: verify pitch multiple for short pitches.          */
/*                                                                  */
/* Q values
   --------
   pitch - Q7
   pcorr - Q14
*/

#define SHORT_PITCH (30*(1<<7))       /* Q7 */

void double_ver(Shortword sig_in[],Shortword *pcorr,Shortword pitch,
		Shortword pmin,Shortword pmax,Shortword pmin_q7,
		Shortword pmax_q7,Shortword lmin)
{
    Shortword multiple;
    Shortword corr,temp_pit;


    /* Verify pitch multiples for short pitches */
    multiple = 1;          
    while (extract_l(L_shr(L_mult(pitch,multiple),1)) < SHORT_PITCH) {
      /* increment complexity for while statement */
      
      multiple=add(multiple,1);     
    }

    /* increment complexity for if statement */
    
    if (multiple > 1) {
	temp_pit = extract_l(L_shr(L_mult(pitch,multiple),1));
	temp_pit = frac_pch(sig_in,&corr,temp_pit,0,pmin,pmax,
				pmin_q7,pmax_q7,lmin);

	/* use smaller of two correlation values */
	/* increment complexity for if statement */
        
	if (corr < *pcorr) {
          *pcorr = corr;    
	}
    }
}
#undef SHORT_PITCH

    static  Shortword   temp_buf[321];



/*                                                                  */
/*  find_pitch.c: Determine pitch value.                            */
/*                                                                  */
/*  Q values:
    sig_in - Q0
    ipitch - Q0
    *pcorr - Q14 */
/*

  WARNING: this function assumes the input buffer has been normalized
  by f_pitch_scale.

*/

Shortword find_pitch(Shortword sig_in[],Shortword *pcorr,Shortword lower,
		     Shortword upper,Shortword length)

{

    unsigned long res;
    Shortword ipitch;

    res = (unsigned long)find_pitch_asm(sig_in,lower,upper,length);

    *pcorr = (sqrt_fxp( (res & 0xffff), 15) >> 1);

    ipitch = (res >> 16);
    return(ipitch);
}


/*
    Name: frac_pch.c
    Description: Determine fractional pitch.
    Inputs:
      sig_in - input signal
      fpitch - initial floating point pitch estimate
      range - range for local integer pitch search (0=none)
      pmin - minimum allowed pitch value
      pmax - maximum allowed pitch value
      lmin - minimum correlation length
    Outputs:
      pcorr - correlation at fractional pitch value
    Returns: fpitch - fractional pitch value

    Copyright (c) 1995 by Texas Instruments, Inc.  All rights reserved.
*/

/* Q values
   --------
   ipitch - Q0
   fpitch - Q7
   sig_in - Q0
   *pcorr - Q14

*/

/*

  WARNING: this function assumes the input buffer has been normalized
  by f_pitch_scale.

*/

#define MAXFRAC (2.0*(1<<13))       /* Q13 */
#define MINFRAC (-1.0*(1<<13))      /* Q13 */
#define X05_Q13 (0.5*(1<<13))       /* Q13 */
#define ONE_Q13 (1*(1<<13))      /* Q13 */
#define ONE_Q14 (1*(1<<14))      /* Q14 */

Shortword frac_pch(Shortword sig_in[],Shortword *pcorr,Shortword fpitch,
		   Shortword range,Shortword pmin,Shortword pmax,
		   Shortword pmin_q7,Shortword pmax_q7,Shortword lmin)
{
    Shortword length,cbegin,lower,upper,ipitch;
    Shortword c0_0,c0_T,c0_T1,cT_T,cT_T1,cT1_T1,c0_Tm1;
    Shortword shift1a,shift1b,shift2,shift;
    Shortword frac,frac1,corr;
    Shortword temp;
    Longword denom,denom1,denom2,denom3,numer;
    Longword mag_sq;
    Longword L_temp1;
    Shortword *sig_inp;

    /* Perform local integer pitch search for better fpitch estimate */
    /* increment complexity for if statement */
    
    if (range > 0) {
        ipitch = shift_r7(fpitch);    
        lower = sub(ipitch,range);    
        upper = add(ipitch,range);    
	/* increment complexity for if statement */
        
	if (upper > pmax) {
          upper = pmax;    
	}
	/* increment complexity for if statement */
        
	if (lower < pmin) {
          lower = pmin;    
	}
	/* increment complexity for if statement */
        
	if (lower < add(shr(ipitch,1),shr(ipitch,2))) {
          lower = add(shr(ipitch,1),shr(ipitch,2));    
	}
        length = ipitch;    
	/* increment complexity for if statement */
        
	if (length < lmin) {
          length = lmin;    
	}
	fpitch = shl(find_pitch(sig_in,&corr,lower,upper,length),7);
        
    }


    /* Estimate needed crosscorrelations */
    ipitch = shift_r7(fpitch);    

    /* increment complexity for if statement */
    
    if (ipitch >= pmax) {
      ipitch = sub(pmax,1);    
    }
    length = ipitch;    
    /* increment complexity for if statement */
    
    if (length < lmin) {
      length = lmin;    
    }
    cbegin = negate(shr(add(length,ipitch),1));    

    /* Calculate normalization for numerator and denominator */
    mag_sq = L_v_magsq(&sig_in[cbegin],length,0,1);
    shift1a = norm_s(extract_h(mag_sq));
    shift1b = norm_s(extract_h(
	L_v_magsq(&sig_in[cbegin+ipitch-1],(Shortword)(length+2),0,1)));
    shift = add(shift1a,shift1b);
    shift2 = shr(shift,1);  /* shift2 = half of total shift */
    if (shl(shift2,1) != shift)
	shift1a = sub(shift1a,1);

    /* Calculate correlations with appropriate normalization */
    c0_0 = extract_h(L_shl(mag_sq,shift1a));

    asm(" CNFD");
    v_equ( (Shortword *)((Shortword)0x200 + cbegin), &sig_in[cbegin], 320);
    asm(" CNFP");

    sig_inp = (Shortword *)0xff00;

    c0_T = extract_h(L_shl(L_v_inner(&sig_inp[cbegin],
				     &sig_in[cbegin+ipitch],length),shift2));
    c0_T1 = extract_h(L_shl(L_v_inner(&sig_inp[cbegin],
				      &sig_in[cbegin+ipitch+1],length),shift2));
    c0_Tm1 = extract_h(L_shl(L_v_inner(&sig_inp[cbegin],
				       &sig_in[cbegin+ipitch-1],length),shift2));

    /* increment complexity for if statement */
    
    if (c0_Tm1 > c0_T1) {
	/* fractional component should be less than 1, so decrement pitch */
        c0_T1 = c0_T;    
        c0_T = c0_Tm1;    
        ipitch = sub(ipitch,1) ;    
    }
    cT_T1 = extract_h(L_shl(L_v_inner(&sig_inp[cbegin+ipitch],
				      &sig_in[cbegin+ipitch+1],length),shift1b));
    cT_T = extract_h(L_shl(L_v_inner(&sig_inp[cbegin+ipitch],
				     &sig_in[cbegin+ipitch],length),shift1b));
    cT1_T1 = extract_h(L_shl(L_v_inner(&sig_inp[cbegin+ipitch+1],
				       &sig_in[cbegin+ipitch+1],length),shift1b));