static void cor_h_x_e( Word16 h[], /* (i) Q12 : impulse response of weighted synthesis filter */ Word16 x[], /* (i) Q0 : correlation between target and h[] */ Word16 dn[] /* (o) Q0 : correlation between target and h[] */ ) { Word16 i, j, k; Word32 s, y32[L_SUBFR], max, tot, L_tmp; /* first keep the result on 32 bits and find absolute maximum */ tot = 5; for (k=0; k<NB_TRACK; k++) { max = 0; for (i=k; i<L_SUBFR; i+=STEP) { s = 0; for (j=i; j<L_SUBFR; j++) s = L_mac(s, x[j], h[j-i]); y32[i] = s; s = L_abs(s); L_tmp = L_sub(s, max); if (L_tmp > (Word32)0) max = s; } tot = L_add(tot, L_shr(max, 1)); /* tot += (2.0 x max) / 4.0 */ } /* Find the number of right shifts to do on y32[] so that */ /* 2.0 x sumation of all max of dn[] in each track not saturate. */ j = sub(norm_l(tot), 2); /* multiply tot by 4 */ for (i=0; i<L_SUBFR; i++) { dn[i] = round(L_shl(y32[i], j)); } return; }
/*-------------------------------------------------------------------* * Function set_sign() * * ~~~~~~~~~~~~~~~~~~~~ * * Set the sign of each pulse position. * *-------------------------------------------------------------------*/ static void set_sign( Word16 fac_cn, /* (i) Q15: residual weight for sign determination */ Word16 cn[], /* (i) Q0 : residual after long term prediction */ Word16 dn[], /* (i) Q0 : correlation between target and h[] */ Word16 sign[], /* (o) Q15: sign vector (sign of each position) */ Word16 inv_sign[], /* (o) Q15: inverse of sign[] */ Word16 pos_max[], /* (o) : pos of max of correlation */ Word32 corr[] /* (o) : correlation of each track */ ) { Word16 i, k, pos, k_cn, k_dn, val; Word32 s, max; /* calculate energy for normalization of cn[] and dn[] */ s = 0; for (i=0; i<L_SUBFR; i++) s = L_mac(s, cn[i], cn[i]); if (s < 512) s = 512; s = Inv_sqrt(s); k_cn = extract_h(L_shl(s, 5)); /* k_cn = 11..23170 */ k_cn = mult(k_cn, fac_cn); s = 0; for (i=0; i<L_SUBFR; i++) s = L_mac(s, dn[i], dn[i]); if (s < 512) s = 512; s = Inv_sqrt(s); k_dn = extract_h(L_shl(s, 5)); /* k_dn = 11..23170 */ /* set sign according to en[] = k_cn*cn[] + k_dn*dn[] */ /* find position of maximum of correlation in each track */ for (k=0; k<NB_TRACK; k++) { max = -1; for (i=k; i<L_SUBFR; i+=STEP) { val = dn[i]; s = L_mac(L_mult(k_cn, cn[i]), k_dn, val); if (s >= 0) { sign[i] = 32767L; /* sign = +1 (Q15) */ inv_sign[i] = -32768L; } else { sign[i] = -32768L; /* sign = -1 (Q15) */ inv_sign[i] = 32767L; val = negate(val); } dn[i] = val; /* modify dn[] according to the fixed sign */ s = L_abs(s); if (s > max) { max = s; pos = i; } } pos_max[k] = pos; corr[k] = max; } return; }
void Cor_h_X( Word16 h[], /* (i) Q12 :Impulse response of filters */ Word16 X[], /* (i) :Target vector */ Word16 D[] /* (o) :Correlations between h[] and D[] */ /* Normalized to 13 bits */ ) { Word16 i, j; Word32 s, max, L_temp; Word32 y32[L_SUBFR]; /* first keep the result on 32 bits and find absolute maximum */ max = 0; for (i = 0; i < L_SUBFR; i++) { s = 0; for (j = i; j < L_SUBFR; j++) s = L_mac(s, X[j], h[j-i]); y32[i] = s; s = L_abs(s); L_temp =L_sub(s,max); if(L_temp>0L) { max = s; } } /* Find the number of right shifts to do on y32[] */ /* so that maximum is on 13 bits */ j = norm_l(max); if( sub(j,16) > 0) { j = 16; } j = sub(18, j); if(j>=0) { for(i=0; i<L_SUBFR; i++) { D[i] = extract_l( L_shr(y32[i], j) ); } } else { Word16 pj = abs_s(j); for(i=0; i<L_SUBFR; i++) { D[i] = extract_l( L_shr(y32[i], pj) ); } } return; }
Word16 coarsepitch( Word16 *xw, /* (i) (normalized) weighted signal */ struct BV16_Encoder_State *cstate) /* (i/o) Coder State */ { Word16 s; /* Q2 */ Word16 a, b; Word16 im; Word16 maxdev, flag, mpflag; Word32 eni, deltae; Word32 cc; Word16 ah,al, bh, bl; Word32 a0, a1, a2, a3; Word32 *lp0; Word16 exp, new_exp; Word16 *fp0, *fp1, *fp2, *fp3, *sp; Word16 *fp1_h, *fp1_l, *fp2_h, *fp2_l; Word16 cor2max, cor2max_exp; Word16 cor2m, cor2m_exp; Word16 s0, t0, t1, exp0, exp1, e2, e3; Word16 threshold; Word16 mplth; /* Q2 */ Word16 i, j, k, n, npeaks, imax, idx[MAXPPD-MINPPD+1]; Word16 cpp; Word16 plag[HMAXPPD], cor2[MAXPPD1], cor2_exp[MAXPPD1]; Word16 cor2i[HMAXPPD], cor2i_exp[HMAXPPD], xwd[LXD]; Word16 tmp_h[DFO+FRSZ], tmp_l[DFO+FRSZ]; /* DPF Q7 */ Word32 cor[MAXPPD1], energy[MAXPPD1], lxwd[FRSZD]; Word16 energy_man[MAXPPD1], energy_exp[MAXPPD1]; Word16 energyi_man[HMAXPPD], energyi_exp[HMAXPPD]; Word16 energym_man, energym_exp; Word16 energymax_man, energymax_exp; /* Lowpass filter xw() to 800 hz; shift & output into xwd() */ /* AP and AZ filtering and decimation */ fp1_h = tmp_h + DFO; fp1_l = tmp_l + DFO; sp = xw; a1 = 1; for (i=0;i<DFO;i++) tmp_h[i] = cstate->dfm_h[2*i+1]; for (i=0;i<DFO;i++) tmp_l[i] = cstate->dfm_h[2*i]; lp0 = lxwd; for (i=0;i<FRSZD;i++) { for (k=0;k<DECF;k++) { a0 = L_shr(L_deposit_h(*sp++),10); fp2_h = fp1_h-1; fp2_l = fp1_l-1; for (j=0;j<DFO;j++) a0=L_sub(a0,Mpy_32(*fp2_h--,*fp2_l--,adf_h[j+1],adf_l[j+1])); a0 = L_shl(a0, 2); /* adf Q13 */ L_Extract(a0, fp1_h++, fp1_l++); } fp2_h = fp1_h-1; fp2_l = fp1_l-1; a0 = Mpy_32_16(*fp2_h--, *fp2_l--, bdf[0]); for (j=0;j<DFO;j++) a0=L_add(a0,Mpy_32_16(*fp2_h--,*fp2_l--,bdf[j+1])); *lp0++ = a0; a0 = L_abs(a0); if (a1 < a0) a1 = a0; } /* copy temp buffer to memory */ fp1_h -= DFO; fp1_l -= DFO; for (i=0;i<DFO;i++) { cstate->dfm_h[2*i+1] = fp1_h[i]; cstate->dfm_h[2*i] = fp1_l[i]; } lp0 = lxwd; new_exp = sub(norm_l(a1), 3); /* headroom to avoid overflow */ exp = sub(cstate->xwd_exp,new_exp); /* increase in bit-resolution */ if (exp < 0) { /* Descending signal level */ new_exp = cstate->xwd_exp; exp = 0; } for (i=0;i<XDOFF;i++) xwd[i] = shr(cstate->xwd[i], exp); /* fill-in new exponent */ fp0 = xwd + XDOFF; for (i=0;i<FRSZD;i++) fp0[i] = intround(L_shl(lp0[i],new_exp)); /* update signal memory for next frame */ exp0 = 1; for (i=0;i<XDOFF;i++) { exp1 = abs_s(xwd[FRSZD+i]); if (exp1 > exp0) exp0 = exp1; } exp0 = sub(norm_s(exp0),3); /* extra exponent for next frame */ exp = sub(exp0, exp); if (exp >=0) { for (i=0;i<XDOFF-FRSZD;i++) cstate->xwd[i] = shl(cstate->xwd[i+FRSZD], exp); } else { exp = -exp; if (exp >=15) exp = 15; for (i=0;i<XDOFF-FRSZD;i++) cstate->xwd[i] = shr(cstate->xwd[i+FRSZD], exp); } for (;i<XDOFF;i++) cstate->xwd[i] = shl(xwd[FRSZD+i],exp0); cstate->xwd_exp = add(new_exp, exp0); /* Compute correlation & energy of prediction basis vector */ /* reset local buffers */ for (i=0;i<MAXPPD1;i++) cor[i] = energy[i] = 0; fp0 = xwd+MAXPPD1; fp1 = xwd+MAXPPD1-M1; a0 = a1 = 0; for (i=0;i<(LXD-MAXPPD1);i++) { a0 = L_mac0(a0, *fp1, *fp1); a1 = L_mac0(a1, *fp0++, *fp1++); } cor[M1-1] = a1; energy[M1-1] = a0; energy_exp[M1-1] = norm_l(energy[M1-1]); energy_man[M1-1] = extract_h(L_shl(energy[M1-1], energy_exp[M1-1])); s0 = cor2_exp[M1-1] = norm_l(a1); t0 = extract_h(L_shl(a1, s0)); cor2[M1-1] = extract_h(L_mult(t0, t0)); if (a1 < 0) cor2[M1-1] = negate(cor2[M1-1]); fp2 = xwd+LXD-M1-1; fp3 = xwd+MAXPPD1-M1-1; for (i=M1;i<M2;i++) { fp0 = xwd+MAXPPD1; fp1 = xwd+MAXPPD1-i-1; a1 = 0; for (j=0;j<(LXD-MAXPPD1);j++) a1 = L_mac0(a1,*fp0++,*fp1++); cor[i] = a1; a0 = L_msu0(a0, *fp2, *fp2); a0 = L_mac0(a0, *fp3, *fp3); fp2--; fp3--; energy[i] = a0; energy_exp[i] = norm_l(energy[i]); energy_man[i] = extract_h(L_shl(energy[i], energy_exp[i])); s0 = cor2_exp[i] = norm_l(a1); t0 = extract_h(L_shl(a1, s0)); cor2[i] = extract_h(L_mult(t0, t0)); if (a1 < 0) cor2[i] = negate(cor2[i]); } /* Find positive correlation peaks */ /* Find maximum of cor*cor/energy among positive correlation peaks */ npeaks = 0; n = MINPPD-1; while ((npeaks < MAX_NPEAKS) && (n<MAXPPD)) { if (cor[n]>0) { a0 = L_mult(energy_man[n-1],cor2[n]); a1 = L_mult(energy_man[n], cor2[n-1]); exp0 = shl(sub(cor2_exp[n], cor2_exp[n-1]),1); exp0 = add(exp0, energy_exp[n-1]); exp0 = sub(exp0, energy_exp[n]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { a0 = L_mult(energy_man[n+1],cor2[n]); a1 = L_mult(energy_man[n], cor2[n+1]); exp0 = shl(sub(cor2_exp[n], cor2_exp[n+1]),1); exp0 = add(exp0, energy_exp[n+1]); exp0 = sub(exp0, energy_exp[n]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { idx[npeaks] = n; npeaks++; } } } n++; } /* Return early if there is no peak or only one peak */ if (npeaks == 0){ /* if there are no positive peak, */ return MINPPD*DECF; /* return minimum pitch period in decimated domain */ } if (npeaks == 1){ /* if there is exactly one peak, */ return (idx[0]+1)*DECF; /* return the time lag for this single peak */ } /* If program proceeds to here, there are 2 or more peaks */ cor2max=(Word16) 0x8000; cor2max_exp= (Word16) 0; energymax_man=1; energymax_exp=0; imax=0; for (i=0; i < npeaks; i++) { /* Use quadratic interpolation to find the interpolated cor[] and energy[] corresponding to interpolated peak of cor2[]/energy[] */ /* first calculate coefficients of y(x)=ax^2+bx+c; */ n=idx[i]; a0=L_sub(L_shr(L_add(cor[n+1],cor[n-1]),1),cor[n]); L_Extract(a0, &ah, &al); a0=L_shr(L_sub(cor[n+1],cor[n-1]),1); L_Extract(a0, &bh, &bl); cc=cor[n]; /* Initialize variables before searching for interpolated peak */ im=0; cor2m_exp = cor2_exp[n]; cor2m = cor2[n]; energym_exp = energy_exp[n]; energym_man = energy_man[n]; eni=energy[n]; /* Determine which side the interpolated peak falls in, then do the search in the appropriate range */ a0 = L_mult(energy_man[n-1],cor2[n+1]); a1 = L_mult(energy_man[n+1], cor2[n-1]); exp0 = shl(sub(cor2_exp[n+1], cor2_exp[n-1]),1); exp0 = add(exp0, energy_exp[n-1]); exp0 = sub(exp0, energy_exp[n+1]); if (exp0>=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { /* if right side */ deltae = L_shr(L_sub(energy[n+1], eni), 2); for (k = 0; k < HDECF; k++) { a0=L_add(L_add(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc); eni = L_add(eni, deltae); a1 = eni; exp0 = norm_l(a0); s0 = extract_h(L_shl(a0, exp0)); s0 = extract_h(L_mult(s0, s0)); e2 = energym_exp; t0 = energym_man; a2 = L_mult(t0, s0); e3 = norm_l(a1); t1 = extract_h(L_shl(a1, e3)); a3 = L_mult(t1, cor2m); exp1 = shl(sub(exp0, cor2m_exp),1); exp1 = add(exp1, e2); exp1 = sub(exp1, e3); if (exp1>=0) a2 = L_shr(a2, exp1); else a3 = L_shl(a3, exp1); if (a2 > a3) { im = k+1; cor2m = s0; cor2m_exp = exp0; energym_exp = e3; energym_man = t1; } } } else { /* if interpolated peak is on the left side */ deltae = L_shr(L_sub(energy[n-1], eni), 2); for (k = 0; k < HDECF; k++) { a0=L_add(L_sub(Mpy_32_16(ah,al,x2[k]),Mpy_32_16(bh,bl,x[k])),cc); eni = L_add(eni, deltae); a1=eni; exp0 = norm_l(a0); s0 = extract_h(L_shl(a0, exp0)); s0 = extract_h(L_mult(s0, s0)); e2 = energym_exp; t0 = energym_man; a2 = L_mult(t0, s0); e3 = norm_l(a1); t1 = extract_h(L_shl(a1, e3)); a3 = L_mult(t1, cor2m); exp1 = shl(sub(exp0, cor2m_exp),1); exp1 = add(exp1, e2); exp1 = sub(exp1, e3); if (exp1>=0) a2 = L_shr(a2, exp1); else a3 = L_shl(a3, exp1); if (a2 > a3) { im = -k-1; cor2m = s0; cor2m_exp = exp0; energym_exp = e3; energym_man = t1; } } } /* Search done; assign cor2[] and energy[] corresponding to interpolated peak */ plag[i]=add(shl(add(idx[i],1),2),im); /* lag of interp. peak */ cor2i[i]=cor2m; cor2i_exp[i]=cor2m_exp; /* interpolated energy[] of i-th interpolated peak */ energyi_exp[i] = energym_exp; energyi_man[i] = energym_man; /* Search for global maximum of interpolated cor2[]/energy[] peak */ a0 = L_mult(cor2m,energymax_man); a1 = L_mult(cor2max, energyi_man[i]); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { imax=i; cor2max=cor2m; cor2max_exp=cor2m_exp; energymax_exp = energyi_exp[i]; energymax_man = energyi_man[i]; } } cpp=plag[imax]; /* first candidate for coarse pitch period */ mplth=plag[npeaks-1]; /* set mplth to the lag of last peak */ /* Find the largest peak (if there is any) around the last pitch */ maxdev= shr(cstate->cpplast,2); /* maximum deviation from last pitch */ im = -1; cor2m=(Word16) 0x8000; cor2m_exp= (Word16) 0; energym_man = 1; energym_exp = 0; for (i=0;i<npeaks;i++) { /* loop thru the peaks before the largest peak */ if (abs_s(sub(plag[i],cstate->cpplast)) <= maxdev) { a0 = L_mult(cor2i[i],energym_man); a1 = L_mult(cor2m, energyi_man[i]); exp0 = shl(sub(cor2i_exp[i], cor2m_exp),1); exp0 = add(exp0, energym_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { im=i; cor2m=cor2i[i]; cor2m_exp=cor2i_exp[i]; energym_man = energyi_man[i]; energym_exp = energyi_exp[i]; } } } /* if there is no peaks around last pitch, then im is still -1 */ /* Now see if we should pick any alternatice peak */ /* first, search first half of pitch range, see if any qualified peak has large enough peaks at every multiple of its lag */ i=0; while (2*plag[i] < mplth) { /* Determine the appropriate threshold for this peak */ if (i != im) { /* if not around last pitch, */ threshold = TH1; /* use a higher threshold */ } else { /* if around last pitch */ threshold = TH2; /* use a lower threshold */ } /* If threshold exceeded, test peaks at multiples of this lag */ a0 = L_mult(cor2i[i],energymax_man); t1 = extract_h(L_mult(energyi_man[i], threshold)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2i_exp[i], cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[i]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { flag=1; j=i+1; k=0; s=shl(plag[i],1); /* initialize t to twice the current lag */ while (s<=mplth) { /* loop thru all multiple lag <= mplth */ mpflag=0; /* initialize multiple pitch flag to 0 */ t0 = mult_r(s,MPDTH); a=sub(s, t0); /* multiple pitch range lower bound */ b=add(s, t0); /* multiple pitch range upper bound */ while (j < npeaks) { /* loop thru peaks with larger lags */ if (plag[j] > b) { /* if range exceeded, */ break; /* break the innermost while loop */ } /* if didn't break, then plag[j] <= b */ if (plag[j] > a) { /* if current peak lag within range, */ /* then check if peak value large enough */ a0 = L_mult(cor2i[j],energymax_man); if (k<4) t1 = MPTH[k]; else t1 = MPTH4; t1 = extract_h(L_mult(t1, energyi_man[j])); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2i_exp[j], cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energyi_exp[j]); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { mpflag=1; /* if peak large enough, set mpflag, */ break; /* and break the innermost while loop */ } } j++; } /* if no qualified peak found at this multiple lag */ if (mpflag == 0) { flag=0; /* disqualify the lag plag[i] */ break; /* and break the while (s<=mplth) loop */ } k++; s = add(s, plag[i]); /* update s to the next multiple pitch lag */ } /* if there is a qualified peak at every multiple of plag[i], */ if (flag == 1) { cpp = plag[i]; /* then accept this as final pitch */ return cpp; /* and return to calling function */ } } i++; if (i == npeaks) break; /* to avoid out of array bound error */ } /* If program proceeds to here, none of the peaks with lags < 0.5*mplth qualifies as the final pitch. in this case, check if there is any peak large enough around last pitch. if so, use its lag as the final pitch. */ if (im != -1) { /* if there is at least one peak around last pitch */ if (im == imax) { /* if this peak is also the global maximum, */ return cpp; /* return first pitch candidate at global max */ } if (im < imax) { /* if lag of this peak < lag of global max, */ a0 = L_mult(cor2m,energymax_man); t1 = extract_h(L_mult(energym_man, LPTH2)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energym_exp); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { if (plag[im] > HMAXPPD*DECF) { cpp=plag[im]; return cpp; } for (k=2; k<=5;k++) { /* check if current candidate pitch */ s=mult(plag[imax],invk[k-2]); /* is a sub-multiple of */ t0 = mult_r(s,SMDTH); a=sub(s, t0); /* the time lag of */ b=add(s, t0); /* the global maximum peak */ if (plag[im]>a && plag[im]<b) { /* if so, */ cpp=plag[im]; /* accept this peak, */ return cpp; /* and return as pitch */ } } } } else { /* if lag of this peak > lag of global max, */ a0 = L_mult(cor2m,energymax_man); t1 = extract_h(L_mult(energym_man, LPTH1)); a1 = L_mult(cor2max, t1); exp0 = shl(sub(cor2m_exp, cor2max_exp),1); exp0 = add(exp0, energymax_exp); exp0 = sub(exp0, energym_exp); if (exp0 >=0) a0 = L_shr(a0, exp0); else a1 = L_shl(a1, exp0); if (a0 > a1) { cpp = plag[im]; /* if this peak is large enough, */ return cpp; /* accept its lag */ } } } /* If program proceeds to here, we have no choice but to accept the lag of the global maximum */ return cpp; }
/******************************************************************************* * * functionname: scfCount * returns : --- * description : count bits used by scalefactors. * ********************************************************************************/ static void scfCount(const Word16 *scalefacGain, const UWord16 *maxValueInSfb, SECTION_DATA * sectionData) { SECTION_INFO *psectionInfo; SECTION_INFO *psectionInfom; /* counter */ Word32 i = 0; /* section counter */ Word32 j = 0; /* sfb counter */ Word32 k = 0; /* current section auxiliary counter */ Word32 m = 0; /* other section auxiliary counter */ Word32 n = 0; /* other sfb auxiliary counter */ /* further variables */ Word32 lastValScf = 0; Word32 deltaScf = 0; Flag found = 0; Word32 scfSkipCounter = 0; sectionData->scalefacBits = 0; if (scalefacGain == NULL) { return; } lastValScf = 0; sectionData->firstScf = 0; psectionInfo = sectionData->sectionInfo; for (i=0; i<sectionData->noOfSections; i++) { if (psectionInfo->codeBook != CODE_BOOK_ZERO_NO) { sectionData->firstScf = psectionInfo->sfbStart; lastValScf = scalefacGain[sectionData->firstScf]; break; } psectionInfo += 1; } psectionInfo = sectionData->sectionInfo; for (i=0; i<sectionData->noOfSections; i++, psectionInfo += 1) { if (psectionInfo->codeBook != CODE_BOOK_ZERO_NO && psectionInfo->codeBook != CODE_BOOK_PNS_NO) { for (j = psectionInfo->sfbStart; j < (psectionInfo->sfbStart + psectionInfo->sfbCnt); j++) { /* check if we can repeat the last value to save bits */ if (maxValueInSfb[j] == 0) { found = 0; if (scfSkipCounter == 0) { /* end of section */ if (j - ((psectionInfo->sfbStart + psectionInfo->sfbCnt) - 1) == 0) { found = 0; } else { for (k = j + 1; k < psectionInfo->sfbStart + psectionInfo->sfbCnt; k++) { if (maxValueInSfb[k] != 0) { int tmp = L_abs(scalefacGain[k] - lastValScf); found = 1; if ( tmp < CODE_BOOK_SCF_LAV) { /* save bits */ deltaScf = 0; } else { /* do not save bits */ deltaScf = lastValScf - scalefacGain[j]; lastValScf = scalefacGain[j]; scfSkipCounter = 0; } break; } /* count scalefactor skip */ scfSkipCounter = scfSkipCounter + 1; } } psectionInfom = psectionInfo + 1; /* search for the next maxValueInSfb[] != 0 in all other sections */ for (m = i + 1; (m < sectionData->noOfSections) && (found == 0); m++) { if ((psectionInfom->codeBook != CODE_BOOK_ZERO_NO) && (psectionInfom->codeBook != CODE_BOOK_PNS_NO)) { for (n = psectionInfom->sfbStart; n < (psectionInfom->sfbStart + psectionInfom->sfbCnt); n++) { if (maxValueInSfb[n] != 0) { found = 1; if ( (abs_s(scalefacGain[n] - lastValScf) < CODE_BOOK_SCF_LAV)) { deltaScf = 0; } else { deltaScf = (lastValScf - scalefacGain[j]); lastValScf = scalefacGain[j]; scfSkipCounter = 0; } break; } /* count scalefactor skip */ scfSkipCounter = scfSkipCounter + 1; } } psectionInfom += 1; } if (found == 0) { deltaScf = 0; scfSkipCounter = 0; } } else { deltaScf = 0; scfSkipCounter = scfSkipCounter - 1; } } else { deltaScf = lastValScf - scalefacGain[j]; lastValScf = scalefacGain[j]; } sectionData->scalefacBits += bitCountScalefactorDelta(deltaScf); } } } }
void fndppf(short *delay, short *beta, short *buf, short dmin, short dmax, short length) { static short b = -10224; /* rom storage */ static short a[3] = {-18739, 16024, -4882}; /* a[] scaled down by 4 */ short dnew = 0; short sum; long Lsum; register short m, i, n; static short DECbuf[FrameSize / 4]; long Lcorrmax, Lcmax, Ltmp; short tap1; short M1, M2, dnewtmp = 0; static short lastgoodpitch = 0; static short lastbeta = 0; static short memory[3]; static int FirstTime = 1; short Lsum_scale; short shift, Lcorr_scale, Lcmax_scale; short n1, n2, nq, nq1; long Ltempf; /* init static variables (should be in init routine for implementation) */ if (FirstTime) { FirstTime = 0; n1 = (shr(FrameSize, 2)); for (i = 0; i < n1; i++) DECbuf[i] = 0; memory[0] = memory[1] = memory[2] = 0; } /* Shift memory of DECbuf */ for (i = 0; i < shr(length, 3); i++) { DECbuf[i] = DECbuf[i + shr(length, 3)]; } /* filter signal and decimate */ for (i = 0, n = shr(length, 3); i < shr(length, 1); i++) { Ltempf = L_shr(L_deposit_h(buf[i + shr(length, 1)]), 4); Ltempf = L_msu(Ltempf, memory[0], a[0]); Ltempf = L_msu(Ltempf, memory[1], a[1]); Ltempf = L_msu(Ltempf, memory[2], a[2]); Ltempf = L_shl(Ltempf, 2); shift = 0; if ((i + 1) % 4 == 0) { Lsum = L_add(Ltempf, L_deposit_h(memory[2])); Lsum = L_mac(Lsum, memory[0], b); Lsum = L_mac(Lsum, memory[1], b); DECbuf[n++] = round(L_shl(Lsum, 1)); } memory[2] = memory[1]; memory[1] = memory[0]; memory[0] = round(Ltempf); } /* perform first search for best delay value in decimated domain */ Lcorrmax = (LW_MIN); Lcorr_scale = 1; for (m = shr(dmin, 2); m <= shr(dmax, 2); m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++) { Ltempf = L_mult(DECbuf[i], DECbuf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax)) || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale)))) ) { Lcorrmax = Lsum; Lcorr_scale = n1; dnew = m; } } /* Compare against lastgoodpitch */ if (lastgoodpitch != 0 && (abs_s(sub(lastgoodpitch, shl(dnew, 2))) > 2)) { M1 = sub(shr(lastgoodpitch, 2), 2); if (M1 < shr(dmin, 2)) M1 = shr(dmin, 2); M2 = add(M1, 4); if (M2 > shr(dmax, 2)) M2 = shr(dmax, 2); Lcmax = LW_MIN; Lcmax_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(shr(length, 2), m); i++) { Ltempf = L_mult(DECbuf[i], DECbuf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax)) || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale)))) ) { /* Gives some bias to low delays */ Lcmax = Lsum; Lcmax_scale = n1; dnewtmp = m; } } Lsum = L_mpy_ls(Lcorrmax, 27361); if ( ((Lcmax_scale >= Lcorr_scale) && (L_shr(Lsum, sub(Lcmax_scale, Lcorr_scale)) < Lcmax)) || ((Lcmax_scale < Lcorr_scale) && (Lsum < L_shr(Lcmax, sub(Lcorr_scale, Lcmax_scale)))) ) { dnew = dnewtmp; } } /* perform first search for best delay value in non-decimated buffer */ M1 = Max(sub(shl(dnew, 2), 3), dmin); if (M1 < dmin) M1 = dmin; M2 = Min(add(shl(dnew, 2), 3), dmax); if (M2 > dmax) M2 = dmax; Lcorrmax = LW_MIN; Lcorr_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < sub(length, m); i++) { Ltempf = L_mult(buf[i], buf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcorr_scale >= n1) && (L_shr(Lsum, sub(Lcorr_scale, n1)) > Lcorrmax)) || ((Lcorr_scale < n1) && (Lsum > L_shr(Lcorrmax, sub(n1, Lcorr_scale)))) ) { Lcorrmax = Lsum; Lcorr_scale = n1; dnew = m; } } Lsum_scale = 1; for (i = 0, Lsum = 0; i < sub(length, dnew); i++) { Ltempf = L_mult(buf[i + dnew], buf[i + dnew]); Ltempf = L_shr(Ltempf, Lsum_scale); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); Lsum_scale++; } } Lcmax_scale = 1; for (i = 0, Lcmax = 0; i < length - dnew; i++) { Ltempf = L_mult(buf[i], buf[i]); Ltempf = L_shr(Ltempf, Lcmax_scale); Lcmax = L_add(Lcmax, Ltempf); if (L_abs(Lcmax) >= 0x40000000) { Lcmax = L_shr(Lcmax, 1); Lcmax_scale++; } } nq = norm_l(Lsum); Lsum = L_shl(Lsum, nq); nq1 = norm_l(Lcmax); Lcmax = L_shl(Lcmax, nq1); Lsum = L_mpy_ll(Lsum, Lcmax); n1 = norm_l(Lsum); Lsum = L_shl(Lsum, n1); sum = sqroot(Lsum); n1 = add(add(n1, nq), nq1); n1 = sub(sub(n1, Lcmax_scale), Lsum_scale); n2 = shr(n1, 1); if (n1 & 1) Lsum = L_mult(sum, 23170); else Lsum = L_deposit_h(sum); n2 = add(n2, Lcorr_scale); Lcorrmax = L_shl(Lcorrmax, n2); if ((Lsum == 0) || (Lcorrmax <= 0)) *beta = 0; else if (Lcorrmax > Lsum) *beta = 0x7fff; else *beta = round(L_divide(Lcorrmax, Lsum)); /* perform search for best delay value in around old pitch delay */ if (lastgoodpitch != 0) { M1 = lastgoodpitch - 6; M2 = lastgoodpitch + 6; if (M1 < dmin) M1 = dmin; if (M2 > dmax) M2 = dmax; if (dnew > M2 || dnew < M1) { Lcmax = LW_MIN; Lcmax_scale = 1; for (m = M1; m <= M2; m++) { n1 = 1; for (i = 0, Lsum = 0; i < length - m; i++) { Ltempf = L_mult(buf[i], buf[i + m]); Ltempf = L_shr(Ltempf, n1); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); n1++; } } if ( ((Lcmax_scale >= n1) && (L_shr(Lsum, sub(Lcmax_scale, n1)) > Lcmax)) || ((Lcmax_scale < n1) && (Lsum > L_shr(Lcmax, sub(n1, Lcmax_scale)))) ) { Lcmax = Lsum; dnewtmp = m; Lcmax_scale = n1; } } Lcorr_scale = 1; for (i = 0, Ltmp = 0; i < length - dnewtmp; i++) { Ltempf = L_mult(buf[i + dnewtmp], buf[i + dnewtmp]); Ltempf = L_shr(Ltempf, Lcorr_scale); Ltmp = L_add(Ltmp, Ltempf); if (L_abs(Ltmp) >= 0x40000000) { Ltmp = L_shr(Ltmp, 1); Lcorr_scale++; } } Lsum_scale = 1; for (i = 0, Lsum = 0; i < length - dnewtmp; i++) { Ltempf = L_mult(buf[i], buf[i]); Ltempf = L_shr(Ltempf, Lsum_scale); Lsum = L_add(Lsum, Ltempf); if (L_abs(Lsum) >= 0x40000000) { Lsum = L_shr(Lsum, 1); Lsum_scale++; } } nq = norm_l(Ltmp); Ltmp = L_shl(Ltmp, nq); nq1 = norm_l(Lsum); Lsum = L_shl(Lsum, nq1); Ltmp = L_mpy_ll(Ltmp, Lsum); n1 = norm_l(Ltmp); Ltmp = L_shl(Ltmp, n1); sum = sqroot(Ltmp); n1 = add(add(n1, nq), nq1); n1 = sub(sub(n1, Lsum_scale), Lcorr_scale); n2 = shr(n1, 1); if (n1 & 1) Ltmp = L_mult(sum, 23170); else Ltmp = L_deposit_h(sum); n2 = add(n2, Lcmax_scale); Lcmax = L_shl(Lcmax, n2); if ((Ltmp == 0) || (Lcmax <= 0)) tap1 = 0; else if (Lcmax >= Ltmp) tap1 = 0x7fff; else tap1 = round(L_divide(Lcmax, Ltmp)); /* Replace dnew with dnewtmp if tap1 is large enough */ if ((dnew > M2 && (shr(tap1, 1) > mult_r(9830, *beta))) || (dnew < M1 && (shr(tap1, 1) > mult_r(19661, *beta)))) { dnew = dnewtmp; *beta = (tap1); } } } *delay = dnew; if (*beta > 13107) { lastgoodpitch = dnew; lastbeta = *beta; } else { lastbeta = mult_r(24576, lastbeta); if (lastbeta < 9830) lastgoodpitch = 0; } }
/*---------------------------------------------------------------------------- ; FUNCTION CODE ----------------------------------------------------------------------------*/ Word16 hp_max( Word32 corr[], /* i : correlation vector. */ Word16 scal_sig[], /* i : scaled signal. */ Word16 L_frame, /* i : length of frame to compute pitch */ Word16 lag_max, /* i : maximum lag */ Word16 lag_min, /* i : minimum lag */ Word16 *cor_hp_max, /* o : max high-pass filtered norm. correlation */ Flag *pOverflow /* i/o : overflow Flag */ ) { Word16 i; Word16 *p, *p1; Word32 max, t0, t1; Word16 max16, t016, cor_max; Word16 shift, shift1, shift2; Word32 L_temp; max = MIN_32; t0 = 0L; for (i = lag_max - 1; i > lag_min; i--) { /* high-pass filtering */ t0 = L_shl(corr[-i], 1, pOverflow); L_temp = L_sub(t0, corr[-i-1], pOverflow); t0 = L_sub(L_temp, corr[-i+1], pOverflow); t0 = L_abs(t0); if (t0 >= max) { max = t0; } } /* compute energy */ p = scal_sig; p1 = &scal_sig[0]; t0 = 0L; for (i = 0; i < L_frame; i++, p++, p1++) { t0 = L_mac(t0, *p, *p1, pOverflow); } p = scal_sig; p1 = &scal_sig[-1]; t1 = 0L; for (i = 0; i < L_frame; i++, p++, p1++) { t1 = L_mac(t1, *p, *p1, pOverflow); } /* high-pass filtering */ L_temp = L_shl(t0, 1, pOverflow); t1 = L_shl(t1, 1, pOverflow); t0 = L_sub(L_temp, t1, pOverflow); t0 = L_abs(t0); /* max/t0 */ /* shift1 = sub(norm_l(max), 1); max16 = extract_h(L_shl(max, shift1)); shift2 = norm_l(t0); t016 = extract_h(L_shl(t0, shift2)); */ t016 = norm_l(max); shift1 = sub(t016, 1, pOverflow); L_temp = L_shl(max, shift1, pOverflow); max16 = (Word16)(L_temp >> 16); shift2 = norm_l(t0); L_temp = L_shl(t0, shift2, pOverflow); t016 = (Word16)(L_temp >> 16); if (t016 != 0) { cor_max = div_s(max16, t016); } else { cor_max = 0; } shift = sub(shift1, shift2, pOverflow); if (shift >= 0) { *cor_hp_max = shr(cor_max, shift, pOverflow); /* Q15 */ } else { *cor_hp_max = shl(cor_max, negate(shift), pOverflow); /* Q15 */ } return 0; }
/* ******************************************************************************** * PUBLIC PROGRAM CODE ******************************************************************************** */ Word16 hp_max ( Word32 corr[], /* i : correlation vector. */ Word16 scal_sig[], /* i : scaled signal. */ Word16 L_frame, /* i : length of frame to compute pitch */ Word16 lag_max, /* i : maximum lag */ Word16 lag_min, /* i : minimum lag */ Word16 *cor_hp_max) /* o : max high-pass filtered norm. correlation */ { Word16 i; Word16 *p, *p1; Word32 max, t0, t1; Word16 max16, t016, cor_max; Word16 shift, shift1, shift2; max = MIN_32; move32 (); t0 = 0L; move32 (); for (i = lag_max-1; i > lag_min; i--) { /* high-pass filtering */ t0 = L_sub (L_sub(L_shl(corr[-i], 1), corr[-i-1]), corr[-i+1]); t0 = L_abs (t0); test (); if (L_sub (t0, max) >= 0) { max = t0; move32 (); } } /* compute energy */ p = scal_sig; move16 (); p1 = &scal_sig[0]; move16 (); t0 = 0L; move32 (); for (i = 0; i < L_frame; i++, p++, p1++) { t0 = L_mac (t0, *p, *p1); } p = scal_sig; move16 (); p1 = &scal_sig[-1]; move16 (); t1 = 0L; move32 (); for (i = 0; i < L_frame; i++, p++, p1++) { t1 = L_mac (t1, *p, *p1); } /* high-pass filtering */ t0 = L_sub(L_shl(t0, 1), L_shl(t1, 1)); t0 = L_abs (t0); /* max/t0 */ shift1 = sub(norm_l(max), 1); max16 = extract_h(L_shl(max, shift1)); shift2 = norm_l(t0); t016 = extract_h(L_shl(t0, shift2)); test (); if (t016 != 0) { cor_max = div_s(max16, t016); } else { cor_max = 0; move16 (); } shift = sub(shift1, shift2); test (); if (shift >= 0) { *cor_hp_max = shr(cor_max, shift); move16 (); /* Q15 */ } else { *cor_hp_max = shl(cor_max, negate(shift)); move16 (); /* Q15 */ } return 0; }
void A_Refl( Word16 a[], /* i : Directform coefficients */ Word16 refl[], /* o : Reflection coefficients */ Flag *pOverflow ) { /* local variables */ Word16 i; Word16 j; Word16 aState[M]; Word16 bState[M]; Word16 normShift; Word16 normProd; Word32 L_acc; Word16 scale; Word32 L_temp; Word16 temp; Word16 mult; /* initialize states */ for (i = 0; i < M; i++) { aState[i] = a[i]; } /* backward Levinson recursion */ for (i = M - 1; i >= 0; i--) { if (abs_s(aState[i]) >= 4096) { for (i = 0; i < M; i++) { refl[i] = 0; } break; } refl[i] = shl(aState[i], 3, pOverflow); L_temp = L_mult(refl[i], refl[i], pOverflow); L_acc = L_sub(MAX_32, L_temp, pOverflow); normShift = norm_l(L_acc); scale = sub(15, normShift, pOverflow); L_acc = L_shl(L_acc, normShift, pOverflow); normProd = pv_round(L_acc, pOverflow); mult = div_s(16384, normProd); for (j = 0; j < i; j++) { L_acc = L_deposit_h(aState[j]); L_acc = L_msu(L_acc, refl[i], aState[i-j-1], pOverflow); temp = pv_round(L_acc, pOverflow); L_temp = L_mult(mult, temp, pOverflow); L_temp = L_shr_r(L_temp, scale, pOverflow); if (L_abs(L_temp) > 32767) { for (i = 0; i < M; i++) { refl[i] = 0; } break; } bState[j] = extract_l(L_temp); } for (j = 0; j < i; j++) { aState[j] = bState[j]; } } return; }
void Levinsone( Word16 m, /* (i) : LPC order */ Word16 Rh[], /* (i) : Rh[M+1] Vector of autocorrelations (msb) */ Word16 Rl[], /* (i) : Rl[M+1] Vector of autocorrelations (lsb) */ Word16 A[], /* (o) Q12 : A[M] LPC coefficients (m = 10) */ Word16 rc[], /* (o) Q15 : rc[M] Reflection coefficients. */ Word16 old_A[], /* (i/o) Q12 : last stable filter LPC coefficients */ Word16 old_rc[] /* (i/o) Q15 : last stable filter Reflection coefficients. */ ) { Word16 i, j; Word16 hi, lo; Word16 Kh, Kl; /* reflection coefficient; hi and lo */ Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */ Word16 Ah[M_BWDP1], Al[M_BWDP1]; /* LPC coef. in double prec. */ Word16 Anh[M_BWDP1], Anl[M_BWDP1]; /* LPC coef.for next iteration in double prec. */ Word32 t0, t1, t2; /* temporary variable */ Word32 tmp; /* K = A[1] = -R[1] / R[0] */ t1 = L_Comp(Rh[1], Rl[1]); /* R[1] in Q31 */ tmp = L_Comp(Rh[0], Rl[0]); if (t1 > tmp) t1 = tmp; t2 = L_abs(t1); /* abs R[1] */ t0 = Div_32(t2, Rh[0], Rl[0]); /* R[1]/R[0] in Q31 */ if(t1 > 0) t0= L_negate(t0); /* -R[1]/R[0] */ L_Extract(t0, &Kh, &Kl); /* K in DPF */ rc[0] = Kh; t0 = L_shr(t0,4); /* A[1] in Q27 */ L_Extract(t0, &Ah[1], &Al[1]); /* A[1] in DPF */ /* Alpha = R[0] * (1-K**2) */ t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */ t0 = L_abs(t0); /* Some case <0 !! */ t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */ L_Extract(t0, &hi, &lo); /* DPF format */ t0 = Mpy_32(Rh[0] ,Rl[0], hi, lo); /* Alpha in Q31 */ /* Normalize Alpha */ alp_exp = norm_l(t0); t0 = L_shl(t0, alp_exp); L_Extract(t0, &alp_h, &alp_l); /* DPF format */ /*--------------------------------------* * ITERATIONS I=2 to m * *--------------------------------------*/ for(i= 2; i<=m; i++) { /* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */ t0 = 0; for(j=1; j<i; j++) t0 = L_add(t0, Mpy_32(Rh[j], Rl[j], Ah[i-j], Al[i-j])); t0 = L_shl(t0,4); /* result in Q27 -> convert to Q31 */ /* No overflow possible */ t1 = L_Comp(Rh[i],Rl[i]); t0 = L_add(t0, t1); /* add R[i] in Q31 */ /* K = -t0 / Alpha */ t1 = L_abs(t0); tmp = L_Comp(alp_h, alp_l); if (t1 > tmp) t1 = tmp; t2 = Div_32(t1, alp_h, alp_l); /* abs(t0)/Alpha */ if(t0 > 0) t2= L_negate(t2); /* K =-t0/Alpha */ t2 = L_shl(t2, alp_exp); /* denormalize; compare to Alpha */ L_Extract(t2, &Kh, &Kl); /* K in DPF */ rc[i-1] = Kh; /* Test for unstable filter. If unstable keep old A(z) */ if (sub(abs_s(Kh), 32750) > 0) { for(j=0; j<=m; j++) { A[j] = old_A[j]; } rc[0] = old_rc[0]; /* only two rc coefficients are needed */ rc[1] = old_rc[1]; return; } /*------------------------------------------* * Compute new LPC coeff. -> An[i] * * An[j]= A[j] + K*A[i-j] , j=1 to i-1 * * An[i]= K * *------------------------------------------*/ for(j=1; j<i; j++) { t0 = Mpy_32(Kh, Kl, Ah[i-j], Al[i-j]); t0 = L_add(t0, L_Comp(Ah[j], Al[j])); L_Extract(t0, &Anh[j], &Anl[j]); } t2 = L_shr(t2, 4); /* t2 = K in Q31 ->convert to Q27 */ L_Extract(t2, &Anh[i], &Anl[i]); /* An[i] in Q27 */ /* Alpha = Alpha * (1-K**2) */ t0 = Mpy_32(Kh ,Kl, Kh, Kl); /* K*K in Q31 */ t0 = L_abs(t0); /* Some case <0 !! */ t0 = L_sub( (Word32)0x7fffffffL, t0 ); /* 1 - K*K in Q31 */ L_Extract(t0, &hi, &lo); /* DPF format */ t0 = Mpy_32(alp_h , alp_l, hi, lo); /* Alpha in Q31 */ /* Normalize Alpha */ j = norm_l(t0); t0 = L_shl(t0, j); L_Extract(t0, &alp_h, &alp_l); /* DPF format */ alp_exp = add(alp_exp, j); /* Add normalization to alp_exp */ /* A[j] = An[j] */ for(j=1; j<=i; j++) { Ah[j] =Anh[j]; Al[j] =Anl[j]; } } /* Truncate A[i] in Q27 to Q12 with rounding */ A[0] = 4096; for(i=1; i<=m; i++) { t0 = L_Comp(Ah[i], Al[i]); old_A[i] = A[i] = round(L_shl(t0, 1)); } old_rc[0] = rc[0]; old_rc[1] = rc[1]; return; }
/* ** ** Function: AtoLsp() ** ** Description: Transforms 10 LPC coefficients to the 10 ** corresponding LSP frequencies for a subframe. ** This transformation is done once per frame, ** for subframe 3 only. The transform algorithm ** generates sum and difference polynomials from ** the LPC coefficients. It then evaluates the ** sum and difference polynomials at uniform ** intervals of pi/256 along the unit circle. ** Intervals where a sign change occurs are ** interpolated to find the zeros of the ** polynomials, which are the LSP frequencies. ** ** Links to text: Section 2.5 ** ** Arguments: ** ** Word16 *LspVect Empty Buffer ** Word16 Lpc[] Unquantized LPC coefficients (10 words) ** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words) ** ** Outputs: ** ** Word16 LspVect[] LSP frequencies for the current frame (10 words) ** ** Return value: None ** **/ void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp ) { int i,j,k ; Word32 Lpq[LpcOrder+2] ; Word16 Spq[LpcOrder+2] ; Word16 Exp ; Word16 LspCnt ; Word32 PrevVal,CurrVal ; Word32 Acc0,Acc1 ; /* * Perform a bandwidth expansion on the LPC coefficients. This * scales the poles of the LPC synthesis filter by a factor of * 0.994. */ for ( i = 0 ; i < LpcOrder ; i ++ ) LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ; /* * Compute the sum and difference polynomials with the roots at z = * -1 (sum) or z = +1 (difference) removed. Let these polynomials * be P(z) and Q(z) respectively, and let their coefficients be * {p_i} amd {q_i}. The coefficients are stored in the array Lpq[] * as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5. There is no need * to store the other coefficients because of symmetry. */ /* * Set p_0 = q_0 = 1. The LPC coefficients are already scaled by * 1/4. P(z) and Q(z) are scaled by an additional scaling factor of * 1/16, for an overall factor of 1/64 = 0x02000000L. */ Lpq[0] = Lpq[1] = (Word32) 0x02000000L ; /* * This loop computes the coefficients of P(z) and Q(z). The long * division (to remove the real zeros) is done recursively. */ for ( i = 0 ; i < LpcOrder/2 ; i ++ ) { /* P(z) */ Acc0 = L_negate( Lpq[2*i+0] ) ; Acc1 = L_deposit_h( LspVect[i] ) ; Acc1 = L_shr( Acc1, (Word16) 4 ) ; Acc0 = L_sub( Acc0, Acc1 ) ; Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ; Acc1 = L_shr( Acc1, (Word16) 4 ) ; Acc0 = L_sub( Acc0, Acc1 ) ; Lpq[2*i+2] = Acc0 ; /* Q(z) */ Acc0 = Lpq[2*i+1] ; Acc1 = L_deposit_h( LspVect[i] ) ; Acc1 = L_shr( Acc1, (Word16) 4 ) ; Acc0 = L_sub( Acc0, Acc1 ) ; Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ; Acc1 = L_shr( Acc1, (Word16) 4 ) ; // Acc0 = L_add( Acc0, Acc1 ) ; L_ADD(Acc0, Acc1, Acc0); Lpq[2*i+3] = Acc0 ; } /* * Divide p_5 and q_5 by 2 for proper weighting during polynomial * evaluation. */ Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ; Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ; /* * Normalize the polynomial coefficients and convert to shorts */ /* Find the maximum */ Acc1 = L_abs( Lpq[0] ) ; for ( i = 1 ; i < LpcOrder+2 ; i ++ ) { Acc0 = L_abs( Lpq[i] ) ; if ( Acc0 > Acc1 ) Acc1 = Acc0 ; } /* Compute the normalization factor */ Exp = norm_l( Acc1 ) ; /* Normalize and convert to shorts */ for ( i = 0 ; i < LpcOrder+2 ; i ++ ) { Acc0 = L_shl( Lpq[i], Exp ) ; Spq[i] = round( Acc0 ) ; } /* * Initialize the search loop */ /* * The variable k is a flag that indicates which polynomial (sum or * difference) the algorithm is currently evaluating. Start with * the sum. */ k = 0 ; /* Evaluate the sum polynomial at frequency zero */ PrevVal = (Word32) 0 ; for ( j = 0 ; j <= LpcOrder/2 ; j ++ ) PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ; /* * Search loop. Evaluate P(z) and Q(z) at uniform intervals of * pi/256 along the unit circle. Check for zero crossings. The * zeros of P(w) and Q(w) alternate, so only one of them need by * evaluated at any given step. */ LspCnt = (Word16) 0 ; for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) { /* Evaluate the selected polynomial */ CurrVal = (Word32) 0 ; for ( j = 0 ; j <= LpcOrder/2 ; j ++ ) CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k], CosineTable[i*j%CosineTableSize] ) ; /* Check for a sign change, indicating a zero crossing */ if ( (CurrVal ^ PrevVal) < (Word32) 0 ) { /* * Interpolate to find the bottom 7 bits of the * zero-crossing frequency */ Acc0 = L_abs( CurrVal ) ; Acc1 = L_abs( PrevVal ) ; // Acc0 = L_add( Acc0, Acc1 ) ; L_ADD(Acc0, Acc1, Acc0); /* Normalize the sum */ Exp = norm_l( Acc0 ) ; Acc0 = L_shl( Acc0, Exp ) ; Acc1 = L_shl( Acc1, Exp ) ; Acc1 = L_shr( Acc1, (Word16) 8 ) ; LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ; /* * Add the upper part of the zero-crossing frequency, * i.e. bits 7-15 */ Exp = shl( (Word16) (i-1), (Word16) 7 ) ; LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ; LspCnt ++ ; /* Check if all zeros have been found */ if ( LspCnt == (Word16) LpcOrder ) break ; /* * Switch the pointer between sum and difference polynomials */ k ^= 1 ; /* * Evaluate the new polynomial at the current frequency */ CurrVal = (Word32) 0 ; for ( j = 0 ; j <= LpcOrder/2 ; j ++ ) CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k], CosineTable[i*j%CosineTableSize] ) ; } /* Update the previous value */ PrevVal = CurrVal ; } /* * Check if all 10 zeros were found. If not, ignore the results of * the search and use the previous frame's LSP frequencies instead. */ if ( LspCnt != (Word16) LpcOrder ) { for ( j = 0 ; j < LpcOrder ; j ++ ) LspVect[j] = PrevLsp[j] ; } return ; }
/*---------------------------------------------------------------------------- * scale_st - control of the subframe gain * gain[n] = AGC_FAC * gain[n-1] + (1 - AGC_FAC) g_in/g_out *---------------------------------------------------------------------------- */ static void scale_st( Word16 *sig_in, /* input : postfilter input signal */ Word16 *sig_out, /* in/out: postfilter output signal */ Word16 *gain_prec /* in/out: last value of gain for subframe */ ) { int i; Word16 scal_in, scal_out; Word32 L_acc, L_temp; Word16 s_g_in, s_g_out, temp, sh_g0, g0; Word16 gain; /* compute input gain */ L_acc = 0L; for(i=0; i<L_SUBFR; i++) { L_temp = L_abs(L_deposit_l(sig_in[i])); L_acc = L_add(L_acc, L_temp); } if(L_acc == 0L) { g0 = 0; } else { scal_in = norm_l(L_acc); L_acc = L_shl(L_acc, scal_in); s_g_in = extract_h(L_acc); /* normalized */ /* Compute output gain */ L_acc = 0L; for(i=0; i<L_SUBFR; i++) { L_temp = L_abs(L_deposit_l(sig_out[i])); L_acc = L_add(L_acc, L_temp); } if(L_acc == 0L) { *gain_prec = 0; return; } scal_out = norm_l(L_acc); L_acc = L_shl(L_acc, scal_out); s_g_out = extract_h(L_acc); /* normalized */ sh_g0 = add(scal_in, 1); sh_g0 = sub(sh_g0, scal_out); /* scal_in - scal_out + 1 */ if(sub(s_g_in ,s_g_out)<0) { g0 = div_s(s_g_in, s_g_out); /* s_g_in/s_g_out in Q15 */ } else { temp = sub(s_g_in, s_g_out); /* sufficient since normalized */ g0 = shr(div_s(temp, s_g_out), 1); g0 = add(g0, (Word16)0x4000);/* s_g_in/s_g_out in Q14 */ sh_g0 = sub(sh_g0, 1); } /* L_gain_in/L_gain_out in Q14 */ /* overflows if L_gain_in > 2 * L_gain_out */ g0 = shr(g0, sh_g0); /* sh_g0 may be >0, <0, or =0 */ g0 = mult_r(g0, AGC_FAC1); /* L_gain_in/L_gain_out * AGC_FAC1 */ } /* gain(n) = AGC_FAC gain(n-1) + AGC_FAC1 gain_in/gain_out */ /* sig_out(n) = gain(n) sig_out(n) */ gain = *gain_prec; for(i=0; i<L_SUBFR; i++) { temp = mult_r(AGC_FAC, gain); gain = add(temp, g0); /* in Q14 */ L_temp = L_mult(gain, sig_out[i]); L_temp = L_shl(L_temp, 1); sig_out[i] = round(L_temp); } *gain_prec = gain; return; }
/* ************************************************************************** * * Function : A_Refl * ************************************************************************** */ void A_Refl( Word16 a[], /* i : Directform coefficients */ Word16 refl[] /* o : Reflection coefficients */ ) { /* local variables */ Word16 i,j; Word16 aState[M]; Word16 bState[M]; Word16 normShift; Word16 normProd; Word32 L_acc; Word16 scale; Word32 L_temp; Word16 temp; Word16 mult; /* initialize states */ for (i = 0; i < M; i++) { aState[i] = a[i]; move16 (); } /* backward Levinson recursion */ for (i = M-1; i >= 0; i--) { if (sub(abs_s(aState[i]), 4096) >= 0) { goto ExitRefl; } refl[i] = shl(aState[i], 3); L_temp = L_mult(refl[i], refl[i]); L_acc = L_sub(MAX_32, L_temp); normShift = norm_l(L_acc); scale = sub(15, normShift); L_acc = L_shl(L_acc, normShift); normProd = round(L_acc); mult = div_s(16384, normProd); for (j = 0; j < i; j++) { L_acc = L_deposit_h(aState[j]); L_acc = L_msu(L_acc, refl[i], aState[i-j-1]); temp = round(L_acc); L_temp = L_mult(mult, temp); L_temp = L_shr_r(L_temp, scale); if (L_sub(L_abs(L_temp), 32767) > 0) { goto ExitRefl; } bState[j] = extract_l(L_temp); } for (j = 0; j < i; j++) { aState[j] = bState[j]; move16 (); } } return; ExitRefl: for (i = 0; i < M; i++) { refl[i] = 0; move16 (); } }
/************************************************************************* * * FUNCTION: Levinson() * * PURPOSE: Levinson-Durbin algorithm in double precision. To compute the * LP filter parameters from the speech autocorrelations. * * DESCRIPTION: * R[i] autocorrelations. * A[i] filter coefficients. * K reflection coefficients. * Alpha prediction gain. * * Initialisation: * A[0] = 1 * K = -R[1]/R[0] * A[1] = K * Alpha = R[0] * (1-K**2] * * Do for i = 2 to M * * S = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] * * K = -S / Alpha * * An[j] = A[j] + K*A[i-j] for j=1 to i-1 * where An[i] = new A[i] * An[i]=K * * Alpha=Alpha * (1-K**2) * * END * *************************************************************************/ int Levinson ( LevinsonState *st, Word16 Rh[], /* i : Rh[m+1] Vector of autocorrelations (msb) */ Word16 Rl[], /* i : Rl[m+1] Vector of autocorrelations (lsb) */ Word16 A[], /* o : A[m] LPC coefficients (m = 10) */ Word16 rc[] /* o : rc[4] First 4 reflection coefficients */ ) { Word16 i, j; Word16 hi, lo; Word16 Kh, Kl; /* reflexion coefficient; hi and lo */ Word16 alp_h, alp_l, alp_exp; /* Prediction gain; hi lo and exponent */ Word16 Ah[M + 1], Al[M + 1]; /* LPC coef. in double prec. */ Word16 Anh[M + 1], Anl[M + 1];/* LPC coef.for next iteration in double prec. */ Word32 t0, t1, t2; /* temporary variable */ /* K = A[1] = -R[1] / R[0] */ t1 = L_Comp (Rh[1], Rl[1]); t2 = L_abs (t1); /* abs R[1] */ t0 = Div_32 (t2, Rh[0], Rl[0]); /* R[1]/R[0] */ if (t1 > 0) t0 = L_negate (t0); /* -R[1]/R[0] */ L_Extract (t0, &Kh, &Kl); /* K in DPF */ rc[0] = round (t0); t0 = L_shr (t0, 4); /* A[1] in */ L_Extract (t0, &Ah[1], &Al[1]); /* A[1] in DPF */ /* Alpha = R[0] * (1-K**2) */ t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */ t0 = L_abs (t0); /* Some case <0 !! */ t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */ L_Extract (t0, &hi, &lo); /* DPF format */ t0 = Mpy_32 (Rh[0], Rl[0], hi, lo); /* Alpha in */ /* Normalize Alpha */ alp_exp = norm_l (t0); t0 = L_shl (t0, alp_exp); L_Extract (t0, &alp_h, &alp_l); /* DPF format */ /*--------------------------------------* * ITERATIONS I=2 to M * *--------------------------------------*/ for (i = 2; i <= M; i++) { /* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */ t0 = 0; for (j = 1; j < i; j++) { t0 = L_add (t0, Mpy_32 (Rh[j], Rl[j], Ah[i - j], Al[i - j])); } t0 = L_shl (t0, 4); t1 = L_Comp (Rh[i], Rl[i]); t0 = L_add (t0, t1); /* add R[i] */ /* K = -t0 / Alpha */ t1 = L_abs (t0); t2 = Div_32 (t1, alp_h, alp_l); /* abs(t0)/Alpha */ if (t0 > 0) t2 = L_negate (t2); /* K =-t0/Alpha */ t2 = L_shl (t2, alp_exp); /* denormalize; compare to Alpha */ L_Extract (t2, &Kh, &Kl); /* K in DPF */ if (sub (i, 5) < 0) { rc[i - 1] = round (t2); } /* Test for unstable filter. If unstable keep old A(z) */ if (sub (abs_s (Kh), 32750) > 0) { for (j = 0; j <= M; j++) { A[j] = st->old_A[j]; } for (j = 0; j < 4; j++) { rc[j] = 0; } return 0; } /*------------------------------------------* * Compute new LPC coeff. -> An[i] * * An[j]= A[j] + K*A[i-j] , j=1 to i-1 * * An[i]= K * *------------------------------------------*/ for (j = 1; j < i; j++) { t0 = Mpy_32 (Kh, Kl, Ah[i - j], Al[i - j]); t0 = L_add(t0, L_Comp(Ah[j], Al[j])); L_Extract (t0, &Anh[j], &Anl[j]); } t2 = L_shr (t2, 4); L_Extract (t2, &Anh[i], &Anl[i]); /* Alpha = Alpha * (1-K**2) */ t0 = Mpy_32 (Kh, Kl, Kh, Kl); /* K*K */ t0 = L_abs (t0); /* Some case <0 !! */ t0 = L_sub ((Word32) 0x7fffffffL, t0); /* 1 - K*K */ L_Extract (t0, &hi, &lo); /* DPF format */ t0 = Mpy_32 (alp_h, alp_l, hi, lo); /* Normalize Alpha */ j = norm_l (t0); t0 = L_shl (t0, j); L_Extract (t0, &alp_h, &alp_l); /* DPF format */ alp_exp = add (alp_exp, j); /* Add normalization to alp_exp */ /* A[j] = An[j] */ for (j = 1; j <= i; j++) { Ah[j] = Anh[j]; Al[j] = Anl[j]; } } A[0] = 4096; for (i = 1; i <= M; i++) { t0 = L_Comp (Ah[i], Al[i]); st->old_A[i] = A[i] = round (L_shl (t0, 1)); } return 0; }