void pitch_unquant_3tap( spx_word16_t exc[], /* Input excitation */ spx_word32_t exc_out[], /* Output excitation */ int start, /* Smallest pitch value allowed */ int end, /* Largest pitch value allowed */ spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ const void *par, int nsf, /* Number of samples in subframe */ int *pitch_val, spx_word16_t *gain_val, SpeexBits *bits, char *stack, int count_lost, int subframe_offset, spx_word16_t last_pitch_gain, int cdbk_offset ) { int i; int pitch; int gain_index; spx_word16_t gain[3]; const signed char *gain_cdbk; int gain_cdbk_size; const ltp_params *params; params = (const ltp_params*) par; gain_cdbk_size = 1<<params->gain_bits; gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits); pitch += start; gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits); /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/ #ifdef FIXED_POINT gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]); gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]); gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]); #else gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5; gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5; gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5; #endif if (count_lost && pitch > subframe_offset) { spx_word16_t gain_sum; if (1) { #ifdef FIXED_POINT spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1); if (tmp>62) tmp=62; #else spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain; if (tmp>.95) tmp=.95; #endif gain_sum = gain_3tap_to_1tap(gain); if (gain_sum > tmp) { spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum); for (i=0;i<3;i++) gain[i]=MULT16_16_Q14(fact,gain[i]); } } } *pitch_val = pitch; gain_val[0]=gain[0]; gain_val[1]=gain[1]; gain_val[2]=gain[2]; gain[0] = SHL16(gain[0],7); gain[1] = SHL16(gain[1],7); gain[2] = SHL16(gain[2],7); SPEEX_MEMSET(exc_out, 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_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]); tmp3=nsf; if (tmp3>pp+pitch) tmp3=pp+pitch; for (j=tmp1;j<tmp3;j++) exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]); } /*for (i=0;i<nsf;i++) exc[i]=PSHR32(exc32[i],13);*/ }
/** 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]; { int bound; VARDECL(spx_mem_t *mm); int pp=pitch-1; ALLOC(mm, p, spx_mem_t); bound = nsf; if (nsf-pp>0) bound = pp; for (j=0;j<bound;j++) e[j]=exc2[j-pp]; bound = nsf; if (nsf-pp-pitch>0) bound = pp+pitch; for (;j<bound;j++) e[j]=exc2[j-pp-pitch]; for (;j<nsf;j++) 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; filter10(e, awk1, awk2, e, nsf, 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 */ int pitch_search_3tap( spx_word16_t target[], /* Target vector */ spx_word16_t *sw, spx_coef_t ak[], /* LPCs for this subframe */ spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */ spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */ spx_sig_t exc[], /* Excitation */ const void *par, int start, /* Smallest pitch value allowed */ int end, /* Largest pitch value allowed */ spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ int p, /* Number of LPC coeffs */ int nsf, /* Number of samples in subframe */ SpeexBits *bits, char *stack, spx_word16_t *exc2, spx_word16_t *r, int complexity, int cdbk_offset, int plc_tuning, spx_word32_t *cumul_gain ) { int i; int cdbk_index, pitch=0, best_gain_index=0; VARDECL(spx_sig_t *best_exc); VARDECL(spx_word16_t *new_target); VARDECL(spx_word16_t *best_target); int best_pitch=0; spx_word32_t err, best_err=-1; int N; const ltp_params *params; const signed char *gain_cdbk; int gain_cdbk_size; int scaledown=0; VARDECL(int *nbest); params = (const ltp_params*) par; gain_cdbk_size = 1<<params->gain_bits; gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; N=complexity; if (N>10) N=10; if (N<1) N=1; ALLOC(nbest, N, int); params = (const ltp_params*) par; if (end<start) { speex_bits_pack(bits, 0, params->pitch_bits); speex_bits_pack(bits, 0, params->gain_bits); SPEEX_MEMSET(exc, 0, nsf); return start; } #ifdef FIXED_POINT /* Check if we need to scale everything down in the pitch search to avoid overflows */ for (i=0;i<nsf;i++) { if (ABS16(target[i])>16383) { scaledown=1; break; } } for (i=-end;i<0;i++) { if (ABS16(exc2[i])>16383) { scaledown=1; break; } } #endif if (N>end-start+1) N=end-start+1; if (end != start) open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack); else nbest[0] = start; ALLOC(best_exc, nsf, spx_sig_t); ALLOC(new_target, nsf, spx_word16_t); ALLOC(best_target, nsf, spx_word16_t); for (i=0;i<N;i++) { pitch=nbest[i]; SPEEX_MEMSET(exc, 0, nsf); err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf, bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain, scaledown); if (err<best_err || best_err<0) { SPEEX_COPY(best_exc, exc, nsf); SPEEX_COPY(best_target, new_target, nsf); best_err=err; best_pitch=pitch; best_gain_index=cdbk_index; } } /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/ speex_bits_pack(bits, best_pitch-start, params->pitch_bits); speex_bits_pack(bits, best_gain_index, params->gain_bits); #ifdef FIXED_POINT *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain)); #else *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3]; #endif /*printf ("%f\n", cumul_gain);*/ /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/ SPEEX_COPY(exc, best_exc, nsf); SPEEX_COPY(target, best_target, nsf); #ifdef FIXED_POINT /* Scale target back up if needed */ if (scaledown) { for (i=0;i<nsf;i++) target[i]=SHL16(target[i],1); } #endif return pitch; }
void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack) { int i,j,k; VARDECL(spx_word32_t *best_score); VARDECL(spx_word32_t *best_ener); spx_word32_t e0; VARDECL(spx_word32_t *corr); #ifdef FIXED_POINT /* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16) arrays for (normalized) 16-bit values */ VARDECL(spx_word16_t *corr16); VARDECL(spx_word16_t *ener16); spx_word32_t *energy; int cshift=0, eshift=0; int scaledown = 0; ALLOC(corr16, end-start+1, spx_word16_t); ALLOC(ener16, end-start+1, spx_word16_t); ALLOC(corr, end-start+1, spx_word32_t); energy = corr; #else /* In floating-point, we need to float arrays and no normalized copies */ VARDECL(spx_word32_t *energy); spx_word16_t *corr16; spx_word16_t *ener16; ALLOC(energy, end-start+2, spx_word32_t); ALLOC(corr, end-start+1, spx_word32_t); corr16 = corr; ener16 = energy; #endif ALLOC(best_score, N, spx_word32_t); ALLOC(best_ener, N, spx_word32_t); for (i=0;i<N;i++) { best_score[i]=-1; best_ener[i]=0; pitch[i]=start; } #ifdef FIXED_POINT for (i=-end;i<len;i++) { if (ABS16(sw[i])>16383) { scaledown=1; break; } } /* If the weighted input is close to saturation, then we scale it down */ if (scaledown) { for (i=-end;i<len;i++) { sw[i]=SHR16(sw[i],1); } } #endif energy[0]=inner_prod(sw-start, sw-start, len); e0=inner_prod(sw, sw, len); for (i=start;i<end;i++) { /* Update energy for next pitch*/ energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6)); if (energy[i-start+1] < 0) energy[i-start+1] = 0; } #ifdef FIXED_POINT eshift = normalize16(energy, ener16, 32766, end-start+1); #endif /* In fixed-point, this actually overrites the energy array (aliased to corr) */ pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack); #ifdef FIXED_POINT /* Normalize to 180 so we can square it and it still fits in 16 bits */ cshift = normalize16(corr, corr16, 180, end-start+1); /* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */ if (scaledown) { for (i=-end;i<len;i++) { sw[i]=SHL16(sw[i],1); } } #endif /* Search for the best pitch prediction gain */ for (i=start;i<=end;i++) { spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]); /* Instead of dividing the tmp by the energy, we multiply on the other side */ if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start]))) { /* We can safely put it last and then check */ best_score[N-1]=tmp; best_ener[N-1]=ener16[i-start]+1; pitch[N-1]=i; /* Check if it comes in front of others */ for (j=0;j<N-1;j++) { if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start]))) { for (k=N-1;k>j;k--) { best_score[k]=best_score[k-1]; best_ener[k]=best_ener[k-1]; pitch[k]=pitch[k-1]; } best_score[j]=tmp; best_ener[j]=ener16[i-start]+1; pitch[j]=i; break; } } } } /* Compute open-loop gain if necessary */ if (gain) { for (j=0;j<N;j++) { spx_word16_t g; i=pitch[j]; g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6)); /* FIXME: g = max(g,corr/energy) */ if (g<0) g = 0; gain[j]=g; } } }
void pitch_unquant_3tap( spx_sig_t exc[], /* Excitation */ int start, /* Smallest pitch value allowed */ int end, /* Largest pitch value allowed */ spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */ const void *par, int nsf, /* Number of samples in subframe */ int *pitch_val, spx_word16_t *gain_val, SpeexBits *bits, char *stack, int count_lost, int subframe_offset, spx_word16_t last_pitch_gain, int cdbk_offset ) { int i; int pitch; int gain_index; spx_word16_t gain[3]; const signed char *gain_cdbk; int gain_cdbk_size; 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; pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits); pitch += start; gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits); /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/ #ifdef FIXED_POINT gain[0] = 32+(spx_word16_t)gain_cdbk[gain_index*3]; gain[1] = 32+(spx_word16_t)gain_cdbk[gain_index*3+1]; gain[2] = 32+(spx_word16_t)gain_cdbk[gain_index*3+2]; #else gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5; gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5; gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5; #endif if (count_lost && pitch > subframe_offset) { float gain_sum; if (1) { float tmp = count_lost < 4 ? GAIN_SCALING_1*last_pitch_gain : 0.4 * GAIN_SCALING_1 * last_pitch_gain; if (tmp>.95) tmp=.95; gain_sum = GAIN_SCALING_1*gain_3tap_to_1tap(gain); if (gain_sum > tmp) { float fact = tmp/gain_sum; for (i=0; i<3; i++) gain[i]*=fact; } } } *pitch_val = pitch; gain_val[0]=gain[0]; gain_val[1]=gain[1]; gain_val[2]=gain[2]; { spx_sig_t *e[3]; VARDECL(spx_sig_t *tmp2); ALLOC(tmp2, 3*nsf, spx_sig_t); e[0]=tmp2; e[1]=tmp2+nsf; e[2]=tmp2+2*nsf; for (i=0; i<3; i++) { int j; int pp=pitch+1-i; #if 0 for (j=0; j<nsf; j++) { if (j-pp<0) e[i][j]=exc[j-pp]; else if (j-pp-pitch<0) e[i][j]=exc[j-pp-pitch]; else e[i][j]=0; } #else { int tmp1, tmp3; tmp1=nsf; if (tmp1>pp) tmp1=pp; for (j=0; j<tmp1; j++) e[i][j]=exc[j-pp]; tmp3=nsf; if (tmp3>pp+pitch) tmp3=pp+pitch; for (j=tmp1; j<tmp3; j++) e[i][j]=exc[j-pp-pitch]; for (j=tmp3; j<nsf; j++) e[i][j]=0; } #endif } #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); } #else for (i=0; i<nsf; i++) exc[i]=VERY_SMALL+gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i]; #endif } }
/** 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];*/ } }
EXPORT SpeexEchoState *speex_echo_state_init_mc(int frame_size, int filter_length, int nb_mic, int nb_speakers) { int i,N,M, C, K; SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState)); st->K = nb_speakers; st->C = nb_mic; C=st->C; K=st->K; #ifdef DUMP_ECHO_CANCEL_DATA if (rFile || pFile || oFile) speex_fatal("Opening dump files twice"); rFile = fopen("aec_rec.sw", "wb"); pFile = fopen("aec_play.sw", "wb"); oFile = fopen("aec_out.sw", "wb"); #endif st->frame_size = frame_size; st->window_size = 2*frame_size; N = st->window_size; M = st->M = (filter_length+st->frame_size-1)/frame_size; st->cancel_count=0; st->sum_adapt = 0; st->saturated = 0; st->screwed_up = 0; /* This is the default sampling rate */ st->sampling_rate = 8000; st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate); #ifdef FIXED_POINT st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate); st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate); #else st->beta0 = (2.0f*st->frame_size)/st->sampling_rate; st->beta_max = (.5f*st->frame_size)/st->sampling_rate; #endif st->leak_estimate = 0; st->fft_table = spx_fft_init(N); st->e = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); st->x = (spx_word16_t*)speex_alloc(K*N*sizeof(spx_word16_t)); st->input = (spx_word16_t*)speex_alloc(C*st->frame_size*sizeof(spx_word16_t)); st->y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); st->last_y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t)); st->X = (spx_word16_t*)speex_alloc(K*(M+1)*N*sizeof(spx_word16_t)); st->Y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); st->E = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t)); st->W = (spx_word32_t*)speex_alloc(C*K*M*N*sizeof(spx_word32_t)); #ifdef TWO_PATH st->foreground = (spx_word16_t*)speex_alloc(M*N*C*K*sizeof(spx_word16_t)); #endif st->PHI = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t)); st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t)); st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t)); st->window = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); st->prop = (spx_word16_t*)speex_alloc(M*sizeof(spx_word16_t)); st->wtmp = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); #ifdef FIXED_POINT st->wtmp2 = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t)); for (i=0;i<N>>1;i++) { st->window[i] = (16383-SHL16(spx_cos(DIV32_16(MULT16_16(25736,i<<1),N)),1)); st->window[N-i-1] = st->window[i]; } #else for (i=0;i<N;i++) st->window[i] = .5-.5*cos(2*M_PI*i/N); #endif for (i=0;i<=st->frame_size;i++) st->power_1[i] = FLOAT_ONE; for (i=0;i<N*M*K*C;i++) st->W[i] = 0; { spx_word32_t sum = 0; /* Ratio of ~10 between adaptation rate of first and last block */ spx_word16_t decay = SHR32(spx_exp(NEG16(DIV32_16(QCONST16(2.4,11),M))),1); st->prop[0] = QCONST16(.7, 15); sum = EXTEND32(st->prop[0]); for (i=1;i<M;i++) { st->prop[i] = MULT16_16_Q15(st->prop[i-1], decay); sum = ADD32(sum, EXTEND32(st->prop[i])); } for (i=M-1;i>=0;i--) { st->prop[i] = DIV32(MULT16_16(QCONST16(.8f,15), st->prop[i]),sum); } } st->memX = (spx_word16_t*)speex_alloc(K*sizeof(spx_word16_t)); st->memD = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t)); st->memE = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t)); st->preemph = QCONST16(.9,15); if (st->sampling_rate<12000) st->notch_radius = QCONST16(.9, 15); else if (st->sampling_rate<24000) st->notch_radius = QCONST16(.982, 15); else st->notch_radius = QCONST16(.992, 15); st->notch_mem = (spx_mem_t*)speex_alloc(2*C*sizeof(spx_mem_t)); st->adapted = 0; st->Pey = st->Pyy = FLOAT_ONE; #ifdef TWO_PATH st->Davg1 = st->Davg2 = 0; st->Dvar1 = st->Dvar2 = FLOAT_ZERO; #endif st->play_buf = (spx_int16_t*)speex_alloc(K*(PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t)); st->play_buf_pos = PLAYBACK_DELAY*st->frame_size; st->play_buf_started = 0; return st; }