asterisk/codecs/ilbc/filter.c

 
/****************************************************************** 
 
    iLBC Speech Coder ANSI-C Source Code 
 
    filter.c  
 
    Copyright (c) 2001, 
    Global IP Sound AB. 
    All rights reserved. 
 
******************************************************************/ 
 
#include "iLBC_define.h" 
 
/*----------------------------------------------------------------* 
 *  all-pole filter 
 *---------------------------------------------------------------*/ 
 
void AllPoleFilter( 
    float *InOut,   /* (i/o) on entrance InOut[-orderCoef] to  
                           InOut[-1] contain the state of the filter  
                           (delayed samples). InOut[0] to  
                           InOut[lengthInOut-1] contain the filter  
                           input, on en exit InOut[-orderCoef] to  
                           InOut[-1] is unchanged and InOut[0] to  
                           InOut[lengthInOut-1] contain filtered  
                           samples */ 
    float *Coef,/* (i) filter coefficients, Coef[0] is assumed to  
                           be 1.0 */ 
    int lengthInOut,/* (i) number of input/output samples */ 
    int orderCoef   /* (i) number of filter coefficients */ 
){   
    int n,k; 
     
    for(n=0;n<lengthInOut;n++){ 
        for(k=1;k<=orderCoef;k++){ 
            *InOut -= Coef[k]*InOut[-k]; 
        } 
        InOut++; 
    } 
} 
 
/*----------------------------------------------------------------* 
 *  all-zero filter                          
 *---------------------------------------------------------------*/ 
 
void AllZeroFilter( 
    float *In,      /* (i) In[0] to In[lengthInOut-1] contain filter  
                           input samples */ 
    float *Coef,/* (i) filter coefficients (Coef[0] is assumed  
                           to be 1.0) */ 
    int lengthInOut,/* (i) number of input/output samples */ 
    int orderCoef,  /* (i) number of filter coefficients */ 
    float *Out      /* (i/o) on entrance Out[-orderCoef] to Out[-1] 
                           contain the filter state, on exit Out[0]  
                           to Out[lengthInOut-1] contain filtered  
                           samples */ 
){   
    int n,k; 
     
    for(n=0;n<lengthInOut;n++){ 
        *Out = Coef[0]*In[0]; 
        for(k=1;k<=orderCoef;k++){ 
            *Out += Coef[k]*In[-k]; 
        } 
        Out++; 
        In++; 
    } 
} 
 
/*----------------------------------------------------------------* 
 *  pole-zero filter 
 *---------------------------------------------------------------*/ 
 
void ZeroPoleFilter( 
    float *In,      /* (i) In[0] to In[lengthInOut-1] contain filter  
                           input samples In[-orderCoef] to In[-1]  
                           contain state of all-zero section */ 
    float *ZeroCoef,/* (i) filter coefficients for all-zero  
                           section (ZeroCoef[0] is assumed to  
                           be 1.0) */ 
    float *PoleCoef,/* (i) filter coefficients for all-pole section  
                           (ZeroCoef[0] is assumed to be 1.0) */ 
    int lengthInOut,/* (i) number of input/output samples */ 
    int orderCoef,  /* (i) number of filter coefficients */ 
    float *Out      /* (i/o) on entrance Out[-orderCoef] to Out[-1]  
                           contain state of all-pole section. On exit  
                           Out[0] to Out[lengthInOut-1] contain  
                           filtered samples */ 
){ 
    AllZeroFilter(In,ZeroCoef,lengthInOut,orderCoef,Out); 
    AllPoleFilter(Out,PoleCoef,lengthInOut,orderCoef); 
} 
 
/*----------------------------------------------------------------* 
 * downsample (LP filter and decimation) 
 *---------------------------------------------------------------*/ 
 
void DownSample ( 
    float  *In,     /* (i) input samples */ 
    float  *Coef,   /* (i) filter coefficients */ 
    int lengthIn,   /* (i) number of input samples */  
    float  *state,  /* (i) filter state */ 
    float  *Out     /* (o) downsampled output */ 
){ 
    float   o; 
    float *Out_ptr = Out; 
    float *Coef_ptr, *In_ptr; 
    float *state_ptr; 
    int i, j, stop; 
 
    /* LP filter and decimate at the same time */ 
 
    for (i = DELAY_DS; i < lengthIn; i+=FACTOR_DS) 
    { 
        Coef_ptr = &Coef[0]; 
        In_ptr = &In[i]; 
        state_ptr = &state[FILTERORDER_DS-2]; 
 
        o = (float)0.0; 
         
        stop = (i < FILTERORDER_DS) ? i + 1 : FILTERORDER_DS; 
 
        for (j = 0;j < stop; j++)  
        { 
            o += *Coef_ptr++ * (*In_ptr--); 
        } 
        for (j = i + 1; j < FILTERORDER_DS; j++)  
        { 
            o += *Coef_ptr++ * (*state_ptr--); 
        } 
 
        *Out_ptr++ = o; 
    } 
 
    /* Get the last part (use zeros as input for the future) */ 
 
    for (i=(lengthIn+FACTOR_DS); i<(lengthIn+DELAY_DS);  
            i+=FACTOR_DS) { 
 
        o=(float)0.0; 
         
        if (i<lengthIn) { 
            Coef_ptr = &Coef[0]; 
            In_ptr = &In[i]; 
            for (j=0;j<FILTERORDER_DS;j++) { 
                    o += *Coef_ptr++ * (*Out_ptr--); 
            } 
        } else { 
            Coef_ptr = &Coef[i-lengthIn]; 
            In_ptr = &In[lengthIn-1]; 
            for (j=0;j<FILTERORDER_DS-(i-lengthIn);j++) { 
                    o += *Coef_ptr++ * (*In_ptr--); 
            } 
        } 
        *Out_ptr++ = o; 
    } 
}