Ignore:
Timestamp:
Jul 26, 2006 5:04:54 PM (18 years ago)
Author:
bennylp
Message:
  • Bring speex codec up to date with their SVN trunk
  • Speex codec should work in FIXED_POINT mode when PJ_HAS_FLOATING_POINT is set to zero.
  • ulaw2linear will return zero if zero is given (this would make the VAD works better, and it also fixed click noise when call is established/hangup).
File:
1 edited

Legend:

Unmodified
Added
Removed

  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/filters.c

    r278 r628  
    7676} 
    7777 
    78 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len) 
     78void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int len) 
    7979{ 
    8080   int i; 
    … …  
    8383      spx_word16_t scale_1; 
    8484      scale = PSHR32(scale, SIG_SHIFT); 
    85       scale_1 = EXTRACT16(DIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale)); 
     85      scale_1 = EXTRACT16(PDIV32_16(SHL32(EXTEND32(SIG_SCALING),7),scale)); 
    8686      for (i=0;i<len;i++) 
    8787      { 
    88          y[i] = SHR32(MULT16_16(scale_1, EXTRACT16(SHR32(x[i],SIG_SHIFT))),7); 
    89       } 
    90    } else { 
     88         y[i] = MULT16_16_P15(scale_1, x[i]); 
     89      } 
     90   } else if (scale > SHR32(EXTEND32(SIG_SCALING), 2)) { 
    9191      spx_word16_t scale_1; 
    9292      scale = PSHR32(scale, SIG_SHIFT-5); 
    … …  
    9494      for (i=0;i<len;i++) 
    9595      { 
    96          y[i] = MULT16_16(scale_1, EXTRACT16(SHR32(x[i],SIG_SHIFT-2))); 
     96         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),8); 
     97      } 
     98   } else { 
     99      spx_word16_t scale_1; 
     100      scale = PSHR32(scale, SIG_SHIFT-7); 
     101      if (scale < 5) 
     102         scale = 5; 
     103      scale_1 = DIV32_16(SHL32(EXTEND32(SIG_SCALING),3),scale); 
     104      for (i=0;i<len;i++) 
     105      { 
     106         y[i] = PSHR32(MULT16_16(scale_1, SHL16(x[i],2)),6); 
    97107      } 
    98108   } 
    … …  
    161171   } 
    162172    
    163    return EXTRACT16(SHR32(SHL32(EXTEND32(spx_sqrt(1+DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT)); 
    164 } 
    165  
     173   return EXTRACT16(PSHR32(SHL32(EXTEND32(spx_sqrt(DIV32(sum,len))),(sig_shift+3)),SIG_SHIFT)); 
     174} 
     175 
     176spx_word16_t compute_rms16(const spx_word16_t *x, int len) 
     177{ 
     178   int i; 
     179   spx_word16_t max_val=10;  
     180 
     181   for (i=0;i<len;i++) 
     182   { 
     183      spx_sig_t tmp = x[i]; 
     184      if (tmp<0) 
     185         tmp = -tmp; 
     186      if (tmp > max_val) 
     187         max_val = tmp; 
     188   } 
     189   if (max_val>16383) 
     190   { 
     191      spx_word32_t sum=0; 
     192      for (i=0;i<len;i+=4) 
     193      { 
     194         spx_word32_t sum2=0; 
     195         sum2 = MAC16_16(sum2,PSHR16(x[i],1),PSHR16(x[i],1)); 
     196         sum2 = MAC16_16(sum2,PSHR16(x[i+1],1),PSHR16(x[i+1],1)); 
     197         sum2 = MAC16_16(sum2,PSHR16(x[i+2],1),PSHR16(x[i+2],1)); 
     198         sum2 = MAC16_16(sum2,PSHR16(x[i+3],1),PSHR16(x[i+3],1)); 
     199         sum = ADD32(sum,SHR32(sum2,6)); 
     200      } 
     201      return SHL16(spx_sqrt(DIV32(sum,len)),4); 
     202   } else { 
     203      spx_word32_t sum=0; 
     204      int sig_shift=0; 
     205      if (max_val < 8192) 
     206         sig_shift=1; 
     207      if (max_val < 4096) 
     208         sig_shift=2; 
     209      if (max_val < 2048) 
     210         sig_shift=3; 
     211      for (i=0;i<len;i+=4) 
     212      { 
     213         spx_word32_t sum2=0; 
     214         sum2 = MAC16_16(sum2,SHL16(x[i],sig_shift),SHL16(x[i],sig_shift)); 
     215         sum2 = MAC16_16(sum2,SHL16(x[i+1],sig_shift),SHL16(x[i+1],sig_shift)); 
     216         sum2 = MAC16_16(sum2,SHL16(x[i+2],sig_shift),SHL16(x[i+2],sig_shift)); 
     217         sum2 = MAC16_16(sum2,SHL16(x[i+3],sig_shift),SHL16(x[i+3],sig_shift)); 
     218         sum = ADD32(sum,SHR32(sum2,6)); 
     219      } 
     220      return SHL16(spx_sqrt(DIV32(sum,len)),3-sig_shift);    
     221   } 
     222} 
    166223 
    167224#ifndef OVERRIDE_NORMALIZE16 
    … …  
    207264   return sqrt(.1+sum/len); 
    208265} 
     266spx_word16_t compute_rms16(const spx_word16_t *x, int len) 
     267{ 
     268   return compute_rms(x, len); 
     269} 
    209270#endif 
    210271 
    … …  
    237298   spx_sig_t xi,yi,nyi; 
    238299 
     300   for (i=0;i<ord;i++) 
     301      mem[i] = SHR32(mem[i],1);    
    239302   for (i=0;i<N;i++) 
    240303   { 
    … …  
    249312      y[i] = yi; 
    250313   } 
    251 } 
    252 #endif 
    253 #endif 
     314   for (i=0;i<ord;i++) 
     315      mem[i] = SHL32(mem[i],1);    
     316} 
     317#endif 
     318#endif 
     319 
     320#ifdef FIXED_POINT 
     321#ifndef OVERRIDE_FILTER_MEM16 
     322void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     323{ 
     324   int i,j; 
     325   spx_word16_t xi,yi,nyi; 
     326   for (i=0;i<N;i++) 
     327   { 
     328      xi= x[i]; 
     329      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767)); 
     330      nyi = NEG16(yi); 
     331      for (j=0;j<ord-1;j++) 
     332      { 
     333         mem[j] = MAC16_16(MAC16_16(mem[j+1], num[j],xi), den[j],nyi); 
     334      } 
     335      mem[ord-1] = ADD32(MULT16_16(num[ord-1],xi), MULT16_16(den[ord-1],nyi)); 
     336      y[i] = yi; 
     337   } 
     338} 
     339#endif 
     340#else 
     341void filter_mem16(const spx_word16_t *x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     342{ 
     343   filter_mem2(x, num, den, y, N, ord, mem); 
     344} 
     345#endif 
     346 
    254347 
    255348#ifndef OVERRIDE_IIR_MEM2 
    … …  
    278371   spx_word32_t xi,yi,nyi; 
    279372 
     373   for (i=0;i<ord;i++) 
     374      mem[i] = SHR32(mem[i],1);    
    280375   for (i=0;i<N;i++) 
    281376   { 
    … …  
    290385      y[i] = yi; 
    291386   } 
    292 } 
    293 #endif 
    294 #endif 
     387   for (i=0;i<ord;i++) 
     388      mem[i] = SHL32(mem[i],1);    
     389} 
     390#endif 
     391#endif 
     392 
     393#ifdef FIXED_POINT 
     394#ifndef OVERRIDE_IIR_MEM16 
     395void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     396{ 
     397   int i,j; 
     398   spx_word16_t yi,nyi; 
     399 
     400   for (i=0;i<N;i++) 
     401   { 
     402      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767)); 
     403      nyi = NEG16(yi); 
     404      for (j=0;j<ord-1;j++) 
     405      { 
     406         mem[j] = MAC16_16(mem[j+1],den[j],nyi); 
     407      } 
     408      mem[ord-1] = MULT16_16(den[ord-1],nyi); 
     409      y[i] = yi; 
     410   } 
     411} 
     412#endif 
     413#else 
     414void iir_mem16(const spx_word16_t *x, const spx_coef_t *den, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     415{ 
     416   iir_mem2(x, den, y, N, ord, mem); 
     417} 
     418#endif 
     419 
    295420 
    296421#ifndef OVERRIDE_FIR_MEM2 
    … …  
    319444   spx_word32_t xi,yi; 
    320445 
     446   for (i=0;i<ord;i++) 
     447      mem[i] = SHR32(mem[i],1);    
    321448   for (i=0;i<N;i++) 
    322449   { 
    … …  
    330457      y[i] = SATURATE(yi,805306368); 
    331458   } 
    332 } 
    333 #endif 
    334 #endif 
    335  
    336  
     459   for (i=0;i<ord;i++) 
     460      mem[i] = SHL32(mem[i],1);    
     461} 
     462#endif 
     463#endif 
     464 
     465#ifdef FIXED_POINT 
     466#ifndef OVERRIDE_FIR_MEM16 
     467void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     468{ 
     469   int i,j; 
     470   spx_word16_t xi,yi; 
     471 
     472   for (i=0;i<N;i++) 
     473   { 
     474      xi=x[i]; 
     475      yi = EXTRACT16(SATURATE(ADD32(EXTEND32(x[i]),PSHR32(mem[0],LPC_SHIFT)),32767)); 
     476      for (j=0;j<ord-1;j++) 
     477      { 
     478         mem[j] = MAC16_16(mem[j+1], num[j],xi); 
     479      } 
     480      mem[ord-1] = MULT16_16(num[ord-1],xi); 
     481      y[i] = yi; 
     482   } 
     483} 
     484#endif 
     485#else 
     486void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack) 
     487{ 
     488   fir_mem2(x, num, y, N, ord, mem); 
     489} 
     490#endif 
    337491 
    338492 
    … …  
    383537   for (;i<N;i++) 
    384538      y[i] = VERY_SMALL; 
    385     
    386539   for (i=0;i<ord;i++) 
    387540      mem1[i] = mem2[i] = 0; 
    … …  
    390543      y1 = ADD16(y[i], EXTRACT16(PSHR32(mem1[0],LPC_SHIFT))); 
    391544      ny1i = NEG16(y1); 
    392       y[i] = ADD16(SHL16(y1,1), EXTRACT16(PSHR32(mem2[0],LPC_SHIFT))); 
     545      y[i] = PSHR32(ADD32(SHL32(EXTEND32(y1),LPC_SHIFT+1),mem2[0]),LPC_SHIFT); 
    393546      ny2i = NEG16(y[i]); 
    394547      for (j=0;j<ord-1;j++) 
    … …  
    427580      for (j=0;j<M2;j++) 
    428581      { 
    429          y1[k]=ADD32(y1[k],SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1)); 
    430          y2[k]=SUB32(y2[k],SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1)); 
     582         y1[k]=ADD32(y1[k],MULT16_16(a[j],ADD16(x[i+j],x2[i-j]))); 
     583         y2[k]=SUB32(y2[k],MULT16_16(a[j],SUB16(x[i+j],x2[i-j]))); 
    431584         j++; 
    432          y1[k]=ADD32(y1[k],SHR(MULT16_16(a[j],ADD16(x[i+j],x2[i-j])),1)); 
    433          y2[k]=ADD32(y2[k],SHR(MULT16_16(a[j],SUB16(x[i+j],x2[i-j])),1)); 
    434       } 
     585         y1[k]=ADD32(y1[k],MULT16_16(a[j],ADD16(x[i+j],x2[i-j]))); 
     586         y2[k]=ADD32(y2[k],MULT16_16(a[j],SUB16(x[i+j],x2[i-j]))); 
     587      } 
     588      y1[k] = SHR32(y1[k],1); 
     589      y2[k] = SHR32(y2[k],1); 
    435590   } 
    436591   for (i=0;i<M-1;i++) 
    … …  
    451606 
    452607   for (i = 0; i < N/2; i++) 
    453       xx[2*i] = SHR(x[N/2-1-i],SIG_SHIFT+1); 
     608      xx[2*i] = PSHR32(x[N/2-1-i],SIG_SHIFT); 
    454609   for (i = 0; i < M - 1; i += 2) 
    455610      xx[N+i] = mem[i+1]; 
    … …  
    470625         x1 = xx[N-2+j-i]; 
    471626 
    472          y0 = ADD32(y0,SHR(MULT16_16(a0, x1),1)); 
    473          y1 = ADD32(y1,SHR(MULT16_16(a1, x1),1)); 
    474          y2 = ADD32(y2,SHR(MULT16_16(a0, x0),1)); 
    475          y3 = ADD32(y3,SHR(MULT16_16(a1, x0),1)); 
     627         y0 = ADD32(y0,SHR(MULT16_16(a0, x1),2)); 
     628         y1 = ADD32(y1,SHR(MULT16_16(a1, x1),2)); 
     629         y2 = ADD32(y2,SHR(MULT16_16(a0, x0),2)); 
     630         y3 = ADD32(y3,SHR(MULT16_16(a1, x0),2)); 
    476631 
    477632         a0 = a[j+2]; 
    … …  
    479634         x0 = xx[N+j-i]; 
    480635 
    481          y0 = ADD32(y0,SHR(MULT16_16(a0, x0),1)); 
    482          y1 = ADD32(y1,SHR(MULT16_16(a1, x0),1)); 
    483          y2 = ADD32(y2,SHR(MULT16_16(a0, x1),1)); 
    484          y3 = ADD32(y3,SHR(MULT16_16(a1, x1),1)); 
     636         y0 = ADD32(y0,SHR(MULT16_16(a0, x0),2)); 
     637         y1 = ADD32(y1,SHR(MULT16_16(a1, x0),2)); 
     638         y2 = ADD32(y2,SHR(MULT16_16(a0, x1),2)); 
     639         y3 = ADD32(y3,SHR(MULT16_16(a1, x1),2)); 
    485640      } 
    486641      y[i] = y0; 
    … …  
    494649} 
    495650 
    496 void comb_filter_mem_init (CombFilterMem *mem) 
    497 { 
    498    mem->last_pitch=0; 
    499    mem->last_pitch_gain[0]=mem->last_pitch_gain[1]=mem->last_pitch_gain[2]=0; 
    500    mem->smooth_gain=1; 
    501 } 
    502  
    503 #ifdef FIXED_POINT 
    504 #define COMB_STEP 32767 
    505 #else 
    506 #define COMB_STEP 1.0 
    507 #endif 
    508  
    509 void comb_filter( 
    510 spx_sig_t *exc,          /*decoded excitation*/ 
    511 spx_sig_t *new_exc,      /*enhanced excitation*/ 
     651#ifdef FIXED_POINT 
     652#if 0 
     653spx_word16_t shift_filt[3][7] = {{-33,    1043,   -4551,   19959,   19959,   -4551,    1043}, 
     654                                 {-98,    1133,   -4425,   29179,    8895,   -2328,     444}, 
     655                                 {444,   -2328,    8895,   29179,   -4425,    1133,     -98}}; 
     656#else 
     657spx_word16_t shift_filt[3][7] = {{-390,    1540,   -4993,   20123,   20123,   -4993,    1540}, 
     658                                {-1064,    2817,   -6694,   31589,    6837,    -990,    -209}, 
     659                                 {-209,    -990,    6837,   31589,   -6694,    2817,   -1064}}; 
     660#endif 
     661#else 
     662#if 0 
     663float shift_filt[3][7] = {{-9.9369e-04, 3.1831e-02, -1.3889e-01, 6.0910e-01, 6.0910e-01, -1.3889e-01, 3.1831e-02}, 
     664                          {-0.0029937, 0.0345613, -0.1350474, 0.8904793, 0.2714479, -0.0710304, 0.0135403}, 
     665                          {0.0135403, -0.0710304, 0.2714479, 0.8904793, -0.1350474, 0.0345613,  -0.0029937}}; 
     666#else 
     667float shift_filt[3][7] = {{-0.011915, 0.046995, -0.152373, 0.614108, 0.614108, -0.152373, 0.046995}, 
     668                          {-0.0324855, 0.0859768, -0.2042986, 0.9640297, 0.2086420, -0.0302054, -0.0063646}, 
     669                          {-0.0063646, -0.0302054, 0.2086420, 0.9640297, -0.2042986, 0.0859768, -0.0324855}}; 
     670#endif 
     671#endif 
     672 
     673int interp_pitch( 
     674spx_word16_t *exc,          /*decoded excitation*/ 
     675spx_word16_t *interp,          /*decoded excitation*/ 
     676int pitch,               /*pitch period*/ 
     677int len 
     678) 
     679{ 
     680   int i,j,k; 
     681   spx_word32_t corr[4][7]; 
     682   spx_word32_t maxcorr; 
     683   int maxi, maxj; 
     684   for (i=0;i<7;i++) 
     685   { 
     686      corr[0][i] = inner_prod(exc, exc-pitch-3+i, len); 
     687   } 
     688   for (i=0;i<3;i++) 
     689   { 
     690      for (j=0;j<7;j++) 
     691      { 
     692         int i1, i2; 
     693         spx_word32_t tmp=0; 
     694         i1 = 3-j; 
     695         if (i1<0) 
     696            i1 = 0; 
     697         i2 = 10-j; 
     698         if (i2>7) 
     699            i2 = 7; 
     700         for (k=i1;k<i2;k++) 
     701            tmp += MULT16_32_Q15(shift_filt[i][k],corr[0][j+k-3]); 
     702         corr[i+1][j] = tmp; 
     703      } 
     704   } 
     705   maxi=maxj=0; 
     706   maxcorr = corr[0][0]; 
     707   for (i=0;i<4;i++) 
     708   { 
     709      for (j=0;j<7;j++) 
     710      { 
     711         if (corr[i][j] > maxcorr) 
     712         { 
     713            maxcorr = corr[i][j]; 
     714            maxi=i; 
     715            maxj=j; 
     716         } 
     717      } 
     718   } 
     719   for (i=0;i<len;i++) 
     720   { 
     721      spx_word32_t tmp = 0; 
     722      if (maxi>0) 
     723      { 
     724         for (k=0;k<7;k++) 
     725         { 
     726            tmp += MULT16_16(exc[i-(pitch-maxj+3)+k-3],shift_filt[maxi-1][k]); 
     727         } 
     728      } else { 
     729         tmp = SHL32(exc[i-(pitch-maxj+3)],15); 
     730      } 
     731      interp[i] = PSHR32(tmp,15); 
     732   } 
     733   return pitch-maxj+3; 
     734} 
     735 
     736void multicomb( 
     737spx_word16_t *exc,          /*decoded excitation*/ 
     738spx_word16_t *new_exc,      /*enhanced excitation*/ 
    512739spx_coef_t *ak,           /*LPC filter coefs*/ 
    513740int p,               /*LPC order*/ 
    514741int nsf,             /*sub-frame size*/ 
    515742int pitch,           /*pitch period*/ 
    516 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/ 
     743int max_pitch, 
    517744spx_word16_t  comb_gain,    /*gain of comb filter*/ 
    518 CombFilterMem *mem 
     745char *stack 
    519746) 
    520747{ 
    521    int i; 
    522    spx_word16_t exc_energy=0, new_exc_energy=0; 
    523    spx_word16_t gain; 
    524    spx_word16_t step; 
    525    spx_word16_t fact; 
    526  
    527    /*Compute excitation amplitude prior to enhancement*/ 
    528    exc_energy = compute_rms(exc, nsf); 
    529    /*for (i=0;i<nsf;i++) 
    530      exc_energy+=((float)exc[i])*exc[i];*/ 
    531  
    532    /*Some gain adjustment if pitch is too high or if unvoiced*/ 
    533 #ifdef FIXED_POINT 
    534    { 
    535       spx_word16_t g = gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain); 
    536       if (g > 166) 
    537          comb_gain = MULT16_16_Q15(DIV32_16(SHL32(EXTEND32(165),15),g), comb_gain); 
    538       if (g < 64) 
    539          comb_gain = MULT16_16_Q15(SHL16(g, 9), comb_gain); 
    540    } 
    541 #else 
    542    { 
    543       float g=0; 
    544       g = GAIN_SCALING_1*.5*(gain_3tap_to_1tap(pitch_gain)+gain_3tap_to_1tap(mem->last_pitch_gain)); 
    545       if (g>1.3) 
    546          comb_gain*=1.3/g; 
    547       if (g<.5) 
    548          comb_gain*=2.*g; 
    549    } 
    550 #endif 
    551    step = DIV32(COMB_STEP, nsf); 
    552    fact=0; 
    553  
    554    /*Apply pitch comb-filter (filter out noise between pitch harmonics)*/ 
     748   int i;  
     749   VARDECL(spx_word16_t *iexc); 
     750   spx_word16_t old_ener, new_ener; 
     751   int corr_pitch; 
     752    
     753   spx_word16_t iexc0_mag, iexc1_mag, exc_mag; 
     754   spx_word32_t corr0, corr1; 
     755   spx_word16_t gain0, gain1; 
     756   spx_word16_t pgain1, pgain2; 
     757   spx_word16_t c1, c2; 
     758   spx_word16_t g1, g2; 
     759   spx_word16_t ngain; 
     760   spx_word16_t gg1, gg2; 
     761 
     762#if 0 /* Set to 1 to enable full pitch search */ 
     763   int nol_pitch[6]; 
     764   spx_word16_t nol_pitch_coef[6]; 
     765   spx_word16_t ol_pitch_coef; 
     766   open_loop_nbest_pitch(exc, 20, 120, nsf,  
     767                         nol_pitch, nol_pitch_coef, 6, stack); 
     768   corr_pitch=nol_pitch[0]; 
     769   ol_pitch_coef = nol_pitch_coef[0]; 
     770   /*Try to remove pitch multiples*/ 
     771   for (i=1;i<6;i++) 
     772   { 
     773#ifdef FIXED_POINT 
     774      if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],19661)) &&  
     775#else 
     776      if ((nol_pitch_coef[i]>.6*nol_pitch_coef[0]) &&  
     777#endif 
     778         (ABS(2*nol_pitch[i]-corr_pitch)<=2 || ABS(3*nol_pitch[i]-corr_pitch)<=3 ||  
     779         ABS(4*nol_pitch[i]-corr_pitch)<=4 || ABS(5*nol_pitch[i]-corr_pitch)<=5)) 
     780      { 
     781         corr_pitch = nol_pitch[i]; 
     782      } 
     783   } 
     784#else 
     785   corr_pitch = pitch; 
     786#endif 
     787    
     788   ALLOC(iexc, 2*nsf, spx_word16_t); 
     789    
     790   interp_pitch(exc, iexc, corr_pitch, 80); 
     791   if (corr_pitch>max_pitch) 
     792      interp_pitch(exc, iexc+nsf, 2*corr_pitch, 80); 
     793   else 
     794      interp_pitch(exc, iexc+nsf, -corr_pitch, 80); 
     795 
     796   /*interp_pitch(exc, iexc+2*nsf, 2*corr_pitch, 80);*/ 
     797    
     798   /*printf ("%d %d %f\n", pitch, corr_pitch, max_corr*ener_1);*/ 
     799   iexc0_mag = spx_sqrt(1000+inner_prod(iexc,iexc,nsf)); 
     800   iexc1_mag = spx_sqrt(1000+inner_prod(iexc+nsf,iexc+nsf,nsf)); 
     801   exc_mag = spx_sqrt(1+inner_prod(exc,exc,nsf)); 
     802   corr0  = inner_prod(iexc,exc,nsf); 
     803   if (corr0<0) 
     804      corr0=0; 
     805   corr1 = inner_prod(iexc+nsf,exc,nsf); 
     806   if (corr1<0) 
     807      corr1=0; 
     808#ifdef FIXED_POINT 
     809   /* Doesn't cost much to limit the ratio and it makes the rest easier */ 
     810   if (SHL32(EXTEND32(iexc0_mag),6) < EXTEND32(exc_mag)) 
     811      iexc0_mag = ADD16(1,PSHR16(exc_mag,6)); 
     812   if (SHL32(EXTEND32(iexc1_mag),6) < EXTEND32(exc_mag)) 
     813      iexc1_mag = ADD16(1,PSHR16(exc_mag,6)); 
     814#endif 
     815   if (corr0 > MULT16_16(iexc0_mag,exc_mag)) 
     816      pgain1 = QCONST16(1., 14); 
     817   else 
     818      pgain1 = PDIV32_16(SHL32(PDIV32(corr0, exc_mag),14),iexc0_mag); 
     819   if (corr1 > MULT16_16(iexc1_mag,exc_mag)) 
     820      pgain2 = QCONST16(1., 14); 
     821   else 
     822      pgain2 = PDIV32_16(SHL32(PDIV32(corr1, exc_mag),14),iexc1_mag); 
     823   gg1 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc0_mag); 
     824   gg2 = PDIV32_16(SHL32(EXTEND32(exc_mag),8), iexc1_mag); 
     825   if (comb_gain>0) 
     826   { 
     827#ifdef FIXED_POINT 
     828      c1 = (MULT16_16_Q15(QCONST16(.4,15),comb_gain)+QCONST16(.07,15)); 
     829      c2 = QCONST16(.5,15)+MULT16_16_Q14(QCONST16(1.72,14),(c1-QCONST16(.07,15))); 
     830#else 
     831      c1 = .4*comb_gain+.07; 
     832      c2 = .5+1.72*(c1-.07); 
     833#endif 
     834   } else  
     835   { 
     836      c1=c2=0; 
     837   } 
     838#ifdef FIXED_POINT 
     839   g1 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain1),pgain1); 
     840   g2 = 32767 - MULT16_16_Q13(MULT16_16_Q15(c2, pgain2),pgain2); 
     841#else 
     842   g1 = 1-c2*pgain1*pgain1; 
     843   g2 = 1-c2*pgain2*pgain2; 
     844#endif 
     845   if (g1<c1) 
     846      g1 = c1; 
     847   if (g2<c1) 
     848      g2 = c1; 
     849   g1 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g1); 
     850   g2 = (spx_word16_t)PDIV32_16(SHL32(EXTEND32(c1),14),(spx_word16_t)g2); 
     851   if (corr_pitch>max_pitch) 
     852   { 
     853      gain0 = MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q14(g1,gg1)); 
     854      gain1 = MULT16_16_Q15(QCONST16(.3,15),MULT16_16_Q14(g2,gg2)); 
     855   } else { 
     856      gain0 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g1,gg1)); 
     857      gain1 = MULT16_16_Q15(QCONST16(.6,15),MULT16_16_Q14(g2,gg2)); 
     858   } 
    555859   for (i=0;i<nsf;i++) 
    556    { 
    557       spx_word32_t exc1, exc2; 
    558  
    559       fact = ADD16(fact,step); 
    560        
    561       exc1 = SHL32(MULT16_32_Q15(SHL16(pitch_gain[0],7),exc[i-pitch+1]) + 
    562                  MULT16_32_Q15(SHL16(pitch_gain[1],7),exc[i-pitch]) + 
    563                  MULT16_32_Q15(SHL16(pitch_gain[2],7),exc[i-pitch-1]) , 2); 
    564       exc2 = SHL32(MULT16_32_Q15(SHL16(mem->last_pitch_gain[0],7),exc[i-mem->last_pitch+1]) + 
    565                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[1],7),exc[i-mem->last_pitch]) + 
    566                  MULT16_32_Q15(SHL16(mem->last_pitch_gain[2],7),exc[i-mem->last_pitch-1]),2); 
    567  
    568       new_exc[i] = exc[i] + MULT16_32_Q15(comb_gain, ADD32(MULT16_32_Q15(fact,exc1), MULT16_32_Q15(SUB16(COMB_STEP,fact), exc2))); 
    569    } 
    570  
    571    mem->last_pitch_gain[0] = pitch_gain[0]; 
    572    mem->last_pitch_gain[1] = pitch_gain[1]; 
    573    mem->last_pitch_gain[2] = pitch_gain[2]; 
    574    mem->last_pitch = pitch; 
    575  
    576    /*Amplitude after enhancement*/ 
    577    new_exc_energy = compute_rms(new_exc, nsf); 
    578  
    579    if (exc_energy > new_exc_energy) 
    580       exc_energy = new_exc_energy; 
    581     
    582    gain = DIV32_16(SHL32(EXTEND32(exc_energy),15),ADD16(1,new_exc_energy)); 
    583  
    584 #ifdef FIXED_POINT 
    585    if (gain < 16384) 
    586       gain = 16384; 
    587 #else 
    588    if (gain < .5) 
    589       gain=.5; 
    590 #endif 
    591  
    592 #ifdef FIXED_POINT 
     860      new_exc[i] = ADD16(exc[i], EXTRACT16(PSHR32(ADD32(MULT16_16(gain0,iexc[i]), MULT16_16(gain1,iexc[i+nsf])),8))); 
     861   /* FIXME: compute_rms16 is currently not quite accurate enough (but close) */ 
     862   new_ener = compute_rms16(new_exc, nsf); 
     863   old_ener = compute_rms16(exc, nsf); 
     864    
     865   if (old_ener < 1) 
     866      old_ener = 1; 
     867   if (new_ener < 1) 
     868      new_ener = 1; 
     869   if (old_ener > new_ener) 
     870      old_ener = new_ener; 
     871   ngain = PDIV32_16(SHL32(EXTEND32(old_ener),14),new_ener); 
     872    
    593873   for (i=0;i<nsf;i++) 
    594    { 
    595       mem->smooth_gain = ADD16(MULT16_16_Q15(31457,mem->smooth_gain), MULT16_16_Q15(1311,gain)); 
    596       new_exc[i] = MULT16_32_Q15(mem->smooth_gain, new_exc[i]); 
    597    } 
    598 #else 
    599    for (i=0;i<nsf;i++) 
    600    { 
    601       mem->smooth_gain = .96*mem->smooth_gain + .04*gain; 
    602       new_exc[i] *= mem->smooth_gain; 
    603    } 
    604 #endif 
    605 } 
     874      new_exc[i] = MULT16_16_Q14(ngain, new_exc[i]); 
     875} 
     876 
Note: See TracChangeset for help on using the changeset viewer.