|
|
//
// ITU-T G.723 Floating Point Speech Coder ANSI C Source Code. Version 1.00
// copyright (c) 1995, AudioCodes, DSP Group, France Telecom,
// Universite de Sherbrooke, Intel Corporation. All rights reserved.
//
#include "opt.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <memory.h>
#include "typedef.h"
#include "cst_lbc.h"
#include "tab_lbc.h"
#include "util_lbc.h"
#include "exc_lbc.h"
#include "timer.h"
#include "mmxutil.h"
// This file contains pitch and excitation related functions.
//------------------------------------------------------
#if COMPILE_MMX
int Estim_Int(float *Dpnt, int Start){ int i,k;
#define NCOR (PitchMax+1-PitchMin) // = 128 (rounded up to mult of 4)
#define NTAPS (2*SubFrLen+12) // = 132
int Pr,Indx = PitchMin; float MaxE = 1.0f; float MaxC = 0.0f; float E,C2,E2,Diff; int corr[NCOR];
typedef struct { short taps[4][NTAPS]; //** These two arrays need
short temp[PitchMax-3+2*SubFrLen]; //** to be 8-byte aligned
double foo; } EstimStruct;
EstimStruct est,*e; e = &est;
ASM { mov eax,e; add e,7; and e,0fffffff8h; mov e,eax; }
// Convert just the necessary portion of Dpnt to 16-bit integers,
// store the result in 'temp'. 4 guard bits are needed since the
// correlations are length 120, which means 7 guard bits are needed.
// So we use 4 so that 4+4=8 guard bits are present in the product.
FloatToShortScaled(&Dpnt[Start-PitchMax+3],e->temp,PitchMax-3+2*SubFrLen,4);
MakeAligned4(&e->temp[PitchMax-3],e->taps[0],2*NTAPS); MakeAligned2(&e->temp[PitchMax-4],e->taps[1],2*NTAPS); MakeAligned0(&e->temp[PitchMax-5],e->taps[2],2*NTAPS); MakeAligned6(&e->temp[PitchMax-6],e->taps[3],2*NTAPS);
for (i=0; i<4; i++) { for (k=0; k<i; k++) e->taps[i][k] = 0; for (k=NTAPS-12+i; k<NTAPS; k++) e->taps[i][k] = 0; }
// Compute cross-correlations, store in corr[] array
CorrelateInt4(e->taps[0],e->temp,corr,NTAPS-12,NCOR>>2); for (i=1; i<4; i++) CorrelateInt4(e->taps[i],e->temp,&corr[i],NTAPS,NCOR>>2);
// Now do the actual pitch search.
Pr = Start - PitchMin; k = PitchMax-PitchMin-3; E = DotProd(&Dpnt[Pr],&Dpnt[Pr],2*SubFrLen); // first energy value
for (i=0; i < (PitchMax-2-PitchMin); i++) {// Update energy
E = E - Dpnt[Pr+2*SubFrLen]*Dpnt[Pr+2*SubFrLen] + Dpnt[Pr]*Dpnt[Pr];
// Check for new maximum
if (corr[k] > 0) { C2 = ((float)corr[k]) * ((float)corr[k]); E2 = C2*MaxE; Diff = (E2 - E*MaxC)*4.0f; if (asint(Diff) > asint(E2) || (Diff > 0.0f && ((i - Indx) < PitchMin))) { Indx = i; MaxE = E; MaxC = C2; } } Pr--; k--; } return(Indx+PitchMin);}
#endif
//------------------------------------------------------
int Estim_Pitch(float *Dpnt, int Start){ int i;
int Pr,Indx = PitchMin; float MaxE = 1.0f; float MaxC = 0.0f; float E,C,C2,E2,Diff; int *dptr,*eptr;
dptr = (int *)&Diff; eptr = (int *)&E2; Pr = Start - PitchMin + 1;
// Init the energy estimate
E = DotProd(&Dpnt[Pr],&Dpnt[Pr],2*SubFrLen);
// Main Open loop pitch search loop
for (i=PitchMin; i <= PitchMax-3; i++) { Pr--;
// Update energy, compute cross
C = DotProd(&Dpnt[Start],&Dpnt[Pr],2*SubFrLen); E = E - Dpnt[Pr+2*SubFrLen]*Dpnt[Pr+2*SubFrLen] + Dpnt[Pr]*Dpnt[Pr]; C2 = C*C;
// Check for new maximum
E2 = C2*MaxE; Diff = (E2 - E*MaxC)*4.0f; if (C > 0.0f && E > 0.0f && (*dptr > *eptr || (Diff > 0.0f && ((i - Indx) < PitchMin)))) { Indx = i; MaxE = E; MaxC = C2; } } return Indx;}
//------------------------------------------------------
PWDEF Comp_Pw(float *Dpnt, int Start, int Olp){
int i,k; float Energy,C,E,C2,MaxE,MaxC2,MaxC,Gopt; PWDEF Pw;
// Compute target energy
Energy = DotProd(&Dpnt[Start],&Dpnt[Start],SubFrLen);
// Find position with maximum C2/E value
MaxE = 1.0f; MaxC = 0.0f; MaxC2 = 0.0f; Pw.Indx = -1; Pw.Gain = 0.0f; k = Start - (Olp-PwRange); E = DotProd(&Dpnt[k],&Dpnt[k],SubFrLen); for (i=0; i <= 2*PwRange; i++) { C = DotProd(&Dpnt[Start],&Dpnt[k],SubFrLen);
if (E > 0.0f && C > 0.0f) { C2 = C*C; if (C2*MaxE > E*MaxC2) { Pw.Indx = i; MaxE = E; MaxC = C; MaxC2 = C2; } } k--;
if (k < 0) { break; }
E = E - Dpnt[k+SubFrLen]*Dpnt[k+SubFrLen] + Dpnt[k]*Dpnt[k];
}
if (Pw.Indx == -1) { Pw.Indx = Olp; return Pw; }
Pw.Gain = 0.0f; if (MaxC2 > MaxE*Energy*0.375f) { if (MaxC > MaxE || MaxE == 0.0f) Gopt = 1.0f; else Gopt = (float) fabs(MaxC)/MaxE;
Pw.Gain = 0.3125f*Gopt; } Pw.Indx = Olp - PwRange + Pw.Indx; return Pw;
}
//--------------------------------------------------------------
void Filt_Pw(float *DataBuff, float *Dpnt, int Start, PWDEF Pw){ int i;
// Perform the harmonic weighting
for (i=0; i < SubFrLen; i++) DataBuff[Start+i] = Dpnt[PitchMax+Start+i] - Pw.Gain*Dpnt[PitchMax+Start-Pw.Indx+i];}
//-----------------------------------------------------------------
void Find_Fcbk(float *Dpnt, float *ImpResp, LINEDEF *Line, int Sfc, enum Crate WrkRate, int flags, int UseMMX){ int i; int Srate,T0_acelp; float gain_T0;
BESTDEF Best = {0};
switch(WrkRate) { case Rate63: Srate = Nb_puls[Sfc]; Best.MaxErr = -99999999.9f;
if (flags & SC_FINDB) { if ((*Line).Olp[Sfc>>1] < SubFrLen-2) Find_Best(&Best, Dpnt, ImpResp, Srate, (*Line).Olp[Sfc>>1]); else Find_Best(&Best, Dpnt, ImpResp, Srate, SubFrLen); } else { Find_Best(&Best, Dpnt, ImpResp, Srate, SubFrLen); if ((*Line).Olp[Sfc>>1] < SubFrLen-2) Find_Best(&Best, Dpnt, ImpResp, Srate, (*Line).Olp[Sfc>>1]); }
// Reconstruct the excitation
for (i=0; i < SubFrLen; i++) Dpnt[i] = 0.0f;
for (i=0; i < Srate; i++) Dpnt[Best.Ploc[i]] = Best.Pamp[i];
// Code the excitation
Fcbk_Pack(Dpnt, &((*Line).Sfs[Sfc]), &Best, Srate);
if (Best.UseTrn == 1) Gen_Trn(Dpnt, Dpnt, (*Line).Olp[Sfc>>1]); break; case Rate53:
T0_acelp = search_T0 ((*Line).Olp[Sfc>>1]-1+(*Line).Sfs[Sfc].AcLg, (*Line).Sfs[Sfc].AcGn, &gain_T0);
#if COMPILE_MMX
if (UseMMX) { (*Line).Sfs[Sfc].Ppos = ACELP_LBC_code_int(Dpnt, ImpResp, T0_acelp, Dpnt, &(*Line).Sfs[Sfc].Mamp, &(*Line).Sfs[Sfc].Grid, &(*Line).Sfs[Sfc].Pamp, gain_T0, flags); } else#endif //COMPILE_MMX
{ (*Line).Sfs[Sfc].Ppos = ACELP_LBC_code(Dpnt, ImpResp, T0_acelp, Dpnt, &(*Line).Sfs[Sfc].Mamp, &(*Line).Sfs[Sfc].Grid, &(*Line).Sfs[Sfc].Pamp, gain_T0, flags);
} (*Line).Sfs[Sfc].Tran = 0; break; }
return;}
//---------------------------------------------------------
void Fcbk_Unpk(float *Tv, SFSDEF Sfs, int Olp, int Sfc, enum Crate WrkRate){ int i,j,Np; float Tv_tmp[SubFrLen+4]; float acelp_gain,gain_T0; int acelp_sign, acelp_shift, acelp_pos; int offset, ipos, T0_acelp; Word32 Acc0;
switch(WrkRate) { case Rate63: { Np = Nb_puls[Sfc];
for (i=0; i < SubFrLen; i++) Tv[i] = 0.0f;
if (Sfs.Ppos >= MaxPosTable[Sfc]) return;
// Decode the amplitudes and positions
j = MaxPulseNum - Np; Acc0 = Sfs.Ppos;
for (i = 0; i < SubFrLen/Sgrid; i++) { Acc0 -= CombinatorialTable[j][i];
if (Acc0 < (Word32) 0) { Acc0 += CombinatorialTable[j][i]; j++;
if ((Sfs.Pamp & (1 << (MaxPulseNum-j))) != 0) Tv[Sfs.Grid + Sgrid*i] = -FcbkGainTable[Sfs.Mamp]; else Tv[Sfs.Grid + Sgrid*i] = FcbkGainTable[Sfs.Mamp];
if (j == MaxPulseNum) break; } }
if (Sfs.Tran == 1) Gen_Trn(Tv, Tv, Olp); break; }
case Rate53: {
for (i = 0; i < SubFrLen+4; i++) Tv_tmp[i] = 0.0f;
acelp_gain = FcbkGainTable[Sfs.Mamp]; acelp_shift = Sfs.Grid; acelp_sign = Sfs.Pamp; acelp_pos = Sfs.Ppos; offset = 0; for(i=0; i<4; i++) { ipos = (acelp_pos & 7); ipos = (ipos << 3) + acelp_shift + offset; if((acelp_sign & 1)== 1) Tv_tmp[ipos] = acelp_gain; else Tv_tmp[ipos] = -acelp_gain; offset += 2; acelp_pos = acelp_pos >> 3; acelp_sign = acelp_sign >> 1; } for (i = 0; i < SubFrLen; i++) Tv[i] = Tv_tmp[i]; T0_acelp = search_T0(Olp-1+Sfs.AcLg, Sfs.AcGn, &gain_T0); if (T0_acelp < SubFrLen-2) { for (i = T0_acelp; i < SubFrLen; i++) Tv[i] += Tv[i-T0_acelp]*gain_T0; } break; } } return;}
//---------------------------------------------------------------------
void Acbk_Filt(float *output,float *input,float fac,float *impresp){#if OPT_ACBKF
ASM { push esi; push edi; push ebx;
mov eax,58; mov esi,input; mov edi,output; mov ebx,impresp;
loop1: fld DP[ebx+4*eax]; fmul fac; fld DP[ebx+4*eax-4]; fmul fac; fld DP[ebx+4*eax-8]; fmul fac; fld DP[ebx+4*eax-12]; fmul fac; // a3 a2 a1 a0
fxch ST(3); fadd DP[esi+4*eax]; // b0 a2 a1 a3
fxch ST(2); fadd DP[esi+4*eax-4]; // b1 a2 b0 a3
fxch ST(1); fadd DP[esi+4*eax-8]; // b2 b1 b0 a3
fxch ST(3); fadd DP[esi+4*eax-12]; // b3 b1 b0 b2
fxch ST(2); fstp DP[edi+4*eax]; // b1 b3 b2
fstp DP[edi+4*eax-4]; // b3 b2
fxch ST(1); fstp DP[edi+4*eax-8]; fstp DP[edi+4*eax-12];
sub eax,4; cmp eax,2; jg loop1;
pop ebx; pop edi; pop esi; }
#else
int i;
for (i=58; i>2; i-=4) { output[i-0] = fac*impresp[i-0] + input[i-0]; output[i-1] = fac*impresp[i-1] + input[i-1]; output[i-2] = fac*impresp[i-2] + input[i-2]; output[i-3] = fac*impresp[i-3] + input[i-3]; }#endif
output[2] = fac*impresp[2] + input[2]; output[1] = fac*impresp[1] + input[1]; output[0] = fac*impresp[0] + input[0];}//---------------------------------------------------------------------
#if COMPILE_MMX
void Find_AcbkInt(float *Tv, float *ImpResp, float *PrevExc, LINEDEF*Line, int Sfc, enum Crate WrkRate, int flags, CODDEF *CodStat){ int i,j,k;
float RezBuf[SubFrLen+ClPitchOrd-1];
short TvInt[SubFrLen]; int Tvxi[SubFrLen];
short *lPntInt,*sPntInt,*PtrInt;
int CorBufInt[4*(2*ClPitchOrd + ClPitchOrd*(ClPitchOrd-1)/2)]; int *lPntd;
long Acc0l, Acc1l; int Olp,Lid,Gid,Hb,t,k1,k2; int Bound[3]; int Lag1, Lag2; int MaxInt,off_filt;
int shift,Tshift,mx;
DECLARE_SHORT(FltBuf0Int,63); DECLARE_SHORT(FltBuf1Int,63); DECLARE_SHORT(FltBuf2Int,63); DECLARE_SHORT(FltBuf3Int,63); DECLARE_SHORT(FltBuf4Int,63) ; DECLARE_SHORT(CorVctInt,4*(2*ClPitchOrd + ClPitchOrd*(ClPitchOrd-1)/2)) ; DECLARE_SHORT(RezTmpInt,16) ; DECLARE_SHORT(RezBufInt,SubFrLen+ClPitchOrd-1) ; DECLARE_SHORT(ImpRespInt,63);
DECLARE_SHORT(Ix,2*SubFrLen+16) ; DECLARE_SHORT(Rx,2*SubFrLen+16); DECLARE_INT(Temp,64);
ALIGN_ARRAY(FltBuf0Int); ALIGN_ARRAY(FltBuf1Int); ALIGN_ARRAY(FltBuf2Int); ALIGN_ARRAY(FltBuf3Int); ALIGN_ARRAY(FltBuf4Int); ALIGN_ARRAY(RezBufInt); ALIGN_ARRAY(RezTmpInt); ALIGN_ARRAY(ImpRespInt); ALIGN_ARRAY(CorVctInt);
ALIGN_ARRAY(Ix); ALIGN_ARRAY(Rx); ALIGN_ARRAY(Temp);
Olp = (*Line).Olp[Sfc>>1]; Lid = Pstep; Gid = 0; Hb = 3 + (Sfc & 1);
// For even frames only
if ((Sfc & 1) == 0) { if (Olp == PitchMin) Olp++; if (Olp > (PitchMax-5)) Olp = PitchMax-5; }
if (flags & SC_LAG1) { lPntInt = &CorVctInt[20]; k1 = 1; k2 = 2; } else { lPntInt = CorVctInt; k1 = 0; k2 = Hb; }
//TIMER_SPOT_ON(Conversion);
//Convert Tv to 16-bit
ConstFloatToInt(Tv, Tvxi, SubFrLen, 32768.0f); for(i=0; i<SubFrLen; i++) TvInt[i] = (short)(((Tvxi[i]<<1)+0x00008000)>>16);
//Convert ImpResp to 16-bit
//Scale by 2^14 & truncate bits right of decimal
ConstFloatToShort(ImpResp,ImpRespInt,SubFrLen,16384.0f);
for (k=k1; k<k2; k++) { lPntd = &CorBufInt[k*20];
// Get residual from the excitation buffer
Get_Rez(RezBuf, PrevExc, Olp-Pstep+k);
//Convert RezBuf to 16-bit
ConstFloatToShort(RezBuf,RezBufInt,SubFrLen+ClPitchOrd-1,1.0f);
// Filter the last one (ClPitchOrd-1) using the impulse responce
//TIMER_SPOT_OFF(Conversion);
//TIMER_SPOT_ON(Convolution);
ab2abbcw(&RezBufInt[ClPitchOrd-1], Rx, SubFrLen);
j=0; for(i=0; i<SubFrLen; i+=2){ Ix[j] =Ix[j+2]=ImpRespInt[SubFrLen-1-i]; Ix[j+1]=Ix[j+3]=ImpRespInt[SubFrLen-2-i]; j+=4; } for(i=0; i<16; i++) Ix[j+i]=0; ConvMMX(Rx, Ix, Temp, 60); for(i=0; i<SubFrLen; i++) FltBuf4Int[i] = (short)(((Temp[i]<<1)+0x00008000)>>16);
//TIMER_SPOT_OFF(Convolution);
//TIMER_SPOT_ON(FbufCalc);
// Update the others (ClPitchOrd-2 down to 0)
Acc0l = ((RezBufInt[3]<<13)+0x00004000)>>15; FltBuf3Int[0] = (short)Acc0l;
Acc0l = ((RezBufInt[2]<<13)+0x00004000)>>15; FltBuf2Int[0] = (short)Acc0l;
Acc0l = ((RezBufInt[1]<<13)+0x00004000)>>15; FltBuf1Int[0] = (short)Acc0l;
Acc0l = ((RezBufInt[0]<<13)+0x00004000)>>15; FltBuf0Int[0] = (short)Acc0l;
DupRezBuf(RezBufInt,RezTmpInt);
FBufCalcInt(FltBuf4Int,FltBuf3Int,ImpRespInt,RezTmpInt,0); FBufCalcInt(FltBuf3Int,FltBuf2Int,ImpRespInt,RezTmpInt,1); FBufCalcInt(FltBuf2Int,FltBuf1Int,ImpRespInt,RezTmpInt,2); FBufCalcInt(FltBuf1Int,FltBuf0Int,ImpRespInt,RezTmpInt,3);
//TIMER_SPOT_OFF(FbufCalc);
//TIMER_SPOT_ON(Dots);
// Compute the cross products with the signal
*lPntd++ = DotMMX60(TvInt,FltBuf0Int)<<1; *lPntd++ = DotMMX60(TvInt,FltBuf1Int)<<1; *lPntd++ = DotMMX60(TvInt,FltBuf2Int)<<1; *lPntd++ = DotMMX60(TvInt,FltBuf3Int)<<1; *lPntd++ = DotMMX60(TvInt,FltBuf4Int)<<1;
// Compute the energies
*lPntd++ = DotMMX60(FltBuf0Int,FltBuf0Int)<<1; *lPntd++ = DotMMX60(FltBuf1Int,FltBuf1Int)<<1; *lPntd++ = DotMMX60(FltBuf2Int,FltBuf2Int)<<1; *lPntd++ = DotMMX60(FltBuf3Int,FltBuf3Int)<<1; *lPntd++ = DotMMX60(FltBuf4Int,FltBuf4Int)<<1;
// Compute the between crosses
*lPntd++ = DotMMX60(FltBuf1Int,FltBuf0Int)<<2;
*lPntd++ = DotMMX60(FltBuf2Int,FltBuf0Int)<<2; *lPntd++ = DotMMX60(FltBuf2Int,FltBuf1Int)<<2;
*lPntd++ = DotMMX60(FltBuf3Int,FltBuf0Int)<<2; *lPntd++ = DotMMX60(FltBuf3Int,FltBuf1Int)<<2; *lPntd++ = DotMMX60(FltBuf3Int,FltBuf2Int)<<2;
*lPntd++ = DotMMX60(FltBuf4Int,FltBuf0Int)<<2; *lPntd++ = DotMMX60(FltBuf4Int,FltBuf1Int)<<2; *lPntd++ = DotMMX60(FltBuf4Int,FltBuf2Int)<<2; *lPntd++ = DotMMX60(FltBuf4Int,FltBuf3Int)<<2;
//TIMER_SPOT_OFF(Dots);
}
//Convert k1 through k2 indices of CorBufInt to 16-bit
// values
Acc1l = 0L; for(j=k1; j<k2; j++) { for(i=0; i<20; i++) { Acc0l = abs(CorBufInt[j*20 + i]); if( Acc0l > Acc1l) Acc1l = Acc0l; } }
//Need a convert_long_to_short routine
shift = norm(Acc1l); for(j=k1; j<k2; j++) { for(i=0; i<20; i++) { CorBufInt[j*20 + i]=CorBufInt[j*20 + i]<<shift; CorBufInt[j*20 + i] += 0x00008000L; //round up to 16 MSBs
*lPntInt++=(short)(CorBufInt[j*20 + i]>>16); } }
/* Test potential error */ Lag1 = Olp - Pstep; Lag2 = Olp - Pstep + Hb - 1;
off_filt = Test_Err(Lag1, Lag2, CodStat);
Bound[0] = NbFilt085_min + (off_filt << 2); if(Bound[0] > NbFilt085) Bound[0] = NbFilt085; Bound[1] = NbFilt170_min + (off_filt << 3); if(Bound[1] > NbFilt170) Bound[1] = NbFilt170;
Bound[2] = 85; //Use subset table in the case t=2
MaxInt = 0;
for (k=k1; k<k2; k++) {
// Select Quantization table
t = 0; if (WrkRate == Rate63) { if ((Sfc & 1) == 0) { if (Olp-Pstep+k >= SubFrLen-2) t = 1; } else { if (Olp >= SubFrLen-2) t = 1; } } else t = 1;
/* If Bound=170 and SC_GAIN=TRUE, use 170subset table.
Else, use full table with limited Bound.*/ //if (t==1 && (flags & SC_GAIN) && Bound[t]==NbFilt170)
if ((WrkRate == Rate53) && (flags & SC_GAIN) && (Bound[t]==NbFilt170)) t = 2; // Search for maximum
//t=1;
sPntInt = AcbkGainTablePtrInt[t]; PtrInt = &CorVctInt[k*20];
//TIMER_SPOT_ON(CodeBook);
CodeBkSrch(PtrInt, sPntInt, Bound[t], &Gid, &MaxInt);
//TIMER_SPOT_OFF(CodeBook);
if (t==2) Gid = GainScramble[Gid]; //else
//Gid = Gid;
Lid = k; }
// Modify Olp for even sub frames
if ((Sfc & 1) == 0) { Olp = Olp - Pstep + Lid; Lid = Pstep; }
// Save Lag, Gain and Olp
(*Line).Sfs[Sfc].AcLg = Lid; (*Line).Sfs[Sfc].AcGn = Gid; (*Line).Olp[Sfc>>1] = Olp;
//ASM emms;
/* ------------------------------ FLOAT -----------------------*/
// Decode the Acbk contribution and subtract it
Decod_Acbk(RezBuf, PrevExc, Olp, Lid, Gid, WrkRate);
//TIMER_SPOT_ON(LastConvolv);
mx = FloatToShortScaled(RezBuf, RezBufInt, SubFrLen+ClPitchOrd-1, 3); Tshift = 11 - (mx-126); if(mx==0) Tshift = 0;
ab2abbcw(RezBufInt, Rx, 60); ConvMMX(Rx, Ix, Temp, SubFrLen);
//ASM emms;
if (Tshift >=0) { for(j=0; j<SubFrLen; j++){ Temp[j] = Temp[j]>>Tshift; Tv[j]=((float)(Tvxi[j] - Temp[j]))*0.00003052f; } } else { for(j=0; j<SubFrLen; j++){ Temp[j] = Temp[j]<<(-Tshift); Tv[j]=((float)(Tvxi[j] - Temp[j]))*0.00003052f; } }
//TIMER_SPOT_OFF(LastConvolv);
}#endif //COMPILE_MMX
short norm(long L_var1){ short var_out;
if (L_var1 == 0L) { var_out = (short)0; } else { if (L_var1 == (long)0xffffffffL) { var_out = (short)31; } else { if (L_var1 < 0L) { L_var1 = ~L_var1; }
for(var_out = (short)0;L_var1 < 0x40000000L;var_out++) { L_var1 <<= 1L; } } }
return(var_out);}/*---------------------------------------------------------------------*/void Find_Acbk(float *Tv, float *ImpResp, float *PrevExc, LINEDEF*Line, int Sfc, enum Crate WrkRate, int flags, CODDEF *CodStat){ int i,j,k;
float Acc0,Max;
float RezBuf[SubFrLen+ClPitchOrd-1]; float FltBuf[ClPitchOrd][SubFrLen]; float CorVct[4*(2*ClPitchOrd + ClPitchOrd*(ClPitchOrd-1)/2)]; float *lPnt; float *sPnt,*Ptr; int Olp,Lid,Gid,Hb,t,k1,k2; int Bound[3]; int Lag1, Lag2; int off_filt;
Olp = (*Line).Olp[Sfc>>1]; Lid = Pstep; Gid = 0; Hb = 3 + (Sfc & 1);
// For even frames only
if ((Sfc & 1) == 0) { if (Olp == PitchMin) Olp++; if (Olp > (PitchMax-5)) Olp = PitchMax-5; }
if (flags & SC_LAG1) { lPnt = &CorVct[20]; k1 = 1; k2 = 2; } else { lPnt = CorVct; k1 = 0; k2 = Hb; }
for (k=k1; k<k2; k++) {
// Get residual from the exitation buffer
Get_Rez(RezBuf, PrevExc, Olp-Pstep+k);
// Filter the last one (ClPitchOrd-1) using the impulse responce
for (i=0; i < SubFrLen; i++) FltBuf[ClPitchOrd-1][i] = DotRev(&RezBuf[ClPitchOrd-1],ImpResp,i+1); // Update the others (ClPitchOrd-2 down to 0)
for (i=ClPitchOrd-2; i >= 0; i --) { FltBuf[i][0] = RezBuf[i]*0.5f; Acbk_Filt(&FltBuf[i][1],&FltBuf[i+1][0],RezBuf[i],&ImpResp[1]);// for (j = 1; j < SubFrLen; j++)
// FltBuf[i][j] = RezBuf[i]*ImpResp[j] + FltBuf[i+1][j-1];
}
// Compute the cross products with the signal
for (i=0; i < ClPitchOrd; i++) *lPnt++ = DotProd(Tv, FltBuf[i], SubFrLen);
// Compute the energies
for (i=0; i < ClPitchOrd; i++) *lPnt++ = 0.5f*DotProd(FltBuf[i], FltBuf[i], SubFrLen);
// Compute the between crosses
for (i=1; i < ClPitchOrd; i++) for (j = 0; j < i; j++) *lPnt++ = DotProd(FltBuf[i], FltBuf[j], SubFrLen);
}
/* Test potential error */ Lag1 = Olp - Pstep; Lag2 = Olp - Pstep + Hb - 1;
off_filt = Test_Err(Lag1, Lag2, CodStat);
Bound[0] = NbFilt085_min + (off_filt << 2); if(Bound[0] > NbFilt085) Bound[0] = NbFilt085; Bound[1] = NbFilt170_min + (off_filt << 3); if(Bound[1] > NbFilt170) Bound[1] = NbFilt170;
Bound[2] = 85; //Use subset table in the case t=2
Max = 0.0f;
for (k=k1; k<k2; k++) {
// Select Quantization table
t = 0; if (WrkRate == Rate63) { if ((Sfc & 1) == 0) { if (Olp-Pstep+k >= SubFrLen-2) t = 1; } else { if (Olp >= SubFrLen-2) t = 1; } } else t = 1;
/* If Bound=170 and SC_GAIN=TRUE, use 170subset table.
Else, use full table with limited Bound.*/ if (t==1 && (flags & SC_GAIN) && Bound[t]==NbFilt170) t = 2; // Search for maximum
sPnt = AcbkGainTablePtr[t]; Ptr = &CorVct[k*20];
for (i=0; i < Bound[t]; i++) { Acc0 = Ptr[0]*sPnt[0] + Ptr[1]*sPnt[1] + Ptr[2]*sPnt[2] + Ptr[3]*sPnt[3] + Ptr[4]*sPnt[4] + Ptr[5]*sPnt[5] + Ptr[6]*sPnt[6] + Ptr[7]*sPnt[7] + Ptr[8]*sPnt[8] + Ptr[9]*sPnt[9] + Ptr[10]*sPnt[10] + Ptr[11]*sPnt[11] + Ptr[12]*sPnt[12] + Ptr[13]*sPnt[13] + Ptr[14]*sPnt[14] + Ptr[15]*sPnt[15] + Ptr[16]*sPnt[16] + Ptr[17]*sPnt[17] + Ptr[18]*sPnt[18] + Ptr[19]*sPnt[19];
sPnt += 20;
if (asint(Acc0) > asint(Max)) // integer cmp, since Max is not negative.
{ Max = Acc0;
if (t==2) Gid = GainScramble[i]; else Gid = i;
Lid = k; } } }
// Modify Olp for even sub frames
if ((Sfc & 1) == 0) { Olp = Olp - Pstep + Lid; Lid = Pstep; }
// Save Lag, Gain and Olp
(*Line).Sfs[Sfc].AcLg = Lid; (*Line).Sfs[Sfc].AcGn = Gid; (*Line).Olp[Sfc>>1] = Olp;
// Decode the Acbk contribution and subtract it
Decod_Acbk(RezBuf, PrevExc, Olp, Lid, Gid, WrkRate);
for (i=0; i < SubFrLen; i++) Tv[i] -= DotRev(RezBuf,ImpResp,i+1);
}
//-----------------------------------------------------------------
void Get_Rez(float *Tv, float *PrevExc, int Lag){ int i,n,div,mod; float *src,*dst;
for (i=0; i < ClPitchOrd/2; i++) Tv[i] = PrevExc[PitchMax - Lag - ClPitchOrd/2 + i];
n = SubFrLen+ClPitchOrd/2; div = n/Lag; mod = n%Lag;
dst = &Tv[ClPitchOrd/2]; src = &PrevExc[PitchMax-Lag]; for (i=0; i<div; i++) { memcpy(dst,src,4*Lag); dst += Lag; } memcpy(dst,src,4*mod);}
//-----------------------------------------------------------------
void Decod_Acbk(float *Tv, float *PrevExc, int Olp, int Lid, int Gid, enum Crate WrkRate){ int i;
float RezBuf[SubFrLen+ClPitchOrd-1]; float *sPnt;
Get_Rez(RezBuf, PrevExc, (Olp + Lid) - Pstep);
// Select Quantization tables
i = 0; if (WrkRate == Rate63) { if (Olp >= (SubFrLen-2)) i++; } else i=1;
sPnt = AcbkGainTablePtr[i] + Gid*20;
// Compute output vector
for (i=0; i < SubFrLen; i++) Tv[i] = RezBuf[i]*sPnt[0] + RezBuf[i+1]*sPnt[1] + RezBuf[i+2]*sPnt[2] + RezBuf[i+3]*sPnt[3] + RezBuf[i+4]*sPnt[4];}
//-----------------------------------------------
int Comp_Info(float Buff[60], int Olp){ int i;
float Acc0;
float Tenr; float Ccr,Enr; int Indx;
if (Olp > (PitchMax-3)) Olp = (PitchMax-3);
Indx = Olp; Ccr = 0.0f;
for (i=Olp-3; i <= Olp+3; i++) { Acc0 = DotProd(&Buff[PitchMax+Frame-2*SubFrLen], &Buff[PitchMax+Frame-2*SubFrLen-i],2*SubFrLen); if (Acc0 > Ccr) { Ccr = Acc0; Indx = i; } }
// Compute target energy
Tenr = DotProd(&Buff[PitchMax+Frame-2*SubFrLen], &Buff[PitchMax+Frame-2*SubFrLen],2*SubFrLen);
// Compute best energy
Enr = DotProd(&Buff[PitchMax+Frame-2*SubFrLen-Indx], &Buff[PitchMax+Frame-2*SubFrLen-Indx],2*SubFrLen);
if (Ccr <= 0.0f) return 0; if (((0.125f*Enr*Tenr) - (Ccr*Ccr)) < 0.0f) return Indx; else return 0;}
//------------------------------------------------------------------
void Regen(float *DataBuff, float *Buff, int Lag, float Gain, int Ecount, int *Sd){ int i;
// Test for clearing
if (Ecount >= ErrMaxNum) { for (i = 0; i < Frame; i++) DataBuff[i] = 0.0f; for (i = 0; i < Frame+PitchMax; i++) Buff[i] = 0.0f; } else { // Interpolate accordingly to the voicing estimation
if (Lag != 0) { // Voiced case
for (i = 0; i < Frame; i++) Buff[PitchMax+i] = Buff[PitchMax-Lag+i]; for (i = 0; i < Frame; i++) DataBuff[i] = Buff[PitchMax+i] = Buff[PitchMax+i] * 0.75f; } else {
//Unvoiced case
for (i = 0; i < Frame; i++) DataBuff[i] = Gain*(float)Rand_lbc(Sd)*(1.0f/16384.0f);
//Clear buffer to reset memory
for (i = 0; i < Frame+PitchMax; i++) Buff[i] = 0.0f; } }}
//------------------------------------------------------
//Comp_Lpf
//------------------------------------------------------
//Find_B
//------------------------------------------------------
//Find_F
//------------------------------------------------------
//Get_Ind
//------------------------------------------------------
//Filt_Lpf
//---------------------------------------------------------------
int search_T0 (int T0, int Gid, float *gain_T0){
int T0_mod;
T0_mod = T0+epsi170[Gid]; *gain_T0 = gain170[Gid];
return(T0_mod);}
/*
**** Function: Update_Err()**** Description: Estimation of the excitation error associated** to the excitation signal when it is disturbed at** the decoder, the disturbing signal being filtered** by the long term synthesis filters** one value for (SubFrLen/2) samples** Updates the table CodStat.Err**** Links to text: Section**** Arguments:**** int Olp Center value for pitch delay** int AcLg Offset value for pitch delay** int AcGn Index of Gain LT filter**** Outputs: None**** Return value: None***/
#define MAX 256.0f
void Update_Err(int Olp, int AcLg, int AcGn, CODDEF *CodStat){ int i, iz, temp2; int Lag; float Worst1, Worst0, wtemp; float beta,*ptr_tab;
Lag = Olp - Pstep + AcLg;
/* Select Quantization tables */ i = 0 ; ptr_tab = tabgain85; if ( CodStat->WrkRate == Rate63 ) { if ( Olp >= (SubFrLen-2) ) ptr_tab = tabgain170; } else { ptr_tab = tabgain170; } beta = ptr_tab[(int)AcGn];
if(Lag <= (SubFrLen/2)) { Worst0 = CodStat->Err[0]*beta + Err0; Worst1 = Worst0; } else { iz = (Lag*1092) >> 15; temp2 = 30*(iz+1);
if (temp2 != Lag) { if(iz == 1) { Worst0 = CodStat->Err[0]*beta + Err0; Worst1 = CodStat->Err[1]*beta + Err0;
if (Worst0 > Worst1) Worst1 = Worst0; else Worst0 = Worst1; } else { wtemp = CodStat->Err[iz-1]*beta + Err0; Worst0 = CodStat->Err[iz-2]*beta + Err0; if (wtemp > Worst0) Worst0 = wtemp; Worst1 = CodStat->Err[iz]*beta + Err0; if (wtemp > Worst1) Worst1 = wtemp; } } else { Worst0 = CodStat->Err[iz-1]*beta + Err0; Worst1 = CodStat->Err[iz]*beta + Err0; } }
if (Worst0 > MAX) Worst0 = MAX; if (Worst1 > MAX) Worst1 = MAX;
for(i=4; i>=2; i--) CodStat->Err[i] = CodStat->Err[i-2];
CodStat->Err[0] = Worst0; CodStat->Err[1] = Worst1;
return;}
/*
**** Function: Test_Err()**** Description: Check the error excitation maximum for** the subframe and computes an index iTest used to** calculate the maximum nb of filters (in Find_Acbk) :** Bound = Min(Nmin + iTest x pas, Nmax) , with** AcbkGainTable085 : pas = 2, Nmin = 51, Nmax = 85** AcbkGainTable170 : pas = 4, Nmin = 93, Nmax = 170** iTest depends on the relative difference between** errmax and a fixed threshold**** Links to text: Section**** Arguments:**** Word16 Lag1 1st long term Lag of the tested zone** Word16 Lag2 2nd long term Lag of the tested zone**** Outputs: None**** Return value:** Word16 index iTest used to compute Acbk number of filters*/
int Test_Err(int Lag1, int Lag2, CODDEF *CodStat){ int i, i1, i2; int zone1, zone2, iTest; float Err_max;
i2 = Lag2 + ClPitchOrd/2; zone2 = i2/30;
i1 = - SubFrLen + 1 + Lag1 - ClPitchOrd/2; if (i1 <= 0) i1 = 1; zone1 = i1/30;
Err_max = -1.0f; for(i=zone2; i>=zone1; i--) { if (CodStat->Err[i] > Err_max) Err_max = CodStat->Err[i]; } if((Err_max > ThreshErr) || (CodStat->SinDet < 0 ) ) { iTest = 0; //ount_clip++;
} else { iTest = (int)(ThreshErr - Err_max); }
return(iTest);}
#if COMPILE_MMX
#if ASM_FACBK
int DotMMX60(short *ind, short *oud){int dotprod;
#define reg0 mm0
#define reg1 mm1
#define reg2 mm2
#define acc0 mm6
#define inx esi
#define oux edi
#define dot eax
#define jcnt ebx
#define l(n) ASM movq reg##n,QP[inx+8*n]
#define m(n) ASM pmaddwd reg##n,QP[oux+8*n]
#define a(n) ASM paddd acc0,reg##n
ASM { mov inx,ind; mov oux,oud; mov jcnt,5; }
//Begin loop
ASM pxor acc0,acc0; ASM pxor reg1,reg1; //make first a(1) a nop
ASM pxor reg2,reg2; //make first a(2) a nop
inner: //------------------
l(0); a(1);m(0); l(1); a(2); m(1); l(2);a(0); m(2);//-------------------
ASM add inx,24;ASM add oux,24;
ASM sub jcnt,1;ASM jg inner;
a(1);a(2);
ASM{ //Add the two halves of acc0
movq reg0,acc0; psrlq acc0,32; paddd acc0,reg0; movd dot,acc0; //store
mov dotprod,dot}
ASM emms;
return(dotprod);#undef reg0
#undef reg1
#undef reg2
#undef acc0
#undef inx
#undef oux
#undef dot
#undef jcnt
#undef l
#undef m
#undef a
}
#else
int DotMMX60(short *in, short *out){int dotprod;int j;
dotprod=0;for(i=0; i < 60; i++){ dotprod += in[j]*out[j];
}
return(dotprod);}
#endif
#if ASM_FACBK
void DupRezBuf(short *rezbuf, short *reztemp){ #define reg0 mm0
#define reg1 mm1
#define reg2 mm2
#define reg3 mm3
#define rbuf edi
#define rztmp esi
//rezbuf duplication operations
#define cr(r0,r1) ASM movq reg##r0,reg##r1
#define uph(r0) ASM punpckhwd reg##r0,reg##r0
#define upl(r0) ASM punpcklwd reg##r0,reg##r0
#define sto(r0,i) ASM movq QP[rztmp+8*i],reg##r0
#define sl(r0) ASM psllw reg##r0,1
#define l(r0) ASM movq reg##r0,QP[rbuf]
//Duplicate first 4 rezbuf values 4 times each
// and store into 4 QWORDS in reztemp
//Multiply by two while we're at it
ASM mov rbuf,rezbuf; ASM mov rztmp,reztemp;
l(0); sl(0); cr(2,0); uph(0); upl(2); cr(1,0); cr(3,2); uph(0); sto(0,0); upl(1); sto(1,1); uph(2); sto(2,2); upl(3); sto(3,3);
ASM emms;
} #undef reg0
#undef reg1
#undef reg2
#undef reg3
#undef rbuf
#undef rztmp
#undef cr
#undef uph
#undef upl
#undef sto
#undef sl
#undef l
#endif
#if ASM_FACBK
void FBufCalcInt(short *fi, short *fo, short *impresp, short *reztemp, int n){ #define reg0 mm0
#define reg1 mm1
#define reg2 mm2
#define reg3 mm3
#define reg4 mm4
#define reg5 mm5
#define reg6 mm6
#define reg7 mm1
#define reg8 mm7
#define fbufi esi
#define rbuf edi
#define imp edx
#define fbufo ebx
#define jcnt ecx
#define rzv eax
//Diagonal array operations
#define l1(r0,j) ASM movq reg##r0,QP[fbufi+8*j]
#define l2(r0,j) ASM movq reg##r0,QP[fbufi+8+8*j]
#define c3(r0) ASM movq reg##r0,QP[rbuf+8*rzv]
#define m1(r0,j) ASM pmulhw reg##r0,QP[imp+8+8*j]
#define a1(r0,r1) ASM paddsw reg##r0,reg##r1
#define sto(r0,j) ASM movq QP[fbufo+8+8*j], reg##r0
#define s1(r0) ASM psrlq reg##r0,48
#define s2(r0) ASM psllq reg##r0,16
#define or(r0,r1) ASM por reg##r0,reg##r1
//Loop setup
ASM { mov rbuf,reztemp mov jcnt,5; mov fbufi,fi; mov fbufo,fo; mov imp,impresp; mov rzv,n}//Compute initial values
//Zero-th QWORD is different
ASM { movq reg0,QP[fbufo]; psllq reg0,48; psrlq reg0,48;
//zero-th part of fbufo now in reg0
movq reg2,QP[rbuf+8*rzv]; pmulhw reg2,QP[imp+2]; paddsw reg2,QP[fbufi]; psllq reg2,16; por reg0,reg2;
movq QP[fbufo],reg0;}//begin loop
l2(0,0); l1(1,0); s2(0); s1(1); c3(2); m1(2,0); l2(3,1); l1(4,1); s2(3); s1(4); or(0,1); inner: //-------------------------
l2(6,2); a1(0,2); c3(5); m1(5,1); or(3,4); l1(7,2); s2(6); sto(0,0); s1(7); l2(0,3); a1(3,5); c3(8); m1(8,2); or(6,7); l1(1,3); s2(0); sto(3,1); s1(1); l2(3,4); a1(6,8); c3(2); m1(2,3); or(0,1); l1(4,4); s2(3); sto(6,2); s1(4); //-------------------------
ASM add fbufo,24; ASM add fbufi,24; ASM add imp,24;
ASM sub jcnt,1; ASM jg inner;
ASM emms;} #undef reg0
#undef reg1
#undef reg2
#undef reg3
#undef reg4
#undef reg5
#undef reg6
#undef reg7
#undef reg8
#undef fbufi
#undef rbuf
#undef imp
#undef fbufo
#undef jcnt
#undef rzv
#undef l1
#undef l2
#undef c3
#undef m1
#undef a1
#undef sto
#undef s1
#undef s2
#undef or
#else
void FBufCalcInt(short *fi, short *fo, short *impresp, short *rezbuf, short *reztemp, int n){ long Acc0l; int j;
#define MAX16 32767
#define MIN16 -32768
for(j=1; j<SubFrLen; j++) { Acc0l = fi[j-1]; Acc0l += (((rezbuf[4-n]<<1)*impresp[j]))>>16; if (Acc0l > MAX16) Acc0l = MAX16; else if(Acc0l < MIN16) Acc0l = MIN16; fo[j] = (short)(Acc0l); } }#endif
#if ASM_FACBK
//#if 0
void CodeBkSrch(short *lpint, short *spint, int numvecs, int *gid, int *max){
#define reg0 mm0
#define reg1 mm1
#define reg2 mm2
#define reg3 mm3
#define reg4 mm4
#define acc1 mm5
#define acc0 mm6
#define gdx mm3
#define gd mm7
#define icx mm2
#define lp esi
#define sp edi
#define maxx eax
#define gidx edx
#define icnt ebx
// In the following macros, 'n' is the column number.
#define l(n) ASM movq reg##n,QP[lp+8*n]
#define m(n) ASM pmaddwd reg##n,QP[sp+8*n]
#define a(n) ASM paddd acc0,reg##n
ASM { mov sp,spint; mov lp,lpint; mov icnt,numvecs; mov gidx,gid; mov maxx,max; } ASM movd gd,numvecs;//load gd with top codebook index
ASM movd acc1,DP[maxx];//load acc1 with previous max
//Begin loop
outer://inner:
ASM pxor acc0,acc0; ASM pxor reg1,reg1; //make first a(1) a nop
ASM pxor reg2,reg2; //make first a(2) a nop
//--------------------------
l(0); a(1);m(0); l(1); a(2); m(1); l(2);a(0); m(2); l(3); a(1); m(3);l(4); a(2);m(4);
ASM add sp,40; a(3);ASM movq gdx,gd;ASM movd icx,icnt;
a(4);
ASM { movq reg0,acc0; psrlq acc0,32;
pxor gd,icx;//gd=MASK
paddd acc0,reg0; movq reg0,acc0; //copy acc0
movq reg1,acc1; //copy old max
pxor reg1,acc0 pcmpgtd reg0,acc1; //reg0=0xFF or 0x00
pand reg1,reg0; //reg1=MASK or 0x00
pxor acc1,reg1; //acc1=acc0 or acc1
pand gd,reg0; //gd=MASK or 0x00
pxor gd,gdx; //gd=icnt or previous value
sub icnt,1; jg outer; }
ASM movd reg0,numvecs; ASM psubd reg0,gd; ASM movd DP[gidx],reg0;//return gid
ASM movd DP[maxx],acc1;//return max
ASM emms; }#undef reg0
#undef reg1
#undef reg2
#undef reg3
#undef reg4
#undef acc1
#undef acc0
#undef gdx
#undef gd
#undef icx
#undef lp
#undef sp
#undef maxx
#undef gidx
#undef icnt
#undef l
#undef m
#undef a
#else
void CodeBkSrch(short *lpint, short *spint, int numvecs, int *gid, int *max){int acc0;int i,j;
for(i=0; i < numvecs; i++){ acc0 = 0;
for(j=0; j<20; j++) acc0 += lpint[j]*spint[j];
if (acc0 > *max) { *max = acc0; *gid = i; }
spint += 20;}
}
#endif
#endif //COMPILE_MMX
|
