static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, const signed char *cdbk, int nbVec, int nbDim) { int i,j; spx_word32_t dist; spx_word16_t tmp; spx_word32_t best_dist=VERY_LARGE32; int best_id=0; const signed char *ptr=cdbk; for (i=0;i<nbVec;i++) { dist=0; for (j=0;j<nbDim;j++) { tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5)); dist=MAC16_32_Q15(dist,weight[j],MULT16_16(tmp,tmp)); } if (dist<best_dist) { best_dist=dist; best_id=i; } } for (j=0;j<nbDim;j++) x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5)); return best_id; }
void filter_mem2(const spx_sig_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem) { int i,j; spx_sig_t xi,yi,nyi; for (i=0;i<N;i++) { int xh,xl,yh,yl; xi=SATURATE(x[i],805306368); yi = SATURATE(ADD32(xi, SHL(mem[0],2)),805306368); nyi = -yi; xh = xi>>15; xl=xi&0x00007fff; yh = yi>>15; yl=yi&0x00007fff; for (j=0;j<ord-1;j++) { mem[j] = MAC16_32_Q15(MAC16_32_Q15(mem[j+1], num[j+1],xi), den[j+1],nyi); } mem[ord-1] = SUB32(MULT16_32_Q15(num[ord],xi), MULT16_32_Q15(den[ord],yi)); y[i] = yi; } }
void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem) { int i,j; spx_word32_t xi,yi; for (i=0;i<N;i++) { int xh,xl; xi=SATURATE(x[i],805306368); yi = xi + (mem[0]<<2); xh = xi>>15; xl=xi&0x00007fff; for (j=0;j<ord-1;j++) { mem[j] = MAC16_32_Q15(mem[j+1], num[j+1],xi); } mem[ord-1] = MULT16_32_Q15(num[ord],xi); y[i] = SATURATE(yi,805306368); } }
void iir_mem2(const spx_sig_t *x, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem) { int i,j; spx_word32_t xi,yi,nyi; for (i=0;i<N;i++) { int yh,yl; xi=SATURATE(x[i],805306368); yi = SATURATE(xi + (mem[0]<<2),805306368); nyi = -yi; yh = yi>>15; yl=yi&0x00007fff; for (j=0;j<ord-1;j++) { mem[j] = MAC16_32_Q15(mem[j+1],den[j+1],nyi); } mem[ord-1] = - MULT16_32_Q15(den[ord],yi); y[i] = yi; } }
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ static spx_word32_t pitch_gain_search_3tap( const spx_word16_t target[], /* Target vector */ const spx_coef_t ak[], /* LPCs for this subframe */ const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ spx_sig_t exc[], /* Excitation */ const signed char* gain_cdbk, int gain_cdbk_size, int pitch, /* Pitch value */ int p, /* Number of LPC coeffs */ int nsf, /* Number of samples in subframe */ SpeexBits* bits, char* stack, const spx_word16_t* exc2, const spx_word16_t* r, spx_word16_t* new_target, int* cdbk_index, int plc_tuning, spx_word32_t cumul_gain, int scaledown ) { int i, j; VARDECL(spx_word16_t * tmp1); VARDECL(spx_word16_t * e); spx_word16_t* x[3]; spx_word32_t corr[3]; spx_word32_t A[3][3]; spx_word16_t gain[3]; spx_word32_t err; spx_word16_t max_gain = 128; int best_cdbk = 0; ALLOC(tmp1, 3 * nsf, spx_word16_t); ALLOC(e, nsf, spx_word16_t); if (cumul_gain > 262144) max_gain = 31; x[0] = tmp1; x[1] = tmp1 + nsf; x[2] = tmp1 + 2 * nsf; for (j = 0; j < nsf; j++) new_target[j] = target[j]; { VARDECL(spx_mem_t * mm); int pp = pitch - 1; ALLOC(mm, p, spx_mem_t); for (j = 0; j < nsf; j++) { if (j - pp < 0) e[j] = exc2[j - pp]; else if (j - pp - pitch < 0) e[j] = exc2[j - pp - pitch]; else e[j] = 0; } #ifdef FIXED_POINT /* Scale target and excitation down if needed (avoiding overflow) */ if (scaledown) { for (j = 0; j < nsf; j++) e[j] = SHR16(e[j], 1); for (j = 0; j < nsf; j++) new_target[j] = SHR16(new_target[j], 1); } #endif for (j = 0; j < p; j++) mm[j] = 0; iir_mem16(e, ak, e, nsf, p, mm, stack); for (j = 0; j < p; j++) mm[j] = 0; filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack); for (j = 0; j < nsf; j++) x[2][j] = e[j]; } for (i = 1; i >= 0; i--) { spx_word16_t e0 = exc2[-pitch - 1 + i]; #ifdef FIXED_POINT /* Scale excitation down if needed (avoiding overflow) */ if (scaledown) e0 = SHR16(e0, 1); #endif x[i][0] = MULT16_16_Q14(r[0], e0); for (j = 0; j < nsf - 1; j++) x[i][j + 1] = ADD32(x[i + 1][j], MULT16_16_P14(r[j + 1], e0)); } for (i = 0; i < 3; i++) corr[i] = inner_prod(x[i], new_target, nsf); for (i = 0; i < 3; i++) for (j = 0; j <= i; j++) A[i][j] = A[j][i] = inner_prod(x[i], x[j], nsf); { spx_word32_t C[9]; #ifdef FIXED_POINT spx_word16_t C16[9]; #else spx_word16_t* C16 = C; #endif C[0] = corr[2]; C[1] = corr[1]; C[2] = corr[0]; C[3] = A[1][2]; C[4] = A[0][1]; C[5] = A[0][2]; C[6] = A[2][2]; C[7] = A[1][1]; C[8] = A[0][0]; /*plc_tuning *= 2;*/ if (plc_tuning < 2) plc_tuning = 2; if (plc_tuning > 30) plc_tuning = 30; #ifdef FIXED_POINT C[0] = SHL32(C[0], 1); C[1] = SHL32(C[1], 1); C[2] = SHL32(C[2], 1); C[3] = SHL32(C[3], 1); C[4] = SHL32(C[4], 1); C[5] = SHL32(C[5], 1); C[6] = MAC16_32_Q15(C[6], MULT16_16_16(plc_tuning, 655), C[6]); C[7] = MAC16_32_Q15(C[7], MULT16_16_16(plc_tuning, 655), C[7]); C[8] = MAC16_32_Q15(C[8], MULT16_16_16(plc_tuning, 655), C[8]); normalize16(C, C16, 32767, 9); #else C[6] *= .5 * (1 + .02 * plc_tuning); C[7] *= .5 * (1 + .02 * plc_tuning); C[8] *= .5 * (1 + .02 * plc_tuning); #endif best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain); #ifdef FIXED_POINT gain[0] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4]); gain[1] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 1]); gain[2] = ADD16(32, (spx_word16_t)gain_cdbk[best_cdbk * 4 + 2]); /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/ #else gain[0] = 0.015625 * gain_cdbk[best_cdbk * 4] + .5; gain[1] = 0.015625 * gain_cdbk[best_cdbk * 4 + 1] + .5; gain[2] = 0.015625 * gain_cdbk[best_cdbk * 4 + 2] + .5; #endif *cdbk_index = best_cdbk; } SPEEX_MEMSET(exc, 0, nsf); for (i = 0; i < 3; i++) { int j; int tmp1, tmp3; int pp = pitch + 1 - i; tmp1 = nsf; if (tmp1 > pp) tmp1 = pp; for (j = 0; j < tmp1; j++) exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp]); tmp3 = nsf; if (tmp3 > pp + pitch) tmp3 = pp + pitch; for (j = tmp1; j < tmp3; j++) exc[j] = MAC16_16(exc[j], SHL16(gain[2 - i], 7), exc2[j - pp - pitch]); } for (i = 0; i < nsf; i++) { spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0], x[2][i]), MULT16_16(gain[1], x[1][i])), MULT16_16(gain[2], x[0][i])); new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp, 6))); } err = inner_prod(new_target, new_target, nsf); return err; }
/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ static spx_word64_t pitch_gain_search_3tap( const spx_sig_t target[], /* Target vector */ const spx_coef_t ak[], /* LPCs for this subframe */ const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ spx_sig_t exc[], /* Excitation */ const void *par, int pitch, /* Pitch value */ int p, /* Number of LPC coeffs */ int nsf, /* Number of samples in subframe */ SpeexBits *bits, char *stack, const spx_sig_t *exc2, const spx_word16_t *r, spx_sig_t *new_target, int *cdbk_index, int cdbk_offset, int plc_tuning ) { int i,j; VARDECL(spx_sig_t *tmp1); VARDECL(spx_sig_t *tmp2); spx_sig_t *x[3]; spx_sig_t *e[3]; spx_word32_t corr[3]; spx_word32_t A[3][3]; int gain_cdbk_size; const signed char *gain_cdbk; spx_word16_t gain[3]; spx_word64_t err; const ltp_params *params; params = (const ltp_params*) par; gain_cdbk_size = 1<<params->gain_bits; gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset; ALLOC(tmp1, 3*nsf, spx_sig_t); ALLOC(tmp2, 3*nsf, spx_sig_t); x[0]=tmp1; x[1]=tmp1+nsf; x[2]=tmp1+2*nsf; e[0]=tmp2; e[1]=tmp2+nsf; e[2]=tmp2+2*nsf; for (i=2; i>=0; i--) { int pp=pitch+1-i; for (j=0; j<nsf; j++) { if (j-pp<0) e[i][j]=exc2[j-pp]; else if (j-pp-pitch<0) e[i][j]=exc2[j-pp-pitch]; else e[i][j]=0; } if (i==2) syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack); else { for (j=0; j<nsf-1; j++) x[i][j+1]=x[i+1][j]; x[i][0]=0; for (j=0; j<nsf; j++) { x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1)); } } } #ifdef FIXED_POINT { /* If using fixed-point, we need to normalize the signals first */ spx_word16_t *y[3]; VARDECL(spx_word16_t *ytmp); VARDECL(spx_word16_t *t); spx_sig_t max_val=1; int sig_shift; ALLOC(ytmp, 3*nsf, spx_word16_t); #if 0 ALLOC(y[0], nsf, spx_word16_t); ALLOC(y[1], nsf, spx_word16_t); ALLOC(y[2], nsf, spx_word16_t); #else y[0] = ytmp; y[1] = ytmp+nsf; y[2] = ytmp+2*nsf; #endif ALLOC(t, nsf, spx_word16_t); for (j=0; j<3; j++) { for (i=0; i<nsf; i++) { spx_sig_t tmp = x[j][i]; if (tmp<0) tmp = -tmp; if (tmp > max_val) max_val = tmp; } } for (i=0; i<nsf; i++) { spx_sig_t tmp = target[i]; if (tmp<0) tmp = -tmp; if (tmp > max_val) max_val = tmp; } sig_shift=0; while (max_val>16384) { sig_shift++; max_val >>= 1; } for (j=0; j<3; j++) { for (i=0; i<nsf; i++) { y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift)); } } for (i=0; i<nsf; i++) { t[i] = EXTRACT16(SHR32(target[i],sig_shift)); } for (i=0; i<3; i++) corr[i]=inner_prod(y[i],t,nsf); for (i=0; i<3; i++) for (j=0; j<=i; j++) A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf); } #else { for (i=0; i<3; i++) corr[i]=inner_prod(x[i],target,nsf); for (i=0; i<3; i++) for (j=0; j<=i; j++) A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf); } #endif { spx_word32_t C[9]; const signed char *ptr=gain_cdbk; int best_cdbk=0; spx_word32_t best_sum=0; C[0]=corr[2]; C[1]=corr[1]; C[2]=corr[0]; C[3]=A[1][2]; C[4]=A[0][1]; C[5]=A[0][2]; C[6]=A[2][2]; C[7]=A[1][1]; C[8]=A[0][0]; /*plc_tuning *= 2;*/ if (plc_tuning<2) plc_tuning=2; #ifdef FIXED_POINT C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]); C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]); C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]); #else C[0]*=1-.01*plc_tuning; C[1]*=1-.01*plc_tuning; C[2]*=1-.01*plc_tuning; C[6]*=.5*(1+.01*plc_tuning); C[7]*=.5*(1+.01*plc_tuning); C[8]*=.5*(1+.01*plc_tuning); #endif for (i=0; i<gain_cdbk_size; i++) { spx_word32_t sum=0; spx_word16_t g0,g1,g2; spx_word16_t pitch_control=64; spx_word16_t gain_sum; ptr = gain_cdbk+3*i; g0=ADD16((spx_word16_t)ptr[0],32); g1=ADD16((spx_word16_t)ptr[1],32); g2=ADD16((spx_word16_t)ptr[2],32); gain_sum = g1; if (g0>0) gain_sum += g0; if (g2>0) gain_sum += g2; if (gain_sum > 64) { gain_sum = SUB16(gain_sum, 64); if (gain_sum > 127) gain_sum = 127; #ifdef FIXED_POINT pitch_control = SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10))); #else pitch_control = 64*(1.-.001*plc_tuning*gain_sum); #endif if (pitch_control < 0) pitch_control = 0; } sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g0,pitch_control),C[0])); sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g1,pitch_control),C[1])); sum = ADD32(sum,MULT16_32_Q14(MULT16_16_16(g2,pitch_control),C[2])); sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g0,g1),C[3])); sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g1),C[4])); sum = SUB32(sum,MULT16_32_Q14(MULT16_16_16(g2,g0),C[5])); sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g0,g0),C[6])); sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g1,g1),C[7])); sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g2,g2),C[8])); /* We could force "safe" pitch values to handle packet loss better */ if (sum>best_sum || i==0) { best_sum=sum; best_cdbk=i; } } #ifdef FIXED_POINT gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]); gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]); gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]); /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/ #else gain[0] = 0.015625*gain_cdbk[best_cdbk*3] + .5; gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5; gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5; #endif *cdbk_index=best_cdbk; } #ifdef FIXED_POINT for (i=0; i<nsf; i++) exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])), MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2); err=0; for (i=0; i<nsf; i++) { spx_word16_t perr2; spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])), MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2); spx_sig_t perr=SUB32(target[i],tmp); new_target[i] = SUB32(target[i], tmp); perr2 = EXTRACT16(PSHR32(perr,15)); err = ADD64(err,MULT16_16(perr2,perr2)); } #else for (i=0; i<nsf; i++) exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i]; err=0; for (i=0; i<nsf; i++) { spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i]; new_target[i] = target[i] - tmp; err+=new_target[i]*new_target[i]; } #endif return err; }
/** Performs echo cancellation on a frame */ EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out) { int i,j, chan, speak; int N,M, C, K; spx_word32_t Syy,See,Sxx,Sdd, Sff; #ifdef TWO_PATH spx_word32_t Dbf; int update_foreground; #endif spx_word32_t Sey; spx_word16_t ss, ss_1; spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE; spx_float_t alpha, alpha_1; spx_word16_t RER; spx_word32_t tmp32; N = st->window_size; M = st->M; C = st->C; K = st->K; st->cancel_count++; #ifdef FIXED_POINT ss=DIV32_16(11469,M); ss_1 = SUB16(32767,ss); #else ss=.35/M; ss_1 = 1-ss; #endif for (chan = 0; chan < C; chan++) { /* Apply a notch filter to make sure DC doesn't end up causing problems */ filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C); /* Copy input data to buffer and apply pre-emphasis */ /* Copy input data to buffer */ for (i=0;i<st->frame_size;i++) { spx_word32_t tmp32; /* FIXME: This core has changed a bit, need to merge properly */ tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan]))); #ifdef FIXED_POINT if (tmp32 > 32767) { tmp32 = 32767; if (st->saturated == 0) st->saturated = 1; } if (tmp32 < -32767) { tmp32 = -32767; if (st->saturated == 0) st->saturated = 1; } #endif st->memD[chan] = st->input[chan*st->frame_size+i]; st->input[chan*st->frame_size+i] = EXTRACT16(tmp32); } } for (speak = 0; speak < K; speak++) { for (i=0;i<st->frame_size;i++) { spx_word32_t tmp32; st->x[speak*N+i] = st->x[speak*N+i+st->frame_size]; tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak]))); #ifdef FIXED_POINT /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */ if (tmp32 > 32767) { tmp32 = 32767; st->saturated = M+1; } if (tmp32 < -32767) { tmp32 = -32767; st->saturated = M+1; } #endif st->x[speak*N+i+st->frame_size] = EXTRACT16(tmp32); st->memX[speak] = far_end[i*K+speak]; } } for (speak = 0; speak < K; speak++) { /* Shift memory: this could be optimized eventually*/ for (j=M-1;j>=0;j--) { for (i=0;i<N;i++) st->X[(j+1)*N*K+speak*N+i] = st->X[j*N*K+speak*N+i]; } /* Convert x (echo input) to frequency domain */ spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]); } Sxx = 0; for (speak = 0; speak < K; speak++) { Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); power_spectrum_accum(st->X+speak*N, st->Xf, N); } Sff = 0; for (chan = 0; chan < C; chan++) { #ifdef TWO_PATH /* Compute foreground filter */ spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K); spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]); Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); #endif } /* Adjust proportional adaption rate */ /* FIXME: Adjust that for C, K*/ if (st->adapted) mdf_adjust_prop (st->W, N, M, C*K, st->prop); /* Compute weight gradient */ if (st->saturated == 0) { for (chan = 0; chan < C; chan++) { for (speak = 0; speak < K; speak++) { for (j=M-1;j>=0;j--) { weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N*K+speak*N], st->E+chan*N, st->PHI, N); for (i=0;i<N;i++) st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i]; } } } } else { st->saturated--; } /* FIXME: MC conversion required */ /* Update weight to prevent circular convolution (MDF / AUMDF) */ for (chan = 0; chan < C; chan++) { for (speak = 0; speak < K; speak++) { for (j=0;j<M;j++) { /* This is a variant of the Alternatively Updated MDF (AUMDF) */ /* Remove the "if" to make this an MDF filter */ if (j==0 || st->cancel_count%(M-1) == j-1) { #ifdef FIXED_POINT for (i=0;i<N;i++) st->wtmp2[i] = EXTRACT16(PSHR32(st->W[chan*N*K*M + j*N*K + speak*N + i],NORMALIZE_SCALEDOWN+16)); spx_ifft(st->fft_table, st->wtmp2, st->wtmp); for (i=0;i<st->frame_size;i++) { st->wtmp[i]=0; } for (i=st->frame_size;i<N;i++) { st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP); } spx_fft(st->fft_table, st->wtmp, st->wtmp2); /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */ for (i=0;i<N;i++) st->W[chan*N*K*M + j*N*K + speak*N + i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); #else spx_ifft(st->fft_table, &st->W[chan*N*K*M + j*N*K + speak*N], st->wtmp); for (i=st->frame_size;i<N;i++) { st->wtmp[i]=0; } spx_fft(st->fft_table, st->wtmp, &st->W[chan*N*K*M + j*N*K + speak*N]); #endif } } } } /* So we can use power_spectrum_accum */ for (i=0;i<=st->frame_size;i++) st->Rf[i] = st->Yf[i] = st->Xf[i] = 0; Dbf = 0; See = 0; #ifdef TWO_PATH /* Difference in response, this is used to estimate the variance of our residual power estimate */ for (chan = 0; chan < C; chan++) { spectral_mul_accum(st->X, st->W+chan*N*K*M, st->Y+chan*N, N, M*K); spx_ifft(st->fft_table, st->Y+chan*N, st->y+chan*N); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->e[chan*N+i+st->frame_size], st->y[chan*N+i+st->frame_size]); Dbf += 10+mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); See += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size); } #endif #ifndef TWO_PATH Sff = See; #endif #ifdef TWO_PATH /* Logic for updating the foreground filter */ /* For two time windows, compute the mean of the energy difference, as well as the variance */ st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See))); st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See))); st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf))); st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf))); /* Equivalent float code: st->Davg1 = .6*st->Davg1 + .4*(Sff-See); st->Davg2 = .85*st->Davg2 + .15*(Sff-See); st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf; st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf; */ update_foreground = 0; /* Check if we have a statistically significant reduction in the residual echo */ /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */ if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf))) update_foreground = 1; else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1)))) update_foreground = 1; else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2)))) update_foreground = 1; /* Do we update? */ if (update_foreground) { st->Davg1 = st->Davg2 = 0; st->Dvar1 = st->Dvar2 = FLOAT_ZERO; /* Copy background filter to foreground filter */ for (i=0;i<N*M*C*K;i++) st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16)); /* Apply a smooth transition so as to not introduce blocking artifacts */ for (chan = 0; chan < C; chan++) for (i=0;i<st->frame_size;i++) st->e[chan*N+i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[chan*N+i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[chan*N+i+st->frame_size]); } else { int reset_background=0; /* Otherwise, check if the background filter is significantly worse */ if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf)))) reset_background = 1; if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1))) reset_background = 1; if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2))) reset_background = 1; if (reset_background) { /* Copy foreground filter to background filter */ for (i=0;i<N*M*C*K;i++) st->W[i] = SHL32(EXTEND32(st->foreground[i]),16); /* We also need to copy the output so as to get correct adaptation */ for (chan = 0; chan < C; chan++) { for (i=0;i<st->frame_size;i++) st->y[chan*N+i+st->frame_size] = st->e[chan*N+i+st->frame_size]; for (i=0;i<st->frame_size;i++) st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->y[chan*N+i+st->frame_size]); } See = Sff; st->Davg1 = st->Davg2 = 0; st->Dvar1 = st->Dvar2 = FLOAT_ZERO; } } #endif Sey = Syy = Sdd = 0; for (chan = 0; chan < C; chan++) { /* Compute error signal (for the output with de-emphasis) */ for (i=0;i<st->frame_size;i++) { spx_word32_t tmp_out; #ifdef TWO_PATH tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->e[chan*N+i+st->frame_size])); #else tmp_out = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(st->y[chan*N+i+st->frame_size])); #endif tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE[chan]))); /* This is an arbitrary test for saturation in the microphone signal */ if (in[i*C+chan] <= -32000 || in[i*C+chan] >= 32000) { if (st->saturated == 0) st->saturated = 1; } out[i*C+chan] = WORD2INT(tmp_out); st->memE[chan] = tmp_out; } #ifdef DUMP_ECHO_CANCEL_DATA dump_audio(in, far_end, out, st->frame_size); #endif /* Compute error signal (filter update version) */ for (i=0;i<st->frame_size;i++) { st->e[chan*N+i+st->frame_size] = st->e[chan*N+i]; st->e[chan*N+i] = 0; } /* Compute a bunch of correlations */ /* FIXME: bad merge */ Sey += mdf_inner_prod(st->e+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); Syy += mdf_inner_prod(st->y+chan*N+st->frame_size, st->y+chan*N+st->frame_size, st->frame_size); Sdd += mdf_inner_prod(st->input+chan*st->frame_size, st->input+chan*st->frame_size, st->frame_size); /* Convert error to frequency domain */ spx_fft(st->fft_table, st->e+chan*N, st->E+chan*N); for (i=0;i<st->frame_size;i++) st->y[i+chan*N] = 0; spx_fft(st->fft_table, st->y+chan*N, st->Y+chan*N); /* Compute power spectrum of echo (X), error (E) and filter response (Y) */ power_spectrum_accum(st->E+chan*N, st->Rf, N); power_spectrum_accum(st->Y+chan*N, st->Yf, N); } /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/ /* Do some sanity check */ if (!(Syy>=0 && Sxx>=0 && See >= 0) #ifndef FIXED_POINT || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9) #endif ) { /* Things have gone really bad */ st->screwed_up += 50; for (i=0;i<st->frame_size*C;i++) out[i] = 0; } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6))) { /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */ st->screwed_up++; } else { /* Everything's fine */ st->screwed_up=0; } if (st->screwed_up>=50) { speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now."); speex_echo_state_reset(st); return; } /* Add a small noise floor to make sure not to have problems when dividing */ See = MAX32(See, SHR32(MULT16_16(N, 100),6)); for (speak = 0; speak < K; speak++) { Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size); power_spectrum_accum(st->X+speak*N, st->Xf, N); } /* Smooth far end energy estimate over time */ for (j=0;j<=st->frame_size;j++) st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]); /* Compute filtered spectra and (cross-)correlations */ for (j=st->frame_size;j>=0;j--) { spx_float_t Eh, Yh; Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]); Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]); Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh)); Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh)); #ifdef FIXED_POINT st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]); st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]); #else st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j]; st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j]; #endif } Pyy = FLOAT_SQRT(Pyy); Pey = FLOAT_DIVU(Pey,Pyy); /* Compute correlation updatete rate */ tmp32 = MULT16_32_Q15(st->beta0,Syy); if (tmp32 > MULT16_32_Q15(st->beta_max,See)) tmp32 = MULT16_32_Q15(st->beta_max,See); alpha = FLOAT_DIV32(tmp32, See); alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha); /* Update correlations (recursive average) */ st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey)); st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy)); if (FLOAT_LT(st->Pyy, FLOAT_ONE)) st->Pyy = FLOAT_ONE; /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */ if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy))) st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy); if (FLOAT_GT(st->Pey, st->Pyy)) st->Pey = st->Pyy; /* leak_estimate is the linear regression result */ st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14)); /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */ if (st->leak_estimate > 16383) st->leak_estimate = 32767; else st->leak_estimate = SHL16(st->leak_estimate,1); /*printf ("%f\n", st->leak_estimate);*/ /* Compute Residual to Error Ratio */ #ifdef FIXED_POINT tmp32 = MULT16_32_Q15(st->leak_estimate,Syy); tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1))); /* Check for y in e (lower bound on RER) */ { spx_float_t bound = PSEUDOFLOAT(Sey); bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy))); if (FLOAT_GT(bound, PSEUDOFLOAT(See))) tmp32 = See; else if (tmp32 < FLOAT_EXTRACT32(bound)) tmp32 = FLOAT_EXTRACT32(bound); } if (tmp32 > SHR32(See,1)) tmp32 = SHR32(See,1); RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15)); #else RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See; /* Check for y in e (lower bound on RER) */ if (RER < Sey*Sey/(1+See*Syy)) RER = Sey*Sey/(1+See*Syy); if (RER > .5) RER = .5; #endif /* We consider that the filter has had minimal adaptation if the following is true*/ if (!st->adapted && st->sum_adapt > SHL32(EXTEND32(M),15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy)) { st->adapted = 1; } if (st->adapted) { /* Normal learning rate calculation once we're past the minimal adaptation phase */ for (i=0;i<=st->frame_size;i++) { spx_word32_t r, e; /* Compute frequency-domain adaptation mask */ r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3)); e = SHL32(st->Rf[i],3)+1; #ifdef FIXED_POINT if (r>SHR32(e,1)) r = SHR32(e,1); #else if (r>.5*e) r = .5*e; #endif r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e))); /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/ st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16); } } else { /* Temporary adaption rate if filter is not yet adapted enough */ spx_word16_t adapt_rate=0; if (Sxx > SHR32(MULT16_16(N, 1000),6)) { tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx); #ifdef FIXED_POINT if (tmp32 > SHR32(See,2)) tmp32 = SHR32(See,2); #else if (tmp32 > .25*See) tmp32 = .25*See; #endif adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15)); } for (i=0;i<=st->frame_size;i++) st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1); /* How much have we adapted so far? */ st->sum_adapt = ADD32(st->sum_adapt,adapt_rate); } /* FIXME: MC conversion required */ for (i=0;i<st->frame_size;i++) st->last_y[i] = st->last_y[st->frame_size+i]; if (st->adapted) { /* If the filter is adapted, take the filtered echo */ for (i=0;i<st->frame_size;i++) st->last_y[st->frame_size+i] = in[i]-out[i]; } else { /* If filter isn't adapted yet, all we can do is take the far end signal directly */ /* moved earlier: for (i=0;i<N;i++) st->last_y[i] = st->x[i];*/ } }