ec_sb_int.c

C__ code For Audio Coding

    for (i = 0; i < state->frameSize; i++) {
        r_in_pwr += r_in[i] * r_in[i];
        s_in_pwr += s_in[i] * s_in[i];
    }

    ADDWEIGHTED_INT(state->r_in_pwr, r_in_pwr, state->td_coeff);
    ADDWEIGHTED_INT(state->s_in_pwr, s_in_pwr, state->td_coeff);

    if (state->td_ress || state->td_resr) {
       if ((!state->td_resr || state->r_in_pwr < state->td_thres) &&
            (!state->td_ress || state->s_in_pwr < state->td_thres))
        {
            /* Restore previous mode */
            state->td_resr = state->td_ress = 0;
            if (state->td_mode & 1)
                ec_isb_ModeOp(state, EC_ADAPTATION_ENABLE);
            if (state->td_mode & 2)
                ec_isb_ModeOp(state, EC_NLP_ENABLE);
        }
    } else if (resr || ress) {
        /* Zero coeffs, disable adaptation and NLP */
        if (resr) state->td_resr = 1;
        if (ress) state->td_ress = 1;

        state->td_mode = state->mode;
        ec_isb_ModeOp(state, EC_COEFFS_ZERO);
        ec_isb_ModeOp(state, EC_ADAPTATION_DISABLE);
        ec_isb_ModeOp(state, EC_NLP_DISABLE);
    }

    return 0;
}
static void isb_ownConstrainCoeff(_isbECState *state, int subbandNum){
   int k,s;
   LOCAL_ALIGN_ARRAY(32,Ipp32s,bsrc,SB_SUBBANDS * 4,state);
   LOCAL_ALIGN_ARRAY(32,Ipp32s,bsrc1,SB_SUBBANDS * 4,state);
   LOCAL_ALIGN_ARRAY(32,Ipp32s,bdst,SB_FFT_LEN,state);

   ippsFFTInv_CCSToR_32s_Sfs((Ipp32s *)(state->ppAdaptiveCoefs[subbandNum]), bdst, state->spec_fft32, 0, state->pBuf);

   ippsZero_32s(bsrc1,SB_SUBBANDS * 4);
   for (k = 0; k < SB_FRAME_SIZE; k++) {
      bsrc1[2 * k] = SB_FFT_LEN * bdst[k];
   }

   ippsFFTFwd_CToC_32sc_Sfs((const Ipp32sc *)bsrc1, (Ipp32sc *)bsrc, state->spec_fftC, 0, state->pBuf);

   for (s = 0; s < SB_SUBBANDS; s++) {
      state->ppAdaptiveCoefs[subbandNum][s].re = bsrc[2 * s];
      state->ppAdaptiveCoefs[subbandNum][s].im = bsrc[2 * s + 1];
   }
   LOCAL_ALIGN_ARRAY_FREE(32,Ipp32s,bdst,SB_FFT_LEN,state);
   LOCAL_ALIGN_ARRAY_FREE(32,Ipp32s,bsrc1,SB_SUBBANDS * 4,state);
   LOCAL_ALIGN_ARRAY_FREE(32,Ipp32s,bsrc,SB_SUBBANDS * 4,state);

}
#define COEF_SF 14
#define COEF_ONE (1 << COEF_SF)
#define COEF_0_5 (COEF_ONE / 2)
#define COEF_0_2 (COEF_ONE / 5)
#define COEF_0_77 (COEF_ONE * 10 / 13)
#define COEF_0_35 (COEF_ONE * 7 / 20)
#define COEF_0_48 (COEF_ONE * 12 / 25)
#define COEF_0_66 (COEF_ONE * 2 / 3)
#define COEF_0_8 (COEF_ONE * 4 / 5)
#define COEF_Q15_ONE ((1 << 15) - 1)
#define THRESH_POW 100 /* 20dBm */
#define PSD_SF   10          /* scale factor for input PSD */
#define CMUL(x, v) SCALE((x) * (v), COEF_SF)
#define WEIGHTED_INT(val, add, coef) SCALE(((val) * (coef) + (COEF_ONE - coef) * (add)), COEF_SF)

typedef struct _SubbandControllerState_EC_16s {
    int numSubbands;
    int frameSize;
    int numSegments;
    int sampleFreq;
    Ipp32s *adaptive_pwr;
    Ipp32s *fixed_pwr;
    Ipp32s *rin_pwr;
    Ipp32s *rin_pwr_n;
    Ipp32s *cf_pwr;
    Ipp32s muQ31;
    Ipp32s sin_pwr;
    Ipp32s coef_werr;
    Ipp32s coef_wcoef;
    Ipp32s coef_nlp0, coef_nlp1;
    Ipp32s sgain, rgain, nlp_val;
    Ipp32s rin_pwr_min;
    Ipp32s_EC_Sfs stepSize_max;
    int nlp_count;
    int sub_use;
} SubbandControllerState_EC_16s;

__INLINE int Cnvrt_64s32s(__INT64 z) {
    if(IPP_MAX_32S < z) return IPP_MAX_32S;
    else if(IPP_MIN_32S > z) return IPP_MIN_32S;
    return (int)z;
}
__INLINE int Add_32s(int x, int y) {
    return Cnvrt_64s32s((__INT64)x + y);
}
__INLINE int Cnvrt_32s16s_NR_Sfs15(int x)
{
    int z = Add_32s(x,0x8000);
    return (z >> 16);
}
__INLINE short Cnvrt_32s16s(int x){
    if (IPP_MAX_16S < x) return IPP_MAX_16S;
    else if (IPP_MIN_16S > x) return IPP_MIN_16S;
    return (short)(x);
}
static void ownSubbandController_EC_16s(const Ipp32sc *pSrcAdaptiveFilterErr,
       const Ipp32sc *pSrcFixedFilterErr,
       Ipp32sc **ppDstAdaptiveCoefs, Ipp32sc **ppDstFixedCoefs,
       Ipp16s *pDstSGain, SubbandControllerState_EC_16s *pState,int *convergedNum)
{
    int i,j;
    Ipp32s powa = 0, powc = 0, powd = 0, uval;
    Ipp32s *cf_pwr, *fixed_pwr, *adaptive_pwr;
    Ipp32s coef_wcoef, coef_werr;

    //IPP_BAD_PTR4_RET(pSrcAdaptiveFilterErr, pSrcFixedFilterErr, ppDstAdaptiveCoefs,
    //    ppDstFixedCoefs);
    //IPP_BAD_PTR2_RET(pDstSGain, pState);

    pState = IPP_ALIGNED_PTR(pState, 16);
    cf_pwr = pState->cf_pwr;
    fixed_pwr = pState->fixed_pwr;
    adaptive_pwr = pState->adaptive_pwr;
    coef_wcoef = pState->coef_wcoef;
    coef_werr = pState->coef_werr;
    (*convergedNum)=0;

    for (i = 0; i < pState->numSubbands; i++) {
        Ipp64s fpwr64, apwr64, cpwr64, cdlt64;
        fpwr64 = SCALE(pSrcFixedFilterErr[i].re * (Ipp64s)pSrcFixedFilterErr[i].re +
            pSrcFixedFilterErr[i].im * (Ipp64s)pSrcFixedFilterErr[i].im, PSD_SF);
        apwr64 = SCALE(pSrcAdaptiveFilterErr[i].re * (Ipp64s)pSrcAdaptiveFilterErr[i].re +
            pSrcAdaptiveFilterErr[i].im * (Ipp64s)pSrcAdaptiveFilterErr[i].im, PSD_SF);
//#if _IPPXSC >= _IPPXSC_S2
//        ownSumSquareColMtrx_NR_32sc64s_Sfs_S2(ppDstAdaptiveCoefs,
//            pState->numSegments, i, &cpwr64);
//        cpwr64 = SCALE(cpwr64, PSD_SF);
//#else
        cpwr64 = 0;
        for (j = 0; j < pState->numSegments; j++)
            cpwr64 += SCALE(ppDstAdaptiveCoefs[j][i].re * (Ipp64s)ppDstAdaptiveCoefs[j][i].re +
            ppDstAdaptiveCoefs[j][i].im * (Ipp64s)ppDstAdaptiveCoefs[j][i].im,PSD_SF);
//#endif
        cdlt64 = (cf_pwr[i] < cpwr64)? cpwr64 - cf_pwr[i] : cf_pwr[i] - cpwr64;

        cf_pwr[i] = Cnvrt_64s32s(WEIGHTED_INT(cf_pwr[i], cpwr64, (Ipp64s)coef_wcoef));
        fixed_pwr[i] = Cnvrt_64s32s(WEIGHTED_INT(fixed_pwr[i], fpwr64, (Ipp64s)coef_werr));
        adaptive_pwr[i] = Cnvrt_64s32s(WEIGHTED_INT(adaptive_pwr[i], apwr64, (Ipp64s)coef_werr));

        if (cf_pwr[i] > cdlt64 * 10 &&
            CMUL((Ipp64s)COEF_0_35, fixed_pwr[i]) > adaptive_pwr[i]) {
//#if _IPPXSC >= _IPPXSC_S2
//                ownCopyColMtrx32sc_S2(ppDstAdaptiveCoefs, ppDstFixedCoefs, pState->numSegments, i);
//#else
                for (j = 0; j < pState->numSegments; j++) {
                    ppDstFixedCoefs[j][i] = ppDstAdaptiveCoefs[j][i];
                }
                (*convergedNum)++;
//#endif
                fixed_pwr[i] = adaptive_pwr[i];
                powc = Add_32s(powc, fixed_pwr[i]);
            } else if (fixed_pwr[i] < CMUL((Ipp64s)COEF_0_66, adaptive_pwr[i])) {
//#if _IPPXSC >= _IPPXSC_S2
//                ownCopyColMtrx32sc_S2(ppDstFixedCoefs, ppDstAdaptiveCoefs, pState->numSegments, i);
//#else
                for (j = 0; j < pState->numSegments; j++) {
                    ppDstAdaptiveCoefs[j][i] = ppDstFixedCoefs[j][i];
                }
//#endif
                adaptive_pwr[i] = fixed_pwr[i];
                powd = Add_32s(powd, fixed_pwr[i]);
            }
            powa = Add_32s(powa, fixed_pwr[i]);
    }


    if (pState->sub_use) {
        if ((Ipp64s)10 * powc > powa && pState->sin_pwr > (Ipp64s)5 * powa) {
            pState->nlp_val = Cnvrt_64s32s(WEIGHTED_INT(pState->nlp_val, 0, pState->coef_nlp0));
        } else if ((Ipp64s)10 * powd >= powa) {
            pState->nlp_val = Cnvrt_64s32s(WEIGHTED_INT(pState->nlp_val, COEF_ONE, pState->coef_nlp0));
        }
    } else {
        if ((Ipp64s)10 * powc > powa) {
            pState->nlp_val = Cnvrt_64s32s(WEIGHTED_INT(pState->nlp_val, 0, pState->coef_nlp0));
        } else if ((Ipp64s)10 * powd >= powa) {
            pState->nlp_val = Cnvrt_64s32s(WEIGHTED_INT(pState->nlp_val, COEF_ONE, pState->coef_nlp0));
        }
    }

    if (pState->nlp_val >= COEF_0_5 || (powd < THRESH_POW && powc < THRESH_POW ) ) {
        uval = COEF_Q15_ONE;
    } else {
        uval = 0;
    }
    pState->sgain = Cnvrt_64s32s(WEIGHTED_INT(pState->sgain, uval, pState->coef_nlp1));

    *pDstSGain = Cnvrt_32s16s(pState->sgain);

    return ;
}
/* Process one frame */
int ec_isb_ProcessFrame(_isbECState *state, Ipp16s *rin, Ipp16s *sin, Ipp16s *sout)
{
    int i;
    int numSegments = state->numSegments;
    LOCAL_ALIGN_ARRAY(32,Ipp32sc,fira_err,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY(32,Ipp32sc,firf_err,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY(32,Ipp32sc,fira_out,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY(32,Ipp32sc,firf_out,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY(32,Ipp32s_EC_Sfs,pStepSize,SB_SUBBANDS*2,state);
    LOCAL_ALIGN_ARRAY(32,Ipp16s,pTimeFiltOut,SB_FFT_LEN,state);
    LOCAL_ALIGN_ARRAY(32,Ipp16s,pTimeError,SB_FFT_LEN,state);
    Ipp32sc* pSegment; /* Pointer to segment of input history */
    int iSegment;
    Ipp16s *pRinHistory = state->pRinHistory;
    int windowLen = state->windowLen;
    int frameSize = state->frameSize;

    /* Tone Disabler */
    if (TD_ENABLED)
        ec_isb_ToneDisabler(state, rin, sin);

    /* update receive-in subband history buffer */
    pSegment = state->ppRinSubbandHistory[state->numSegments-1];
    for(iSegment = state->numSegments-1; iSegment > 0; iSegment--)
    {
      state->ppRinSubbandHistory[iSegment] = state->ppRinSubbandHistory[iSegment-1];
    }
    state->ppRinSubbandHistory[0] = pSegment;

    /* update receive-in history buffer */
    for (i = 0; i < windowLen - frameSize; i++)
        pRinHistory[i] = pRinHistory[i + frameSize];

    for (i = 0; i < frameSize; i++)
        pRinHistory[i + windowLen - frameSize] = rin[i];

    /* Apply FFT to receive-in signal */
    ippsFFTFwd_RToCCS_16s32s_Sfs(pRinHistory, (Ipp32s *)(state->ppRinSubbandHistory[0]), state->spec_fft, 0, state->pBuf);

    /* Do filtering with fixed coeffs */
    ippsFIRSubband_EC_32sc_Sfs(state->ppRinSubbandHistory, state->ppFixedCoefs, firf_out,
        numSegments, state->numSubbands, F_SF);

    /* Get fixed filter output in time domain: apply inverse FFT */
    ippsFFTInv_CCSToR_32s16s_Sfs((Ipp32s *)firf_out, pTimeFiltOut, state->spec_fft, 0, state->pBuf);

    /* First half of error is set to zero. */
    for (i = 0; i < SB_FRAME_SIZE; i++)
        pTimeError[i] = 0;

    /* Subtract microphone signal from fixed filter output. */
    for (i = 0; i < SB_FRAME_SIZE; i++) {
        pTimeError[SB_FRAME_SIZE + i] = SAT_32S_16S(sin[i] - pTimeFiltOut[SB_FRAME_SIZE + i]);
        sout[i] = pTimeError[SB_FRAME_SIZE + i];
    }

    if (ADAPTATION_ENABLED) // Adaptation Enabled
    {
        int numSegAdapt = numSegments;

        /* Get fixed filter error in frequency domain */
        ippsFFTFwd_RToCCS_16s32s_Sfs(pTimeError, (Ipp32s *)firf_err, state->spec_fft, 0, state->pBuf);

        /* Do filtering with adaptive coeffs */
        ippsFIRSubband_EC_32sc_Sfs(state->ppRinSubbandHistory, state->ppAdaptiveCoefs, fira_out,
            numSegments, state->numSubbands, F_SF);
        /* Get adaptive filter output in time domain */
        ippsFFTInv_CCSToR_32s16s_Sfs((Ipp32s *)fira_out, pTimeFiltOut, state->spec_fft, 0, state->pBuf);

        /* Subtract microphone signal from adaptive filter output. */
        for (i = 0; i < SB_FRAME_SIZE; i++) {
            pTimeError[SB_FRAME_SIZE + i] = SAT_32S_16S(sin[i] - pTimeFiltOut[SB_FRAME_SIZE + i]);
        }

        ippsFFTFwd_RToCCS_16s32s_Sfs(pTimeError, (Ipp32s *)fira_err, state->spec_fft, 0, state->pBuf);

        /* Update subband controller */
        ippsSubbandControllerUpdate_EC_16s(rin, sin, (const Ipp32sc **)(state->ppRinSubbandHistory),
            0, pStepSize, state->controller);

        /* Update adaptive coeffs */
        ippsFIRSubbandCoeffUpdate_EC_32sc_I(pStepSize, (const Ipp32sc **)(state->ppRinSubbandHistory), fira_err,
            state->ppAdaptiveCoefs, numSegments, state->numSubbands, F_SF);


        if(state->mode & 8){ /* lite */
            /* Constrain coeffs each 10th frame. */
           if(!(state->constrainSubbandNum)) {
               for (i = 0; i < numSegments; i++) {
                  isb_ownConstrainCoeff(state,i);
               }
           }
           state->constrainSubbandNum++;
           if(state->constrainSubbandNum >= 10) state->constrainSubbandNum=0;
        }else{
            /* Constrain coeffs. */
            for (i = 0; i < numSegments; i++) {
                  isb_ownConstrainCoeff(state,i);
            }
        }

        /* Apply subband controller */
         ownSubbandController_EC_16s(fira_err, firf_err, state->ppAdaptiveCoefs, state->ppFixedCoefs,
            &state->sgain,(SubbandControllerState_EC_16s*) state->controller,&state->convergedSBNum);
         //state->convergedSBNum+=convergedSBNum;
         //if(state->convergedSBNum==state->subbandsNum){
         //    i=0;
         //}
        /* Apply NLP coeff */
       if (NLP_ENABLED)
            for (i = 0; i < frameSize; i++)
                sout[i] = SCALE((Ipp32s)sout[i] * state->sgain, 15);
    }

    LOCAL_ALIGN_ARRAY_FREE(32,Ipp16s,pTimeError,SB_FFT_LEN,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp16s,pTimeFiltOut,SB_FFT_LEN,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp32s_EC_Sfs,pStepSize,SB_SUBBANDS*2,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp32sc,firf_out,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp32sc,fira_out,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp32sc,firf_err,SB_SUBBANDS,state);
    LOCAL_ALIGN_ARRAY_FREE(32,Ipp32sc,fira_err,SB_SUBBANDS,state);


    return 0;
}