语音压缩

开发平台：
C/C++

mat_lib.c：源码内容
							/*
  mat_lib.c: Matrix and vector manipulation library
*/
#include "spbstd.h"
#include "mathhalf.h"
#include "wmops.h"
#include "mat.h"
/***************************************************************************
 *
 *   FUNCTION NAME: v_add
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 16 bit input vector with
 *     saturation.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     vec2            16 bit short signed integer (Shortword) vector whose 
 *                     values falls in the range 
 *                     0xffff 8000 <= vec2 <= 0x0000 7fff.
 *
 *     n               size of input vectors.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 16 bit input vectors with
 *     saturation.
 *
 *     vec1 = vec1 + vec2
 *
 *     vec1[] is set to 0x7fff if the operation results in an
 *     overflow.  vec1[] is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/
Shortword *v_add(Shortword *vec1,Shortword *vec2,Shortword n)
{
    Shortword i;
	
	Longword L_temp;
    for(i=0; i < n; i++) {
      /* vec1[i] = add(vec1[i],vec2[i]);  //  data_move();mark del */
        L_temp = _sadd2((Longword)(vec1[i]),(Longword)(vec2[i]));
        vec1[i] = (Shortword) (0x0000ffffL & L_temp);
    }
    return(vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: L_v_add
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 32 bit input vector with
 *     saturation.
 *
 *   INPUTS:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values fall in the range 
 *                     0x8000 0000 <= L_vec1 <= 0x7fff ffff.
 *
 *     L_vec2          32 bit long signed integer (Longword) vector whose 
 *                     values falls in the range 
 *                     0x8000 0000 <= L_vec2 <= 0x7fff ffff.
 *
 *     n               size of input vectors.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values fall in the range
 *                     0x8000 0000 <= L_vec1[] <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 32 bit input vectors with
 *     saturation.
 *
 *     L_vec1 = L_vec1 + L_vec2
 *
 *     L_vec1[] is set to 0x7fff ffff if the operation results in an
 *     overflow.  L_vec1[] is set to 0x8000 0000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/
Longword *L_v_add(Longword *L_vec1,Longword *L_vec2,Shortword n)
{
    Shortword i;
    for(i=0; i < n; i++) {
      L_vec1[i] = L_add(L_vec1[i],L_vec2[i]); //   L_data_move();mark del
    }
    return(L_vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_equ
 *
 *   PURPOSE:
 *
 *     Copy the contents of one 16 bit input vector to another
 *
 *   INPUTS:
 *
 *     vec2            16 bit short signed integer (Shortword) vector whose 
 *                     values falls in the range 
 *                     0xffff 8000 <= vec2 <= 0x0000 7fff.
 *
 *     n               size of input vector
 *
 *   OUTPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Copy the contents of one 16 bit input vector to another
 *
 *     vec1 = vec2
 *
 *   KEYWORDS: equate, copy
 *
 *************************************************************************/
 Shortword *v_equ(Shortword *vec1,Shortword *vec2,Shortword n) 
 {                                                             
     Shortword i;                                              
                                                               
     for(i=0; i < n; i++) {                                    
       vec1[i] = vec2[i];   // data_move();mark del            
     }                                                         
     return(vec1);                                             
 }                                                             
/***************************************************************************
 *
 *   FUNCTION NAME: v_equ_shr
 *
 *   PURPOSE:
 *
 *     Copy the contents of one 16 bit input vector to another with shift
 *
 *   INPUTS:
 *
 *     vec2            16 bit short signed integer (Shortword) vector whose 
 *                     values falls in the range 
 *                     0xffff 8000 <= vec2 <= 0x0000 7fff.
 *     scale           right shift factor
 *     n               size of input vector
 *
 *   OUTPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Copy the contents of one 16 bit input vector to another with shift
 *
 *     vec1 = vec2>>scale
 *
 *   KEYWORDS: equate, copy
 *
 *************************************************************************/
Shortword *v_equ_shr(Shortword *vec1,Shortword *vec2,Shortword scale,
		     Shortword n)
{
    Shortword i;
    Longword L_vec2,L_vec1;
    for(i=0; i < n; i++) {
/* 	vec1[i] = shr(vec2[i],scale);  //  data_move();mark del	   */
 	L_vec2 = (Longword)vec2[i];                     
    L_vec1 = _sshvr(L_vec2,scale);                
 	vec1[i] = (Shortword) (0x0000ffffL & L_vec1);	
    }
    return(vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: L_v_equ
 *
 *   PURPOSE:
 *
 *     Copy the contents of one 32 bit input vector to another
 *
 *   INPUTS:
 *
 *     L_vec2          32 bit long signed integer (Longword) vector whose 
 *                     values falls in the range 
 *                     0x8000 0000 <= L_vec2 <= 0x7fff ffff.
 *
 *     n               size of input vector
 *
 *   OUTPUTS:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values fall in the range 
 *                     0x8000 0000 <= L_vec1 <= 0x7fff ffff.
 *
 *   RETURN VALUE:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values fall in the range
 *                     0x8000 0000 <= L_vec1[] <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Copy the contents of one 32 bit input vector to another
 *
 *     vec1 = vec2
 *
 *   KEYWORDS: equate, copy
 *
 *************************************************************************/
Longword *L_v_equ(Longword *L_vec1,Longword *L_vec2,Shortword n)
{
    Shortword i;
    for(i=0; i < n; i++) {
      L_vec1[i] = L_vec2[i];  //   L_data_move();mark del
    }
    return(L_vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_inner
 *
 *   PURPOSE:
 *
 *     Compute the inner product of two 16 bit input vectors
 *     with saturation and truncation.  Output is a 16 bit number.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     vec2            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec2 <= 0x0000 7fff.
 *
 *     n               size of input vectors
 *
 *     qvec1           Q value of vec1
 *
 *     qvec2           Q value of vec2
 *
 *     qout            Q value of output
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     innerprod       16 bit short signed integer (Shortword) whose value
 *                     falls in the range
 *                     0xffff 8000 <= innerprod <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Compute the inner product of the two 16 bit input vectors.
 *     The output is a 16 bit number.
 *
 *   KEYWORDS: inner product
 *
 *************************************************************************/
Shortword v_inner(Shortword *vec1,Shortword *vec2,Shortword n,
		  Shortword qvec1,Shortword qvec2,Shortword qout)
{
    Shortword i;
    Shortword innerprod;
    Longword L_temp;
    L_temp = 0;   //data_move();mark del
    for(i = 0; i < n; i++)
        L_temp = L_mac(L_temp,vec1[i],vec2[i]);
    innerprod = extract_h(L_shl(L_temp,(Shortword)(qout-((qvec1+qvec2+1)-16))));
    return(innerprod);
}
/***************************************************************************
 *
 *   FUNCTION NAME: L_v_inner
 *
 *   PURPOSE:
 *
 *     Compute the inner product of two 16 bit input vectors
 *     with saturation and truncation.  Output is a 32 bit number.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     vec2            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec2 <= 0x0000 7fff.
 *
 *     n               size of input vectors
 *
 *     qvec1           Q value of vec1
 *
 *     qvec2           Q value of vec2
 *
 *     qout            Q value of output
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_innerprod     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_innerprod <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Compute the inner product of the two 16 bit vectors
 *     The output is a 32 bit number.
 *
 *   KEYWORDS: inner product
 *
 *************************************************************************/
Longword L_v_inner(Shortword *vec1,Shortword *vec2,Shortword n,
		   Shortword qvec1,Shortword qvec2,Shortword qout)
{
    Shortword i,shift;
    Longword L_innerprod;
    Longword L_temp,L_qvec1,L_qvec2,L_qout,L_mul;
    L_temp = 0;  // data_move();mark del
    for(i = 0; i < n; i++){
    /*    L_temp = L_mac(L_temp,vec1[i],vec2[i]);*/
        L_mul = _smpy(vec1[i],vec2[i]);
     	L_temp = _sadd(L_temp,L_mul);
	}
    /* ((qout-16)-((qvec1+qvec2+1)-16)) */
	L_qvec1 = (Longword)(qvec1);
	L_qvec2 = (Longword)(qvec2);
	L_qout =(Longword)(qout);
	L_mul = _sadd2(L_qvec1,L_qvec2);
	L_mul = _sadd2(L_mul,(Longword)1);
	L_mul = _sub2(L_qout,L_mul);             
 	shift = (Shortword)(0x0000ffffL & L_mul);	
    /*mul = (Shortword) (0x0000ffffL & L_mul);*/
	/*shift = _sub(qout,mul);*/
	L_innerprod = _sshl(L_temp,shift);
/*  shift = sub(qout,add(add(qvec1,qvec2),1)); */
/*     L_innerprod = L_shl(L_temp,shift); */
    return(L_innerprod);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_magsq
 *
 *   PURPOSE:
 *
 *     Compute the sum of square magnitude of a 16 bit input vector
 *     with saturation and truncation.  Output is a 16 bit number.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     n               size of input vectors
 *
 *     qvec1           Q value of vec1
 *
 *     qout            Q value of output
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     magsq           16 bit short signed integer (Shortword) whose value
 *                     falls in the range
 *                     0xffff 8000 <= magsq <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Compute the sum of square magnitude of a 16 bit input vector.
 *     The output is a 16 bit number.
 *
 *   KEYWORDS: square magnitude
 *
 *************************************************************************/
Shortword v_magsq(Shortword *vec1,Shortword n,Shortword qvec1,Shortword qout)
{
    Shortword i,shift;
    Shortword magsq;
    Longword L_temp;
    L_temp = 0;  // L_data_move();mark del
    for(i = 0; i < n; i++)
        L_temp = L_mac(L_temp,vec1[i],vec1[i]);
    /* qout-((2*qvec1+1)-16) */
    shift = sub(qout,sub(add(shl(qvec1,1),1),16));
    magsq = extract_h(L_shl(L_temp,shift));
    return(magsq);
} /* v_magsq */
/***************************************************************************
 *
 *   FUNCTION NAME: L_v_magsq
 *
 *   PURPOSE:
 *
 *     Compute the sum of square magnitude of a 16 bit input vector
 *     with saturation and truncation.  Output is a 32 bit number.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     n               size of input vectors
 *
 *     qvec1           Q value of vec1
 *
 *     qout            Q value of output
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_magsq         32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_magsq <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Compute the sum of square magnitude of a 16 bit input vector.
 *     The output is a 32 bit number.
 *
 *   KEYWORDS: square magnitude
 *
 *************************************************************************/
Longword L_v_magsq(Shortword *vec1,Shortword n,Shortword qvec1,Shortword qout)
{
    Shortword i,shift;
    Longword L_magsq;
    Longword L_temp,L_mult,L_qvec1,L_qout;
    L_temp = 0; //  L_data_move();mark del
    for(i = 0; i < n; i++){
	   L_mult = _smpy(vec1[i],vec1[i]);
	   L_temp = _sadd(L_temp,L_mult);
	}
       /* L_temp = L_mac(L_temp,vec1[i],vec1[i]);*/
    /* ((qout-16)-((2*qvec1+1)-16)) */
   /* shift = sub(sub(qout,shl(qvec1,1)),1);*/
	L_qvec1 = (Longword)qvec1;                                     
 	L_qout = (Longword)qout;                                           
    L_mult = _sshl(L_qvec1,1);                                    
	L_mult = _sub2(L_qout,L_mult);                                     
 	L_mult = _sub2(L_mult,1);                                     
  	shift = (Shortword) (0x0000ffffL & L_mult); 
    /*L_magsq = L_shl(L_temp,shift); //  L_data_move();mark del*/
    L_magsq = _sshvl(L_temp,shift);  
    return(L_magsq);
} /* L_v_magsq */
                                                
/***************************************************************************
 *
 *   FUNCTION NAME: v_scale
 *
 *   PURPOSE:
 *
 *     Perform a multipy of the 16 bit input vector with a 16 bit input
 *     scale with saturation and truncation.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *     scale
 *                     16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *     n               size of vec1
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform a multipy of the 16 bit input vector with the 16 bit input
 *     scale.  The output is a 16 bit vector.
 *
 *   KEYWORDS: scale
 *
 *************************************************************************/
Shortword *v_scale(Shortword *vec1,Shortword scale,Shortword n)
{
    Shortword i;
	Longword L_temp;
    for(i=0; i < n; i++) {
       /*  vec1[i] = mult(vec1[i],scale); //  data_move();mark del */
	   L_temp = _smpy(vec1[i],scale);
       vec1[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
    }
    return(vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_scale_shl
 *
 *   PURPOSE:
 *
 *     Perform a multipy of the 16 bit input vector with a 16 bit input
 *     scale and shift to left with saturation and truncation.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     scale           16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *     n               size of vec1
 *
 *     shift           16 bit short signed integer (Shortword) whose value
 *                     falls in the range 0x0000 0000 <= var1 <= 0x0000 1f.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform a multipy of the 16 bit input vector with the 16 bit input
 *     scale.  The output is a 16 bit vector.
 *
 *   KEYWORDS: scale
 *
 *************************************************************************/
Shortword *v_scale_shl(Shortword *vec1,Shortword scale,Shortword n,
		       Shortword shift)
{
    Shortword i;
    
	Longword L_temp;
    for(i=0; i < n; i++){
       /*  vec1[i] = extract_h(L_shl(L_mult(vec1[i],scale),shift)); */
	   L_temp = _sshvl(_smpy(vec1[i],scale),shift);
       vec1[i] = (Shortword) (0x0000ffffL & (L_temp >> 16));
		}
    return(vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_sub
 *
 *   PURPOSE:
 *
 *     Perform the subtraction of the two 16 bit input vector with
 *     saturation.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     vec2            16 bit short signed integer (Shortword) vector whose 
 *                     values falls in the range 
 *                     0xffff 8000 <= vec2 <= 0x0000 7fff.
 *
 *     n               size of input vectors.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range
 *                     0xffff 8000 <= vec1[] <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the subtraction of the two 16 bit input vectors with
 *     saturation.
 *
 *     vec1 = vec1 - vec2
 *
 *     vec1[] is set to 0x7fff if the operation results in an
 *     overflow.  vec1[] is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: sub, subtraction
 *
 *************************************************************************/
Shortword *v_sub(Shortword *vec1,Shortword *vec2,Shortword n)
{
    Shortword i;
    for(i=0; i < n; i++) {
        vec1[i] = sub(vec1[i],vec2[i]); // data_move();mark del
    }
    return(vec1);
}
/***************************************************************************
 *
 *   FUNCTION NAME: v_zap
 *
 *   PURPOSE:
 *
 *     Set the elements of a 16 bit input vector to zero.
 *
 *   INPUTS:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values fall in the range 
 *                     0xffff 8000 <= vec1 <= 0x0000 7fff.
 *
 *     n               size of vec1.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     vec1            16 bit short signed integer (Shortword) vector whose 
 *                     values are equal to 0x0000 0000.
 *
 *   IMPLEMENTATION:
 *
 *     Set the elements of 16 bit input vector to zero.
 *
 *     vec1 = 0
 *
 *   KEYWORDS: zap, clear, reset
 *
 *************************************************************************/
/* Shortword *v_zap(Shortword *vec1,Shortword n) */
/* {                                             */
/*     Shortword i;                              */
/*                                               */
/*     for(i = 0; i < n; i++) {                  */
/*         vec1[i] = 0; //  data_move();mark del */
/*     }                                         */
/*                                               */
/*     return(vec1);                             */
/* }                                             */
/***************************************************************************
 *
 *   FUNCTION NAME: L_v_zap
 *
 *   PURPOSE:
 *
 *     Set the elements of a 32 bit input vector to zero.
 *
 *   INPUTS:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values fall in the range 
 *                     0x8000 0000 <= vec1 <= 0x7fff ffff.
 *
 *     n               size of L_vec1.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_vec1          32 bit long signed integer (Longword) vector whose 
 *                     values are equal to 0x0000 0000.
 *
 *   IMPLEMENTATION:
 *
 *     Set the elements of 32 bit input vector to zero.
 *
 *     L_vec1 = 0
 *
 *   KEYWORDS: zap, clear, reset
 *
 *************************************************************************/
Longword *L_v_zap(Longword *L_vec1,Shortword n)
{
    Shortword i;
    for(i = 0; i < n; i++) {
        L_vec1[i] = 0;//   data_move();mark del
    }
    return(L_vec1);
}

						