static inline spx_word32_t compute_pitch_error(spx_word16_t* C, spx_word16_t* g, spx_word16_t pitch_control) { spx_word32_t sum = 0; sum = ADD32(sum, MULT16_16(MULT16_16_16(g[0], pitch_control), C[0])); sum = ADD32(sum, MULT16_16(MULT16_16_16(g[1], pitch_control), C[1])); sum = ADD32(sum, MULT16_16(MULT16_16_16(g[2], pitch_control), C[2])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[0], g[1]), C[3])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[1]), C[4])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[0]), C[5])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[0], g[0]), C[6])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[1], g[1]), C[7])); sum = SUB32(sum, MULT16_16(MULT16_16_16(g[2], g[2]), C[8])); return sum; }
/** 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; }
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; } } }
/** 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; }
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); VARDECL(spx_word32_t *energy); ALLOC(best_score, N, spx_word32_t); ALLOC(best_ener, N, spx_word32_t); ALLOC(corr, end-start+1, spx_word32_t); ALLOC(energy, end-start+2, spx_word32_t); for (i=0;i<N;i++) { best_score[i]=-1; best_ener[i]=0; pitch[i]=start; } 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; } pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack); /* FIXME: Fixed-point and floating-point code should be merged */ #ifdef FIXED_POINT { VARDECL(spx_word16_t *corr16); VARDECL(spx_word16_t *ener16); ALLOC(corr16, end-start+1, spx_word16_t); ALLOC(ener16, end-start+1, spx_word16_t); /* Normalize to 180 so we can square it and it still fits in 16 bits */ normalize16(corr, corr16, 180, end-start+1); normalize16(energy, ener16, 180, end-start+1); 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; } } } } } #else for (i=start;i<=end;i++) { float tmp = corr[i-start]*corr[i-start]; if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start])) { for (j=0;j<N;j++) { if (tmp*best_ener[j]>best_score[j]*(1+energy[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]=energy[i-start]+1; pitch[j]=i; break; } } } } #endif /* Compute open-loop gain */ if (gain) { for (j=0;j<N;j++) { spx_word16_t g; i=pitch[j]; g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6)); /* FIXME: g = max(g,corr/energy) */ if (g<0) g = 0; gain[j]=g; } } }