static Word16 MR795_gain_code_quant_mod( /* o : index of quantization. */ Word16 gain_pit, /* i : pitch gain, Q14 */ Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */ Word16 gcode0, /* i : predicted CB gain (norm.), Q14 */ Word16 frac_en[], /* i : energy coefficients (4), fraction part, Q15 */ Word16 exp_en[], /* i : energy coefficients (4), eponent part, Q0 */ Word16 alpha, /* i : gain adaptor factor (>0), Q15 */ Word16 gain_cod_unq, /* i : Code gain (unquantized) */ /* (scaling: Q10 - exp_gcode0) */ Word16 *gain_cod, /* i/o: Code gain (pre-/quantized), Q1 */ Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */ /* (for MR122 MA predictor update) */ Word16 *qua_ener, /* o : quantized energy error, Q10 */ /* (for other MA predictor update) */ const Word16* qua_gain_code_ptr, /* i : ptr to read-only ptr */ Flag *pOverflow /* o : overflow indicator */ ) { const Word16 *p; Word16 i; Word16 index; Word16 tmp; Word16 one_alpha; Word16 exp; Word16 e_max; Word16 g2_pitch; Word16 g_code; Word16 g2_code_h; Word16 g2_code_l; Word16 d2_code_h; Word16 d2_code_l; Word16 coeff[5]; Word16 coeff_lo[5]; Word16 exp_coeff[5]; Word32 L_tmp; Word32 L_t0; Word32 L_t1; Word32 dist_min; Word16 gain_code; /* Steps in calculation of the error criterion (dist): --------------------------------------------------- underlined = constant; alp = FLP value of alpha, alpha = FIP ---------- ExEn = gp^2 * LtpEn + 2.0*gp*gc[i] * XC + gc[i]^2 * InnEn; ------------ ------ -- ----- aExEn= alp * ExEn = alp*gp^2*LtpEn + 2.0*alp*gp*XC* gc[i] + alp*InnEn* gc[i]^2 -------------- ------------- --------- = t[1] + t[2] + t[3] dist = d1 + d2; d1 = (1.0 - alp) * InnEn * (gcu - gc[i])^2 = t[4] ------------------- --- d2 = alp * (ResEn - 2.0 * sqrt(ResEn*ExEn) + ExEn); --- ----- --- ----- = alp * (sqrt(ExEn) - sqrt(ResEn))^2 --- ----------- = (sqrt(aExEn) - sqrt(alp*ResEn))^2 --------------- = (sqrt(aExEn) - t[0] )^2 ---- */ /* * calculate scalings of the constant terms */ gain_code = shl(*gain_cod, (10 - exp_gcode0), pOverflow); /* Q1 -> Q11 (-ec0) */ g2_pitch = mult(gain_pit, gain_pit, pOverflow); /* Q14 -> Q13 */ /* 0 < alpha <= 0.5 => 0.5 <= 1-alpha < 1, i.e one_alpha is normalized */ one_alpha = add_16((32767 - alpha), 1, pOverflow); /* 32768 - alpha */ /* alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */ L_t1 = L_mult(alpha, frac_en[1], pOverflow); L_t1 = L_shl(L_t1, 1, pOverflow); tmp = (Word16)(L_t1 >> 16); /* directly store in 32 bit variable because no further mult. required */ L_t1 = L_mult(tmp, g2_pitch, pOverflow); exp_coeff[1] = exp_en[1] - 15; tmp = (Word16)(L_shl(L_mult(alpha, frac_en[2], pOverflow), 1, pOverflow) >> 16); coeff[2] = mult(tmp, gain_pit, pOverflow); exp = exp_gcode0 - 10; exp_coeff[2] = add_16(exp_en[2], exp, pOverflow); /* alpha <= 0.5 -> mult. by 2 to keep precision; compensate in exponent */ coeff[3] = (Word16)(L_shl(L_mult(alpha, frac_en[3], pOverflow), 1, pOverflow) >> 16); exp = shl(exp_gcode0, 1, pOverflow) - 7; exp_coeff[3] = add_16(exp_en[3], exp, pOverflow); coeff[4] = mult(one_alpha, frac_en[3], pOverflow); exp_coeff[4] = add_16(exp_coeff[3], 1, pOverflow); L_tmp = L_mult(alpha, frac_en[0], pOverflow); /* sqrt_l returns normalized value and 2*exponent -> result = val >> (exp/2) exp_coeff holds 2*exponent for c[0] */ /* directly store in 32 bit variable because no further mult. required */ L_t0 = sqrt_l_exp(L_tmp, &exp, pOverflow); /* normalization included in sqrt_l_exp */ exp += 47; exp_coeff[0] = exp_en[0] - exp; /* * Determine the maximum exponent occuring in the distance calculation * and adjust all fractions accordingly (including a safety margin) * */ /* find max(e[1..4],e[0]+31) */ e_max = exp_coeff[0] + 31; for (i = 1; i <= 4; i++) { if (exp_coeff[i] > e_max) { e_max = exp_coeff[i]; } } /* scale c[1] (requires no further multiplication) */ tmp = e_max - exp_coeff[1]; L_t1 = L_shr(L_t1, tmp, pOverflow); /* scale c[2..4] (used in Mpy_32_16 in the quantizer loop) */ for (i = 2; i <= 4; i++) { tmp = e_max - exp_coeff[i]; L_tmp = ((Word32)coeff[i] << 16); L_tmp = L_shr(L_tmp, tmp, pOverflow); L_Extract(L_tmp, &coeff[i], &coeff_lo[i], pOverflow); } /* scale c[0] (requires no further multiplication) */ exp = e_max - 31; /* new exponent */ tmp = exp - exp_coeff[0]; L_t0 = L_shr(L_t0, shr(tmp, 1, pOverflow), pOverflow); /* perform correction by 1/sqrt(2) if exponent difference is odd */ if ((tmp & 0x1) != 0) { L_Extract(L_t0, &coeff[0], &coeff_lo[0], pOverflow); L_t0 = Mpy_32_16(coeff[0], coeff_lo[0], 23170, pOverflow); /* 23170 Q15 = 1/sqrt(2)*/ } /* search the quantizer table for the lowest value of the search criterion */ dist_min = MAX_32; index = 0; p = &qua_gain_code_ptr[0]; for (i = 0; i < NB_QUA_CODE; i++) { g_code = *p++; /* this is g_fac (Q11) */ p++; /* skip log2(g_fac) */ p++; /* skip 20*log10(g_fac) */ g_code = mult(g_code, gcode0, pOverflow); /* only continue if gc[i] < 2.0*gc which is equiv. to g_code (Q10-ec0) < gain_code (Q11-ec0) */ if (g_code >= gain_code) { break; } L_tmp = L_mult(g_code, g_code, pOverflow); L_Extract(L_tmp, &g2_code_h, &g2_code_l, pOverflow); tmp = sub(g_code, gain_cod_unq, pOverflow); L_tmp = L_mult(tmp, tmp, pOverflow); L_Extract(L_tmp, &d2_code_h, &d2_code_l, pOverflow); /* t2, t3, t4 */ L_tmp = Mac_32_16(L_t1, coeff[2], coeff_lo[2], g_code, pOverflow); L_tmp = Mac_32(L_tmp, coeff[3], coeff_lo[3], g2_code_h, g2_code_l, pOverflow); L_tmp = sqrt_l_exp(L_tmp, &exp, pOverflow); L_tmp = L_shr(L_tmp, shr(exp, 1, pOverflow), pOverflow); /* d2 */ tmp = pv_round(L_sub(L_tmp, L_t0, pOverflow), pOverflow); L_tmp = L_mult(tmp, tmp, pOverflow); /* dist */ L_tmp = Mac_32(L_tmp, coeff[4], coeff_lo[4], d2_code_h, d2_code_l, pOverflow); /* store table index if distance measure for this index is lower than the minimum seen so far */ if (L_tmp < dist_min) { dist_min = L_tmp; index = i; } } /*------------------------------------------------------------------* * read quantized gains and new values for MA predictor memories * * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * *------------------------------------------------------------------*/ /* Read the quantized gains */ p = &qua_gain_code_ptr[(index<<2) - index]; g_code = *p++; *qua_ener_MR122 = *p++; *qua_ener = *p; /*------------------------------------------------------------------* * calculate final fixed codebook gain: * * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * * * * gc = gc0 * g * *------------------------------------------------------------------*/ L_tmp = L_mult(g_code, gcode0, pOverflow); L_tmp = L_shr(L_tmp, 9 - exp_gcode0, pOverflow); *gain_cod = (Word16)(L_tmp >> 16); return index; }

/*---------------------------------------------------------------------------* * Function Dec_gain * * ~~~~~~~~~~~~~~~~~~ * * Decode the pitch and codebook gains * * * *---------------------------------------------------------------------------* * input arguments: * * * * index :Quantization index * * code[] :Innovative code vector * * L_subfr :Subframe size * * bfi :Bad frame indicator * * * * output arguments: * * * * gain_pit :Quantized pitch gain * * gain_cod :Quantized codebook gain * * * *---------------------------------------------------------------------------*/ void WebRtcG729fix_Dec_gain( Decod_ld8a_state *st, int16_t index, /* (i) :Index of quantization. */ int16_t code[], /* (i) Q13 :Innovative vector. */ int16_t L_subfr, /* (i) :Subframe length. */ int16_t bfi, /* (i) :Bad frame indicator */ int16_t *gain_pit, /* (o) Q14 :Pitch gain. */ int16_t *gain_cod /* (o) Q1 :Code gain. */ ) { int16_t index1, index2, tmp; int16_t gcode0, exp_gcode0; int32_t L_gbk12, L_acc, L_accb; /*-------------- Case of erasure. ---------------*/ if(bfi != 0){ *gain_pit = mult( *gain_pit, 29491 ); /* *0.9 in Q15 */ if (*gain_pit > 29491) *gain_pit = 29491; *gain_cod = mult( *gain_cod, 32111 ); /* *0.98 in Q15 */ /*----------------------------------------------* * update table of past quantized energies * * (frame erasure) * *----------------------------------------------*/ WebRtcG729fix_Gain_update_erasure(st->past_qua_en); return; } /*-------------- Decode pitch gain ---------------*/ index1 = WebRtcG729fix_imap1[ shr(index,NCODE2_B) ] ; index2 = WebRtcG729fix_imap2[ index & (NCODE2-1) ] ; *gain_pit = WebRtcSpl_AddSatW16( WebRtcG729fix_gbk1[index1][0], WebRtcG729fix_gbk2[index2][0] ); /*-------------- Decode codebook gain ---------------*/ /*---------------------------------------------------* *- energy due to innovation -* *- predicted energy -* *- predicted codebook gain => gcode0[exp_gcode0] -* *---------------------------------------------------*/ WebRtcG729fix_Gain_predict(st->past_qua_en, code, L_subfr, &gcode0, &exp_gcode0 ); /*-----------------------------------------------------------------* * *gain_code = (gbk1[indice1][1]+gbk2[indice2][1]) * gcode0; * *-----------------------------------------------------------------*/ L_acc = L_deposit_l( WebRtcG729fix_gbk1[index1][1] ); L_accb = L_deposit_l( WebRtcG729fix_gbk2[index2][1] ); L_gbk12 = WebRtcSpl_AddSatW32( L_acc, L_accb ); /* Q13 */ tmp = extract_l( L_shr( L_gbk12,1 ) ); /* Q12 */ L_acc = L_mult(tmp, gcode0); /* Q[exp_gcode0+12+1] */ L_acc = L_shl(L_acc, WebRtcSpl_AddSatW16( negate(exp_gcode0),(-12-1+1+16) )); *gain_cod = extract_h( L_acc ); /* Q1 */ /*----------------------------------------------* * update table of past quantized energies * *----------------------------------------------*/ WebRtcG729fix_Gain_update(st->past_qua_en, L_gbk12 ); return; }

void perc_var ( Word16 *gamma1, /* Bandwidth expansion parameter */ Word16 *gamma2, /* Bandwidth expansion parameter */ Word16 *LsfInt, /* Interpolated LSP vector : 1st subframe */ Word16 *LsfNew, /* New LSP vector : 2nd subframe */ Word16 *r_c /* Reflection coefficients */ ) { Word32 L_temp; Word16 cur_rc; /* Q11 */ Word16 Lar[4]; /* Q11 */ Word16 *LarNew; /* Q11 */ Word16 *Lsf; /* Q15 */ Word16 CritLar0, CritLar1; /* Q11 */ Word16 temp; Word16 d_min; /* Q10 */ Word16 i, k; for (k=0; k<M; k++) { LsfInt[k] = shl(LsfInt[k], 1); LsfNew[k] = shl(LsfNew[k], 1); } LarNew = &Lar[2]; /* ---------------------------------------- */ /* Reflection coefficients ---> Lar */ /* Lar(i) = log10( (1+rc) / (1-rc) ) */ /* Approximated by */ /* x <= SEG1 y = x */ /* SEG1 < x <= SEG2 y = A1 x - B1_L */ /* SEG2 < x <= SEG3 y = A2 x - B2_L */ /* x > SEG3 y = A3 x - B3_L */ /* ---------------------------------------- */ for (i=0; i<2; i++) { cur_rc = abs_s(r_c[i]); cur_rc = shr(cur_rc, 4); if (sub(cur_rc ,SEG1)<= 0) { LarNew[i] = cur_rc; } else { if (sub(cur_rc,SEG2)<= 0) { cur_rc = shr(cur_rc, 1); L_temp = L_mult(cur_rc, A1); L_temp = L_sub(L_temp, L_B1); L_temp = L_shr(L_temp, 11); LarNew[i] = extract_l(L_temp); } else { if (sub(cur_rc ,SEG3)<= 0) { cur_rc = shr(cur_rc, 1); L_temp = L_mult(cur_rc, A2); L_temp = L_sub(L_temp, L_B2); L_temp = L_shr(L_temp, 11); LarNew[i] = extract_l(L_temp); } else { cur_rc = shr(cur_rc, 1); L_temp = L_mult(cur_rc, A3); L_temp = L_sub(L_temp, L_B3); L_temp = L_shr(L_temp, 11); LarNew[i] = extract_l(L_temp); } } } if (r_c[i] < 0) { LarNew[i] = sub(0, LarNew[i]); } } /* Interpolation of Lar for the 1st subframe */ temp = add(LarNew[0], LarOld[0]); Lar[0] = shr(temp, 1); LarOld[0] = LarNew[0]; temp = add(LarNew[1], LarOld[1]); Lar[1] = shr(temp, 1); LarOld[1] = LarNew[1]; for (k=0; k<2; k++) { /* LOOP : gamma2 for 1st to 2nd subframes */ /* ---------------------------------------------------------- */ /* First criterion based on the first two Lars */ /* smooth == 1 ==> gamma2 can vary from 0.4 to 0.7 */ /* smooth == 0 ==> gamma2 is set to 0.6 */ /* */ /* Double threshold + hysteresis : */ /* if smooth = 1 */ /* if (CritLar0 < THRESH_L1) and (CritLar1 > THRESH_H1) */ /* smooth = 0 */ /* if smooth = 0 */ /* if (CritLar0 > THRESH_L2) or (CritLar1 < THRESH_H2) */ /* smooth = 1 */ /* ---------------------------------------------------------- */ CritLar0 = Lar[2*k]; CritLar1 = Lar[2*k+1]; if (smooth != 0) { if ((sub(CritLar0,THRESH_L1)<0)&&( sub(CritLar1,THRESH_H1)>0)) { smooth = 0; } } else { if ( (sub(CritLar0 ,THRESH_L2)>0) || (sub(CritLar1,THRESH_H2) <0) ) { smooth = 1; } } if (smooth == 0) { /* ------------------------------------------------------ */ /* Second criterion based on the minimum distance between */ /* two successives LSPs */ /* */ /* gamma2[k] = -6.0 * pi * d_min + 1.0 */ /* */ /* with Lsfs normalized range 0.0 <= val <= 1.0 */ /* ------------------------------------------------------ */ gamma1[k] = GAMMA1_0; if (k == 0) { Lsf = LsfInt; } else { Lsf = LsfNew; } d_min = sub(Lsf[1], Lsf[0]); for (i=1; i<M-1; i++) { temp = sub(Lsf[i+1],Lsf[i]); if (sub(temp,d_min)<0) { d_min = temp; } } temp = mult(ALPHA, d_min); temp = sub(BETA, temp); temp = shl(temp, 5); gamma2[k] = temp; if (sub(gamma2[k] , GAMMA2_0_H)>0) { gamma2[k] = GAMMA2_0_H; } if (sub(gamma2[k] ,GAMMA2_0_L)<0) { gamma2[k] = GAMMA2_0_L; } } else { gamma1[k] = GAMMA1_1; gamma2[k] = GAMMA2_1; } } return; }

/*-----------------------------------------------------------------* * Functions vad * * ~~~ * * Input: * * rc : reflection coefficient * * lsf[] : unquantized lsf vector * * r_h[] : upper 16-bits of the autocorrelation vector * * r_l[] : lower 16-bits of the autocorrelation vector * * exp_R0 : exponent of the autocorrelation vector * * sigpp[] : preprocessed input signal * * frm_count : frame counter * * prev_marker : VAD decision of the last frame * * pprev_marker : VAD decision of the frame before last frame * * * * Output: * * * * marker : VAD decision of the current frame * * * *-----------------------------------------------------------------*/ void vad( Word16 rc, Word16 *lsf, Word16 *r_h, Word16 *r_l, Word16 exp_R0, Word16 *sigpp, Word16 frm_count, Word16 prev_marker, Word16 pprev_marker, Word16 *marker) { /* scalar */ Word32 acc0; Word16 i, j, exp, frac; Word16 ENERGY, ENERGY_low, SD, ZC, dSE, dSLE, dSZC; Word16 COEF, C_COEF, COEFZC, C_COEFZC, COEFSD, C_COEFSD; /* compute the frame energy */ acc0 = L_Comp(r_h[0], r_l[0]); Log2(acc0, &exp, &frac); acc0 = Mpy_32_16(exp, frac, 9864); i = sub(exp_R0, 1); i = sub(i, 1); acc0 = L_mac(acc0, 9864, i); acc0 = L_shl(acc0, 11); ENERGY = extract_h(acc0); ENERGY = sub(ENERGY, 4875); /* compute the low band energy */ acc0 = 0; for (i=1; i<=NP; i++) acc0 = L_mac(acc0, r_h[i], lbf_corr[i]); acc0 = L_shl(acc0, 1); acc0 = L_mac(acc0, r_h[0], lbf_corr[0]); Log2(acc0, &exp, &frac); acc0 = Mpy_32_16(exp, frac, 9864); i = sub(exp_R0, 1); i = sub(i, 1); acc0 = L_mac(acc0, 9864, i); acc0 = L_shl(acc0, 11); ENERGY_low = extract_h(acc0); ENERGY_low = sub(ENERGY_low, 4875); /* compute SD */ acc0 = 0; for (i=0; i<M; i++){ j = sub(lsf[i], MeanLSF[i]); acc0 = L_mac(acc0, j, j); } SD = extract_h(acc0); /* Q15 */ /* compute # zero crossing */ ZC = 0; for (i=ZC_START+1; i<=ZC_END; i++) if (mult(sigpp[i-1], sigpp[i]) < 0) ZC = add(ZC, 410); /* Q15 */ /* Initialize and update Mins */ if(sub(frm_count, 129) < 0){ if (sub(ENERGY, Min) < 0){ Min = ENERGY; Prev_Min = ENERGY; } if((frm_count & 0x0007) == 0){ i = sub(shr(frm_count,3),1); Min_buffer[i] = Min; Min = MAX_16; } } if((frm_count & 0x0007) == 0){ Prev_Min = Min_buffer[0]; for (i=1; i<16; i++){ if (sub(Min_buffer[i], Prev_Min) < 0) Prev_Min = Min_buffer[i]; } } if(sub(frm_count, 129) >= 0){ if(((frm_count & 0x0007) ^ (0x0001)) == 0){ Min = Prev_Min; Next_Min = MAX_16; } if (sub(ENERGY, Min) < 0) Min = ENERGY; if (sub(ENERGY, Next_Min) < 0) Next_Min = ENERGY; if((frm_count & 0x0007) == 0){ for (i=0; i<15; i++) Min_buffer[i] = Min_buffer[i+1]; Min_buffer[15] = Next_Min; Prev_Min = Min_buffer[0]; for (i=1; i<16; i++) if (sub(Min_buffer[i], Prev_Min) < 0) Prev_Min = Min_buffer[i]; } } if (sub(frm_count, INIT_FRAME) <= 0) if(sub(ENERGY, 3072) < 0){ *marker = NOISE; less_count++; } else{ *marker = VOICE; acc0 = L_deposit_h(MeanE); acc0 = L_mac(acc0, ENERGY, 1024); MeanE = extract_h(acc0); acc0 = L_deposit_h(MeanSZC); acc0 = L_mac(acc0, ZC, 1024); MeanSZC = extract_h(acc0); for (i=0; i<M; i++){ acc0 = L_deposit_h(MeanLSF[i]); acc0 = L_mac(acc0, lsf[i], 1024); MeanLSF[i] = extract_h(acc0); } } if (sub(frm_count, INIT_FRAME) >= 0){ if (sub(frm_count, INIT_FRAME) == 0){ acc0 = L_mult(MeanE, factor_fx[less_count]); acc0 = L_shl(acc0, shift_fx[less_count]); MeanE = extract_h(acc0); acc0 = L_mult(MeanSZC, factor_fx[less_count]); acc0 = L_shl(acc0, shift_fx[less_count]); MeanSZC = extract_h(acc0); for (i=0; i<M; i++){ acc0 = L_mult(MeanLSF[i], factor_fx[less_count]); acc0 = L_shl(acc0, shift_fx[less_count]); MeanLSF[i] = extract_h(acc0); } MeanSE = sub(MeanE, 2048); /* Q11 */ MeanSLE = sub(MeanE, 2458); /* Q11 */ } dSE = sub(MeanSE, ENERGY); dSLE = sub(MeanSLE, ENERGY_low); dSZC = sub(MeanSZC, ZC); if(sub(ENERGY, 3072) < 0) *marker = NOISE; else *marker = MakeDec(dSLE, dSE, SD, dSZC); v_flag = 0; if((prev_marker==VOICE) && (*marker==NOISE) && (add(dSE,410) < 0) && (sub(ENERGY, 3072)>0)){ *marker = VOICE; v_flag = 1; } if(flag == 1){ if((pprev_marker == VOICE) && (prev_marker == VOICE) && (*marker == NOISE) && (sub(abs_s(sub(prev_energy,ENERGY)), 614) <= 0)){ count_ext++; *marker = VOICE; v_flag = 1; if(sub(count_ext, 4) <= 0) flag=1; else{ count_ext=0; flag=0; } } } else flag=1; if(*marker == NOISE) count_sil++; if((*marker == VOICE) && (sub(count_sil, 10) > 0) && (sub(sub(ENERGY,prev_energy), 614) <= 0)){ *marker = NOISE; count_sil=0; } if(*marker == VOICE) count_sil=0; if ((sub(sub(ENERGY, 614), MeanSE) < 0) && (sub(frm_count, 128) > 0) && (!v_flag) && (sub(rc, 19661) < 0)) *marker = NOISE; if ((sub(sub(ENERGY,614),MeanSE) < 0) && (sub(rc, 24576) < 0) && (sub(SD, 83) < 0)){ count_update++; if (sub(count_update, INIT_COUNT) < 0){ COEF = 24576; C_COEF = 8192; COEFZC = 26214; C_COEFZC = 6554; COEFSD = 19661; C_COEFSD = 13017; } else if (sub(count_update, INIT_COUNT+10) < 0){ COEF = 31130; C_COEF = 1638; COEFZC = 30147; C_COEFZC = 2621; COEFSD = 21299; C_COEFSD = 11469; } else if (sub(count_update, INIT_COUNT+20) < 0){ COEF = 31785; C_COEF = 983; COEFZC = 30802; C_COEFZC = 1966; COEFSD = 22938; C_COEFSD = 9830; } else if (sub(count_update, INIT_COUNT+30) < 0){ COEF = 32440; C_COEF = 328; COEFZC = 31457; C_COEFZC = 1311; COEFSD = 24576; C_COEFSD = 8192; } else if (sub(count_update, INIT_COUNT+40) < 0){ COEF = 32604; C_COEF = 164; COEFZC = 32440; C_COEFZC = 328; COEFSD = 24576; C_COEFSD = 8192; } else{ COEF = 32604; C_COEF = 164; COEFZC = 32702; C_COEFZC = 66; COEFSD = 24576; C_COEFSD = 8192; } /* compute MeanSE */ acc0 = L_mult(COEF, MeanSE); acc0 = L_mac(acc0, C_COEF, ENERGY); MeanSE = extract_h(acc0); /* compute MeanSLE */ acc0 = L_mult(COEF, MeanSLE); acc0 = L_mac(acc0, C_COEF, ENERGY_low); MeanSLE = extract_h(acc0); /* compute MeanSZC */ acc0 = L_mult(COEFZC, MeanSZC); acc0 = L_mac(acc0, C_COEFZC, ZC); MeanSZC = extract_h(acc0); /* compute MeanLSF */ for (i=0; i<M; i++){ acc0 = L_mult(COEFSD, MeanLSF[i]); acc0 = L_mac(acc0, C_COEFSD, lsf[i]); MeanLSF[i] = extract_h(acc0); } } if((sub(frm_count, 128) > 0) && (((sub(MeanSE,Min) < 0) && (sub(SD, 83) < 0)) || (sub(MeanSE,Min) > 2048))){ MeanSE = Min; count_update = 0; } } prev_energy = ENERGY; }

inline ivect mult(ivect v, imat M) { return matr2vec(mult(vec2matr(v), M), 0); }

static int three_register_execute(UM_machine machine, Um_instruction instruction) { unsigned op = Bitpack_getu(instruction, BITS_PER_OP_CODE, 28); unsigned A = Bitpack_getu(instruction, BITS_PER_REG_NUM, 6); unsigned B = Bitpack_getu(instruction, BITS_PER_REG_NUM, 3); unsigned C = Bitpack_getu(instruction, BITS_PER_REG_NUM, 0); switch(op){ case 0: // movc(machine, A, B, C); return 1; case 1: segl(machine, A, B, C); return 1; case 2: segs(machine, A, B, C); return 1; case 3: add(machine, A, B, C); return 1; case 4: mult(machine, A, B, C); return 1; case 5: divi(machine, A, B, C); return 1; case 6: nand(machine, A, B, C); return 1; case 7: return 0; case 8: map(machine, B, C); return 1; case 9: unmap(machine, C); return 1; case 10: out(machine, C); return 1; case 11: in(machine, C); return 1; case 12: loadp(machine, B, C); return 1; default: return 0; } }

std::uint16_t VirtualCPU::executeInstructionAtAddress(std::uint16_t address) { debugger_.pc_ = address; //HACK FOR NOW switch (memoryController_.readAtAddress(address)) { case 0: return address + halt(); case 1: return address + set(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 2: return address + push(memoryController_.readAtAddress(address + 1)); case 3: return address + pop(memoryController_.readAtAddress(address + 1)); case 4: return address + eq(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 5: return address + gt(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address +2), memoryController_.readAtAddress(address + 3)); case 6: return jump(memoryController_.readAtAddress(address + 1), address); case 7: return jt(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), address); case 8: return jf(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), address); case 9: return address + add(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 10: return address + mult(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 11: return address + mod(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 12: return address + and_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 13: return address + or_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2), memoryController_.readAtAddress(address + 3)); case 14: return address + not_i(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 15: return address + rmem(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 16: return address + wmem(memoryController_.readAtAddress(address + 1), memoryController_.readAtAddress(address + 2)); case 17: return call(memoryController_.readAtAddress(address + 1), address); case 18: return ret(); case 19: return address + out(memoryController_.readAtAddress(address + 1)); case 21: return address + noop(); case 20: return address + in(memoryController_.readAtAddress(address + 1)); default: std::cout << "CPU ERROR illegal op code: " << memoryController_.readAtAddress(address) << std::endl; return address + 1; } }

void Dec_lag3( Word16 index, /* input : received pitch index */ Word16 pit_min, /* input : minimum pitch lag */ Word16 pit_max, /* input : maximum pitch lag */ Word16 i_subfr, /* input : subframe flag */ Word16 *T0, /* output: integer part of pitch lag */ Word16 *T0_frac /* output: fractional part of pitch lag */ ) { Word16 i; Word16 T0_min, T0_max; if (i_subfr == 0) /* if 1st subframe */ { if (sub(index, 197) < 0) { /* *T0 = (index+2)/3 + 19 */ *T0 = add(mult(add(index, 2), 10923), 19); /* *T0_frac = index - *T0*3 + 58 */ i = add(add(*T0, *T0), *T0); *T0_frac = add(sub(index, i), 58); } else { *T0 = sub(index, 112); *T0_frac = 0; } } else /* second subframe */ { /* find T0_min and T0_max for 2nd subframe */ T0_min = sub(*T0, 5); if (sub(T0_min, pit_min) < 0) { T0_min = pit_min; } T0_max = add(T0_min, 9); if (sub(T0_max, pit_max) > 0) { T0_max = pit_max; T0_min = sub(T0_max, 9); } /* i = (index+2)/3 - 1 */ /* *T0 = i + t0_min; */ i = sub(mult(add(index, 2), 10923), 1); *T0 = add(i, T0_min); /* t0_frac = index - 2 - i*3; */ i = add(add(i, i), i); *T0_frac = sub(sub(index, 2), i); } return; }

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// inline void mult_left(const quaternion& left) { mult(left, *this); }

DiagonalMatrix &DM::multT(const DiagonalMatrix &S, DiagonalMatrix &ans, double scal) const { return mult(S, ans, scal); }

void Coder_ld8a( g729a_encoder_state *state, Word16 ana[] /* output : Analysis parameters */ ) { /* LPC analysis */ Word16 Aq_t[(MP1)*2]; /* A(z) quantized for the 2 subframes */ Word16 Ap_t[(MP1)*2]; /* A(z/gamma) for the 2 subframes */ Word16 *Aq, *Ap; /* Pointer on Aq_t and Ap_t */ /* Other vectors */ Word16 h1[L_SUBFR]; /* Impulse response h1[] */ Word16 xn[L_SUBFR]; /* Target vector for pitch search */ Word16 xn2[L_SUBFR]; /* Target vector for codebook search */ Word16 code[L_SUBFR]; /* Fixed codebook excitation */ Word16 y1[L_SUBFR]; /* Filtered adaptive excitation */ Word16 y2[L_SUBFR]; /* Filtered fixed codebook excitation */ Word16 g_coeff[4]; /* Correlations between xn & y1 */ Word16 g_coeff_cs[5]; Word16 exp_g_coeff_cs[5]; /* Correlations between xn, y1, & y2 <y1,y1>, -2<xn,y1>, <y2,y2>, -2<xn,y2>, 2<y1,y2> */ /* Scalars */ Word16 i, j, k, i_subfr; Word16 T_op, T0, T0_min, T0_max, T0_frac; Word16 gain_pit, gain_code, index; Word16 temp, taming; Word32 L_temp; /*------------------------------------------------------------------------* * - Perform LPC analysis: * * * autocorrelation + lag windowing * * * Levinson-durbin algorithm to find a[] * * * convert a[] to lsp[] * * * quantize and code the LSPs * * * find the interpolated LSPs and convert to a[] for the 2 * * subframes (both quantized and unquantized) * *------------------------------------------------------------------------*/ { /* Temporary vectors */ Word16 r_l[MP1], r_h[MP1]; /* Autocorrelations low and hi */ Word16 rc[M]; /* Reflection coefficients. */ Word16 lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe */ /* LP analysis */ Autocorr(state->p_window, M, r_h, r_l); /* Autocorrelations */ Lag_window(M, r_h, r_l); /* Lag windowing */ Levinson(r_h, r_l, Ap_t, rc); /* Levinson Durbin */ Az_lsp(Ap_t, lsp_new, state->lsp_old); /* From A(z) to lsp */ /* LSP quantization */ Qua_lsp(state, lsp_new, lsp_new_q, ana); ana += 2; /* Advance analysis parameters pointer */ /*--------------------------------------------------------------------* * Find interpolated LPC parameters in all subframes * * The interpolated parameters are in array Aq_t[]. * *--------------------------------------------------------------------*/ Int_qlpc(state->lsp_old_q, lsp_new_q, Aq_t); /* Compute A(z/gamma) */ Weight_Az(&Aq_t[0], GAMMA1, M, &Ap_t[0]); Weight_Az(&Aq_t[MP1], GAMMA1, M, &Ap_t[MP1]); /* update the LSPs for the next frame */ Copy(lsp_new, state->lsp_old, M); Copy(lsp_new_q, state->lsp_old_q, M); } /*----------------------------------------------------------------------* * - Find the weighted input speech w_sp[] for the whole speech frame * * - Find the open-loop pitch delay * *----------------------------------------------------------------------*/ Residu(&Aq_t[0], &(state->speech[0]), &(state->exc[0]), L_SUBFR); Residu(&Aq_t[MP1], &(state->speech[L_SUBFR]), &(state->exc[L_SUBFR]), L_SUBFR); { Word16 Ap1[MP1]; Ap = Ap_t; Ap1[0] = 4096; for(i=1; i<=M; i++) /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */ Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938)); Syn_filt(Ap1, &(state->exc[0]), &(state->wsp[0]), L_SUBFR, state->mem_w, 1); Ap += MP1; for(i=1; i<=M; i++) /* Ap1[i] = Ap[i] - 0.7 * Ap[i-1]; */ Ap1[i] = sub(Ap[i], mult(Ap[i-1], 22938)); Syn_filt(Ap1, &(state->exc[L_SUBFR]), &(state->wsp[L_SUBFR]), L_SUBFR, state->mem_w, 1); } /* Find open loop pitch lag */ T_op = Pitch_ol_fast(state->wsp, PIT_MAX, L_FRAME); /* Range for closed loop pitch search in 1st subframe */ T0_min = T_op - 3; T0_max = T0_min + 6; if (T0_min < PIT_MIN) { T0_min = PIT_MIN; T0_max = PIT_MIN + 6; } else if (T0_max > PIT_MAX) { T0_max = PIT_MAX; T0_min = PIT_MAX - 6; } /*------------------------------------------------------------------------* * Loop for every subframe in the analysis frame * *------------------------------------------------------------------------* * To find the pitch and innovation parameters. The subframe size is * * L_SUBFR and the loop is repeated 2 times. * * - find the weighted LPC coefficients * * - find the LPC residual signal res[] * * - compute the target signal for pitch search * * - compute impulse response of weighted synthesis filter (h1[]) * * - find the closed-loop pitch parameters * * - encode the pitch delay * * - find target vector for codebook search * * - codebook search * * - VQ of pitch and codebook gains * * - update states of weighting filter * *------------------------------------------------------------------------*/ Aq = Aq_t; /* pointer to interpolated quantized LPC parameters */ Ap = Ap_t; /* pointer to weighted LPC coefficients */ for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) { /*---------------------------------------------------------------* * Compute impulse response, h1[], of weighted synthesis filter * *---------------------------------------------------------------*/ h1[0] = 4096; Set_zero(&h1[1], L_SUBFR-1); Syn_filt(Ap, h1, h1, L_SUBFR, &h1[1], 0); /*----------------------------------------------------------------------* * Find the target vector for pitch search: * *----------------------------------------------------------------------*/ Syn_filt(Ap, &(state->exc[i_subfr]), xn, L_SUBFR, state->mem_w0, 0); /*---------------------------------------------------------------------* * Closed-loop fractional pitch search * *---------------------------------------------------------------------*/ T0 = Pitch_fr3_fast(&(state->exc[i_subfr]), xn, h1, L_SUBFR, T0_min, T0_max, i_subfr, &T0_frac); index = Enc_lag3(T0, T0_frac, &T0_min, &T0_max,PIT_MIN,PIT_MAX,i_subfr); *ana++ = index; if (i_subfr == 0) { *ana++ = Parity_Pitch(index); } /*-----------------------------------------------------------------* * - find filtered pitch exc * * - compute pitch gain and limit between 0 and 1.2 * * - update target vector for codebook search * *-----------------------------------------------------------------*/ Syn_filt(Ap, &(state->exc[i_subfr]), y1, L_SUBFR, state->mem_zero, 0); gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR); /* clip pitch gain if taming is necessary */ taming = test_err(state, T0, T0_frac); if( taming == 1){ if (gain_pit > GPCLIP) { gain_pit = GPCLIP; } } /* xn2[i] = xn[i] - y1[i] * gain_pit */ for (i = 0; i < L_SUBFR; i++) { //L_temp = L_mult(y1[i], gain_pit); //L_temp = L_shl(L_temp, 1); /* gain_pit in Q14 */ L_temp = ((Word32)y1[i] * gain_pit) << 2; xn2[i] = sub(xn[i], extract_h(L_temp)); } /*-----------------------------------------------------* * - Innovative codebook search. * *-----------------------------------------------------*/ index = ACELP_Code_A(xn2, h1, T0, state->sharp, code, y2, &i); *ana++ = index; /* Positions index */ *ana++ = i; /* Signs index */ /*-----------------------------------------------------* * - Quantization of gains. * *-----------------------------------------------------*/ g_coeff_cs[0] = g_coeff[0]; /* <y1,y1> */ exp_g_coeff_cs[0] = negate(g_coeff[1]); /* Q-Format:XXX -> JPN */ g_coeff_cs[1] = negate(g_coeff[2]); /* (xn,y1) -> -2<xn,y1> */ exp_g_coeff_cs[1] = negate(add(g_coeff[3], 1)); /* Q-Format:XXX -> JPN */ Corr_xy2( xn, y1, y2, g_coeff_cs, exp_g_coeff_cs ); /* Q0 Q0 Q12 ^Qx ^Q0 */ /* g_coeff_cs[3]:exp_g_coeff_cs[3] = <y2,y2> */ /* g_coeff_cs[4]:exp_g_coeff_cs[4] = -2<xn,y2> */ /* g_coeff_cs[5]:exp_g_coeff_cs[5] = 2<y1,y2> */ *ana++ = Qua_gain(code, g_coeff_cs, exp_g_coeff_cs, L_SUBFR, &gain_pit, &gain_code, taming); /*------------------------------------------------------------* * - Update pitch sharpening "sharp" with quantized gain_pit * *------------------------------------------------------------*/ state->sharp = gain_pit; if (state->sharp > SHARPMAX) { state->sharp = SHARPMAX; } else if (state->sharp < SHARPMIN) { state->sharp = SHARPMIN; } /*------------------------------------------------------* * - Find the total excitation * * - update filters memories for finding the target * * vector in the next subframe * *------------------------------------------------------*/ for (i = 0; i < L_SUBFR; i++) { /* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */ /* exc[i] in Q0 gain_pit in Q14 */ /* code[i] in Q13 gain_cod in Q1 */ //L_temp = L_mult(exc[i+i_subfr], gain_pit); //L_temp = L_mac(L_temp, code[i], gain_code); //L_temp = L_shl(L_temp, 1); L_temp = (Word32)(state->exc[i+i_subfr]) * (Word32)gain_pit + (Word32)code[i] * (Word32)gain_code; L_temp <<= 2; state->exc[i+i_subfr] = g_round(L_temp); } update_exc_err(state, gain_pit, T0); for (i = L_SUBFR-M, j = 0; i < L_SUBFR; i++, j++) { temp = ((Word32)y1[i] * (Word32)gain_pit) >> 14; k = ((Word32)y2[i] * (Word32)gain_code) >> 13; state->mem_w0[j] = sub(xn[i], add(temp, k)); } Aq += MP1; /* interpolated LPC parameters for next subframe */ Ap += MP1; } /*--------------------------------------------------* * Update signal for next frame. * * -> shift to the left by L_FRAME: * * speech[], wsp[] and exc[] * *--------------------------------------------------*/ Copy(&(state->old_speech[L_FRAME]), &(state->old_speech[0]), L_TOTAL-L_FRAME); Copy(&(state->old_wsp[L_FRAME]), &(state->old_wsp[0]), PIT_MAX); Copy(&(state->old_exc[L_FRAME]), &(state->old_exc[0]), PIT_MAX+L_INTERPOL); }

void loop(void) { const float spd1=30.0f*dt(); const float spd2=15.0f*dt(); float r=0.0f; float dc=0.0f, ds=0.0f; float tc=0.0f, ts=0.0f; /*viewport(0,0,nsw,nsh);*/ use_basic(); send_lpview(pview); if(mm() || mw()!=0.0f) { if(mm()) { dir+=mx()*spd1; if(dir<-0.0f) { dir+=360.0f; ndir+=360.0f; } else if(dir>360.0f) { dir-=360.0f; ndir-=360.0f; } tilt-=my()*spd1; if(tilt<0.001f) tilt=0.001f; else if(tilt>100.0f) tilt=100.0f; } if(mw()!=0.0f) { zoom-=mw(); if(zoom<7.0f) zoom=7.0f; else if(zoom>12.0f) zoom=12.0f; } } if(mp(3)) zoom=10.0f; ndir-=(ndir-dir)*spd2; r=d2r(ndir); dc=cosf(r); ds=sinf(r); ntilt-=(ntilt-tilt)*spd2; r=d2r(ntilt); tc=cosf(r); ts=sinf(r); nzoom-=(nzoom-zoom)*spd2; look(view,v3(ds*ts*nzoom,dc*ts*nzoom,3.0f+(tc*nzoom)),v3(0.0f,0.0f,3.0f)); mult(pview,proj,view); send_pview(pview); /* ----- */ use_fb(fbos[0]); clear(); /* ----- */ use_tex(area_tex); /* ----- */ draw_vbo(&area_mod); use_fb(0); blit_fb(fbos[0],fbos[1],nsw,nsh); /* ----- */ use_fb(fbos[2]); set_drawbufs(2); clear(); draw_vbo(&area_mod); use_fb(0); use_mblur(); use_tex(texs[0]); /* ----- */ active_tex(GL_TEXTURE1,texs[2]); default_tex(); use_fb(fbos[3]); set_drawbufs(1); clear(); quad(); use_fb(0); active_tex(GL_TEXTURE1,0); default_tex(); /*viewport(0,0,sw,sh);*/ use_vig(); use_tex(texs[3]); clear(); quad(); if(kp(SDL_SCANCODE_DELETE) || kp(SDL_SCANCODE_ESCAPE)) quit(); }

/*inline*/ vertex3f vertex3f::operator*(const float op) const { return mult(op); }

int main(int argc, char**argv) { int block_sz = atoi(argv[1]); int blocks_nr = atoi(argv[2]); int edge_sz = atoi(argv[3]); std::vector<Block> bs(blocks_nr, Block(block_sz,edge_sz) ); Edge e(edge_sz,blocks_nr,block_sz); for (int i=0; i < bs.size(); ++i) bs[i].fill(); e.fill(); // for (int i=0; i < bs.size(); ++i) // std::cout << bs[i] << '\n'; // std::cout << e << '\n'; std::vector<Block> bs0 = bs; Edge e0 = e; double total_time=0.0; clock_t st, end; st = clock(); for (int i=0; i < bs.size(); ++i) bs[i].fwd(); end = clock(); total_time += double((end - st)) / CLOCKS_PER_SEC; for (int b=0; b < bs.size(); ++b) { for (int br=0; br < bs[b].rows; ++br) { double& diag = bs[b](br,br); RowNO r; r.v = &diag; r.sz = bs[b].cols-br; r.row_no = bs[b].rows*b + br; st = clock(); elim_col(e,r); end = clock(); total_time += double((end - st)) / CLOCKS_PER_SEC; } } st = clock(); e.fwd(); e.bwd(); for (int i=0; i < bs.size(); ++i){ bs[i].bwd(e.edge_soln); } end = clock(); total_time += double((end - st)) / CLOCKS_PER_SEC; std::cout << "elapsed " << total_time << '\n'; std::vector<double> soln(bs[0].rows*bs.size() + edge_sz); for (int i=0; i < bs.size(); ++i) for (int k=0; k < bs[i].sol.size(); ++k) soln[i*block_sz+k] = bs[i].sol[k]; for (int k=0; k < e.edge_soln.size(); ++k) soln[blocks_nr*block_sz+k] = e.edge_soln[k]; // for (int k=0; k < soln.size(); ++k) // std::cout << soln[k] << '\n'; std::vector<double> Rhs; for(int i = 0; i < bs0.size(); ++i) { for(int r=0; r < bs0[i].rows; ++r) Rhs.push_back(bs0[i](r,bs0[i].cols-1)); } for(int i = 0; i < e0.rows; ++i) { Rhs.push_back(e0(i,e0.cols-1)); } std::vector<double> rhs = mult(bs0,e0,soln); double residue=0.0; for (int i=0; i < rhs.size(); ++i) residue += fabs(rhs[i] - Rhs[i]); std::cout << "\n\nres = " << residue << '\n'; }

Node Node::operator*(const Node &node) const { return mult(node); }

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// inline void mult_right(const quaternion& right) { mult(*this, right); }

void D_plsf_3( D_plsfState *st, /* i/o: State struct */ enum Mode mode, /* i : coder mode */ Word16 bfi, /* i : bad frame indicator (set to 1 if a */ /* bad frame is received) */ Word16 * indice, /* i : quantization indices of 3 submatrices, Q0 */ Word16 * lsp1_q /* o : quantized 1st LSP vector, Q15 */ ) { Word16 i, index; Word16 *p_cb1, *p_cb2, *p_cb3, *p_dico, temp; Word16 lsf1_r[M]; Word16 lsf1_q[M]; if (bfi != 0) /* if bad frame */ { /* use the past LSFs slightly shifted towards their mean */ for (i = 0; i < M; i++) { /* lsfi_q[i] = ALPHA*past_lsf_q[i] + ONE_ALPHA*mean_lsf[i]; */ lsf1_q[i] = add(mult(st->past_lsf_q[i], ALPHA), mult(mean_lsf[i], ONE_ALPHA)); } /* estimate past quantized residual to be used in next frame */ if (mode != MRDTX) { for (i = 0; i < M; i++) { /* temp = mean_lsf[i] + past_r2_q[i] * PRED_FAC; */ temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i])); st->past_r_q[i] = sub(lsf1_q[i], temp); } } else { for (i = 0; i < M; i++) { /* temp = mean_lsf[i] + past_r2_q[i]; */ temp = add(mean_lsf[i], st->past_r_q[i]); st->past_r_q[i] = sub(lsf1_q[i], temp); } } } else /* if good LSFs received */ { if (mode == MR475 || mode == MR515) { /* MR475, MR515 */ p_cb1 = dico1_lsf; p_cb2 = dico2_lsf; p_cb3 = mr515_3_lsf; } else if (mode == MR795) { /* MR795 */ p_cb1 = mr795_1_lsf; p_cb2 = dico2_lsf; p_cb3 = dico3_lsf; } else { /* MR59, MR67, MR74, MR102, MRDTX */ p_cb1 = dico1_lsf; p_cb2 = dico2_lsf; p_cb3 = dico3_lsf; } /* decode prediction residuals from 3 received indices */ index = *indice++; p_dico = &p_cb1[/*add(index, add(index, index))*/ 3 * index]; lsf1_r[0] = *p_dico++; lsf1_r[1] = *p_dico++; lsf1_r[2] = *p_dico++; index = *indice++; if (mode == MR475 || mode == MR515) { /* MR475, MR515 only using every second entry */ /*index = shl(index,1);*/ index <<= 1; } p_dico = &p_cb2[add(index, add(index, index))]; lsf1_r[3] = *p_dico++; lsf1_r[4] = *p_dico++; lsf1_r[5] = *p_dico++; index = *indice++; p_dico = &p_cb3[shl(index, 2)]; lsf1_r[6] = *p_dico++; lsf1_r[7] = *p_dico++; lsf1_r[8] = *p_dico++; lsf1_r[9] = *p_dico++; /* Compute quantized LSFs and update the past quantized residual */ if (mode != MRDTX) for (i = 0; i < M; i++) { temp = add(mean_lsf[i], mult(st->past_r_q[i], pred_fac[i])); lsf1_q[i] = add(lsf1_r[i], temp); st->past_r_q[i] = lsf1_r[i]; } else for (i = 0; i < M; i++) { temp = add(mean_lsf[i], st->past_r_q[i]); lsf1_q[i] = add(lsf1_r[i], temp); st->past_r_q[i] = lsf1_r[i]; } } /* verification that LSFs has minimum distance of LSF_GAP Hz */ Reorder_lsf(lsf1_q, LSF_GAP, M); Copy (lsf1_q, st->past_lsf_q, M); /* convert LSFs to the cosine domain */ Lsf_lsp(lsf1_q, lsp1_q, M); }

void pulsesequence() { /* Internal variable declarations *************************/ int shapelist90,shapelist180; double seqtime,tau1,tau2,tau3,te1_delay,te2_delay,te3_delay,tr_delay; double freq90[MAXNSLICE], freq180[MAXNSLICE]; /* Diffusion variables */ double te1, te1min, del1, del2, del3, del4; double te_diff1, te_diff2, tmp1, tmp2; double diffamp; char diffpat[MAXSTR]; /* Navigator variables */ double etlnav; /* Variable crushers */ double cscale; double vcrush; // flag /* Diffusion parameters */ #define MAXDIR 1024 /* Will anybody do more than 1024 directions or b-values? */ double roarr[MAXDIR], pearr[MAXDIR], slarr[MAXDIR]; int nbval, /* Total number of bvalues*directions */ nbro, nbpe, nbsl, i; double bro[MAXDIR], bpe[MAXDIR], bsl[MAXDIR], /* b-values along RO, PE, SL */ brs[MAXDIR], brp[MAXDIR], bsp[MAXDIR], /* and the cross-terms */ btrace[MAXDIR], /* and the trace */ max_bval=0, dcrush, dgss2, /* "delta" for crusher and gss2 gradients */ Dro, Dcrush, Dgss2; /* "DELTA" for readout, crusher and gss2 gradients */ /* Real-time variables used in this sequence **************/ int vpe_ctr = v1; // PE loop counter int vpe_mult = v2; // PE multiplier, ranges from -PE/2 to PE/2 int vpe2_ctr = v3; // PE loop counter int vpe2_mult = v4; // PE multiplier, ranges from -PE/2 to PE/2 int vpe2_offset = v5; int vpe2_steps = v6; int vms_slices = v7; // Number of slices int vms_ctr = v8; // Slice loop counter int vseg = v9; // Number of ETL segments int vseg_ctr = v10; // Segment counter int vetl = v11; // Echo train length int vetl_ctr = v12; // Echo train loop counter int vssc = v13; // Compressed steady-states int vtrimage = v14; // Counts down from nt, trimage delay when 0 int vacquire = v15; // Argument for setacqvar, to skip steady state acquires int vphase180 = v16; // phase of 180 degree refocusing pulse int vetl_loop = v17; // Echo train length MINUS ONE, used on etl loop int vnav = v18; // Echo train length int vcr_ctr = v19; // variable crusher, index into table int vcr1 = v20; // multiplier along RO int vcr2 = v21; // multiplier along PE int vcr3 = v22; // multiplier along SL int vetl1 = v23; // = etl-1, determine navigator echo location in echo loop int vcr_reset = v24; // check for navigator echoes, reset crushers /* Initialize paramaters **********************************/ get_parameters(); te1 = getval("te1"); /* te1 is the echo time for the first echo */ cscale = getval("cscale"); /* Scaling factor on first 180 crushers */ vcrush = getval("vcrush"); /* Variable crusher or set amplitude? */ getstr("diffpat",diffpat); /* Load external PE table ********************************/ if (strcmp(petable,"n") && strcmp(petable,"N") && strcmp(petable,"")) { loadtable(petable); } else { abort_message("petable undefined"); } /* Hold variable crushers in tables 5, 6, 7 */ settable(t5,8,crro); settable(t6,8,crpe); settable(t7,8,crss); seqtime = 0.0; espmin = 0.0; /* RF Power & Bandwidth Calculations **********************/ init_rf(&p1_rf,p1pat,p1,flip1,rof1,rof2); init_rf(&p2_rf,p2pat,p2,flip2,rof1,rof2); // shape_rf(&p1_rf,"p1",p1pat,p1,flip1,rof1,rof2); // shape_rf(&p2_rf,"p2",p2pat,p2,flip2,rof1,rof2); calc_rf(&p1_rf,"tpwr1","tpwr1f"); calc_rf(&p2_rf,"tpwr2","tpwr2f"); /* Initialize gradient structures *************************/ init_readout(&ro_grad,"ro",lro,np,sw); init_readout_refocus(&ror_grad,"ror"); init_phase(&pe_grad,"pe",lpe,nv); init_phase(&pe2_grad,"pe2",lpe2,nv2); init_slice(&ss_grad,"ss",thk); /* NOTE assume same band widths for p1 and p2 */ init_slice(&ss2_grad,"ss2",thk); /* not butterfly, want to scale crushers w/ echo */ init_slice_refocus(&ssr_grad,"ssr"); /* Gradient calculations **********************************/ calc_readout(&ro_grad,WRITE,"gro","sw","at"); calc_readout_refocus(&ror_grad,&ro_grad,NOWRITE,"gror"); calc_phase(&pe_grad,NOWRITE,"gpe","tpe"); calc_phase(&pe2_grad,NOWRITE,"gpe2","tpe2"); calc_slice(&ss_grad,&p1_rf,WRITE,"gss"); calc_slice(&ss2_grad,&p1_rf,WRITE,""); calc_slice_refocus(&ssr_grad,&ss_grad,WRITE,"gssr"); /* Equalize refocus and PE gradient durations *************/ calc_sim_gradient(&ror_grad,&pe_grad,&pe2_grad,0.0,WRITE); /* Variable crusher */ init_generic(&crush_grad,"crush",gcrush,tcrush); calc_generic(&crush_grad,WRITE,"",""); /* Create optional prepulse events ************************/ if (sat[0] == 'y') create_satbands(); if (fsat[0] == 'y') create_fatsat(); if (mt[0] == 'y') create_mtc(); /* Optional Diffusion gradient */ if (diff[0] == 'y') { init_generic(&diff_grad,"diff",gdiff,tdelta); if (!strcmp("sine",diffpat)) { diff_grad.shape = SINE; diffamp = gdiff*1; } /* adjust duration, so tdelta is from start ramp up to start ramp down */ if ((ix == 1) && (diff_grad.shape == TRAPEZOID)) { calc_generic(&diff_grad,NOWRITE,"",""); diff_grad.duration += diff_grad.tramp; } calc_generic(&diff_grad,WRITE,"",""); } /* Set up frequency offset pulse shape list ********/ offsetlist(pss,ss_grad.amp,0,freq90,ns,seqcon[1]); offsetlist(pss,ss2_grad.ssamp,0,freq180,ns,seqcon[1]); shapelist90 = shapelist(p1_rf.pulseName,ss_grad.rfDuration, freq90, ns,0,seqcon[1]); shapelist180 = shapelist(p2_rf.pulseName,ss2_grad.rfDuration,freq180,ns,0,seqcon[1]); /* same slice selection gradient and RF pattern used */ if (ss_grad.rfFraction != 0.5) abort_message("RF pulse must be symmetric (RF fraction = %.2f)",ss_grad.rfFraction); if (ro_grad.echoFraction != 1) abort_message("Echo Fraction must be 1"); /*****************************************************/ /* TIMING FOR ECHOES *********************************/ /*****************************************************/ /* First echo time, without diffusion */ tau1 = ss_grad.rfCenterBack + ssr_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront; tau2 = ss2_grad.rfCenterBack + crush_grad.duration + pe_grad.duration + ro_grad.timeToEcho; te1min = 2*MAX(tau1,tau2); if (te1 < te1min + 2*4e-6) { abort_message("First echo time too small, minimum is %.2fms\n",(te1min+2*4e-6)*1000); } /* Each half-echo period in the ETL loop ********/ tau3 = ro_grad.timeFromEcho + pe_grad.duration + crush_grad.duration + ss2_grad.rfCenterFront; espmin = 2*MAX(tau2,tau3); // Minimum echo spacing if (minesp[0] == 'y') { esp = espmin + 2*4e-6; putvalue("esp",esp); } if (esp - (espmin + 2*4e-6) < -12.5e-9) { abort_message("Echo spacing too small, minimum is %.2fms\n",(espmin+2*4e-6)*1000); } te1_delay = te1/2.0 - tau1; te2_delay = esp/2.0 - tau2; te3_delay = esp/2.0 - tau3; /*****************************************************/ /* TIMING FOR DIFFUSION ******************************/ /*****************************************************/ del1 = te1/2.0 - tau1; del2 = 0; del3 = te1/2.0 - tau2; del4 = 0; if (diff[0] == 'y') { tau1 += diff_grad.duration; tau2 += diff_grad.duration; te1min = 2*MAX(tau1,tau2); if (te1 < te1min + 4*4e-6) { /* te1 is split into 4 delays, each of which must be >= 4us */ abort_message("ERROR %s: First echo time too small, minimum is %.2fms\n",seqfil,te1min*1000); } /* te1 is the echo time for the first echo */ te_diff1 = te1/2 - tau1; /* Available time in first half of first echo */ te_diff2 = te1/2 - tau2; /* Available time in second half of first echo */ tmp1 = ss2_grad.duration + 2*crush_grad.duration; /* duration of 180 block */ /* Is tDELTA long enough? */ if (tDELTA < diff_grad.duration + tmp1) abort_message("DELTA too short, increase to %.2fms", (diff_grad.duration + tmp1)*1000); /* Is tDELTA too long? */ tmp2 = diff_grad.duration + te_diff1 + tmp1 + te_diff2; if (tDELTA > tmp2) { abort_message("DELTA too long, increase te1 to %.2fms", (te1 + (tDELTA-tmp2))*1000); } /* First attempt to put lobes right after slice select, ie del1 = 0 */ del1 = 4e-6; /* At least 4us after setting power for 180 */ del2 = te_diff1 - del1; del3 = tDELTA - (diff_grad.duration + del2 + tmp1); if (del3 < 4e-6) { /* shift diffusion block towards acquisition */ del3 = 4e-6; del2 = tDELTA - (diff_grad.duration + tmp1 + del3); } del1 = te_diff1 - del2; del4 = te_diff2 - del3; if (fabs(del4) < 12.5e-9) del4 = 0; } te = te1 + (kzero-1)*esp; // Return effective TE putvalue("te",te); /* How many echoes in the echo loop, including navigators? */ etlnav = (etl-1)+(navigator[0]=='y')*2.0; /* Minimum TR **************************************/ seqtime = 4e-6 + 2*nseg*ns*4e-6; /* count all the 4us delays */ seqtime += ns*(ss_grad.duration/2 + te1 + (etlnav)*esp + ro_grad.timeFromEcho + pe_grad.duration + te3_delay); /* Increase TR if any options are selected****************/ if (sat[0] == 'y') seqtime += ns*satTime; if (fsat[0] == 'y') seqtime += ns*fsatTime; if (mt[0] == 'y') seqtime += ns*mtTime; trmin = seqtime + ns*4e-6; /* Add 4us to ensure that tr_delay is always >= 4us */ if (mintr[0] == 'y'){ tr = trmin; putvalue("tr",tr+1e-6); } if (tr < trmin) { abort_message("TR too short. Minimum TR = %.2fms\n",trmin*1000); } tr_delay = (tr - seqtime)/ns; /* Set number of segments for profile or full image **********/ nseg = prep_profile(profile[0],nv/etl,&pe_grad,&per_grad); pe2_steps = prep_profile(profile[1],nv2,&pe2_grad,&pe2r_grad); /* Calculate total scan time */ g_setExpTime(tr*(nt*nseg*pe2_steps*arraydim + ssc)); /***************************************************/ /* CALCULATE B VALUES ******************************/ if (diff[0] == 'y') { /* Get multiplication factors and make sure they have same # elements */ /* All this is only necessary because putCmd only work for ix==1 */ nbro = (int) getarray("dro",roarr); nbval = nbro; nbpe = (int) getarray("dpe",pearr); if (nbpe > nbval) nbval = nbpe; nbsl = (int) getarray("dsl",slarr); if (nbsl > nbval) nbval = nbsl; if ((nbro != nbval) && (nbro != 1)) abort_message("%s: Number of directions/b-values must be the same for all axes (readout)",seqfil); if ((nbpe != nbval) && (nbpe != 1)) abort_message("%s: Number of directions/b-values must be the same for all axes (phase)",seqfil); if ((nbsl != nbval) && (nbsl != 1)) abort_message("%s: Number of directions/b-values must be the same for all axes (slice)",seqfil); if (nbro == 1) for (i = 1; i < nbval; i++) roarr[i] = roarr[0]; if (nbpe == 1) for (i = 1; i < nbval; i++) pearr[i] = pearr[0]; if (nbsl == 1) for (i = 1; i < nbval; i++) slarr[i] = slarr[0]; } else { nbval = 1; roarr[0] = 0; pearr[0] = 0; slarr[0] = 0; } for (i = 0; i < nbval; i++) { dcrush = crush_grad.duration; //"delta" for crusher Dcrush = dcrush + ss_grad.duration; //"DELTA" for crusher /* Readout */ Dro = ror_grad.duration; bro[i] = bval(gdiff*roarr[i],tdelta,tDELTA); bro[i] += bval(ro_grad.amp,ro_grad.timeToEcho,Dro); bro[i] += bval(crush_grad.amp,dcrush,Dcrush); bro[i] += bval_nested(gdiff*roarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); /* Slice */ dgss2 = Dgss2 = ss_grad.rfCenterFront; bsl[i] = bval(gdiff*slarr[i],tdelta,tDELTA); bsl[i] += bval(crush_grad.amp,dcrush,Dcrush); bsl[i] += bval(ss2_grad.ssamp,dgss2,Dgss2); bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); bsl[i] += bval_nested(gdiff*slarr[i],tdelta,tDELTA,ss2_grad.ssamp,dgss2,Dgss2); bsl[i] += bval_nested(ss2_grad.ssamp,dgss2,Dgss2, crush_grad.amp,dcrush,Dcrush); /* Phase */ bpe[i] = bval(gdiff*pearr[i],tdelta,tDELTA); bpe[i] += bval(crush_grad.amp,dcrush,Dcrush); bpe[i] += bval_nested(gdiff*pearr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); /* Readout/Slice Cross-terms */ brs[i] = bval2(gdiff*roarr[i],gdiff*slarr[i],tdelta,tDELTA); brs[i] += bval2(crush_grad.amp, crush_grad.amp,dcrush,Dcrush); brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); brs[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); brs[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA, ss2_grad.ssamp,dgss2,Dgss2); brs[i] += bval_cross(crush_grad.amp,dcrush,Dcrush, ss2_grad.ssamp,dgss2,Dgss2); /* Readout/Phase Cross-terms */ brp[i] = bval2(gdiff*roarr[i],gdiff*pearr[i],tdelta,tDELTA); brp[i] += bval2(crush_grad.amp, crush_grad.amp,dcrush,Dcrush); brp[i] += bval_cross(gdiff*roarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); brp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); /* Slice/Phase Cross-terms */ bsp[i] = bval2(gdiff*pearr[i],gdiff*slarr[i],tdelta,tDELTA); bsp[i] += bval2(crush_grad.amp, crush_grad.amp,dcrush,Dcrush); bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); bsp[i] += bval_cross(gdiff*slarr[i],tdelta,tDELTA, crush_grad.amp,dcrush,Dcrush); bsp[i] += bval_cross(gdiff*pearr[i],tdelta,tDELTA, ss2_grad.ssamp,dgss2,Dgss2); bsp[i] += bval_cross(crush_grad.amp,dcrush,Dcrush, ss2_grad.ssamp,dgss2,Dgss2); btrace[i] = (bro[i]+bsl[i]+bpe[i]); if (max_bval < btrace[i]) { max_bval = (bro[i]+bsl[i]+bpe[i]); } } /* End for-all-directions */ putarray("bvalrr",bro,nbval); putarray("bvalpp",bpe,nbval); putarray("bvalss",bsl,nbval); putarray("bvalrp",brp,nbval); putarray("bvalrs",brs,nbval); putarray("bvalsp",bsp,nbval); putarray("bvalue",btrace,nbval); putvalue("max_bval",max_bval); /* Shift DDR for pro *******************************/ roff = -poffset(pro,ro_grad.roamp); /* PULSE SEQUENCE *************************************/ if (ix == 1) grad_advance(tep); initval(fabs(ssc),vssc); // Compressed steady-state counter setacqvar(vacquire); // Control acquisition through vacquire assign(one,vacquire); // Turn on acquire when vacquire is zero /* Phase cycle: Alternate 180 phase to cancel residual FID */ mod2(ct,vphase180); // 0101 dbl(vphase180,vphase180); // 0202 add(vphase180,one,vphase180); // 1313 Phase difference from 90 add(vphase180,oph,vphase180); obsoffset(resto); delay(4e-6); initval(nseg,vseg); initval(pe2_steps/2.0,vpe2_offset); initval(etl,vetl); initval(etl-1,vetl1); peloop2(seqcon[3],pe2_steps,vpe2_steps,vpe2_ctr); /* Use standard encoding order for 2nd PE dimension */ sub(vpe2_ctr,vpe2_offset,vpe2_mult); loop(vseg,vseg_ctr); /* Compressed steady-states: 1st array & transient, all arrays if ssc is negative */ if ((ix > 1) && (ssc > 0)) assign(zero,vssc); sub(vseg_ctr,vssc,vseg_ctr); // vpe_ctr counts up from -ssc assign(zero,vssc); ifzero(vseg_ctr); assign(zero,vacquire); // Start acquiring when vseg_ctr reaches zero endif(vseg_ctr); msloop(seqcon[1],ns,vms_slices,vms_ctr); if (ticks) { xgate(ticks); grad_advance(tep); } sp1on(); delay(4e-6); sp1off(); // Scope trigger /* Prepulse options ***********************************/ if (sat[0] == 'y') satbands(); if (fsat[0] == 'y') fatsat(); if (mt[0] == 'y') mtc(); /* 90 degree pulse ************************************/ rotate(); obspower(p1_rf.powerCoarse); obspwrf(p1_rf.powerFine); delay(4e-6); obl_shapedgradient(ss_grad.name,ss_grad.duration,0,0,ss_grad.amp,NOWAIT); delay(ss_grad.rfDelayFront); shapedpulselist(shapelist90,ss_grad.rfDuration,oph,rof1,rof2,seqcon[1],vms_ctr); delay(ss_grad.rfDelayBack); /* Read dephase and Slice refocus *********************/ obl_shapedgradient(ssr_grad.name,ssr_grad.duration,0.0,0.0,-ssr_grad.amp,WAIT); /* First half-TE delay ********************************/ obspower(p2_rf.powerCoarse); obspwrf(p2_rf.powerFine); delay(del1); /* DIFFUSION GRADIENT */ if (diff[0] == 'y') obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT); delay(del2); /*****************************************************/ /* FIRST ECHO OUTSIDE LOOP ***************************/ /*****************************************************/ ifzero(vacquire); // real acquisition, get PE multiplier from table mult(vseg_ctr,vetl,vpe_ctr); getelem(t1,vpe_ctr,vpe_mult); elsenz(vacquire); // steady state scan assign(zero,vpe_mult); endif(vacquire); /* Variable crusher */ assign(zero,vcr_ctr); getelem(t5,vcr_ctr,vcr1); getelem(t6,vcr_ctr,vcr2); getelem(t7,vcr_ctr,vcr3); if(vcrush) phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name, crush_grad.duration, (double)0,(double)0,(double)0, // base levels crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size vcr1,vcr2,vcr3, // multipliers (double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers 1,WAIT,0); else obl_shapedgradient(crush_grad.name,crush_grad.duration, crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT); /* 180 degree pulse *******************************/ obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT); delay(ss2_grad.rfDelayFront); shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr); delay(ss2_grad.rfDelayBack); if (vcrush) phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name, crush_grad.duration, (double)0,(double)0,(double)0, // base levels crush_grad.amp*cscale,crush_grad.amp*cscale,crush_grad.amp*cscale, // step size vcr1,vcr2,vcr3, // multipliers (double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers 1,WAIT,0); else obl_shapedgradient(crush_grad.name,crush_grad.duration, crush_grad.amp,crush_grad.amp,crush_grad.amp,WAIT); delay(del3); /* DIFFUSION GRADIENT */ if (diff[0] == 'y') obl_shapedgradient(diff_grad.name,diff_grad.duration,diff_grad.amp*dro,diff_grad.amp*dpe,diff_grad.amp*dsl,WAIT); delay(del4); /* Phase-encode gradient ******************************/ pe2_shapedgradient(pe_grad.name,pe_grad.duration,-ror_grad.amp,0,0, -pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT); /* Readout gradient ************************************/ obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT); delay(ro_grad.atDelayFront); /* Acquire data ****************************************/ startacq(10e-6); acquire(np,1.0/sw); endacq(); delay(ro_grad.atDelayBack); /* Rewinding phase-encode gradient ********************/ pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0, pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT); /* Second half-TE delay *******************************/ delay(te3_delay); /*****************************************************/ /* LOOP THROUGH THE REST OF ETL **********************/ /*****************************************************/ peloop(seqcon[2],etlnav,vetl_loop,vetl_ctr); ifzero(vacquire); // real acquisition, get PE multiplier from table mult(vseg_ctr,vetl,vpe_ctr); add(vpe_ctr,vetl_ctr,vpe_ctr); add(vpe_ctr,one,vpe_ctr); getelem(t1,vpe_ctr,vpe_mult); elsenz(vacquire); // steady state scan assign(zero,vpe_mult); endif(vacquire); /* But don't phase encode navigator echoes */ ifrtGE(vetl_ctr,vetl1,vnav); assign(zero,vpe_mult); endif(vnav); /* Variable crusher */ incr(vcr_ctr); /* Get next crusher level */ /* Except if we're doing navigators, start over */ sub(vetl1,vetl_ctr,vcr_reset); ifzero(vcr_reset); assign(zero,vcr_ctr); endif(vcr_reset); getelem(t5,vcr_ctr,vcr1); getelem(t6,vcr_ctr,vcr2); getelem(t7,vcr_ctr,vcr3); phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name, crush_grad.duration, (double)0,(double)0,(double)0, // base levels crush_grad.amp,crush_grad.amp,crush_grad.amp, // step size vcr1,vcr2,vcr3, // multipliers (double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers 1,WAIT,0); /* 180 degree pulse *******************************/ obl_shapedgradient(ss2_grad.name,ss2_grad.duration,0,0,ss2_grad.amp,NOWAIT); delay(ss2_grad.rfDelayFront); shapedpulselist(shapelist180,ss2_grad.rfDuration,vphase180,rof1,rof2,seqcon[1],vms_ctr); delay(ss2_grad.rfDelayBack); /* Variable crusher */ phase_encode3_oblshapedgradient(crush_grad.name,crush_grad.name,crush_grad.name, crush_grad.duration, (double)0,(double)0,(double)0, // base levels crush_grad.amp,crush_grad.amp,crush_grad.amp, // step size vcr1,vcr2,vcr3, // multipliers (double)1.0,(double)1.0,(double)1.0, // upper limit on multipliers 1,WAIT,0); /* Phase-encode gradient ******************************/ pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0, -pe_grad.increment,-pe2_grad.increment,vpe_mult,vpe2_mult,WAIT); /* Second half-TE period ******************************/ delay(te2_delay); /* Readout gradient ************************************/ obl_shapedgradient(ro_grad.name,ro_grad.duration,ro_grad.roamp,0,0,NOWAIT); delay(ro_grad.atDelayFront); startacq(10e-6); acquire(np,1.0/sw); endacq(); delay(ro_grad.atDelayBack); /* Rewinding phase-encode gradient ********************/ pe2_shapedgradient(pe_grad.name,pe_grad.duration,0,0,0, pe_grad.increment,pe2_grad.increment,vpe_mult,vpe2_mult,WAIT); /* Second half-TE delay *******************************/ delay(te3_delay); endpeloop(seqcon[2],vetl_ctr); /* Relaxation delay ***********************************/ if (!trtype) delay(tr_delay); endmsloop(seqcon[1],vms_ctr); if (trtype) delay(ns*tr_delay); endloop(vseg_ctr); endpeloop(seqcon[3],vpe2_ctr); /* Inter-image delay **********************************/ sub(ntrt,ct,vtrimage); decr(vtrimage); ifzero(vtrimage); delay(trimage); endif(vtrimage); }

Word16 code_2i40_9bits( Word16 subNr, /* i : subframe number */ Word16 x[], /* i : target vector */ Word16 h[], /* i : impulse response of weighted synthesis */ /* filter h[-L_subfr..-1] must be set to 0. */ Word16 T0, /* i : Pitch lag */ Word16 pitch_sharp, /* i : Last quantized pitch gain */ Word16 code[], /* o : Innovative codebook */ Word16 y[], /* o : filtered fixed codebook excitation */ Word16 * sign, /* o : Signs of 2 pulses */ Flag * pOverflow /* o : Flag set when overflow occurs */ ) { Word16 codvec[NB_PULSE]; Word16 dn[L_CODE]; Word16 dn2[L_CODE]; Word16 dn_sign[L_CODE]; Word16 rr[L_CODE][L_CODE]; register Word16 i; Word16 index; Word16 sharp; Word16 temp; Word32 L_temp; L_temp = ((Word32) pitch_sharp) << 1; /* Check for overflow condition */ if (L_temp != (Word32)((Word16) L_temp)) { *(pOverflow) = 1; sharp = (pitch_sharp > 0) ? MAX_16 : MIN_16; } else { sharp = (Word16) L_temp; } if (T0 < L_CODE) { for (i = T0; i < L_CODE; i++) { temp = mult( *(h + i - T0), sharp, pOverflow); *(h + i) = add( *(h + i), temp, pOverflow); } } cor_h_x( h, x, dn, 1, pOverflow); /* dn2[] not used in this codebook search */ set_sign( dn, dn_sign, dn2, 8); cor_h( h, dn_sign, rr, pOverflow); search_2i40( subNr, dn, rr, codvec, pOverflow); index = build_code( subNr, codvec, dn_sign, code, h, y, sign, pOverflow); /*-----------------------------------------------------------------* * Compute innovation vector gain. * * Include fixed-gain pitch contribution into code[]. * *-----------------------------------------------------------------*/ if (T0 < L_CODE) { for (i = T0; i < L_CODE; i++) { temp = mult( *(code + i - T0), sharp, pOverflow); *(code + i) = add( *(code + i), temp, pOverflow); } } return(index); }

Vec reflection_reflection(const Vec &x , int depth, const Sphere &sph, const Ray &r, const Vec &n, const Vec &f){ Ray rr2; rr2.origin = x; rr2.direction = r.direction-n*2*dot(n,r.direction); return sph.light + mult(f,calculate_light(rr2,depth)); }

/************************************************************************* * * Function: gain_adapt() * Purpose: calculate pitch/codebook gain adaptation factor alpha * (and update the adaptor state) * ************************************************************************** */ void gain_adapt( GainAdaptState *st, /* i : state struct */ Word16 ltpg, /* i : ltp coding gain (log2()), Q13 */ Word16 gain_cod, /* i : code gain, Q1 */ Word16 *alpha /* o : gain adaptation factor, Q15 */ ) { Word16 adapt; /* adaptdation status; 0, 1, or 2 */ Word16 result; /* alpha factor, Q13 */ Word16 filt; /* median-filtered LTP coding gain, Q13 */ Word16 tmp, i; /* basic adaptation */ test (); if (sub (ltpg, LTP_GAIN_THR1) <= 0) { adapt = 0; move16 (); } else { test (); if (sub (ltpg, LTP_GAIN_THR2) <= 0) { adapt = 1; move16 (); } else { adapt = 2; move16 (); } } /* * // onset indicator * if ((cbGain > onFact * cbGainMem[0]) && (cbGain > 100.0)) * onset = 8; * else * if (onset) * onset--; */ /* tmp = cbGain / onFact; onFact = 2.0; 200 Q1 = 100.0 */ tmp = shr_r (gain_cod, 1); test (); test (); if ((sub (tmp, st->prev_gc) > 0) && sub(gain_cod, 200) > 0) { st->onset = 8; move16 (); } else { test (); if (st->onset != 0) { st->onset = sub (st->onset, 1); move16 (); } } /* * // if onset, increase adaptor state * if (onset && (gainAdapt < 2)) gainAdapt++; */ test(); test (); if ((st->onset != 0) && (sub (adapt, 2) < 0)) { adapt = add (adapt, 1); } st->ltpg_mem[0] = ltpg; move16 (); filt = gmed_n (st->ltpg_mem, 5); move16 (); /* function result */ test (); if (adapt == 0) { test (); if (sub (filt, 5443) > 0) /* 5443 Q13 = 0.66443... */ { result = 0; move16 (); } else { test (); if (filt < 0) { result = 16384; move16 (); /* 16384 Q15 = 0.5 */ } else { /* result = 0.5 - 0.75257499*filt */ /* result (Q15) = 16384 - 24660 * (filt << 2) */ filt = shl (filt, 2); /* Q15 */ result = sub (16384, mult (24660, filt)); } } } else { result = 0; move16 (); } /* * if (prevAlpha == 0.0) result = 0.5 * (result + prevAlpha); */ test (); if (st->prev_alpha == 0) { result = shr (result, 1); } /* store the result */ *alpha = result; move16 (); /* update adapter state memory */ st->prev_alpha = result; move16 (); st->prev_gc = gain_cod; move16 (); for (i = LTPG_MEM_SIZE-1; i > 0; i--) { st->ltpg_mem[i] = st->ltpg_mem[i-1]; move16 (); } /* mem[0] is just present for convenience in calling the gmed_n[5] * function above. The memory depth is really LTPG_MEM_SIZE-1. */ }

void cbsearch(Word16 x[], /* i : target vector, Q0 */ Word16 h[], /* i : impulse response of weighted synthesis*/ /* filter h[-L_subfr..-1] must be set to */ /* zero. Q12 */ Word16 T0, /* i : Pitch lag */ Word16 pitch_sharp,/* i : Last quantized pitch gain, Q14 */ Word16 gain_pit, /* i : Pitch gain, Q14 */ Word16 res2[], /* i : Long term prediction residual, Q0 */ Word16 code[], /* o : Innovative codebook, Q13 */ Word16 y[], /* o : filtered fixed codebook excitation */ /* Q12 */ Word16 **anap, /* o : Signs of the pulses */ enum Mode mode, /* i : coder mode */ Word16 subNr, /* i : subframe number */ CommonAmrTbls* common_amr_tbls, /* ptr to struct of tables */ Flag *pOverflow) /* o : Flag set when overflow occurs */ { Word16 index; Word16 i; Word16 temp; Word16 pit_sharpTmp; /* For MR74, the pre and post CB pitch sharpening is included in the * codebook search routine, while for MR122 is it not. */ if ((mode == MR475) || (mode == MR515)) { /* MR475, MR515 */ *(*anap)++ = code_2i40_9bits( subNr, x, h, T0, pitch_sharp, code, y, &index, common_amr_tbls->startPos_ptr, pOverflow); *(*anap)++ = index; /* sign index */ } else if (mode == MR59) { /* MR59 */ *(*anap)++ = code_2i40_11bits( x, h, T0, pitch_sharp, code, y, &index, pOverflow); *(*anap)++ = index; /* sign index */ } else if (mode == MR67) { /* MR67 */ *(*anap)++ = code_3i40_14bits( x, h, T0, pitch_sharp, code, y, &index, pOverflow); *(*anap)++ = index; /* sign index */ } else if ((mode == MR74) || (mode == MR795)) { /* MR74, MR795 */ *(*anap)++ = code_4i40_17bits( x, h, T0, pitch_sharp, code, y, &index, common_amr_tbls->gray_ptr, pOverflow); *(*anap)++ = index; /* sign index */ } else if (mode == MR102) { /* MR102 */ /*-------------------------------------------------------------* * - include pitch contribution into impulse resp. h1[] * *-------------------------------------------------------------*/ /* pit_sharpTmp = pit_sharp; */ /* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0; */ pit_sharpTmp = shl( pitch_sharp, 1, pOverflow); for (i = T0; i < L_SUBFR; i++) { temp = mult( h[i - T0], pit_sharpTmp, pOverflow); h[i] = add_16( h[i], temp, pOverflow); } /*--------------------------------------------------------------* * - Innovative codebook search (find index and gain) * *--------------------------------------------------------------*/ code_8i40_31bits( x, res2, h, code, y, *anap, pOverflow); *anap += 7; /*-------------------------------------------------------* * - Add the pitch contribution to code[]. * *-------------------------------------------------------*/ for (i = T0; i < L_SUBFR; i++) { temp = mult( code[i - T0], pit_sharpTmp, pOverflow); code[i] = add_16( code[i], temp, pOverflow); } } else { /* MR122 */ /*-------------------------------------------------------------* * - include pitch contribution into impulse resp. h1[] * *-------------------------------------------------------------*/ /* pit_sharpTmp = gain_pit; */ /* if (pit_sharpTmp > 1.0) pit_sharpTmp = 1.0; */ pit_sharpTmp = shl(gain_pit, 1, pOverflow); for (i = T0; i < L_SUBFR; i++) { temp = ((Word32)h[i - T0] * pit_sharpTmp) >> 15; /* mult( h[i - T0], , pOverflow); */ h[i] = add_16( h[i], temp, pOverflow); } /*--------------------------------------------------------------* * - Innovative codebook search (find index and gain) * *--------------------------------------------------------------*/ code_10i40_35bits( x, res2, h, code, y, *anap, common_amr_tbls->gray_ptr, pOverflow); *anap += 10; /*-------------------------------------------------------* * - Add the pitch contribution to code[]. * *-------------------------------------------------------*/ for (i = T0; i < L_SUBFR; i++) { temp = mult( code[i - T0], pit_sharpTmp, pOverflow); code[i] = add_16( code[i], temp, pOverflow); } } }

/************************************************************************* * * FUNCTION: MR795_gain_code_quant3 * * PURPOSE: Pre-quantization of codebook gains, given three possible * LTP gains (using predicted codebook gain) * *************************************************************************/ void MR795_gain_code_quant3( Word16 exp_gcode0, /* i : predicted CB gain (exponent), Q0 */ Word16 gcode0, /* i : predicted CB gain (norm.), Q14 */ Word16 g_pitch_cand[], /* i : Pitch gain candidates (3), Q14 */ Word16 g_pitch_cind[], /* i : Pitch gain cand. indices (3), Q0 */ Word16 frac_coeff[], /* i : coefficients (5), Q15 */ Word16 exp_coeff[], /* i : energy coefficients (5), Q0 */ /* coefficients from calc_filt_ener()*/ Word16 *gain_pit, /* o : Pitch gain, Q14 */ Word16 *gain_pit_ind, /* o : Pitch gain index, Q0 */ Word16 *gain_cod, /* o : Code gain, Q1 */ Word16 *gain_cod_ind, /* o : Code gain index, Q0 */ Word16 *qua_ener_MR122, /* o : quantized energy error, Q10 */ /* (for MR122 MA predictor update) */ Word16 *qua_ener /* o : quantized energy error, Q10 */ /* (for other MA predictor update) */ ) { const Word16 *p; Word16 i, j, cod_ind, pit_ind; Word16 e_max, exp_code; Word16 g_pitch, g2_pitch, g_code, g2_code_h, g2_code_l; Word16 g_pit_cod_h, g_pit_cod_l; Word16 coeff[5], coeff_lo[5]; Word16 exp_max[5]; Word32 L_tmp, L_tmp0, dist_min; /* * The error energy (sum) to be minimized consists of five terms, t[0..4]. * * t[0] = gp^2 * <y1 y1> * t[1] = -2*gp * <xn y1> * t[2] = gc^2 * <y2 y2> * t[3] = -2*gc * <xn y2> * t[4] = 2*gp*gc * <y1 y2> * */ /* determine the scaling exponent for g_code: ec = ec0 - 10 */ exp_code = sub(exp_gcode0, 10); /* calculate exp_max[i] = s[i]-1 */ exp_max[0] = sub(exp_coeff[0], 13); exp_max[1] = sub(exp_coeff[1], 14); exp_max[2] = add(exp_coeff[2], add(15, shl(exp_code, 1))); exp_max[3] = add(exp_coeff[3], exp_code); exp_max[4] = add(exp_coeff[4], add(exp_code,1)); /*-------------------------------------------------------------------* * Find maximum exponent: * * ~~~~~~~~~~~~~~~~~~~~~~ * * * * For the sum operation, all terms must have the same scaling; * * that scaling should be low enough to prevent cgOverflow. There- * * fore, the maximum scale is determined and all coefficients are * * re-scaled: * * * * e_max = max(exp_max[i]) + 1; * * e = exp_max[i]-e_max; e <= 0! * * c[i] = c[i]*2^e * *-------------------------------------------------------------------*/ e_max = exp_max[0]; for (i = 1; i < 5; i++) /* implemented flattened */ { if (sub(exp_max[i], e_max) > 0) { e_max = exp_max[i]; } } e_max = add(e_max, 1); /* To avoid cgOverflow */ for (i = 0; i < 5; i++) { j = sub(e_max, exp_max[i]); L_tmp = L_deposit_h(frac_coeff[i]); L_tmp = L_shr(L_tmp, j); L_Extract(L_tmp, &coeff[i], &coeff_lo[i]); } /*-------------------------------------------------------------------* * Codebook search: * * ~~~~~~~~~~~~~~~~ * * * * For each of the candiates LTP gains in g_pitch_cand[], the terms * * t[0..4] are calculated from the values in the table (and the * * pitch gain candidate) and summed up; the result is the mean * * squared error for the LPT/CB gain pair. The index for the mini- * * mum MSE is stored and finally used to retrieve the quantized CB * * gain * *-------------------------------------------------------------------*/ /* start with "infinite" MSE */ dist_min = MAX_32; cod_ind = 0; pit_ind = 0; /* loop through LTP gain candidates */ for (j = 0; j < 3; j++) { /* pre-calculate terms only dependent on pitch gain */ g_pitch = g_pitch_cand[j]; g2_pitch = mult(g_pitch, g_pitch); L_tmp0 = Mpy_32_16( coeff[0], coeff_lo[0], g2_pitch); L_tmp0 = Mac_32_16(L_tmp0, coeff[1], coeff_lo[1], g_pitch); p = &qua_gain_code[0]; for (i = 0; i < NB_QUA_CODE; i++) { g_code = *p++; /* this is g_fac Q11 */ p++; /* skip log2(g_fac) */ p++; /* skip 20*log10(g_fac) */ g_code = mult(g_code, gcode0); L_tmp = L_mult (g_code, g_code); L_Extract (L_tmp, &g2_code_h, &g2_code_l); L_tmp = L_mult(g_code, g_pitch); L_Extract (L_tmp, &g_pit_cod_h, &g_pit_cod_l); L_tmp = Mac_32 (L_tmp0, coeff[2], coeff_lo[2], g2_code_h, g2_code_l); L_tmp = Mac_32_16(L_tmp, coeff[3], coeff_lo[3], g_code); L_tmp = Mac_32 (L_tmp, coeff[4], coeff_lo[4], g_pit_cod_h, g_pit_cod_l); /* store table index if MSE for this index is lower than the minimum MSE seen so far; also store the pitch gain for this (so far) lowest MSE */ if (L_sub(L_tmp, dist_min) < (Word32) 0) { dist_min = L_tmp; cod_ind = i; pit_ind = j; } } } /*------------------------------------------------------------------* * read quantized gains and new values for MA predictor memories * * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * *------------------------------------------------------------------*/ /* Read the quantized gains */ p = &qua_gain_code[add (add (cod_ind, cod_ind), cod_ind)]; g_code = *p++; *qua_ener_MR122 = *p++; *qua_ener = *p; /*------------------------------------------------------------------* * calculate final fixed codebook gain: * * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ * * * * gc = gc0 * g * *------------------------------------------------------------------*/ L_tmp = L_mult(g_code, gcode0); L_tmp = L_shr(L_tmp, sub(9, exp_gcode0)); *gain_cod = extract_h(L_tmp); *gain_cod_ind = cod_ind; *gain_pit = g_pitch_cand[pit_ind]; *gain_pit_ind = g_pitch_cind[pit_ind]; }

static Word16 D4i40_17_fast(/*(o) : Index of pulses positions. */ Word16 dn[], /* (i) : Correlations between h[] and Xn[]. */ Word16 rr[], /* (i) : Correlations of impulse response h[]. */ Word16 h[], /* (i) Q12: Impulse response of filters. */ Word16 cod[], /* (o) Q13: Selected algebraic codeword. */ Word16 y[], /* (o) Q12: Filtered algebraic codeword. */ Word16 *sign /* (o) : Signs of 4 pulses. */ ) { Word16 i0, i1, i2, i3, ip0, ip1, ip2, ip3; Word16 i, j, ix, iy, track, trk, max; Word16 prev_i0, i1_offset; Word16 psk, ps, ps0, ps1, ps2, sq, sq2; Word16 alpk, alp, alp_16; Word32 s, alp0, alp1, alp2; Word16 *p0, *p1, *p2, *p3, *p4; Word16 sign_dn[L_SUBFR], sign_dn_inv[L_SUBFR], *psign; Word16 tmp_vect[NB_POS]; Word16 *rri0i0, *rri1i1, *rri2i2, *rri3i3, *rri4i4; Word16 *rri0i1, *rri0i2, *rri0i3, *rri0i4; Word16 *rri1i2, *rri1i3, *rri1i4; Word16 *rri2i3, *rri2i4; Word16 *ptr_rri0i3_i4; Word16 *ptr_rri1i3_i4; Word16 *ptr_rri2i3_i4; Word16 *ptr_rri3i3_i4; /* Init pointers */ rri0i0 = rr; rri1i1 = rri0i0 + NB_POS; rri2i2 = rri1i1 + NB_POS; rri3i3 = rri2i2 + NB_POS; rri4i4 = rri3i3 + NB_POS; rri0i1 = rri4i4 + NB_POS; rri0i2 = rri0i1 + MSIZE; rri0i3 = rri0i2 + MSIZE; rri0i4 = rri0i3 + MSIZE; rri1i2 = rri0i4 + MSIZE; rri1i3 = rri1i2 + MSIZE; rri1i4 = rri1i3 + MSIZE; rri2i3 = rri1i4 + MSIZE; rri2i4 = rri2i3 + MSIZE; /*-----------------------------------------------------------------------* * Chose the sign of the impulse. * *-----------------------------------------------------------------------*/ for (i=0; i<L_SUBFR; i++) { if (dn[i] >= 0) { sign_dn[i] = MAX_16; sign_dn_inv[i] = MIN_16; } else { sign_dn[i] = MIN_16; sign_dn_inv[i] = MAX_16; dn[i] = negate(dn[i]); } } /*-------------------------------------------------------------------* * Modification of rrixiy[] to take signs into account. * *-------------------------------------------------------------------*/ p0 = rri0i1; p1 = rri0i2; p2 = rri0i3; p3 = rri0i4; for(i0=0; i0<L_SUBFR; i0+=STEP) { psign = sign_dn; if (psign[i0] < 0) psign = sign_dn_inv; for(i1=1; i1<L_SUBFR; i1+=STEP) { *p0++ = mult(*p0, psign[i1]); *p1++ = mult(*p1, psign[i1+1]); *p2++ = mult(*p2, psign[i1+2]); *p3++ = mult(*p3, psign[i1+3]); } } p0 = rri1i2; p1 = rri1i3; p2 = rri1i4; for(i1=1; i1<L_SUBFR; i1+=STEP) { psign = sign_dn; if (psign[i1] < 0) psign = sign_dn_inv; for(i2=2; i2<L_SUBFR; i2+=STEP) { *p0++ = mult(*p0, psign[i2]); *p1++ = mult(*p1, psign[i2+1]); *p2++ = mult(*p2, psign[i2+2]); } } p0 = rri2i3; p1 = rri2i4; for(i2=2; i2<L_SUBFR; i2+=STEP) { psign = sign_dn; if (psign[i2] < 0) psign = sign_dn_inv; for(i3=3; i3<L_SUBFR; i3+=STEP) { *p0++ = mult(*p0, psign[i3]); *p1++ = mult(*p1, psign[i3+1]); } } /*-------------------------------------------------------------------* * Search the optimum positions of the four pulses which maximize * * square(correlation) / energy * *-------------------------------------------------------------------*/ psk = -1; alpk = 1; ptr_rri0i3_i4 = rri0i3; ptr_rri1i3_i4 = rri1i3; ptr_rri2i3_i4 = rri2i3; ptr_rri3i3_i4 = rri3i3; /* Initializations only to remove warning from some compilers */ ip0=0; ip1=1; ip2=2; ip3=3; ix=0; iy=0; ps=0; /* search 2 times: track 3 and 4 */ for (track=3, trk=0; track<5; track++, trk++) { /*------------------------------------------------------------------* * depth first search 3, phase A: track 2 and 3/4. * *------------------------------------------------------------------*/ sq = -1; alp = 1; /* i0 loop: 2 positions in track 2 */ prev_i0 = -1; for (i=0; i<2; i++) { max = -1; /* search "dn[]" maximum position in track 2 */ for (j=2; j<L_SUBFR; j+=STEP) { if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0)) { max = dn[j]; i0 = j; } } prev_i0 = i0; j = mult(i0, 6554); /* j = i0/5 */ p0 = rri2i2 + j; ps1 = dn[i0]; alp1 = L_mult(*p0, _1_4); /* i1 loop: 8 positions in track 2 */ p0 = ptr_rri2i3_i4 + shl(j, 3); p1 = ptr_rri3i3_i4; for (i1=track; i1<L_SUBFR; i1+=STEP) { ps2 = add(ps1, dn[i1]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */ alp2 = L_mac(alp1, *p0++, _1_2); alp2 = L_mac(alp2, *p1++, _1_4); sq2 = mult(ps2, ps2); alp_16 = round(alp2); s = L_msu(L_mult(alp,sq2),sq,alp_16); if (s > 0) { sq = sq2; ps = ps2; alp = alp_16; ix = i0; iy = i1; } } } i0 = ix; i1 = iy; i1_offset = shl(mult(i1, 6554), 3); /* j = 8*(i1/5) */ /*------------------------------------------------------------------* * depth first search 3, phase B: track 0 and 1. * *------------------------------------------------------------------*/ ps0 = ps; alp0 = L_mult(alp, _1_4); sq = -1; alp = 1; /* build vector for next loop to decrease complexity */ p0 = rri1i2 + mult(i0, 6554); p1 = ptr_rri1i3_i4 + mult(i1, 6554); p2 = rri1i1; p3 = tmp_vect; for (i3=1; i3<L_SUBFR; i3+=STEP) { /* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */ s = L_mult(*p0, _1_4); p0 += NB_POS; s = L_mac(s, *p1, _1_4); p1 += NB_POS; s = L_mac(s, *p2++, _1_8); *p3++ = round(s); } /* i2 loop: 8 positions in track 0 */ p0 = rri0i2 + mult(i0, 6554); p1 = ptr_rri0i3_i4 + mult(i1, 6554); p2 = rri0i0; p3 = rri0i1; for (i2=0; i2<L_SUBFR; i2+=STEP) { ps1 = add(ps0, dn[i2]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */ alp1 = L_mac(alp0, *p0, _1_8); p0 += NB_POS; alp1 = L_mac(alp1, *p1, _1_8); p1 += NB_POS; alp1 = L_mac(alp1, *p2++, _1_16); /* i3 loop: 8 positions in track 1 */ p4 = tmp_vect; for (i3=1; i3<L_SUBFR; i3+=STEP) { ps2 = add(ps1, dn[i3]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */ alp2 = L_mac(alp1, *p3++, _1_8); alp2 = L_mac(alp2, *p4++, _1_2); sq2 = mult(ps2, ps2); alp_16 = round(alp2); s = L_msu(L_mult(alp,sq2),sq,alp_16); if (s > 0) { sq = sq2; alp = alp_16; ix = i2; iy = i3; } } } /*----------------------------------------------------------------* * depth first search 3: compare codevector with the best case. * *----------------------------------------------------------------*/ s = L_msu(L_mult(alpk,sq),psk,alp); if (s > 0) { psk = sq; alpk = alp; ip2 = i0; ip3 = i1; ip0 = ix; ip1 = iy; } /*------------------------------------------------------------------* * depth first search 4, phase A: track 3 and 0. * *------------------------------------------------------------------*/ sq = -1; alp = 1; /* i0 loop: 2 positions in track 3/4 */ prev_i0 = -1; for (i=0; i<2; i++) { max = -1; /* search "dn[]" maximum position in track 3/4 */ for (j=track; j<L_SUBFR; j+=STEP) { if ((sub(dn[j], max) > 0) && (sub(prev_i0,j) != 0)) { max = dn[j]; i0 = j; } } prev_i0 = i0; j = mult(i0, 6554); /* j = i0/5 */ p0 = ptr_rri3i3_i4 + j; ps1 = dn[i0]; alp1 = L_mult(*p0, _1_4); /* i1 loop: 8 positions in track 0 */ p0 = ptr_rri0i3_i4 + j; p1 = rri0i0; for (i1=0; i1<L_SUBFR; i1+=STEP) { ps2 = add(ps1, dn[i1]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i1] + 1/2*rr[i1][i1]; */ alp2 = L_mac(alp1, *p0, _1_2); p0 += NB_POS; alp2 = L_mac(alp2, *p1++, _1_4); sq2 = mult(ps2, ps2); alp_16 = round(alp2); s = L_msu(L_mult(alp,sq2),sq,alp_16); if (s > 0) { sq = sq2; ps = ps2; alp = alp_16; ix = i0; iy = i1; } } } i0 = ix; i1 = iy; i1_offset = shl(mult(i1, 6554), 3); /* j = 8*(i1/5) */ /*------------------------------------------------------------------* * depth first search 4, phase B: track 1 and 2. * *------------------------------------------------------------------*/ ps0 = ps; alp0 = L_mult(alp, _1_4); sq = -1; alp = 1; /* build vector for next loop to decrease complexity */ p0 = ptr_rri2i3_i4 + mult(i0, 6554); p1 = rri0i2 + i1_offset; p2 = rri2i2; p3 = tmp_vect; for (i3=2; i3<L_SUBFR; i3+=STEP) { /* rrv[i3] = rr[i3][i3] + rr[i0][i3] + rr[i1][i3]; */ s = L_mult(*p0, _1_4); p0 += NB_POS; s = L_mac(s, *p1++, _1_4); s = L_mac(s, *p2++, _1_8); *p3++ = round(s); } /* i2 loop: 8 positions in track 1 */ p0 = ptr_rri1i3_i4 + mult(i0, 6554); p1 = rri0i1 + i1_offset; p2 = rri1i1; p3 = rri1i2; for (i2=1; i2<L_SUBFR; i2+=STEP) { ps1 = add(ps0, dn[i2]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i2] + rr[i1][i2] + 1/2*rr[i2][i2]; */ alp1 = L_mac(alp0, *p0, _1_8); p0 += NB_POS; alp1 = L_mac(alp1, *p1++, _1_8); alp1 = L_mac(alp1, *p2++, _1_16); /* i3 loop: 8 positions in track 2 */ p4 = tmp_vect; for (i3=2; i3<L_SUBFR; i3+=STEP) { ps2 = add(ps1, dn[i3]); /* index increment = STEP */ /* alp1 = alp0 + rr[i0][i3] + rr[i1][i3] + rr[i2][i3] + 1/2*rr[i3][i3]; */ alp2 = L_mac(alp1, *p3++, _1_8); alp2 = L_mac(alp2, *p4++, _1_2); sq2 = mult(ps2, ps2); alp_16 = round(alp2); s = L_msu(L_mult(alp,sq2),sq,alp_16); if (s > 0) { sq = sq2; alp = alp_16; ix = i2; iy = i3; } } } /*----------------------------------------------------------------* * depth first search 1: compare codevector with the best case. * *----------------------------------------------------------------*/ s = L_msu(L_mult(alpk,sq),psk,alp); if (s > 0) { psk = sq; alpk = alp; ip3 = i0; ip0 = i1; ip1 = ix; ip2 = iy; } ptr_rri0i3_i4 = rri0i4; ptr_rri1i3_i4 = rri1i4; ptr_rri2i3_i4 = rri2i4; ptr_rri3i3_i4 = rri4i4; } /* Set the sign of impulses */ i0 = sign_dn[ip0]; i1 = sign_dn[ip1]; i2 = sign_dn[ip2]; i3 = sign_dn[ip3]; /* Find the codeword corresponding to the selected positions */ for(i=0; i<L_SUBFR; i++) { cod[i] = 0; } cod[ip0] = shr(i0, 2); /* From Q15 to Q13 */ cod[ip1] = shr(i1, 2); cod[ip2] = shr(i2, 2); cod[ip3] = shr(i3, 2); /* find the filtered codeword */ for (i = 0; i < ip0; i++) y[i] = 0; if(i0 > 0) for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = h[j]; else for(i=ip0, j=0; i<L_SUBFR; i++, j++) y[i] = negate(h[j]); if(i1 > 0) for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]); else for(i=ip1, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]); if(i2 > 0) for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]); else for(i=ip2, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]); if(i3 > 0) for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = add(y[i], h[j]); else for(i=ip3, j=0; i<L_SUBFR; i++, j++) y[i] = sub(y[i], h[j]); /* find codebook index; 17-bit address */ i = 0; if(i0 > 0) i = add(i, 1); if(i1 > 0) i = add(i, 2); if(i2 > 0) i = add(i, 4); if(i3 > 0) i = add(i, 8); *sign = i; ip0 = mult(ip0, 6554); /* ip0/5 */ ip1 = mult(ip1, 6554); /* ip1/5 */ ip2 = mult(ip2, 6554); /* ip2/5 */ i = mult(ip3, 6554); /* ip3/5 */ j = add(i, shl(i, 2)); /* j = i*5 */ j = sub(ip3, add(j, 3)); /* j= ip3%5 -3 */ ip3 = add(shl(i, 1), j); i = add(ip0, shl(ip1, 3)); i = add(i , shl(ip2, 6)); i = add(i , shl(ip3, 9)); return i; }

inline ivect mult(imat M, ivect v) { return matc2vec(mult(M, vec2matc(v)), 0); }

static int random(int seed) { int tmp; tmp = (mult(seed,CONST_b)+1); return tmp; }

void UART2_ProcessSpektrumData( ) { // PROCESS Spektrum receiver data -- see http://www.desertrc.com/spektrum_protocol.htm for protocol // Determine what to do with received character uint8_t i; int16_t scaleInt; _Q16 tmpQ16, scaleQ16, scale2Q16; // Data is 14 bytes long for( i=0; i<14; i+=2) { uint16_t rcdata = spektrumData[i] << 8; // MSB rcdata += spektrumData[i+1]; // LSB #ifdef DSMX // 11-bit data mode uint16_t cmddata = (int16_t) (rcdata & 0b0000011111111111); // get last 11 bits uint8_t channel = rcdata >> 11; // get 5 first bits scaleQ16 = num1024; scale2Q16 = num2048; scaleInt = 8; #else // 10-bit data mode uint16_t cmddata = (int16_t) (rcdata & 0b0000001111111111); // get last 10 bits uint8_t channel = rcdata >> 10; // get 6 first bits scaleQ16 = num512; scale2Q16 = num1024; scaleInt = 4; #endif switch(channel){ // process channel data case 0: RCdata.ch0 = cmddata; break; case 1: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch1 = _IQ16div(tmpQ16,scale2Q16); break; case 2: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch2 = _IQ16div(tmpQ16,scale2Q16); break; case 3: int16toQ16(&tmpQ16,&cmddata); tmpQ16 -= scaleQ16; RCdata.ch3 = _IQ16div(tmpQ16,scaleQ16); break; case 4: RCdata.ch4 = cmddata; break; case 5: RCdata.ch5 = cmddata; break; case 6: RCdata.ch6 = cmddata; break; default: break; } // End switch channel } // End for each data byte // manual mode if (RCdata.ch4 > 600) { CmdData.AttCmd = 1; //Debug: Scaling throttle down 2 times //CmdData.throttle = RCdata.ch0/(scaleInt*2); // RCdata is between 0 and 1023, this will be approximately between 0 and 255 //Normal throttle CmdData.throttle = RCdata.ch0/(scaleInt); // RCdata is between 0 and 1023, this will be approximately between 0 and 255 _Q16 halfRoll = -RCdata.ch1; //mult(RCdata.ch1,num0p5); // this is about -0.6 -> 0.6 which is +/- ~70 degrees _Q16 halfPitch = RCdata.ch2; //mult(RCdata.ch2,num0p5); _Q16 cosRoll = _Q16cos(halfRoll); _Q16 sinRoll = _Q16sin(halfRoll); _Q16 cosPitch = _Q16cos(halfPitch); _Q16 sinPitch = _Q16sin(halfPitch); CmdData.q_cmd.o = mult(cosRoll,cosPitch); CmdData.q_cmd.x = mult(sinRoll,cosPitch); CmdData.q_cmd.y = mult(cosRoll,sinPitch); CmdData.q_cmd.z = -mult(sinRoll,sinPitch); CmdData.p = CmdData.q = 0; CmdData.r = mult(RCdata.ch3,num2p0); // Turn on green LED to signify manual control mode ON led_on(LED_GREEN); } else { // Turn off green LED to signify manual control mode OFF led_off(LED_GREEN); CmdData.AttCmd = 0; } }

Node Node::operator*(const IntLeaf &leaf) const { return mult(leaf); }

void prim_conv11::operator() ( signal_& quality__io, signal_& wiregroup__io, signal_& hstrip__io, signal_& clctpat__io, signal_& ph__io, signal_& th__io, signal_& vl__io, signal_& phzvl__io, signal_& me11a__io, signal_& clctpat_r__io, signal_& ph_hit__io, signal_& th_hit__io, signal_& sel__io, signal_& addr__io, signal_& r_in__io, signal_& r_out__io, signal_& we__io, signal_& clk__io, signal_& control_clk__io ) { if (!built) { seg_ch = 2; bw_ph = 8; bw_th = 7; bw_fph = 12; bw_fth = 8; bw_wg = 7; bw_ds = 7; bw_hs = 8; pat_w_st3 = 3; pat_w_st1 = pat_w_st3 + 1; full_pat_w_st3 = (1 << (pat_w_st3+1)) - 1; full_pat_w_st1 = (1 << (pat_w_st1+1)) - 1; padding_w_st1 = full_pat_w_st1 / 2; padding_w_st3 = full_pat_w_st3 / 2; red_pat_w_st3 = pat_w_st3 * 2 + 1; red_pat_w_st1 = pat_w_st1 * 2 + 1; fold = 4; th_ch11 = seg_ch*seg_ch; bw_q = 4; bw_addr = 7; ph_raw_w = (1 << pat_w_st3) * 15; th_raw_w = (1 << bw_th); max_drift = 3; bw_phi = 12; bw_eta = 7; ph_hit_w = 40+4; ph_hit_w20 = ph_hit_w; ph_hit_w10 = 20+4; th_hit_w = 56 + 8; endcap = 1; n_strips = (station <= 1 && cscid <= 2) ? 64 : (station <= 1 && cscid >= 6) ? 64 : 80; n_wg = (station <= 1 && cscid <= 3) ? 48 : (station <= 1 && cscid >= 6) ? 32 : (station == 2 && cscid <= 3) ? 112 : (station >= 3 && cscid <= 3) ? 96 : 64; th_coverage = (station <= 1 && cscid <= 2) ? 45 : (station <= 1 && cscid >= 6) ? 27 : (station <= 1 && cscid >= 3) ? 39 : (station == 2 && cscid <= 2) ? 43 : (station == 2 && cscid >= 3) ? 56 : (station == 3 && cscid <= 2) ? 34 : (station == 3 && cscid >= 3) ? 52 : (station == 4 && cscid <= 2) ? 28 : (station == 4 && cscid >= 3) ? 50 : 0; ph_coverage = (station <= 1 && cscid >= 6) ? 15 : //30 : (station >= 2 && cscid <= 2) ? 40 : 20; th_ch = (station <= 1 && cscid <= 2) ? (seg_ch*seg_ch) : seg_ch; ph_reverse = (endcap == 1 && station >= 3) ? 1 : (endcap == 2 && station < 3) ? 1 : 0; th_mem_sz = (1 << bw_addr); th_corr_mem_sz = (1 << bw_addr); mult_bw = bw_fph + 11; ph_zone_bnd1 = (station <= 1 && cscid <= 2) ? 41 : (station == 2 && cscid <= 2) ? 41 : (station == 2 && cscid > 2) ? 87 : (station == 3 && cscid > 2) ? 49 : (station == 4 && cscid > 2) ? 49 : 127; ph_zone_bnd2 = (station == 3 && cscid > 2) ? 87 : 127; zone_overlap = 2; bwr = 6; bpow = 6; cnr = (1 << bpow); cnrex = ph_raw_w; build(); // input parameters from MPC quality.attach(quality__io); wiregroup.attach(wiregroup__io); hstrip.attach(hstrip__io); clctpat.attach(clctpat__io); sel.attach(sel__io); addr.attach(addr__io); r_in.attach(r_in__io); we.attach(we__io); clk.attach(clk__io); control_clk.attach(control_clk__io); // outputs // low-precision ph, only for detection // high-precision ph with displacement correction will be calculated when // 3 best tracks are found. ph.attach(ph__io); // full precision th, but without displacement correction, takes th duplication into account th.attach(th__io); // one-bit valid flags vl.attach(vl__io); phzvl.attach(phzvl__io); me11a.attach(me11a__io); clctpat_r.attach(clctpat_r__io); // ph and th raw hits ph_hit.attach(ph_hit__io); th_hit.attach(th_hit__io); r_out.attach(r_out__io); } pc_id(3,0) = cscid; pc_id(7,4) = station; r_out = (sel == const_(2, 0x0UL)) ? params[addr] : (sel == const_(2, 0x1UL)) ? th_mem[addr] : (sel == const_(2, 0x2UL)) ? th_corr_mem[addr] : pc_id; beginalways(); if (posedge (control_clk)) { if (( (sel) == 0)) { { if (we) params [addr] = r_in; } } else if (( (sel) == 1)) { { if (we) th_mem [addr] = r_in; } } else if (( (sel) == 2)) { { if (we) th_corr_mem[addr] = r_in(3,0); } } } endalways(); beginalways(); if (posedge (clk)) { // zero outputs vl = 0; phzvl = 0; for (i = 0; i < seg_ch; i = i+1) { fph[i] = 0; clctpat_r[i] = 0; } for (i = 0; i < th_ch; i = i+1) th[i] = 0; ph_hit = 0; th_hit = 0; for (i = 0; i < seg_ch; i = i+1) { factor[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127) ? 1707 : // ME1/1a 1301; // ME1/1b //if(factor[i]) // std::cout<<"factor 11 = "<<factor[i]<<std::endl; me11a_w[i] = (station <= 1 && cscid <= 2 && hstrip[i] > 127); if (( (clctpat[i]) == 0)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 1)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 2)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else if (( (clctpat[i]) == 3)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 4)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 1; } } else if (( (clctpat[i]) == 5)) { { clct_pat_corr = const_(3, 0x5UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 6)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else if (( (clctpat[i]) == 7)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 8)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 1; } } else if (( (clctpat[i]) == 9)) { { clct_pat_corr = const_(3, 0x2UL); clct_pat_sign = 0; } } else if (( (clctpat[i]) == 10)) { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } else { { clct_pat_corr = const_(3, 0x0UL); clct_pat_sign = 0; } } // reverse clct pattern correction if chamber is reversed // if (ph_reverse) clct_pat_sign = ~clct_pat_sign; // convert into 1/8 strips and remove ME1/1a offset (512=128*4) eight_str[i] = (const_s(2, 0x0UL), hstrip [i], const_s(2, 0x0UL)) - (me11a_w[i] ? 512 : 0); // clct pattern correction if (clct_pat_sign == 0) eight_str[i] = eight_str[i] + clct_pat_corr(2,1); else eight_str[i] = eight_str[i] - clct_pat_corr(2,1); if (quality[i]) { vl[i] = 1; // ph conversion // for factors 1024 and 2048 the multiplier should be replaced with shifts by synthesizer mult = eight_str[i] * factor[i]; ph_tmp = mult(mult_bw-1 , 10); //std::cout<<"ph_tmp = "<<ph_tmp<<std::endl; ph_init_ix = me11a_w[i] ? const_(3, 2UL) : const_(3, 0UL); // index of ph_init parameter to apply (different for ME11a and b) //std::cout<<"ph_init_ix = "<<ph_init_ix<<std::endl; if (ph_reverse) { fph[i] = params[ph_init_ix] - ph_tmp; // set ph raw hits ph_hit[ph_coverage - ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1; } else { fph[i] = params[ph_init_ix] + ph_tmp; // set ph raw hits ph_hit[ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)] = 1; /* -----\/----- EXCLUDED -----\/----- // add hits to take ME11a strip ganging into account // offsets of 14 and 28 is what I observe from MC. Calculations show 11.6 and 23.2 (???) if (me11a_w[i]) { ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 14] = 1; ph_hit[ph_tmp(bw_fph-1,5) + params[2] + 28] = 1; } -----/\----- EXCLUDED -----/\----- */ } //std::cout<<"estr = "<<eight_str[i]<<", factor = "<<factor[i]<<", ph_rev = "<<ph_reverse<<", ph_tmp = "<<ph_tmp<<std::endl; //std::cout<<"ph_hit = "<<ph_tmp(bw_fph-1,5) + params[ph_init_ix + const_(3, 1UL)](7,1)<<std::endl; //std::cout<<"strip = "<<hstrip[i]<<", Id = "<<cscid + 1<<",phinit = "<<(params[ph_init_ix])<<"ph_cov = "<<ph_coverage<<", and phshift = "<<(ph_tmp(bw_fph-1,5))<<", and phdisp = "<<(params[ph_init_ix + const_(3, 1UL)](7,1))<<"\n\n\n"; wg = wiregroup[i]; // th conversion // call appropriate LUT, it returns th[i] relative to wg0 of that chamber th_orig = th_mem[wg]; //std::cout<<"wire = "<<wg<<", th_mem[wg] = "<<th_orig<<std::endl; // need th duplication here for (j = 0; j < seg_ch; j = j+1) { if (quality[j]) { // calculate correction for each strip number // index is: (wiregroup(2 MS bits), dblstrip(5-bit for these chambers)) index = (wg(5,4), eight_str[j](8,4)); th_corr = th_corr_mem[index]; //std::cout<<"eightstrip = "<<eight_str[j]<<", eightstrip(8,4) = "<<eight_str[j](8,4)<<", index = "<<index<<", th_corr = "<<th_corr<<std::endl; //std::cout<<"th_corr = "<<th_corr<<" and th_orig = "<<th_orig<<std::endl; // apply correction to the corresponding output if (ph_reverse) th_tmp = (th_orig - th_corr) & const_(6, 0x3fUL); else th_tmp = (th_orig + th_corr) & const_(6, 0x3fUL); //std::cout<<"ph_reverse = "<<ph_reverse<<" "; //if(th_tmp)std::cout<<"th_tmp = "<<th_tmp<<" and thcoverage = "<<th_coverage<<std::endl; // check that correction did not make invalid value outside chamber coverage // this will actually take care of both positive and negative illegal values if (th_tmp < th_coverage) { // apply initial th value for that chamber th[i*seg_ch+j] = th_tmp + params[4]; //std::cout<<"params[4] = "<<params[4]<<"\n"; // th hits th_hit[th_tmp + params[5]] = 1; // check which zones ph hits should be applied to if (th[i*seg_ch+j] <= (ph_zone_bnd1 + zone_overlap)) phzvl[0] = 1; if (th[i*seg_ch+j] > (ph_zone_bnd2 - zone_overlap)) phzvl[2] = 1; if ( (th[i*seg_ch+j] > (ph_zone_bnd1 - zone_overlap)) && (th[i*seg_ch+j] <= (ph_zone_bnd2 + zone_overlap)) ) phzvl[1] = 1; //std::cout<<"ph_zone_bnd1 = "<<ph_zone_bnd1<<std::endl; //std::cout<<"phzvl = "<<phzvl<<std::endl; } } } clctpat_r[i] = clctpat[i]; // just propagate pattern downstream } // if (quality[i]) ph[i] = fph[i]; //if(fph[i]) std::cout<<"fph["<<i<<"] = "<<fph[i]<<" and vl[i] = "<<vl[i]<<std::endl; } // for (i = 0; i < seg_ch; i = i+1) me11a = me11a_w; } endalways(); }

void WebRtcG729fix_Decod_ld8a( Decod_ld8a_state *st, int16_t parm[], /* (i) : vector of synthesis parameters parm[0] = bad frame indicator (bfi) */ int16_t synth[], /* (o) : synthesis speech */ int16_t A_t[], /* (o) : decoded LP filter in 2 subframes */ int16_t *T2, /* (o) : decoded pitch lag in 2 subframes */ int16_t *Vad, /* (o) : frame type */ int16_t bad_lsf /* (i) : bad LSF indicator */ ) { int16_t *Az; /* Pointer on A_t */ int16_t lsp_new[M]; /* LSPs */ int16_t code[L_SUBFR]; /* ACELP codevector */ /* Scalars */ int16_t i, j, i_subfr; int16_t T0, T0_frac, index; int16_t bfi; int32_t L_temp; int16_t bad_pitch; /* bad pitch indicator */ /* for G.729B */ int16_t ftyp; int16_t lsfq_mem[MA_NP][M]; int Overflow; /* Test bad frame indicator (bfi) */ bfi = *parm++; /* for G.729B */ ftyp = *parm; if(bfi == 1) { if(st->past_ftyp == 1) { ftyp = 1; parm[4] = 1; /* G.729 maintenance */ } else ftyp = 0; *parm = ftyp; /* modification introduced in version V1.3 */ } *Vad = ftyp; /* Processing non active frames (SID & not transmitted) */ if(ftyp != 1) { WebRtcG729fix_Get_decfreq_prev(st, lsfq_mem); WebRtcG729fix_Dec_cng(st, st->past_ftyp, st->sid_sav, st->sh_sid_sav, parm, st->exc, st->lsp_old, A_t, &st->seed, lsfq_mem); WebRtcG729fix_Update_decfreq_prev(st, lsfq_mem); Az = A_t; for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) { Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0); if(Overflow != 0) { /* In case of overflow in the synthesis */ /* -> Scale down vector exc[] and redo synthesis */ for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++) st->old_exc[i] = shr(st->old_exc[i], 2); WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1); } else WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M); Az += MP1; *T2++ = st->old_T0; } st->sharp = SHARPMIN; } /* Processing active frame */ else { st->seed = INIT_SEED; parm++; /* Decode the LSPs */ WebRtcG729fix_D_lsp(st, parm, lsp_new, WebRtcSpl_AddSatW16(bfi, bad_lsf)); parm += 2; /* Note: "bad_lsf" is introduce in case the standard is used with channel protection. */ /* Interpolation of LPC for the 2 subframes */ WebRtcG729fix_Int_qlpc(st->lsp_old, lsp_new, A_t); /* update the LSFs for the next frame */ WEBRTC_SPL_MEMCPY_W16(st->lsp_old, lsp_new, M); /*------------------------------------------------------------------------* * Loop for every subframe in the analysis frame * *------------------------------------------------------------------------* * The subframe size is L_SUBFR and the loop is repeated L_FRAME/L_SUBFR * * times * * - decode the pitch delay * * - decode algebraic code * * - decode pitch and codebook gains * * - find the excitation and compute synthesis speech * *------------------------------------------------------------------------*/ Az = A_t; /* pointer to interpolated LPC parameters */ for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) { index = *parm++; /* pitch index */ if(i_subfr == 0) { i = *parm++; /* get parity check result */ bad_pitch = WebRtcSpl_AddSatW16(bfi, i); if( bad_pitch == 0) { WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac); st->old_T0 = T0; } else /* Bad frame, or parity error */ { T0 = st->old_T0; T0_frac = 0; st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1); if(st->old_T0 > PIT_MAX) { st->old_T0 = PIT_MAX; } } } else /* second subframe */ { if(bfi == 0) { WebRtcG729fix_Dec_lag3(index, PIT_MIN, PIT_MAX, i_subfr, &T0, &T0_frac); st->old_T0 = T0; } else { T0 = st->old_T0; T0_frac = 0; st->old_T0 = WebRtcSpl_AddSatW16(st->old_T0, 1); if (st->old_T0 > PIT_MAX) { st->old_T0 = PIT_MAX; } } } *T2++ = T0; /*-------------------------------------------------* * - Find the adaptive codebook vector. * *-------------------------------------------------*/ WebRtcG729fix_Pred_lt_3(&st->exc[i_subfr], T0, T0_frac, L_SUBFR); /*-------------------------------------------------------* * - Decode innovative codebook. * * - Add the fixed-gain pitch contribution to code[]. * *-------------------------------------------------------*/ if(bfi != 0) /* Bad frame */ { parm[0] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x1fff; /* 13 bits random */ parm[1] = WebRtcG729fix_Random(&st->seed_fer) & (int16_t)0x000f; /* 4 bits random */ } WebRtcG729fix_Decod_ACELP(parm[1], parm[0], code); parm +=2; j = shl(st->sharp, 1); /* From Q14 to Q15 */ if(T0 < L_SUBFR) { for (i = T0; i < L_SUBFR; i++) { code[i] = WebRtcSpl_AddSatW16(code[i], mult(code[i-T0], j)); } } /*-------------------------------------------------* * - Decode pitch and codebook gains. * *-------------------------------------------------*/ index = *parm++; /* index of energy VQ */ WebRtcG729fix_Dec_gain(st, index, code, L_SUBFR, bfi, &st->gain_pitch, &st->gain_code); /*-------------------------------------------------------------* * - Update pitch sharpening "sharp" with quantized gain_pitch * *-------------------------------------------------------------*/ st->sharp = st->gain_pitch; if (st->sharp > SHARPMAX) { st->sharp = SHARPMAX; } else if (st->sharp < SHARPMIN) { st->sharp = SHARPMIN; } /*-------------------------------------------------------* * - Find the total excitation. * * - Find synthesis speech corresponding to exc[]. * *-------------------------------------------------------*/ for (i = 0; i < L_SUBFR; i++) { /* exc[i] = gain_pitch*exc[i] + gain_code*code[i]; */ /* exc[i] in Q0 gain_pitch in Q14 */ /* code[i] in Q13 gain_codeode in Q1 */ L_temp = L_mult(st->exc[i+i_subfr], st->gain_pitch); L_temp = L_mac(L_temp, code[i], st->gain_code); L_temp = L_shl(L_temp, 1); st->exc[i+i_subfr] = L_round(L_temp); } Overflow = WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 0); if(Overflow != 0) { /* In case of overflow in the synthesis */ /* -> Scale down vector exc[] and redo synthesis */ for (i = 0; i < PIT_MAX + L_INTERPOL + L_FRAME; i++) st->old_exc[i] = shr(st->old_exc[i], 2); WebRtcG729fix_Syn_filt(Az, &st->exc[i_subfr], &synth[i_subfr], L_SUBFR, st->mem_syn, 1); } else WEBRTC_SPL_MEMCPY_W16(st->mem_syn, &synth[i_subfr+L_SUBFR-M], M); Az += MP1; /* interpolated LPC parameters for next subframe */ } } /*------------* * For G729b *-----------*/ if(bfi == 0) { L_temp = 0L; for (i = 0; i < L_FRAME; i++) { L_temp = L_mac(L_temp, st->exc[i], st->exc[i]); } /* may overflow => last level of SID quantizer */ st->sh_sid_sav = WebRtcSpl_NormW32(L_temp); st->sid_sav = L_round(L_shl(L_temp, st->sh_sid_sav)); st->sh_sid_sav = WebRtcSpl_SubSatW16(16, st->sh_sid_sav); } /*--------------------------------------------------* * Update signal for next frame. * * -> shift to the left by L_FRAME exc[] * *--------------------------------------------------*/ Copy(&st->old_exc[L_FRAME], &st->old_exc[0], PIT_MAX+L_INTERPOL); /* for G729b */ st->past_ftyp = ftyp; return; }