Word32 L_divide(Word32 L_num, Word32 L_denom) { Word16 approx; Word32 L_div; if (L_num < 0 || L_denom < 0 || L_num > L_denom) { printf("ERROR: Invalid input into L_divide!\n"); return (0); } /* First approximation: 1 / L_denom = 1/extract_h(L_denom) */ approx = divide_s((Word16) 0x3fff, extract_h(L_denom)); /* 1/L_denom = approx * (2.0 - L_denom * approx) */ L_div = L_mpy_ls(L_denom, approx); L_div = L_sub((Word32) 0x7fffffffL, L_div); L_div = L_mpy_ls(L_div, approx); /* L_num * (1/L_denom) */ L_div = L_mpy_ll(L_num, L_div); L_div = L_shl(L_div, 2); return (L_div); }
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; } }
void modifyorig(short *residualm, short *accshift, short beta, short *dpm, short shiftrange, short resolution, short *TARGET, short *residual, short dp, short sfend) { static short FirstTime = 1; short best; short tmp; long ex0y[(RSHIFT * 2 + 2) * RRESOLUTION + 1]; /* Fraction sampled correlation function */ long ex0y2[RSHIFT * 2 + 2]; /* Integer sampled correlation function */ short Residual[2 * SubFrameSize + 2 * RSHIFT + 1]; static short a1[RRESOLUTION]; static short a2[RRESOLUTION]; static short a3[RRESOLUTION]; register short i, j, k, n; short sfstart, length, shiftrangel, shiftranger; long e01, e02; short shft_fctr1, shft_fctr2, shft_fctr3; long ltmp; long laccshift; long y; /****** INITIALIZATION *****/ if (FirstTime) { FirstTime = 0; /* Calculate interpolation coefficients */ a1[0] = 12288; a1[1] = 8448; a1[2] = 5120; a1[3] = 2304; a1[4] = 0; a1[5] = -1792; a1[6] = -3072; a1[7] = -3840; a2[0] = 24576; a2[1] = 28160; a2[2] = 30720; a2[3] = 32256; a2[4] = 32767; a2[5] = 32256; a2[6] = 30720; a2[7] = 28160; a3[0] = -4096; a3[1] = -3840; a3[2] = -3072; a3[3] = -1792; a3[4] = 0; a3[5] = 2304; a3[6] = 5120; a3[7] = 8448; } /******************** * CORRELATION MATCH * ********************/ length = sub(sfend, dp); sfstart = dp; /*laccshift = L_shl(L_deposit_h(*accshift), 8); */ laccshift = L_deposit_h(*accshift); /* accshift scaled by 8 */ /* Perform before if * * statement. */ if (shiftrange != 0) { /* Limit the search range to control accshift */ shiftrangel = shiftranger = shiftrange; if (*accshift < 0) shiftrangel = add(shiftrangel, 1); if (*accshift > 0) shiftranger = add(shiftranger, 1); tmp = abs_s(*accshift); /* For non-periodic signals */ if ((beta < 6554 && tmp > 15 * 256) || (beta < 9830 && tmp > 30 * 256)) { if (*accshift < 0) shiftranger = 1; else shiftrangel = 1; } if (add(shiftrangel, shr(*accshift, 8)) > 72) { shiftrangel = sub(72, shr(*accshift, 8)); fprintf(stderr, "mdfyorig:*** Buffer limit. shiftrangel is:%d\n", shiftrangel); } if (sub(shiftranger, shr(*accshift, 8)) > 72) { shiftranger = add(72, shr(*accshift, 8)); fprintf(stderr, "mdfyorig:*** Buffer limit. shiftranger is:%d\n", shiftranger); } /* Create a buffer of modify residual for match at low cut-off frequency */ tmp = add(length, shiftrangel); tmp = add(tmp, shiftranger); ltmp = L_deposit_h(add(*accshift, shl(shiftrangel, 8))); for (i = 0; i <= tmp; i++) { /* POINTER ADDITION NOT CONVERTED -- NEED UNSIGNED ADDITION */ bl_intrp(Residual + i, residual + dp + i, ltmp, 16384, 3); } tmp = add(shiftrangel, shiftranger); /* Search for all integer delays of residual */ for (n = 0; n <= tmp; n++) { ex0y2[n] = 0; for (i = 0; i < length; i++) { ex0y2[n] = L_mac(ex0y2[n], Residual[n + i], TARGET[sfstart + i]); } ex0y2[n] = L_shr(ex0y2[n], 1); } /* Do quadratic interpolation of ex0y */ for (n = 1, k = 0; n < shiftrangel + shiftranger; n++) { for (j = 0; j < resolution; j++) { ex0y[k] = L_mpy_ls(ex0y2[n - 1], a1[j]); ltmp = L_mpy_ls(ex0y2[n], a2[j]); ex0y[k] = L_add(ex0y[k], ltmp); ltmp = L_mpy_ls(ex0y2[n + 1], a3[j]); ex0y[k] = L_add(ex0y[k], ltmp); k++; } } /* Find maximum with positive correlation */ y = 0; best = sub(shl(shiftrangel, 3), 4); for (n = 0; n < k; n++) { if (ex0y[n] > y) { y = ex0y[n]; best = n; } } /* best value not very accurate since ex0y[] calculation not percise. To correct * Residual[] should have more percision. This error does not seem to affect the * final output using the test data. */ /* Calculate energy in selected shift index */ e01 = e02 = 0; for (i = shiftrangel; i < length + shiftrangel; i++) e01 = L_mac(e01, Residual[i], Residual[i]); for (i = 0; i < length; i++) e02 = L_mac(e02, TARGET[i + sfstart], TARGET[i + sfstart]); if (e01 == 0 || e02 == 0) y = 0; else { shft_fctr1 = norm_l(y); y = L_shl(y, shft_fctr1); y = L_mpy_ll(y, y); /* TO RECOVER Y VALUE: * y = L_shl(y, 31-2*shft_fctr1) */ shft_fctr2 = norm_l(e01); e01 = L_shl(e01, shft_fctr2); shft_fctr3 = norm_l(e02); e02 = L_shl(e02, shft_fctr3); ltmp = L_mpy_ll(e01, e02); ltmp = L_mpy_ls(ltmp, 16056); ltmp = L_shl(ltmp, shl(shft_fctr1, 1) - add(add(2, shft_fctr2), shft_fctr3)); if (y > ltmp) { /*tmp = shl(shr(*accshift,8) + shiftrangel, 3) - (best + 4); */ tmp = *accshift + shl(shiftrangel, 8) - shl((best + 4), 5); *accshift = tmp; laccshift = L_deposit_h(tmp); /**accshift = shift_r(tmp, -3);*/ /* * if (laccshift == -75497472) * laccshift = -88080384; */ } } } for (k = 0; k < length; k++) { bl_intrp(residualm + dp + k, residual + dp + k, laccshift, BLFREQ, BLPRECISION); } *dpm = add(dp, length); }