Changeset 628


Ignore:
Timestamp:
Jul 26, 2006 5:04:54 PM (17 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).
Location:
pjproject/trunk/pjmedia
Files:
1 added
49 edited

Legend:

Unmodified
Added
Removed
  • pjproject/trunk/pjmedia/build/Makefile

    r584 r628  
    104104                speex/quant_lsp.o speex/sb_celp.o speex/smallft.o \ 
    105105                speex/speex.o speex/speex_callbacks.o speex/speex_header.o \ 
    106                 speex/stereo.o speex/vbr.o speex/vq.o 
    107 SPEEX_CFLAGS := -DHAVE_CONFIG=1 -I../src/pjmedia-codec 
     106                speex/stereo.o speex/vbr.o speex/vq.o speex/window.o 
     107SPEEX_CFLAGS := -DHAVE_CONFIG_H=1 -I../src/pjmedia-codec 
    108108 
    109109export PJMEDIA_CODEC_SRCDIR = ../src/pjmedia-codec 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/_kiss_fft_guts.h

    r516 r628  
    2121   typedef struct { kiss_fft_scalar r; kiss_fft_scalar i; }kiss_fft_cpx; */ 
    2222#include "kiss_fft.h" 
    23 #include <limits.h> 
    2423 
    2524#define MAXFACTORS 32 
     
    4645 * */ 
    4746#ifdef FIXED_POINT 
     47#include "misc.h" 
    4848# define FRACBITS 15 
    49 # define SAMPPROD int32_t  
     49# define SAMPPROD spx_int32_t  
    5050#define SAMP_MAX 32767 
    5151 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/arch.h

    r278 r628  
    4242#define MAX16(a,b) ((a) > (b) ? (a) : (b))   /**< Maximum 16-bit value.   */ 
    4343#define ABS32(x) ((x) < 0 ? (-(x)) : (x))    /**< Absolute 32-bit value.  */ 
     44#define MAX32(a,b) ((a) > (b) ? (a) : (b))   /**< Maximum 32-bit value.   */ 
    4445 
    4546#ifdef FIXED_POINT 
     
    4748typedef spx_int16_t spx_word16_t; 
    4849typedef spx_int32_t   spx_word32_t; 
    49 #ifdef _MSC_VER 
    50 typedef __int64      spx_word64_t; 
    51 #elif defined NO_LONGLONG 
    52 typedef double    spx_word64_t; 
    53 #else 
    54 typedef long long    spx_word64_t; 
    55 #endif 
    5650typedef spx_word32_t spx_mem_t; 
    5751typedef spx_word16_t spx_coef_t; 
     
    10498typedef float spx_word16_t; 
    10599typedef float spx_word32_t; 
    106 typedef float spx_word64_t; 
    107100 
    108101#define Q15ONE 1.0f 
     
    147140#define ADD32(a,b) ((a)+(b)) 
    148141#define SUB32(a,b) ((a)-(b)) 
    149 #define ADD64(a,b) ((a)+(b)) 
    150142#define MULT16_16_16(a,b)     ((a)*(b)) 
    151143#define MULT16_16(a,b)     ((spx_word32_t)(a)*(spx_word32_t)(b)) 
     
    162154#define MAC16_16_Q11(c,a,b)     ((c)+(a)*(b)) 
    163155#define MAC16_16_Q13(c,a,b)     ((c)+(a)*(b)) 
     156#define MAC16_16_P13(c,a,b)     ((c)+(a)*(b)) 
    164157#define MULT16_16_Q11_32(a,b)     ((a)*(b)) 
    165158#define MULT16_16_Q13(a,b)     ((a)*(b)) 
     
    167160#define MULT16_16_Q15(a,b)     ((a)*(b)) 
    168161#define MULT16_16_P15(a,b)     ((a)*(b)) 
     162#define MULT16_16_P13(a,b)     ((a)*(b)) 
     163#define MULT16_16_P14(a,b)     ((a)*(b)) 
    169164 
    170 #define DIV32_16(a,b)     ((a)/(b)) 
    171 #define DIV32(a,b)     ((a)/(b)) 
     165#define DIV32_16(a,b)     (((spx_word32_t)(a))/(spx_word16_t)(b)) 
     166#define PDIV32_16(a,b)     (((spx_word32_t)(a))/(spx_word16_t)(b)) 
     167#define DIV32(a,b)     (((spx_word32_t)(a))/(spx_word32_t)(b)) 
     168#define PDIV32(a,b)     (((spx_word32_t)(a))/(spx_word32_t)(b)) 
    172169 
    173170 
     
    175172 
    176173 
    177 #ifdef CONFIG_TI_C55X 
     174#if defined (CONFIG_TI_C54X) || defined (CONFIG_TI_C55X) 
    178175 
    179176/* 2 on TI C5x DSP */ 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/bits.c

    r278 r628  
    9494{ 
    9595   int i; 
    96    if (len > bits->buf_size) 
     96   int nchars = len / BYTES_PER_CHAR; 
     97   if (nchars > bits->buf_size) 
    9798   { 
    9899      speex_warning_int("Packet is larger than allocated buffer: ", len); 
    99100      if (bits->owner) 
    100101      { 
    101          char *tmp = (char*)speex_realloc(bits->chars, len); 
     102         char *tmp = (char*)speex_realloc(bits->chars, nchars); 
    102103         if (tmp) 
    103104         { 
    104             bits->buf_size=len; 
     105            bits->buf_size=nchars; 
    105106            bits->chars=tmp; 
    106107         } else { 
    107             len=bits->buf_size; 
     108            nchars=bits->buf_size; 
    108109            speex_warning("Could not resize input buffer: truncating input"); 
    109110         } 
    110111      } else { 
    111112         speex_warning("Do not own input buffer: truncating input"); 
    112          len=bits->buf_size; 
    113       } 
    114    } 
    115    for (i=0;i<len;i++) 
    116       bits->chars[i]=chars[i]; 
    117    bits->nbBits=len<<3; 
     113         nchars=bits->buf_size; 
     114      } 
     115   } 
     116#if (BYTES_PER_CHAR==2) 
     117/* Swap bytes to proper endian order (could be done externally) */ 
     118#define HTOLS(A) ((((A) >> 8)&0xff)|(((A) & 0xff)<<8)) 
     119#else 
     120#define HTOLS(A) (A) 
     121#endif 
     122   for (i=0;i<nchars;i++) 
     123      bits->chars[i]=HTOLS(chars[i]); 
     124 
     125   bits->nbBits=nchars<<LOG2_BITS_PER_CHAR; 
    118126   bits->charPtr=0; 
    119127   bits->bitPtr=0; 
     
    162170   pos=bits->nbBits>>LOG2_BITS_PER_CHAR; 
    163171   for (i=0;i<nchars;i++) 
    164       bits->chars[pos+i]=chars[i]; 
     172      bits->chars[pos+i]=HTOLS(chars[i]); 
    165173   bits->nbBits+=nchars<<LOG2_BITS_PER_CHAR; 
    166174} 
     
    183191   if (max_nchars > ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR)) 
    184192      max_nchars = ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR); 
    185 #if BYTES_PER_CHAR==1 
    186 #define HTOLS(A) (A) 
    187 #else 
    188 #define HTOLS(A) ((((A) >> 8)&0xff)|(((A) & 0xff)<<8)) 
    189 #endif 
     193 
    190194   for (i=0;i<max_nchars;i++) 
    191195      chars[i]=HTOLS(bits->chars[i]); 
     
    200204      max_nchars = ((bits->nbBits)>>LOG2_BITS_PER_CHAR); 
    201205   for (i=0;i<max_nchars;i++) 
    202       chars[i]=bits->chars[i]; 
    203     
     206      chars[i]=HTOLS(bits->chars[i]); 
     207 
    204208   if (bits->bitPtr>0) 
    205209      bits->chars[0]=bits->chars[max_nchars]; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/cb_search.c

    r278 r628  
    7171            resj = MAC16_16(resj,shape[k],r[j-k]); 
    7272#ifdef FIXED_POINT 
    73          res16 = EXTRACT16(SHR32(resj, 11)); 
     73         res16 = EXTRACT16(SHR32(resj, 13)); 
    7474#else 
    7575         res16 = 0.03125f*resj; 
     
    8989{ 
    9090   int n; 
    91    int q=0; 
    92    for (n=0;n<len;n++,q++) 
    93       t[n] = SUB32(t[n],MULT16_16_Q11_32(g,r[q])); 
     91   for (n=0;n<len;n++) 
     92      t[n] = SUB16(t[n],PSHR32(MULT16_16(g,r[n]),13)); 
    9493} 
    9594#endif 
     
    9897 
    9998static void split_cb_search_shape_sign_N1( 
    100 spx_sig_t target[],                     /* target vector */ 
     99spx_word16_t target[],                  /* target vector */ 
    101100spx_coef_t ak[],                        /* LPCs for this subframe */ 
    102101spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */ 
     
    114113{ 
    115114   int i,j,m,q; 
    116 #ifndef FIXED_POINT 
    117    int n; 
    118 #endif 
    119115   VARDECL(spx_word16_t *resp); 
    120116#ifdef _USE_SSE 
     
    159155   /* FIXME: make that adaptive? */ 
    160156   for (i=0;i<nsf;i++) 
    161       t[i]=EXTRACT16(PSHR32(target[i],6)); 
     157      t[i]=target[i]; 
    162158 
    163159   compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack); 
     
    223219#ifdef FIXED_POINT 
    224220         g=sign*shape_cb[rind*subvect_size+m]; 
     221#else 
     222         g=sign*0.03125*shape_cb[rind*subvect_size+m]; 
     223#endif 
    225224         target_update(t+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1)); 
    226 #else 
    227          g=sign*0.03125*shape_cb[rind*subvect_size+m]; 
    228          /*FIXME: I think that one too can be replaced by target_update */ 
    229          for (n=subvect_size*(i+1);n<nsf;n++,q++) 
    230             t[n] = SUB32(t[n],g*r[q]); 
    231 #endif 
    232225      } 
    233226   } 
     
    245238      syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack); 
    246239      for (j=0;j<nsf;j++) 
    247          target[j]=SUB32(target[j],r2[j]); 
     240         target[j]=SUB16(target[j],EXTRACT16(PSHR32(r2[j],8))); 
    248241   } 
    249242} 
     
    252245 
    253246void split_cb_search_shape_sign( 
    254 spx_sig_t target[],                     /* target vector */ 
     247spx_word16_t target[],                  /* target vector */ 
    255248spx_coef_t ak[],                        /* LPCs for this subframe */ 
    256249spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */ 
     
    357350   /* FIXME: make that adaptive? */ 
    358351   for (i=0;i<nsf;i++) 
    359       t[i]=EXTRACT16(PSHR32(target[i],6)); 
     352      t[i]=target[i]; 
    360353 
    361354   for (j=0;j<N;j++) 
     
    445438#ifdef FIXED_POINT 
    446439            g=sign*shape_cb[rind*subvect_size+m]; 
     440#else 
     441            g=sign*0.03125*shape_cb[rind*subvect_size+m]; 
     442#endif 
    447443            target_update(nt[j]+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1)); 
    448 #else 
    449             g=sign*0.03125*shape_cb[rind*subvect_size+m]; 
    450             /*FIXME: I think that one too can be replaced by target_update */ 
    451             for (n=subvect_size*(i+1);n<nsf;n++,q++) 
    452                nt[j][n] = SUB32(nt[j][n],g*r[q]); 
    453 #endif 
    454444         } 
    455445 
     
    515505      syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack); 
    516506      for (j=0;j<nsf;j++) 
    517          target[j]=SUB32(target[j],r2[j]); 
     507         target[j]=SUB16(target[j],EXTRACT16(PSHR32(r2[j],8))); 
    518508   } 
    519509} 
     
    578568 
    579569void noise_codebook_quant( 
    580 spx_sig_t target[],                     /* target vector */ 
     570spx_word16_t target[],                  /* target vector */ 
    581571spx_coef_t ak[],                        /* LPCs for this subframe */ 
    582572spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */ 
     
    596586   VARDECL(spx_sig_t *tmp); 
    597587   ALLOC(tmp, nsf, spx_sig_t); 
    598    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack); 
     588   for (i=0;i<nsf;i++) 
     589      tmp[i]=PSHR32(EXTEND32(target[i]),SIG_SHIFT); 
     590   residue_percep_zero(tmp, ak, awk1, awk2, tmp, nsf, p, stack); 
    599591 
    600592   for (i=0;i<nsf;i++) 
     
    602594   for (i=0;i<nsf;i++) 
    603595      target[i]=0; 
    604  
    605596} 
    606597 
     
    614605) 
    615606{ 
    616    speex_rand_vec(1, exc, nsf); 
     607   int i; 
     608   /* FIXME: This is bad, but I don't think the function ever gets called anyway */ 
     609   spx_int32_t seed = 0; 
     610   for (i=0;i<nsf;i++) 
     611      exc[i]=SHL32(EXTEND32(speex_rand(1, &seed)),SIG_SHIFT); 
    617612} 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/cb_search.h

    r278 r628  
    5050 
    5151void split_cb_search_shape_sign( 
    52 spx_sig_t target[],             /* target vector */ 
     52spx_word16_t target[],             /* target vector */ 
    5353spx_coef_t ak[],                /* LPCs for this subframe */ 
    5454spx_coef_t awk1[],              /* Weighted LPCs for this subframe */ 
     
    7575 
    7676void noise_codebook_quant( 
    77 spx_sig_t target[],             /* target vector */ 
     77spx_word16_t target[],             /* target vector */ 
    7878spx_coef_t ak[],                /* LPCs for this subframe */ 
    7979spx_coef_t awk1[],              /* Weighted LPCs for this subframe */ 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/cb_search_bfin.h

    r278 r628  
    6262            "LOOP_END inner%=;\n\t" 
    6363            "R0 = A0;\n\t" 
    64             "R0 >>>= 11;\n\t" 
     64            "R0 >>>= 13;\n\t" 
    6565            "A1 += R0.L*R0.L (IS);\n\t" 
    6666            "W[P3++] = R0;\n\t" 
     
    7373         : 
    7474      : "m" (subvect_size), "m" (shape_cb), "m" (r), "m" (resp), "m" (E) 
    75       : "A0", "P0", "P1", "P2", "P3", "P4", "R0", "R1", "R2", "I0", "I1", "L0", "L1", "A0", "A1", "memory" 
     75      : "A0", "P0", "P1", "P2", "P3", "P4", "R0", "R1", "R2", "I0", "I1", "L0",  
     76        "L1", "A0", "A1", "memory", "LC0", "LC1" 
    7677      ); 
    7778      shape_cb += subvect_size; 
     
    8485static inline void target_update(spx_word16_t *t, spx_word16_t g, spx_word16_t *r, int len) 
    8586{ 
     87   if (!len) 
     88      return; 
    8689   __asm__ __volatile__ 
    8790         ( 
     
    9093         "L0 = 0;\n\t" 
    9194         "L1 = 0;\n\t" 
     95         "R2 = 4096;\n\t" 
    9296         "LOOP tupdate%= LC0 = %3;\n\t" 
    9397         "LOOP_BEGIN tupdate%=;\n\t" 
    9498            "R0.L = W[I0] || R1.L = W[I1++];\n\t" 
    9599            "R1 = (A1 = R1.L*%2.L) (IS);\n\t" 
    96             "R1 >>>= 11;\n\t" 
     100            "R1 = R1 + R2;\n\t" 
     101            "R1 >>>= 13;\n\t" 
    97102            "R0.L = R0.L - R1.L;\n\t" 
    98103            "W[I0++] = R0.L;\n\t" 
     
    100105   : 
    101106   : "a" (t), "a" (r), "d" (g), "a" (len) 
    102    : "R0", "R1", "A1", "I0", "I1", "L0", "L1" 
     107   : "R0", "R1", "R2", "A1", "I0", "I1", "L0", "L1" 
    103108         ); 
    104109} 
    105  
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/config.h

    r279 r628  
    11 
    2 #include <pj/config.h> 
     2/* Check if we need to use the fixed point version */ 
     3#if !defined(PJ_HAS_FLOATING_POINT) || PJ_HAS_FLOATING_POINT==0 
     4#   define FIXED_POINT 
     5#endif 
     6 
    37 
    48#define inline __inline 
     
    610 
    711#include "misc.h" 
    8  
    9 #if !defined(PJ_HAS_FLOATING_POINT) || PJ_HAS_FLOATING_POINT==0 
    10 #   define FIXED_POINT 
    11 #endif 
    1212 
    1313#ifdef _MSC_VER 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/fftwrap.c

    r516 r628  
    4343#include "misc.h" 
    4444 
     45#define MAX_FFT_SIZE 2048 
    4546 
    4647#ifdef FIXED_POINT 
     
    226227 
    227228 
    228 int fixed_point = 1; 
    229229#ifdef FIXED_POINT 
    230 #include "smallft.h" 
     230/*#include "smallft.h"*/ 
    231231 
    232232 
     
    240240#else 
    241241#endif 
     242#ifdef VAR_ARRAYS 
    242243   spx_word16_t _in[N]; 
    243244   spx_word16_t _out[N]; 
     245#else 
     246   spx_word16_t _in[MAX_FFT_SIZE]; 
     247   spx_word16_t _out[MAX_FFT_SIZE]; 
     248#endif 
    244249   for (i=0;i<N;i++) 
    245250      _in[i] = (int)floor(.5+in[i]); 
     
    247252   for (i=0;i<N;i++) 
    248253      out[i] = _out[i]; 
     254#if 0 
    249255   if (!fixed_point) 
    250256   { 
     
    258264      spx_drft_clear(&t); 
    259265   } 
     266#endif 
    260267} 
    261268 
     
    269276#else 
    270277#endif 
     278#ifdef VAR_ARRAYS 
    271279   spx_word16_t _in[N]; 
    272280   spx_word16_t _out[N]; 
     281#else 
     282   spx_word16_t _in[MAX_FFT_SIZE]; 
     283   spx_word16_t _out[MAX_FFT_SIZE]; 
     284#endif 
    273285   for (i=0;i<N;i++) 
    274286      _in[i] = (int)floor(.5+in[i]); 
     
    276288   for (i=0;i<N;i++) 
    277289      out[i] = _out[i]; 
     290#if 0 
    278291   if (!fixed_point) 
    279292   { 
     
    286299      spx_drft_clear(&t); 
    287300   } 
     301#endif 
    288302} 
    289303 
  • 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 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/filters.h

    r278 r628  
    3939 
    4040spx_word16_t compute_rms(const spx_sig_t *x, int len); 
     41spx_word16_t compute_rms16(const spx_word16_t *x, int len); 
    4142void signal_mul(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len); 
    42 void signal_div(const spx_sig_t *x, spx_sig_t *y, spx_word32_t scale, int len); 
     43void signal_div(const spx_word16_t *x, spx_word16_t *y, spx_word32_t scale, int len); 
    4344 
    4445#ifdef FIXED_POINT 
     
    4748 
    4849#endif 
    49  
    50 /** Combined filter memory. */ 
    51 typedef struct { 
    52    int   last_pitch; 
    53    spx_word16_t last_pitch_gain[3]; 
    54    spx_word16_t smooth_gain; 
    55 } CombFilterMem; 
    5650 
    5751 
     
    6357void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem); 
    6458void iir_mem2(const spx_sig_t *x, const spx_coef_t *den, spx_sig_t *y, int N, int ord, spx_mem_t *mem); 
     59 
     60void 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); 
     61void 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); 
     62void 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); 
    6563 
    6664/* Apply bandwidth expansion on LPC coef */ 
     
    7573void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack); 
    7674 
    77 void comb_filter_mem_init (CombFilterMem *mem); 
    78  
    79 void comb_filter( 
    80 spx_sig_t *exc,          /*decoded excitation*/ 
    81 spx_sig_t *new_exc,      /*enhanced excitation*/ 
     75void multicomb( 
     76spx_word16_t *exc,          /*decoded excitation*/ 
     77spx_word16_t *new_exc,      /*enhanced excitation*/ 
    8278spx_coef_t *ak,           /*LPC filter coefs*/ 
    8379int p,               /*LPC order*/ 
    8480int nsf,             /*sub-frame size*/ 
    8581int pitch,           /*pitch period*/ 
    86 spx_word16_t *pitch_gain,   /*pitch gain (3-tap)*/ 
     82int max_pitch,   /*pitch gain (3-tap)*/ 
    8783spx_word16_t  comb_gain,    /*gain of comb filter*/ 
    88 CombFilterMem *mem 
     84char *stack 
    8985); 
    9086 
    91  
    9287#endif 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/filters_arm4.h

    r278 r628  
    102102   spx_sig_t xi,yi,nyi; 
    103103 
     104   for (i=0;i<ord;i++) 
     105      mem[i] = SHR32(mem[i],1);    
    104106   for (i=0;i<N;i++) 
    105107   { 
     
    253255    
    254256   } 
     257   for (i=0;i<ord;i++) 
     258      mem[i] = SHL32(mem[i],1);    
    255259} 
    256260 
     
    260264   int i,j; 
    261265   spx_sig_t xi,yi,nyi; 
     266 
     267   for (i=0;i<ord;i++) 
     268      mem[i] = SHR32(mem[i],1);    
    262269 
    263270   for (i=0;i<N;i++) 
     
    377384    
    378385   } 
     386   for (i=0;i<ord;i++) 
     387      mem[i] = SHL32(mem[i],1);    
     388 
    379389} 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/filters_bfin.h

    r278 r628  
    3333*/ 
    3434 
    35 #include <stdio.h> 
    36  
    3735#define OVERRIDE_NORMALIZE16 
    3836int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len) 
     
    4038   spx_sig_t max_val=1; 
    4139   int sig_shift; 
    42  
    4340   __asm__  
    4441   ( 
     
    6865   "I0 = %0;\n\t" 
    6966   "L0 = 0;\n\t" 
    70    "I1 = %1;\n\t" 
    71    "L1 = 0;\n\t" 
     67   "P1 = %1;\n\t" 
    7268   "R0 = [I0++];\n\t" 
    73    "LOOP norm_shift%= LC0 = %3 >> 1;\n\t" 
     69   "LOOP norm_shift%= LC0 = %3;\n\t" 
    7470   "LOOP_BEGIN norm_shift%=;\n\t" 
    75       "R1 = ASHIFT R0 by %2.L || R2 = [I0++];\n\t" 
    76       "R3 = ASHIFT R2 by %2.L || R0 = [I0++];\n\t" 
    77       "R3 = PACK(R3.L, R1.L);\n\t" 
    78       "[I1++] = R3;\n\t" 
     71      "R1 = ASHIFT R0 by %2.L || R0 = [I0++];\n\t" 
     72      "W[P1++] = R1;\n\t" 
    7973   "LOOP_END norm_shift%=;\n\t" 
    80    : : "a" (x), "a" (y), "d" (-sig_shift), "a" (len) 
    81    : "I0", "L0", "I1", "L1", "R0", "R1", "R2", "R3", "memory" 
     74   "R1 = ASHIFT R0 by %2.L;\n\t" 
     75   "W[P1++] = R1;\n\t" 
     76   : : "a" (x), "a" (y), "d" (-sig_shift), "a" (len-1) 
     77   : "I0", "L0", "P1", "R0", "R1", "memory" 
    8278   ); 
    8379   return sig_shift; 
     
    104100   "P0 = %3;\n\t" 
    105101   "I0 = P0;\n\t" 
    106    "B0 = P0;\n\t" 
     102   "B0 = P0;\n\t" /* numden */ 
    107103   "L0 = 0;\n\t" 
    108104       
    109    "P2 = %0;\n\t" 
     105   "P2 = %0;\n\t" /* Fused xy */ 
    110106   "I2 = P2;\n\t" 
    111107   "L2 = 0;\n\t" 
    112108    
    113    "P4 = %6;\n\t" 
    114    "P0 = %1;\n\t" 
    115    "P1 = %2;\n\t" 
     109   "P4 = %6;\n\t" /* mem */ 
     110   "P0 = %1;\n\t" /* _x */ 
     111   "P1 = %2;\n\t" /* _y */ 
    116112    
    117113   /* First sample */ 
    118114   "R1 = [P4++];\n\t" 
    119    "R1 <<= 1;\n\t" 
    120    "R2 = [P0++];\n\t" 
     115   "R1 <<= 1;\n\t" /* shift mem */ 
     116   "R2 = [P0++];\n\t" /* load x[0] */ 
    121117   "R1 = R1 + R2;\n\t" 
    122    "[P1++] = R1;\n\t" 
     118   "[P1++] = R1;\n\t" /* store y[0] */ 
    123119   "R1 <<= 2;\n\t" 
    124120   "R2 <<= 2;\n\t" 
    125    "R2 = PACK(R1.H, R2.H);\n\t" 
     121   "R2 = PACK(R1.H, R2.H);\n\t" /* pack x16 and y16 */ 
    126122   "[P2] = R2;\n\t" 
    127123                
     
    148144      "A0 += A1;\n\t" 
    149145      "R4 = A0;\n\t" 
    150       "R4 <<= 1;\n\t" 
    151       "R2 = [P0++];\n\t" 
     146      "R4 <<= 1;\n\t" /* shift mem */ 
     147      "R2 = [P0++];\n\t" /* load x */ 
    152148      "R4 = R4 + R2;\n\t" 
    153       "[P1++] = R4;\n\t" 
     149      "[P1++] = R4;\n\t" /* store y */ 
    154150      "R4 <<= 2;\n\t" 
    155151      "R2 <<= 2;\n\t" 
    156       "R2 = PACK(R4.H, R2.H);\n\t" 
     152      "R2 = PACK(R4.H, R2.H);\n\t" /* pack x16 and y16 */ 
    157153      "[P2] = R2;\n\t" 
    158154 
     
    162158   "R0 = %5;\n\t" 
    163159   "R0 <<= 1;\n\t" 
    164    "I0 = B0;\n\t" 
     160   "I0 = B0;\n\t" /* numden */ 
    165161   "R0 <<= 1;\n\t"    
    166162   "L0 = R0;\n\t" 
    167163    
    168    "R0 = %5;\n\t" 
    169    "R2 = %4;\n\t" 
     164   "R0 = %5;\n\t" /* org */ 
     165   "R2 = %4;\n\t" /* N */ 
    170166   "R2 = R2 - R0;\n\t" 
    171    "R4 = [I0++];\n\t" 
     167   "R4 = [I0++];\n\t" /* numden */ 
    172168   "LC0 = R2;\n\t" 
    173169   "P3 = R0;\n\t" 
     
    177173   "M0 = R0;\n\t" 
    178174   "A1 = A0 = 0;\n\t" 
    179    "R5 = [I2--];\n\t" 
     175   "R5 = [I2--];\n\t" /* load xy */ 
    180176   "LOOP filter_mid%= LC0;\n\t" 
    181177   "LOOP_BEGIN filter_mid%=;\n\t" 
     
    185181      "LOOP_END filter_mid_inner%=;\n\t" 
    186182      "R0 = (A0 += A1) || I2 += M0;\n\t" 
    187       "R0 = R0 << 1 || R5 = [P0++];\n\t" 
     183      "R0 = R0 << 1 || R5 = [P0++];\n\t" /* load x */ 
    188184      "R0 = R0 + R5;\n\t" 
    189       "R0 = R0 << 2 || [P1++] = R0;\n\t" 
     185      "R0 = R0 << 2 || [P1++] = R0;\n\t" /* shift y | store y */ 
    190186      "R5 = R5 << 2;\n\t" 
    191187      "R5 = PACK(R0.H, R5.H);\n\t" 
     188      "A1 = A0 = 0 || [I2--] = R5\n\t" 
     189      "LOOP_END filter_mid%=;\n\t" 
     190   "I2 += 4;\n\t" 
     191   "P2 = I2;\n\t" 
     192   /* Update memory */ 
     193   "P4 = %6;\n\t" 
     194   "R0 = %5;\n\t" 
     195   "LC0 = R0;\n\t" 
     196   "P0 = B0;\n\t" 
     197   "A1 = A0 = 0;\n\t" 
     198   "LOOP mem_update%= LC0;\n\t" 
     199   "LOOP_BEGIN mem_update%=;\n\t" 
     200      "I2 = P2;\n\t" 
     201      "I0 = P0;\n\t" 
     202      "P0 += 4;\n\t" 
     203      "R0 = LC0;\n\t" 
     204      "LC1 = R0;\n\t" 
     205      "R5 = [I2--] || R4 = [I0++];\n\t" 
     206      "LOOP mem_accum%= LC1;\n\t" 
     207      "LOOP_BEGIN mem_accum%=;\n\t" 
     208         "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t" 
     209      "LOOP_END mem_accum%=;\n\t" 
     210      "R0 = (A0 += A1);\n\t" 
     211      "A1 = A0 = 0 || [P4++] = R0;\n\t" 
     212   "LOOP_END mem_update%=;\n\t" 
     213   "L0 = 0;\n\t" 
     214   : : "m" (xy), "m" (_x), "m" (_y), "m" (numden), "m" (N), "m" (ord), "m" (mem) 
     215   : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B0", "I0", "I2", "L0", "L2", "M0", "memory" 
     216   ); 
     217 
     218} 
     219 
     220 
     221#define OVERRIDE_FILTER_MEM16 
     222void 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) 
     223{ 
     224   VARDECL(spx_word32_t *xy2); 
     225   VARDECL(spx_word32_t *numden_a); 
     226   spx_word32_t *xy; 
     227   spx_word16_t *numden; 
     228   int i; 
     229 
     230   ALLOC(xy2, (N+1), spx_word32_t); 
     231   ALLOC(numden_a, (2*ord+2), spx_word32_t); 
     232   xy = xy2+1;   
     233   numden = (spx_word16_t*) numden_a; 
     234 
     235   for (i=0;i<ord;i++) 
     236   { 
     237      numden[2*i] = num[i]; 
     238      numden[2*i+1] = den[i]; 
     239   } 
     240   __asm__ __volatile__ 
     241   ( 
     242   /* Register setup */ 
     243   "R0 = %5;\n\t"      /*ord */ 
     244    
     245   "P0 = %3;\n\t" 
     246   "I0 = P0;\n\t" 
     247   "B0 = P0;\n\t" /* numden */ 
     248   "L0 = 0;\n\t" 
     249       
     250   "P2 = %0;\n\t" /* Fused xy */ 
     251   "I2 = P2;\n\t" 
     252   "L2 = 0;\n\t" 
     253    
     254   "P4 = %6;\n\t" /* mem */ 
     255   "P0 = %1;\n\t" /* _x */ 
     256   "P1 = %2;\n\t" /* _y */ 
     257    
     258   /* First sample */ 
     259   "R1 = [P4++];\n\t" 
     260   "R1 <<= 3;\n\t" /* shift mem */ 
     261   "R1.L = R1 (RND);\n\t" 
     262   "R2 = W[P0++];\n\t" /* load x[0] */ 
     263   "R1.L = R1.L + R2.L;\n\t" 
     264   "W[P1++] = R1;\n\t" /* store y[0] */ 
     265   "R2 = PACK(R1.L, R2.L);\n\t" /* pack x16 and y16 */ 
     266   "[P2] = R2;\n\t" 
     267                
     268   /* Samples 1 to ord-1 (using memory) */ 
     269   "R0 += -1;\n\t" 
     270   "R3 = 0;\n\t" 
     271   "LC0 = R0;\n\t" 
     272   "LOOP filter_start%= LC0;\n\t" 
     273   "LOOP_BEGIN filter_start%=;\n\t" 
     274      "R3 += 1;\n\t" 
     275      "LC1 = R3;\n\t" 
     276       
     277      "R1 = [P4++];\n\t" 
     278      "A1 = R1;\n\t" 
     279      "A0 = 0;\n\t" 
     280      "I0 = B0;\n\t" 
     281      "I2 = P2;\n\t" 
     282      "P2 += 4;\n\t" 
     283      "R4 = [I0++] || R5 = [I2--];\n\t" 
     284      "LOOP filter_start_inner%= LC1;\n\t" 
     285      "LOOP_BEGIN filter_start_inner%=;\n\t" 
     286         "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t" 
     287      "LOOP_END filter_start_inner%=;\n\t" 
     288      "A0 += A1;\n\t" 
     289      "R4 = A0;\n\t" 
     290      "R4 <<= 3;\n\t" /* shift mem */ 
     291      "R4.L = R4 (RND);\n\t" 
     292      "R2 = W[P0++];\n\t" /* load x */ 
     293      "R4.L = R4.L + R2.L;\n\t" 
     294      "W[P1++] = R4;\n\t" /* store y */ 
     295      //"R4 <<= 2;\n\t" 
     296      //"R2 <<= 2;\n\t" 
     297      "R2 = PACK(R4.L, R2.L);\n\t" /* pack x16 and y16 */ 
     298      "[P2] = R2;\n\t" 
     299 
     300   "LOOP_END filter_start%=;\n\t" 
     301 
     302   /* Samples ord to N*/    
     303   "R0 = %5;\n\t" 
     304   "R0 <<= 1;\n\t" 
     305   "I0 = B0;\n\t" /* numden */ 
     306   "R0 <<= 1;\n\t"    
     307   "L0 = R0;\n\t" 
     308    
     309   "R0 = %5;\n\t" /* org */ 
     310   "R2 = %4;\n\t" /* N */ 
     311   "R2 = R2 - R0;\n\t" 
     312   "R4 = [I0++];\n\t" /* numden */ 
     313   "LC0 = R2;\n\t" 
     314   "P3 = R0;\n\t" 
     315   "R0 <<= 2;\n\t" 
     316   "R0 += 8;\n\t" 
     317   "I2 = P2;\n\t" 
     318   "M0 = R0;\n\t" 
     319   "A1 = A0 = 0;\n\t" 
     320   "R5 = [I2--];\n\t" /* load xy */ 
     321   "LOOP filter_mid%= LC0;\n\t" 
     322   "LOOP_BEGIN filter_mid%=;\n\t" 
     323      "LOOP filter_mid_inner%= LC1=P3;\n\t" 
     324      "LOOP_BEGIN filter_mid_inner%=;\n\t" 
     325         "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t" 
     326      "LOOP_END filter_mid_inner%=;\n\t" 
     327      "R0 = (A0 += A1) || I2 += M0;\n\t" 
     328      "R0 = R0 << 3 || R5 = W[P0++];\n\t" /* load x */ 
     329      "R0.L = R0 (RND);\n\t" 
     330      "R0.L = R0.L + R5.L;\n\t" 
     331      "R5 = PACK(R0.L, R5.L) || W[P1++] = R0;\n\t" /* shift y | store y */ 
    192332      "A1 = A0 = 0 || [I2--] = R5\n\t" 
    193333      "LOOP_END filter_mid%=;\n\t" 
     
    347487} 
    348488 
     489 
     490#define OVERRIDE_IIR_MEM16 
     491void 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) 
     492{ 
     493   VARDECL(spx_word16_t *y); 
     494   spx_word16_t *yy; 
     495 
     496   ALLOC(y, (N+2), spx_word16_t); 
     497   yy = y+2; 
     498 
     499   __asm__ __volatile__ 
     500   ( 
     501   /* Register setup */ 
     502   "R0 = %5;\n\t"      /*ord */ 
     503    
     504   "P1 = %3;\n\t" 
     505   "I1 = P1;\n\t" 
     506   "B1 = P1;\n\t" 
     507   "L1 = 0;\n\t" 
     508    
     509   "P3 = %0;\n\t" 
     510   "I3 = P3;\n\t" 
     511   "L3 = 0;\n\t" 
     512    
     513   "P4 = %6;\n\t" 
     514   "P0 = %1;\n\t" 
     515   "P1 = %2;\n\t" 
     516    
     517   /* First sample */ 
     518   "R1 = [P4++];\n\t" 
     519   "R1 = R1 << 3 (S);\n\t" 
     520   "R1.L = R1 (RND);\n\t" 
     521   "R2 = W[P0++];\n\t" 
     522   "R1 = R1 + R2;\n\t" 
     523   "W[P1++] = R1;\n\t" 
     524   "W[P3] = R1;\n\t" 
     525 
     526   /* Samples 1 to ord-1 (using memory) */ 
     527   "R0 += -1;\n\t" 
     528   "R3 = 0;\n\t" 
     529   "LC0 = R0;\n\t" 
     530   "LOOP filter_start%= LC0;\n\t" 
     531   "LOOP_BEGIN filter_start%=;\n\t" 
     532      "R3 += 1;\n\t" 
     533      "LC1 = R3;\n\t" 
     534       
     535      "R1 = [P4++];\n\t" 
     536      "A1 = R1;\n\t" 
     537      "I1 = B1;\n\t" 
     538      "I3 = P3;\n\t" 
     539      "P3 += 2;\n\t" 
     540      "LOOP filter_start_inner%= LC1;\n\t" 
     541      "LOOP_BEGIN filter_start_inner%=;\n\t" 
     542         "R4.L = W[I1++];\n\t" 
     543         "R5.L = W[I3--];\n\t" 
     544         "A1 -= R4.L*R5.L (IS);\n\t" 
     545      "LOOP_END filter_start_inner%=;\n\t" 
     546    
     547      "R1 = A1;\n\t" 
     548      "R1 <<= 3;\n\t" 
     549      "R1.L = R1 (RND);\n\t" 
     550      "R2 = W[P0++];\n\t" 
     551      "R1 = R1 + R2;\n\t" 
     552      "W[P1++] = R1;\n\t" 
     553      "W[P3] = R1;\n\t" 
     554   "LOOP_END filter_start%=;\n\t" 
     555 
     556   /* Samples ord to N*/    
     557   "R0 = %5;\n\t" 
     558   "R0 <<= 1;\n\t" 
     559   "I1 = B1;\n\t" 
     560   "L1 = R0;\n\t" 
     561    
     562   "R0 = %5;\n\t" 
     563   "R2 = %4;\n\t" 
     564   "R2 = R2 - R0;\n\t" 
     565   "R4.L = W[I1++];\n\t" 
     566   "LC0 = R2;\n\t" 
     567   "LOOP filter_mid%= LC0;\n\t" 
     568   "LOOP_BEGIN filter_mid%=;\n\t" 
     569      "LC1 = R0;\n\t" 
     570      "A1 = 0;\n\t" 
     571      "I3 = P3;\n\t" 
     572      "P3 += 2;\n\t" 
     573      "R5.L = W[I3--];\n\t" 
     574      "LOOP filter_mid_inner%= LC1;\n\t" 
     575      "LOOP_BEGIN filter_mid_inner%=;\n\t" 
     576         "A1 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t" 
     577      "LOOP_END filter_mid_inner%=;\n\t" 
     578      "R1 = A1;\n\t" 
     579      "R1 = R1 << 3 || R2 = W[P0++];\n\t" 
     580      "R1.L = R1 (RND);\n\t" 
     581      "R1 = R1 + R2;\n\t" 
     582      "W[P1++] = R1;\n\t" 
     583      "W[P3] = R1;\n\t" 
     584   "LOOP_END filter_mid%=;\n\t" 
     585      
     586   /* Update memory */ 
     587   "P4 = %6;\n\t" 
     588   "R0 = %5;\n\t" 
     589   "LC0 = R0;\n\t" 
     590   "P1 = B1;\n\t" 
     591   "LOOP mem_update%= LC0;\n\t" 
     592   "LOOP_BEGIN mem_update%=;\n\t" 
     593      "A0 = 0;\n\t" 
     594      "I3 = P3;\n\t" 
     595      "I1 = P1;\n\t" 
     596      "P1 += 2;\n\t" 
     597      "R0 = LC0;\n\t" 
     598      "LC1=R0;\n\t" 
     599      "R5.L = W[I3--] || R4.L = W[I1++];\n\t" 
     600      "LOOP mem_accum%= LC1;\n\t" 
     601      "LOOP_BEGIN mem_accum%=;\n\t" 
     602         "A0 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t" 
     603      "LOOP_END mem_accum%=;\n\t" 
     604      "R0 = A0;\n\t" 
     605      "[P4++] = R0;\n\t" 
     606   "LOOP_END mem_update%=;\n\t" 
     607   "L1 = 0;\n\t" 
     608   : : "m" (yy), "m" (_x), "m" (_y), "m" (den), "m" (N), "m" (ord), "m" (mem) 
     609   : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B1", "I1", "I3", "L1", "L3", "memory" 
     610   ); 
     611 
     612} 
     613 
     614 
    349615#define OVERRIDE_FIR_MEM2 
    350616void fir_mem2(const spx_sig_t *x, const spx_coef_t *num, spx_sig_t *y, int N, int ord, spx_mem_t *mem) 
     
    357623      den[i] = 0; 
    358624   filter_mem2(x, num, den, y, N, ord, mem); 
     625} 
     626 
     627#define OVERRIDE_FIR_MEM16 
     628void 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) 
     629{ 
     630   int i; 
     631   spx_coef_t den2[12]; 
     632   spx_coef_t *den; 
     633   den = (spx_coef_t*)((((int)den2)+4)&0xfffffffc); 
     634   for (i=0;i<10;i++) 
     635      den[i] = 0; 
     636   filter_mem16(x, num, den, y, N, ord, mem, stack); 
    359637} 
    360638 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/fixed_bfin.h

    r278 r628  
    3737#define FIXED_BFIN_H 
    3838 
     39#undef PDIV32_16 
     40static inline spx_word16_t PDIV32_16(spx_word32_t a, spx_word16_t b) 
     41{ 
     42   spx_word32_t res, bb; 
     43   bb = b; 
     44   a += b>>1; 
     45   __asm__  ( 
     46         "P0 = 15;\n\t" 
     47         "R0 = %1;\n\t" 
     48         "R1 = %2;\n\t" 
     49         //"R0 = R0 + R1;\n\t" 
     50         "R0 <<= 1;\n\t" 
     51         "DIVS (R0, R1);\n\t" 
     52         "LOOP divide%= LC0 = P0;\n\t" 
     53         "LOOP_BEGIN divide%=;\n\t" 
     54            "DIVQ (R0, R1);\n\t" 
     55         "LOOP_END divide%=;\n\t" 
     56         "R0 = R0.L;\n\t" 
     57         "%0 = R0;\n\t" 
     58   : "=m" (res) 
     59   : "m" (a), "m" (bb) 
     60   : "P0", "R0", "R1", "cc"); 
     61   return res; 
     62} 
     63 
    3964#undef DIV32_16 
    4065static inline spx_word16_t DIV32_16(spx_word32_t a, spx_word16_t b) 
     
    4267   spx_word32_t res, bb; 
    4368   bb = b; 
     69   /* Make the roundinf consistent with the C version  
     70      (do we need to do that?)*/ 
     71   if (a<0)  
     72      a += (b-1); 
    4473   __asm__  ( 
    4574         "P0 = 15;\n\t" 
     
    80109   __asm__ 
    81110   ( 
    82          "%1 <<= 1;\n\t" 
    83          "A1 = %2.L*%1.L (M,IS);\n\t" 
    84          "A1 = A1 >>> 16;\n\t" 
    85          "R1 = (A1 += %2.L*%1.H) (IS);\n\t" 
    86          "%0 = R1;\n\t" 
    87    : "=&d" (res), "=&d" (b) 
     111         "A1 = %2.L*%1.L (M);\n\t" 
     112         "A1 = A1 >>> 15;\n\t" 
     113         "%0 = (A1 += %2.L*%1.H) ;\n\t" 
     114   : "=&W" (res), "=&d" (b) 
    88115   : "d" (a), "1" (b) 
    89    : "A1", "R1" 
     116   : "A1" 
    90117   ); 
    91118   return res; 
     
    98125   __asm__ 
    99126         ( 
    100          "%1 <<= 1;\n\t" 
    101          "A1 = %2.L*%1.L (M,IS);\n\t" 
    102          "A1 = A1 >>> 16;\n\t" 
    103          "R1 = (A1 += %2.L*%1.H) (IS);\n\t" 
    104          "%0 = R1 + %4;\n\t" 
    105    : "=&d" (res), "=&d" (b) 
     127         "A1 = %2.L*%1.L (M);\n\t" 
     128         "A1 = A1 >>> 15;\n\t" 
     129         "%0 = (A1 += %2.L*%1.H);\n\t" 
     130         "%0 = %0 + %4;\n\t" 
     131   : "=&W" (res), "=&d" (b) 
    106132   : "d" (a), "1" (b), "d" (c) 
    107    : "A1", "R1" 
     133   : "A1" 
    108134         ); 
    109135   return res; 
     
    116142   __asm__ 
    117143         ( 
    118          "%2 <<= 2;\n\t" 
    119          "A1 = %1.L*%2.L (M,IS);\n\t" 
    120          "A1 = A1 >>> 16;\n\t" 
    121          "R1 = (A1 += %1.L*%2.H) (IS);\n\t" 
    122          "%0 = R1;\n\t" 
    123    : "=d" (res), "=d" (a), "=d" (b) 
     144         "%2 <<= 1;\n\t" 
     145         "A1 = %1.L*%2.L (M);\n\t" 
     146         "A1 = A1 >>> 15;\n\t" 
     147         "%0 = (A1 += %1.L*%2.H);\n\t" 
     148   : "=W" (res), "=d" (a), "=d" (b) 
    124149   : "1" (a), "2" (b) 
    125    : "A1", "R1" 
     150   : "A1" 
    126151         ); 
    127152   return res; 
     
    134159   __asm__ 
    135160         ( 
    136          "%1 <<= 2;\n\t" 
    137          "A1 = %2.L*%1.L (M,IS);\n\t" 
    138          "A1 = A1 >>> 16;\n\t" 
    139          "R1 = (A1 += %2.L*%1.H) (IS);\n\t" 
    140          "%0 = R1 + %4;\n\t" 
    141    : "=&d" (res), "=&d" (b) 
     161         "%1 <<= 1;\n\t" 
     162         "A1 = %2.L*%1.L (M);\n\t" 
     163         "A1 = A1 >>> 15;\n\t" 
     164         "%0 = (A1 += %2.L*%1.H);\n\t" 
     165         "%0 = %0 + %4;\n\t" 
     166   : "=&W" (res), "=&d" (b) 
    142167   : "d" (a), "1" (b), "d" (c) 
    143    : "A1", "R1" 
     168   : "A1" 
    144169         ); 
    145170   return res; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/fixed_debug.h

    r278 r628  
    4141#define MIPS_INC spx_mips++, 
    4242 
    43 #define QCONST16(x,bits) ((spx_word16_t)((x)*(1<<(bits))+(1<<((bits)-1)))) 
    44 #define QCONST32(x,bits) ((spx_word32_t)((x)*(1<<(bits))+(1<<((bits)-1)))) 
     43#define QCONST16(x,bits) ((spx_word16_t)(.5+(x)*(((spx_word32_t)1)<<(bits)))) 
     44#define QCONST32(x,bits) ((spx_word32_t)(.5+(x)*(((spx_word32_t)1)<<(bits)))) 
     45 
    4546 
    4647#define VERIFY_SHORT(x) ((x)<=32767&&(x)>=-32768) 
     
    170171   res = a+b; 
    171172   if (!VERIFY_SHORT(res)) 
    172       fprintf (stderr, "ADD16: output is not short: %d\n", res); 
     173      fprintf (stderr, "ADD16: output is not short: %d+%d=%d\n", a,b,res); 
    173174   spx_mips++; 
    174175   return res; 
     
    197198   res = a+b; 
    198199   if (!VERIFY_INT(res)) 
     200   { 
    199201      fprintf (stderr, "ADD32: output is not int: %d\n", (int)res); 
     202   } 
    200203   spx_mips++; 
    201204   return res; 
     
    252255#define MAC16_16_Q11(c,a,b)     (ADD16((c),EXTRACT16(SHR32(MULT16_16((a),(b)),11)))) 
    253256#define MAC16_16_Q13(c,a,b)     (ADD16((c),EXTRACT16(SHR32(MULT16_16((a),(b)),13)))) 
     257#define MAC16_16_P13(c,a,b)     (ADD32((c),SHR(ADD32(4096,MULT16_16((a),(b))),13))) 
     258 
    254259 
    255260static inline int MULT16_32_QX(int a, long long b, int Q) 
     
    438443   return res; 
    439444} 
    440  
    441  
     445#define PDIV32(a,b) DIV32(ADD32((a),(b)>>1),b) 
     446#define PDIV32_16(a,b) DIV32_16(ADD32((a),(b)>>1),b) 
    442447 
    443448#endif 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/fixed_generic.h

    r278 r628  
    3636#define FIXED_GENERIC_H 
    3737 
    38 #define QCONST16(x,bits) ((spx_word16_t)(.5+(x)*(1<<(bits)))) 
    39 #define QCONST32(x,bits) ((spx_word32_t)(.5+(x)*(1<<(bits)))) 
     38#define QCONST16(x,bits) ((spx_word16_t)(.5+(x)*(((spx_word32_t)1)<<(bits)))) 
     39#define QCONST32(x,bits) ((spx_word32_t)(.5+(x)*(((spx_word32_t)1)<<(bits)))) 
    4040 
    4141#define NEG16(x) (-(x)) 
    4242#define NEG32(x) (-(x)) 
    43 #define EXTRACT16(x) ((spx_word16_t)x) 
    44 #define EXTEND32(x) ((spx_word32_t)x) 
     43#define EXTRACT16(x) ((spx_word16_t)(x)) 
     44#define EXTEND32(x) ((spx_word32_t)(x)) 
    4545#define SHR16(a,shift) ((a) >> (shift)) 
    4646#define SHL16(a,shift) ((a) << (shift)) 
     
    6262#define ADD32(a,b) ((spx_word32_t)(a)+(spx_word32_t)(b)) 
    6363#define SUB32(a,b) ((spx_word32_t)(a)-(spx_word32_t)(b)) 
    64 #define ADD64(a,b) ((spx_word64_t)(a)+(spx_word64_t)(b)) 
    6564 
    6665 
     
    8584#define MAC16_16_Q11(c,a,b)     (ADD32((c),SHR(MULT16_16((a),(b)),11))) 
    8685#define MAC16_16_Q13(c,a,b)     (ADD32((c),SHR(MULT16_16((a),(b)),13))) 
     86#define MAC16_16_P13(c,a,b)     (ADD32((c),SHR(ADD32(4096,MULT16_16((a),(b))),13))) 
    8787 
    8888#define MULT16_16_Q11_32(a,b) (SHR(MULT16_16((a),(b)),11)) 
     
    9898 
    9999#define DIV32_16(a,b) ((spx_word16_t)(((spx_word32_t)(a))/((spx_word16_t)(b)))) 
     100#define PDIV32_16(a,b) ((spx_word16_t)(((spx_word32_t)(a)+((spx_word16_t)(b)>>1))/((spx_word16_t)(b)))) 
    100101#define DIV32(a,b) (((spx_word32_t)(a))/((spx_word32_t)(b))) 
     102#define PDIV32(a,b) (((spx_word32_t)(a)+((spx_word16_t)(b)>>1))/((spx_word32_t)(b))) 
    101103 
    102104#endif 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/gain_table.c

    r278 r628  
    3030*/ 
    3131 
    32 const signed char gain_cdbk_nb[384] = { 
    33 -32,-32,-32, 
    34 -28,-67,-5, 
    35 -42,-6,-32, 
    36 -57,-10,-54, 
    37 -16,27,-41, 
    38 19,-19,-40, 
    39 -45,24,-21, 
    40 -8,-14,-18, 
    41 1,14,-58, 
    42 -18,-88,-39, 
    43 -38,21,-18, 
    44 -19,20,-43, 
    45 10,17,-48, 
    46 -52,-58,-13, 
    47 -44,-1,-11, 
    48 -12,-11,-34, 
    49 14,0,-46, 
    50 -37,-35,-34, 
    51 -25,44,-30, 
    52 6,-4,-63, 
    53 -31,43,-41, 
    54 -23,30,-43, 
    55 -43,26,-14, 
    56 -33,1,-13, 
    57 -13,18,-37, 
    58 -46,-73,-45, 
    59 -36,24,-25, 
    60 -36,-11,-20, 
    61 -25,12,-18, 
    62 -36,-69,-59, 
    63 -45,6,8, 
    64 -22,-14,-24, 
    65 -1,13,-44, 
    66 -39,-48,-26, 
    67 -32,31,-37, 
    68 -33,15,-46, 
    69 -24,30,-36, 
    70 -41,31,-23, 
    71 -50,22,-4, 
    72 -22,2,-21, 
    73 -17,30,-34, 
    74 -7,-60,-28, 
    75 -38,42,-28, 
    76 -44,-11,21, 
    77 -16,8,-44, 
    78 -39,-55,-43, 
    79 -11,-35,26, 
    80 -9,0,-34, 
    81 -8,121,-81, 
    82 7,-16,-22, 
    83 -37,33,-31, 
    84 -27,-7,-36, 
    85 -34,70,-57, 
    86 -37,-11,-48, 
    87 -40,17,-1, 
    88 -33,6,-6, 
    89 -9,0,-20, 
    90 -21,69,-33, 
    91 -29,33,-31, 
    92 -55,12,-1, 
    93 -33,27,-22, 
    94 -50,-33,-47, 
    95 -50,54,51, 
    96 -1,-5,-44, 
    97 -4,22,-40, 
    98 -39,-66,-25, 
    99 -33,1,-26, 
    100 -24,-23,-25, 
    101 -11,21,-45, 
    102 -25,-45,-19, 
    103 -43,105,-16, 
    104 5,-21,1, 
    105 -16,11,-33, 
    106 -13,-99,-4, 
    107 -37,33,-15, 
    108 -25,37,-63, 
    109 -36,24,-31, 
    110 -53,-56,-38, 
    111 -41,-4,4, 
    112 -33,13,-30, 
    113 49,52,-94, 
    114 -5,-30,-15, 
    115 1,38,-40, 
    116 -23,12,-36, 
    117 -17,40,-47, 
    118 -37,-41,-39, 
    119 -49,34,0, 
    120 -18,-7,-4, 
    121 -16,17,-27, 
    122 30,5,-62, 
    123 4,48,-68, 
    124 -43,11,-11, 
    125 -18,19,-15, 
    126 -23,-62,-39, 
    127 -42,10,-2, 
    128 -21,-13,-13, 
    129 -9,13,-47, 
    130 -23,-62,-24, 
    131 -44,60,-21, 
    132 -18,-3,-52, 
    133 -22,22,-36, 
    134 -75,57,16, 
    135 -19,3,10, 
    136 -29,23,-38, 
    137 -5,-62,-51, 
    138 -51,40,-18, 
    139 -42,13,-24, 
    140 -34,14,-20, 
    141 -56,-75,-26, 
    142 -26,32,15, 
    143 -26,17,-29, 
    144 -7,28,-52, 
    145 -12,-30,5, 
    146 -5,-48,-5, 
    147 2,2,-43, 
    148 21,16,16, 
    149 -25,-45,-32, 
    150 -43,18,-10, 
    151 9,0,-1, 
    152 -1,7,-30, 
    153 19,-48,-4, 
    154 -28,25,-29, 
    155 -22,0,-31, 
    156 -32,17,-10, 
    157 -64,-41,-62, 
    158 -52,15,16, 
    159 -30,-22,-32, 
    160 -7,9,-38}; 
     32const signed char gain_cdbk_nb[512] = { 
     33-32, -32, -32, 0, 
     34-28, -67, -5, 33, 
     35-42, -6, -32, 18, 
     36-57, -10, -54, 35, 
     37-16, 27, -41, 42, 
     3819, -19, -40, 36, 
     39-45, 24, -21, 40, 
     40-8, -14, -18, 28, 
     411, 14, -58, 53, 
     42-18, -88, -39, 39, 
     43-38, 21, -18, 37, 
     44-19, 20, -43, 38, 
     4510, 17, -48, 54, 
     46-52, -58, -13, 33, 
     47-44, -1, -11, 32, 
     48-12, -11, -34, 22, 
     4914, 0, -46, 46, 
     50-37, -35, -34, 5, 
     51-25, 44, -30, 43, 
     526, -4, -63, 49, 
     53-31, 43, -41, 43, 
     54-23, 30, -43, 41, 
     55-43, 26, -14, 44, 
     56-33, 1, -13, 27, 
     57-13, 18, -37, 37, 
     58-46, -73, -45, 34, 
     59-36, 24, -25, 34, 
     60-36, -11, -20, 19, 
     61-25, 12, -18, 33, 
     62-36, -69, -59, 34, 
     63-45, 6, 8, 46, 
     64-22, -14, -24, 18, 
     65-1, 13, -44, 44, 
     66-39, -48, -26, 15, 
     67-32, 31, -37, 34, 
     68-33, 15, -46, 31, 
     69-24, 30, -36, 37, 
     70-41, 31, -23, 41, 
     71-50, 22, -4, 50, 
     72-22, 2, -21, 28, 
     73-17, 30, -34, 40, 
     74-7, -60, -28, 29, 
     75-38, 42, -28, 42, 
     76-44, -11, 21, 43, 
     77-16, 8, -44, 34, 
     78-39, -55, -43, 21, 
     79-11, -35, 26, 41, 
     80-9, 0, -34, 29, 
     81-8, 121, -81, 113, 
     827, -16, -22, 33, 
     83-37, 33, -31, 36, 
     84-27, -7, -36, 17, 
     85-34, 70, -57, 65, 
     86-37, -11, -48, 21, 
     87-40, 17, -1, 44, 
     88-33, 6, -6, 33, 
     89-9, 0, -20, 34, 
     90-21, 69, -33, 57, 
     91-29, 33, -31, 35, 
     92-55, 12, -1, 49, 
     93-33, 27, -22, 35, 
     94-50, -33, -47, 17, 
     95-50, 54, 51, 94, 
     96-1, -5, -44, 35, 
     97-4, 22, -40, 45, 
     98-39, -66, -25, 24, 
     99-33, 1, -26, 20, 
     100-24, -23, -25, 12, 
     101-11, 21, -45, 44, 
     102-25, -45, -19, 17, 
     103-43, 105, -16, 82, 
     1045, -21, 1, 41, 
     105-16, 11, -33, 30, 
     106-13, -99, -4, 57, 
     107-37, 33, -15, 44, 
     108-25, 37, -63, 54, 
     109-36, 24, -31, 31, 
     110-53, -56, -38, 26, 
     111-41, -4, 4, 37, 
     112-33, 13, -30, 24, 
     11349, 52, -94, 114, 
     114-5, -30, -15, 23, 
     1151, 38, -40, 56, 
     116-23, 12, -36, 29, 
     117-17, 40, -47, 51, 
     118-37, -41, -39, 11, 
     119-49, 34, 0, 58, 
     120-18, -7, -4, 34, 
     121-16, 17, -27, 35, 
     12230, 5, -62, 65, 
     1234, 48, -68, 76, 
     124-43, 11, -11, 38, 
     125-18, 19, -15, 41, 
     126-23, -62, -39, 23, 
     127-42, 10, -2, 41, 
     128-21, -13, -13, 25, 
     129-9, 13, -47, 42, 
     130-23, -62, -24, 24, 
     131-44, 60, -21, 58, 
     132-18, -3, -52, 32, 
     133-22, 22, -36, 34, 
     134-75, 57, 16, 90, 
     135-19, 3, 10, 45, 
     136-29, 23, -38, 32, 
     137-5, -62, -51, 38, 
     138-51, 40, -18, 53, 
     139-42, 13, -24, 32, 
     140-34, 14, -20, 30, 
     141-56, -75, -26, 37, 
     142-26, 32, 15, 59, 
     143-26, 17, -29, 29, 
     144-7, 28, -52, 53, 
     145-12, -30, 5, 30, 
     146-5, -48, -5, 35, 
     1472, 2, -43, 40, 
     14821, 16, 16, 75, 
     149-25, -45, -32, 10, 
     150-43, 18, -10, 42, 
     1519, 0, -1, 52, 
     152-1, 7, -30, 36, 
     15319, -48, -4, 48, 
     154-28, 25, -29, 32, 
     155-22, 0, -31, 22, 
     156-32, 17, -10, 36, 
     157-64, -41, -62, 36, 
     158-52, 15, 16, 58, 
     159-30, -22, -32, 6, 
     160-7, 9, -38, 36}; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/gain_table_lbr.c

    r278 r628  
    3030*/ 
    3131 
    32 const signed char gain_cdbk_lbr[96] = { 
    33 -32,-32,-32, 
    34 -31,-58,-16, 
    35 -41,-24,-43, 
    36 -56,-22,-55, 
    37 -13,33,-41, 
    38 -4,-39,-9, 
    39 -41,15,-12, 
    40 -8,-15,-12, 
    41 1,2,-44, 
    42 -22,-66,-42, 
    43 -38,28,-23, 
    44 -21,14,-37, 
    45 0,21,-50, 
    46 -53,-71,-27, 
    47 -37,-1,-19, 
    48 -19,-5,-28, 
    49 6,65,-44, 
    50 -33,-48,-33, 
    51 -40,57,-14, 
    52 -17,4,-45, 
    53 -31,38,-33, 
    54 -23,28,-40, 
    55 -43,29,-12, 
    56 -34,13,-23, 
    57 -16,15,-27, 
    58 -14,-82,-15, 
    59 -31,25,-32, 
    60 -21,5,-5, 
    61 -47,-63,-51, 
    62 -46,12,3, 
    63 -28,-17,-29, 
    64 -10,14,-40}; 
     32const signed char gain_cdbk_lbr[128] = { 
     33-32, -32, -32, 0, 
     34-31, -58, -16, 22, 
     35-41, -24, -43, 14, 
     36-56, -22, -55, 29, 
     37-13, 33, -41, 47, 
     38-4, -39, -9, 29, 
     39-41, 15, -12, 38, 
     40-8, -15, -12, 31, 
     411, 2, -44, 40, 
     42-22, -66, -42, 27, 
     43-38, 28, -23, 38, 
     44-21, 14, -37, 31, 
     450, 21, -50, 52, 
     46-53, -71, -27, 33, 
     47-37, -1, -19, 25, 
     48-19, -5, -28, 22, 
     496, 65, -44, 74, 
     50-33, -48, -33, 9, 
     51-40, 57, -14, 58, 
     52-17, 4, -45, 32, 
     53-31, 38, -33, 36, 
     54-23, 28, -40, 39, 
     55-43, 29, -12, 46, 
     56-34, 13, -23, 28, 
     57-16, 15, -27, 34, 
     58-14, -82, -15, 43, 
     59-31, 25, -32, 29, 
     60-21, 5, -5, 38, 
     61-47, -63, -51, 33, 
     62-46, 12, 3, 47, 
     63-28, -17, -29, 11, 
     64-10, 14, -40, 38}; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/jitter.c

    r523 r628  
    3737#endif 
    3838 
    39 #ifndef NULL 
    40 #define NULL 0 
    41 #endif 
    4239 
    4340#include "misc.h" 
     
    4744#include <stdio.h> 
    4845 
    49 #define LATE_BINS 4 
    50  
    51 void speex_jitter_init(SpeexJitter *jitter, void *decoder, int sampling_rate) 
     46#define LATE_BINS 10 
     47#define MAX_MARGIN 30                     /**< Number of bins in margin histogram */ 
     48 
     49#define SPEEX_JITTER_MAX_BUFFER_SIZE 200   /**< Maximum number of packets in jitter buffer */ 
     50 
     51 
     52 
     53#define GT32(a,b) (((spx_int32_t)((a)-(b)))>0) 
     54#define GE32(a,b) (((spx_int32_t)((a)-(b)))>=0) 
     55#define LT32(a,b) (((spx_int32_t)((a)-(b)))<0) 
     56#define LE32(a,b) (((spx_int32_t)((a)-(b)))<=0) 
     57 
     58/** Jitter buffer structure */ 
     59struct JitterBuffer_ { 
     60   spx_uint32_t pointer_timestamp;                                        /**< Timestamp of what we will *get* next */ 
     61   spx_uint32_t current_timestamp;                                        /**< Timestamp of the local clock (what we will *play* next) */ 
     62 
     63   char *buf[SPEEX_JITTER_MAX_BUFFER_SIZE];                               /**< Buffer of packets (NULL if slot is free) */ 
     64   spx_uint32_t timestamp[SPEEX_JITTER_MAX_BUFFER_SIZE];                  /**< Timestamp of packet                 */ 
     65   int span[SPEEX_JITTER_MAX_BUFFER_SIZE];                                /**< Timestamp of packet                 */ 
     66   int len[SPEEX_JITTER_MAX_BUFFER_SIZE];                                 /**< Number of bytes in packet           */ 
     67 
     68   int tick_size;                                                         /**< Output granularity                  */ 
     69   int reset_state;                                                       /**< True if state was just reset        */ 
     70   int buffer_margin;                                                     /**< How many frames we want to keep in the buffer (lower bound) */ 
     71    
     72   int lost_count;                                                        /**< Number of consecutive lost packets  */ 
     73   float shortterm_margin[MAX_MARGIN];                                    /**< Short term margin histogram         */ 
     74   float longterm_margin[MAX_MARGIN];                                     /**< Long term margin histogram          */ 
     75   float loss_rate;                                                       /**< Average loss rate                   */ 
     76}; 
     77 
     78/** Initialise jitter buffer */ 
     79JitterBuffer *jitter_buffer_init(int tick) 
     80{ 
     81   JitterBuffer *jitter = speex_alloc(sizeof(JitterBuffer)); 
     82   if (jitter) 
     83   { 
     84      int i; 
     85      for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
     86         jitter->buf[i]=NULL; 
     87      jitter->tick_size = tick; 
     88      jitter->buffer_margin = 1; 
     89      jitter_buffer_reset(jitter); 
     90   } 
     91   return jitter; 
     92} 
     93 
     94/** Reset jitter buffer */ 
     95void jitter_buffer_reset(JitterBuffer *jitter) 
    5296{ 
    5397   int i; 
    5498   for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
    5599   { 
    56       jitter->len[i]=-1; 
    57       jitter->timestamp[i]=-1; 
    58    } 
    59  
    60    jitter->dec = decoder; 
    61    speex_decoder_ctl(decoder, SPEEX_GET_FRAME_SIZE, &jitter->frame_size); 
    62    jitter->frame_time = jitter->frame_size; 
    63  
    64    speex_bits_init(&jitter->current_packet); 
    65    jitter->valid_bits = 0; 
    66  
    67    jitter->buffer_size = 4; 
    68  
    69    jitter->pointer_timestamp = -jitter->frame_time * jitter->buffer_size; 
     100      if (jitter->buf[i]) 
     101      { 
     102         speex_free(jitter->buf[i]); 
     103         jitter->buf[i] = NULL; 
     104      } 
     105   } 
     106   /* Timestamp is actually undefined at this point */ 
     107   jitter->pointer_timestamp = 0; 
     108   jitter->current_timestamp = 0; 
    70109   jitter->reset_state = 1; 
    71110   jitter->lost_count = 0; 
    72111   jitter->loss_rate = 0; 
    73 } 
    74  
    75 void speex_jitter_destroy(SpeexJitter *jitter) 
    76 { 
    77    speex_bits_destroy(&jitter->current_packet); 
    78 } 
    79  
    80  
    81 void speex_jitter_put(SpeexJitter *jitter, char *packet, int len, int timestamp) 
     112   for (i=0;i<MAX_MARGIN;i++) 
     113   { 
     114      jitter->shortterm_margin[i] = 0; 
     115      jitter->longterm_margin[i] = 0; 
     116   } 
     117   /*fprintf (stderr, "reset\n");*/ 
     118} 
     119 
     120/** Destroy jitter buffer */ 
     121void jitter_buffer_destroy(JitterBuffer *jitter) 
     122{ 
     123   jitter_buffer_reset(jitter); 
     124   speex_free(jitter); 
     125} 
     126 
     127/** Put one packet into the jitter buffer */ 
     128void jitter_buffer_put(JitterBuffer *jitter, const JitterBufferPacket *packet) 
    82129{ 
    83130   int i,j; 
    84    int arrival_margin; 
    85  
     131   spx_int32_t arrival_margin; 
     132   /*fprintf (stderr, "put packet %d %d\n", timestamp, span);*/ 
    86133   if (jitter->reset_state) 
    87134   { 
    88135      jitter->reset_state=0; 
    89       jitter->pointer_timestamp = timestamp-jitter->frame_time * jitter->buffer_size; 
    90       for (i=0;i<MAX_MARGIN;i++) 
    91       { 
    92          jitter->shortterm_margin[i] = 0; 
    93          jitter->longterm_margin[i] = 0; 
    94       } 
    95       for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
    96       { 
    97          jitter->len[i]=-1; 
    98          jitter->timestamp[i]=-1; 
    99       } 
    100       fprintf(stderr, "reset to %d\n", timestamp); 
     136      jitter->pointer_timestamp = packet->timestamp; 
     137      jitter->current_timestamp = packet->timestamp; 
     138      /*fprintf(stderr, "reset to %d\n", timestamp);*/ 
    101139   } 
    102140    
     
    104142   for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
    105143   { 
    106       if (jitter->timestamp[i]<jitter->pointer_timestamp) 
    107       { 
    108          jitter->len[i]=-1; 
    109          /*if (jitter->timestamp[i] != -1) 
    110             fprintf (stderr, "discarding %d %d\n", jitter->timestamp[i], jitter->pointer_timestamp);*/ 
     144      if (jitter->buf[i] && LE32(jitter->timestamp[i] + jitter->span[i], jitter->pointer_timestamp)) 
     145      { 
     146         /*fprintf (stderr, "cleaned (not played)\n");*/ 
     147         speex_free(jitter->buf[i]); 
     148         jitter->buf[i] = NULL; 
    111149      } 
    112150   } 
     
    115153   for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
    116154   { 
    117       if (jitter->len[i]==-1) 
     155      if (jitter->buf[i]==NULL) 
    118156         break; 
    119157   } 
    120158 
    121159   /*fprintf(stderr, "%d %d %f\n", timestamp, jitter->pointer_timestamp, jitter->drift_average);*/ 
     160   /*No place left in the buffer*/ 
    122161   if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) 
    123162   { 
     
    126165      for (j=1;j<SPEEX_JITTER_MAX_BUFFER_SIZE;j++) 
    127166      { 
    128          if (jitter->timestamp[j]<earliest) 
     167         if (!jitter->buf[i] || LT32(jitter->timestamp[j],earliest)) 
    129168         { 
    130169            earliest = jitter->timestamp[j]; 
     
    132171         } 
    133172      } 
    134       /*fprintf (stderr, "Buffer is full, discarding earliest frame %d (currently at %d)\n", timestamp, jitter->pointer_timestamp);*/ 
    135       /*No place left in the buffer*/ 
    136        
    137       /*skip some frame(s) */ 
    138       /*return;*/ 
     173      speex_free(jitter->buf[i]); 
     174      jitter->buf[i]=NULL; 
     175      if (jitter->lost_count>20) 
     176      { 
     177         jitter_buffer_reset(jitter); 
     178      } 
     179      /*fprintf (stderr, "Buffer is full, discarding earliest frame %d (currently at %d)\n", timestamp, jitter->pointer_timestamp);*/       
    139180   } 
    140181    
    141182   /* Copy packet in buffer */ 
    142    if (len>SPEEX_JITTER_MAX_PACKET_SIZE) 
    143       len=SPEEX_JITTER_MAX_PACKET_SIZE; 
    144    for (j=0;j<len/BYTES_PER_CHAR;j++) 
    145       jitter->buf[i][j]=packet[j]; 
    146    jitter->timestamp[i]=timestamp; 
    147    jitter->len[i]=len; 
    148     
    149    /* Don't count late packets when adjusting the synchro (we're taking care of them elsewhere) */ 
    150    /*if (timestamp <= jitter->pointer_timestamp) 
    151    { 
    152       fprintf (stderr, "frame for timestamp %d arrived too late (at time %d)\n", timestamp, jitter->pointer_timestamp); 
    153    }*/ 
    154  
     183   jitter->buf[i]=speex_alloc(packet->len); 
     184   for (j=0;j<packet->len;j++) 
     185      jitter->buf[i][j]=packet->data[j]; 
     186   jitter->timestamp[i]=packet->timestamp; 
     187   jitter->span[i]=packet->span; 
     188   jitter->len[i]=packet->len; 
     189    
    155190   /* Adjust the buffer size depending on network conditions */ 
    156    arrival_margin = (timestamp - jitter->pointer_timestamp - jitter->frame_time); 
    157     
    158    if (arrival_margin >= -LATE_BINS*jitter->frame_time) 
    159    { 
    160       int int_margin; 
     191   arrival_margin = (packet->timestamp - jitter->current_timestamp) - jitter->buffer_margin*jitter->tick_size; 
     192    
     193   if (arrival_margin >= -LATE_BINS*jitter->tick_size) 
     194   { 
     195      spx_int32_t int_margin; 
    161196      for (i=0;i<MAX_MARGIN;i++) 
    162197      { 
     
    164199         jitter->longterm_margin[i] *= .995; 
    165200      } 
    166       int_margin = (arrival_margin + LATE_BINS*jitter->frame_time)/jitter->frame_time; 
     201      int_margin = LATE_BINS + arrival_margin/jitter->tick_size; 
    167202      if (int_margin>MAX_MARGIN-1) 
    168203         int_margin = MAX_MARGIN-1; 
     
    172207         jitter->longterm_margin[int_margin] += .005; 
    173208      } 
    174    } 
    175     
    176    /*fprintf (stderr, "margin : %d %d %f %f %f %f\n", arrival_margin, jitter->buffer_size, 100*jitter->loss_rate, 100*jitter->late_ratio, 100*jitter->ontime_ratio, 100*jitter->early_ratio);*/ 
    177 } 
    178  
    179 void speex_jitter_get(SpeexJitter *jitter, short *out, int *current_timestamp) 
    180 { 
    181    int i; 
    182    int ret; 
     209   } else { 
     210       
     211      /*fprintf (stderr, "way too late = %d\n", arrival_margin);*/ 
     212      if (jitter->lost_count>20) 
     213      { 
     214         jitter_buffer_reset(jitter); 
     215      } 
     216   } 
     217#if 0 /* Enable to check how much is being buffered */ 
     218   if (rand()%1000==0) 
     219   { 
     220      int count = 0; 
     221      for (j=0;j<SPEEX_JITTER_MAX_BUFFER_SIZE;j++) 
     222      { 
     223         if (jitter->buf[j]) 
     224            count++; 
     225      } 
     226      fprintf (stderr, "buffer_size = %d\n", count); 
     227   } 
     228#endif 
     229} 
     230 
     231/** Get one packet from the jitter buffer */ 
     232int jitter_buffer_get(JitterBuffer *jitter, JitterBufferPacket *packet, spx_uint32_t *start_offset) 
     233{ 
     234   int i, j; 
    183235   float late_ratio_short; 
    184236   float late_ratio_long; 
     
    187239   float early_ratio_short; 
    188240   float early_ratio_long; 
     241   int chunk_size; 
     242   int incomplete = 0; 
     243    
     244   if (LT32(jitter->current_timestamp+jitter->tick_size, jitter->pointer_timestamp)) 
     245   { 
     246      jitter->current_timestamp = jitter->pointer_timestamp; 
     247      speex_warning("did you forget to call jitter_buffer_tick() by any chance?"); 
     248   } 
     249   /*fprintf (stderr, "get packet %d %d\n", jitter->pointer_timestamp, jitter->current_timestamp);*/ 
     250 
     251   /* FIXME: This should be only what remaining of the current tick */ 
     252   chunk_size = jitter->tick_size; 
     253    
     254   /* Compiling arrival statistics */ 
    189255    
    190256   late_ratio_short = 0; 
     
    205271   if (0&&jitter->pointer_timestamp%1000==0) 
    206272   { 
    207       fprintf (stderr, "%f %f %f %f %f %f\n", early_ratio_short, early_ratio_long, ontime_ratio_short, ontime_ratio_long, late_ratio_short, late_ratio_long); 
     273      /*fprintf (stderr, "%f %f %f %f %f %f\n", early_ratio_short, early_ratio_long, ontime_ratio_short, ontime_ratio_long, late_ratio_short, late_ratio_long);*/ 
    208274      /*fprintf (stderr, "%f %f\n", early_ratio_short + ontime_ratio_short + late_ratio_short, early_ratio_long + ontime_ratio_long + late_ratio_long);*/ 
    209275   } 
    210276    
     277   /* Adjusting the buffering */ 
     278    
    211279   if (late_ratio_short > .1 || late_ratio_long > .03) 
    212280   { 
     281      /* If too many packets are arriving late */ 
    213282      jitter->shortterm_margin[MAX_MARGIN-1] += jitter->shortterm_margin[MAX_MARGIN-2]; 
    214283      jitter->longterm_margin[MAX_MARGIN-1] += jitter->longterm_margin[MAX_MARGIN-2]; 
     
    220289      jitter->shortterm_margin[0] = 0; 
    221290      jitter->longterm_margin[0] = 0;             
    222       /*fprintf (stderr, "interpolate frame\n");*/ 
    223       speex_decode_int(jitter->dec, NULL, (spx_int16_t*)out); 
    224       if (current_timestamp) 
    225          *current_timestamp = jitter->pointer_timestamp; 
    226       return; 
    227    } 
    228     
    229    /* Increment timestamp */ 
    230    jitter->pointer_timestamp += jitter->frame_time; 
    231     
    232    if (late_ratio_short + ontime_ratio_short < .005 && late_ratio_long + ontime_ratio_long < .01 && early_ratio_short > .8) 
    233    { 
     291      jitter->pointer_timestamp -= jitter->tick_size; 
     292      jitter->current_timestamp -= jitter->tick_size; 
     293      /*fprintf (stderr, "i");*/ 
     294      /*fprintf (stderr, "interpolate (getting some slack)\n");*/ 
     295   } else if (late_ratio_short + ontime_ratio_short < .005 && late_ratio_long + ontime_ratio_long < .01 && early_ratio_short > .8) 
     296   { 
     297      /* Many frames arriving early */ 
    234298      jitter->shortterm_margin[0] += jitter->shortterm_margin[1]; 
    235299      jitter->longterm_margin[0] += jitter->longterm_margin[1]; 
     
    242306      jitter->longterm_margin[MAX_MARGIN-1] = 0;       
    243307      /*fprintf (stderr, "drop frame\n");*/ 
    244       jitter->pointer_timestamp += jitter->frame_time; 
    245    } 
    246  
    247    if (current_timestamp) 
    248       *current_timestamp = jitter->pointer_timestamp; 
    249  
    250    /* Send zeros while we fill in the buffer */ 
    251    if (jitter->pointer_timestamp<0) 
    252    { 
    253       for (i=0;i<jitter->frame_size;i++) 
    254          out[i]=0; 
    255       return; 
    256    } 
    257     
    258    /* Search the buffer for a packet with the right timestamp */ 
     308      /*fprintf (stderr, "d");*/ 
     309      jitter->pointer_timestamp += jitter->tick_size; 
     310      jitter->current_timestamp += jitter->tick_size; 
     311      /*fprintf (stderr, "dropping packet (getting more aggressive)\n");*/ 
     312   } 
     313    
     314   /* Searching for the packet that fits best */ 
     315    
     316   /* Search the buffer for a packet with the right timestamp and spanning the whole current chunk */ 
    259317   for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
    260318   { 
    261       if (jitter->len[i]!=-1 && jitter->timestamp[i]==jitter->pointer_timestamp) 
     319      if (jitter->buf[i] && jitter->timestamp[i]==jitter->pointer_timestamp && GE32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp+chunk_size)) 
    262320         break; 
    263321   } 
    264322    
     323   /* If no match, try for an "older" packet that still spans (fully) the current chunk */ 
    265324   if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) 
    266325   { 
     326      for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
     327      { 
     328         if (jitter->buf[i] && jitter->timestamp[i]<=jitter->pointer_timestamp && GE32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp+chunk_size)) 
     329            break; 
     330      } 
     331   } 
     332    
     333   /* If still no match, try for an "older" packet that spans part of the current chunk */ 
     334   if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) 
     335   { 
     336      for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
     337      { 
     338         if (jitter->buf[i] && jitter->timestamp[i]<=jitter->pointer_timestamp && GT32(jitter->timestamp[i]+jitter->span[i],jitter->pointer_timestamp)) 
     339            break; 
     340      } 
     341   } 
     342    
     343   /* If still no match, try for earliest packet possible */ 
     344   if (i==SPEEX_JITTER_MAX_BUFFER_SIZE) 
     345   { 
     346      int found = 0; 
     347      spx_uint32_t best_time=0; 
     348      int best_span=0; 
     349      int besti=0; 
     350      for (i=0;i<SPEEX_JITTER_MAX_BUFFER_SIZE;i++) 
     351      { 
     352         /* check if packet starts within current chunk */ 
     353         if (jitter->buf[i] && LT32(jitter->timestamp[i],jitter->pointer_timestamp+chunk_size) && GE32(jitter->timestamp[i],jitter->pointer_timestamp)) 
     354         { 
     355            if (!found || LT32(jitter->timestamp[i],best_time) || (jitter->timestamp[i]==best_time && GT32(jitter->span[i],best_span))) 
     356            { 
     357               best_time = jitter->timestamp[i]; 
     358               best_span = jitter->span[i]; 
     359               besti = i; 
     360               found = 1; 
     361            } 
     362         } 
     363      } 
     364      if (found) 
     365      { 
     366         i=besti; 
     367         incomplete = 1; 
     368         /*fprintf (stderr, "incomplete: %d %d %d %d\n", jitter->timestamp[i], jitter->pointer_timestamp, chunk_size, jitter->span[i]);*/ 
     369      } 
     370   } 
     371 
     372   /* If we find something */ 
     373   if (i!=SPEEX_JITTER_MAX_BUFFER_SIZE) 
     374   { 
     375      /* We (obviously) haven't lost this packet */ 
     376      jitter->lost_count = 0; 
     377      jitter->loss_rate = .999*jitter->loss_rate; 
     378      /* Check for potential overflow */ 
     379      packet->len = jitter->len[i]; 
     380      /* Copy packet */ 
     381      for (j=0;j<packet->len;j++) 
     382         packet->data[j] = jitter->buf[i][j]; 
     383      /* Remove packet */ 
     384      speex_free(jitter->buf[i]); 
     385      jitter->buf[i] = NULL; 
     386      /* Set timestamp and span (if requested) */ 
     387      if (start_offset) 
     388         *start_offset = jitter->timestamp[i]-jitter->pointer_timestamp; 
     389      packet->timestamp = jitter->timestamp[i]; 
     390      packet->span = jitter->span[i]; 
     391      /* Point at the end of the current packet */ 
     392      jitter->pointer_timestamp = jitter->timestamp[i]+jitter->span[i]; 
     393      if (incomplete) 
     394         return JITTER_BUFFER_INCOMPLETE; 
     395      else 
     396         return JITTER_BUFFER_OK; 
     397   } 
     398    
     399    
     400   /* If we haven't found anything worth returning */ 
     401   /*fprintf (stderr, "not found\n");*/ 
     402   jitter->lost_count++; 
     403   /*fprintf (stderr, "m");*/ 
     404   /*fprintf (stderr, "lost_count = %d\n", jitter->lost_count);*/ 
     405   jitter->loss_rate = .999*jitter->loss_rate + .001; 
     406   if (start_offset) 
     407      *start_offset = 0; 
     408   packet->timestamp = jitter->pointer_timestamp; 
     409   packet->span = jitter->tick_size; 
     410   jitter->pointer_timestamp += chunk_size; 
     411   packet->len = 0; 
     412   return JITTER_BUFFER_MISSING; 
     413 
     414} 
     415 
     416/** Get pointer timestamp of jitter buffer */ 
     417int jitter_buffer_get_pointer_timestamp(JitterBuffer *jitter) 
     418{ 
     419   return jitter->pointer_timestamp; 
     420} 
     421 
     422void jitter_buffer_tick(JitterBuffer *jitter) 
     423{ 
     424   jitter->current_timestamp += jitter->tick_size; 
     425} 
     426 
     427 
     428 
     429 
     430 
     431void speex_jitter_init(SpeexJitter *jitter, void *decoder, int sampling_rate) 
     432{ 
     433   jitter->dec = decoder; 
     434   speex_decoder_ctl(decoder, SPEEX_GET_FRAME_SIZE, &jitter->frame_size); 
     435 
     436   jitter->packets = jitter_buffer_init(jitter->frame_size); 
     437 
     438   speex_bits_init(&jitter->current_packet); 
     439   jitter->valid_bits = 0; 
     440 
     441} 
     442 
     443void speex_jitter_destroy(SpeexJitter *jitter) 
     444{ 
     445   jitter_buffer_destroy(jitter->packets); 
     446   speex_bits_destroy(&jitter->current_packet); 
     447} 
     448 
     449void speex_jitter_put(SpeexJitter *jitter, char *packet, int len, int timestamp) 
     450{ 
     451   JitterBufferPacket p; 
     452   p.data = packet; 
     453   p.len = len; 
     454   p.timestamp = timestamp; 
     455   p.span = jitter->frame_size; 
     456   jitter_buffer_put(jitter->packets, &p); 
     457} 
     458 
     459void speex_jitter_get(SpeexJitter *jitter, short *out, int *current_timestamp) 
     460{ 
     461   int i; 
     462   int ret; 
     463   char data[2048]; 
     464   JitterBufferPacket packet; 
     465   packet.data = data; 
     466    
     467   if (jitter->valid_bits) 
     468   { 
     469      /* Try decoding last received packet */ 
     470      ret = speex_decode_int(jitter->dec, &jitter->current_packet, out); 
     471      if (ret == 0) 
     472      { 
     473         jitter_buffer_tick(jitter->packets); 
     474         return; 
     475      } else { 
     476         jitter->valid_bits = 0; 
     477      } 
     478   } 
     479 
     480   ret = jitter_buffer_get(jitter->packets, &packet, NULL); 
     481    
     482   if (ret != JITTER_BUFFER_OK) 
     483   { 
    267484      /* No packet found */ 
    268       if (jitter->valid_bits) 
    269       { 
    270          /* Try decoding last received packet */ 
    271          ret = speex_decode_int(jitter->dec, &jitter->current_packet, (spx_int16_t*)out); 
    272          if (ret == 0) 
    273          { 
    274             jitter->lost_count = 0; 
    275             return; 
    276          } else { 
    277             jitter->valid_bits = 0; 
    278          } 
    279       } 
    280  
    281       /*fprintf (stderr, "lost/late frame %d\n", jitter->pointer_timestamp);*/ 
     485 
     486      /*fprintf (stderr, "lost/late frame\n");*/ 
    282487      /*Packet is late or lost*/ 
    283       speex_decode_int(jitter->dec, NULL, (spx_int16_t*)out); 
    284       jitter->lost_count++; 
    285       if (jitter->lost_count>=25) 
    286       { 
    287          jitter->lost_count = 0; 
    288          jitter->reset_state = 1; 
    289          speex_decoder_ctl(jitter->dec, SPEEX_RESET_STATE, NULL); 
    290       } 
    291       jitter->loss_rate = .999*jitter->loss_rate + .001; 
     488      speex_decode_int(jitter->dec, NULL, out); 
    292489   } else { 
    293       jitter->lost_count = 0; 
    294       /* Found the right packet */ 
    295       speex_bits_read_from(&jitter->current_packet, jitter->buf[i], jitter->len[i]); 
    296       jitter->len[i]=-1; 
     490      speex_bits_read_from(&jitter->current_packet, packet.data, packet.len); 
    297491      /* Decode packet */ 
    298       ret = speex_decode_int(jitter->dec, &jitter->current_packet, (spx_int16_t*)out); 
     492      ret = speex_decode_int(jitter->dec, &jitter->current_packet, out); 
    299493      if (ret == 0) 
    300494      { 
     
    305499            out[i]=0; 
    306500      } 
    307       jitter->loss_rate = .999*jitter->loss_rate; 
    308    } 
    309  
    310  
     501   } 
     502   jitter_buffer_tick(jitter->packets); 
    311503} 
    312504 
    313505int speex_jitter_get_pointer_timestamp(SpeexJitter *jitter) 
    314506{ 
    315    return jitter->pointer_timestamp; 
    316 } 
     507   return jitter_buffer_get_pointer_timestamp(jitter->packets); 
     508} 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/kiss_fft.c

    r523 r628  
    3333    do { \ 
    3434        if ( nbuf < (size_t)(n) ) {\ 
    35             free(buf); \ 
     35            speex_free(buf); \ 
    3636            buf = (kiss_fft_cpx*)KISS_FFT_MALLOC(sizeof(kiss_fft_cpx)*(n)); \ 
    3737            nbuf = (size_t)(n); \ 
     
    8888       int i; 
    8989       kiss_fft_cpx *x=Fout; 
    90        for (i=0;i<(int)(4*m);i++) 
     90       for (i=0;i<4*m;i++) 
    9191       { 
    9292          x[i].r = PSHR16(x[i].r,2); 
     
    405405        CHECKBUF(tmpbuf,ntmpbuf,st->nfft); 
    406406        kf_work(tmpbuf,fin,1,in_stride, st->factors,st); 
    407         memcpy(fout,tmpbuf,sizeof(kiss_fft_cpx)*st->nfft); 
     407        speex_move(fout,tmpbuf,sizeof(kiss_fft_cpx)*st->nfft); 
    408408    }else{ 
    409409        kf_work( fout, fin, 1,in_stride, st->factors,st ); 
     
    422422void kiss_fft_cleanup(void) 
    423423{ 
    424     free(scratchbuf); 
     424    speex_free(scratchbuf); 
    425425    scratchbuf = NULL; 
    426426    nscratchbuf=0; 
    427     free(tmpbuf); 
     427    speex_free(tmpbuf); 
    428428    tmpbuf=NULL; 
    429429    ntmpbuf=0; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/kiss_fft.h

    r529 r628  
    33 
    44#include <stdlib.h> 
    5 #include <stdio.h> 
    65#include <math.h> 
    7 #include <memory.h> 
    8 //Not available in gcc MacOS X (bennylp) 
    9 //#include <malloc.h> 
     6#include "misc.h" 
    107 
    118#ifdef __cplusplus 
     
    3128#define KISS_FFT_MALLOC(nbytes) memalign(16,nbytes) 
    3229#else    
    33 #define KISS_FFT_MALLOC malloc 
     30#define KISS_FFT_MALLOC speex_alloc 
    3431#endif   
    3532 
    3633 
    3734#ifdef FIXED_POINT 
    38 #include <sys/types.h>   
    39 #  define kiss_fft_scalar int16_t 
     35#include "misc.h"        
     36#  define kiss_fft_scalar spx_int16_t 
    4037#else 
    4138# ifndef kiss_fft_scalar 
     
    9693/* If kiss_fft_alloc allocated a buffer, it is one contiguous  
    9794   buffer and can be simply free()d when no longer needed*/ 
    98 #define kiss_fft_free free 
     95#define kiss_fft_free speex_free 
    9996 
    10097/* 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/kiss_fftr.c

    r516 r628  
    3636 
    3737    if (nfft & 1) { 
    38         fprintf(stderr,"Real FFT optimization must be even.\n"); 
     38        speex_warning("Real FFT optimization must be even.\n"); 
    3939        return NULL; 
    4040    } 
     
    7676 
    7777    if ( st->substate->inverse) { 
    78         fprintf(stderr,"kiss fft usage error: improper alloc\n"); 
     78        speex_warning("kiss fft usage error: improper alloc\n"); 
    7979        exit(1); 
    8080    } 
     
    131131 
    132132    if (st->substate->inverse == 0) { 
    133         fprintf (stderr, "kiss fft usage error: improper alloc\n"); 
     133        speex_warning ("kiss fft usage error: improper alloc\n"); 
    134134        exit (1); 
    135135    } 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/kiss_fftr.h

    r516 r628  
    3939*/ 
    4040 
    41 #define kiss_fftr_free free 
     41#define kiss_fftr_free speex_free 
    4242 
    4343#ifdef __cplusplus 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/lpc.c

    r516 r628  
    9595         rr = SUB32(rr,MULT16_16(lpc[j],ac[i - j])); 
    9696#ifdef FIXED_POINT 
    97       r = DIV32_16(rr,ADD16(error,16)); 
     97      r = DIV32_16(rr+PSHR32(error,1),ADD16(error,8)); 
    9898#else 
    9999      r = rr/(error+.003*ac[0]); 
     
    104104      { 
    105105         spx_word16_t tmp  = lpc[j]; 
    106          lpc[j]     = MAC16_16_Q13(lpc[j],r,lpc[i-1-j]); 
    107          lpc[i-1-j] = MAC16_16_Q13(lpc[i-1-j],r,tmp); 
     106         lpc[j]     = MAC16_16_P13(lpc[j],r,lpc[i-1-j]); 
     107         lpc[i-1-j] = MAC16_16_P13(lpc[i-1-j],r,tmp); 
    108108      } 
    109109      if (i & 1)  
    110          lpc[j] = MAC16_16_Q13(lpc[j],lpc[j],r); 
     110         lpc[j] = MAC16_16_P13(lpc[j],lpc[j],r); 
    111111 
    112112      error = SUB16(error,MULT16_16_Q13(r,MULT16_16_Q13(error,r))); 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/lsp.c

    r278 r628  
    11/*---------------------------------------------------------------------------*\ 
    22Original copyright 
    3         FILE........: AKSLSPD.C 
    4         TYPE........: Turbo C 
    5         COMPANY.....: Voicetronix 
     3        FILE........: lsp.c 
    64        AUTHOR......: David Rowe 
    75        DATE CREATED: 24/2/93 
     
    4543*/ 
    4644 
     45/*---------------------------------------------------------------------------*\ 
     46 
     47  Introduction to Line Spectrum Pairs (LSPs) 
     48  ------------------------------------------ 
     49 
     50  LSPs are used to encode the LPC filter coefficients {ak} for 
     51  transmission over the channel.  LSPs have several properties (like 
     52  less sensitivity to quantisation noise) that make them superior to 
     53  direct quantisation of {ak}. 
     54 
     55  A(z) is a polynomial of order lpcrdr with {ak} as the coefficients. 
     56 
     57  A(z) is transformed to P(z) and Q(z) (using a substitution and some 
     58  algebra), to obtain something like: 
     59 
     60    A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1) 
     61 
     62  As you can imagine A(z) has complex zeros all over the z-plane. P(z) 
     63  and Q(z) have the very neat property of only having zeros _on_ the 
     64  unit circle.  So to find them we take a test point z=exp(jw) and 
     65  evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0 
     66  and pi. 
     67 
     68  The zeros (roots) of P(z) also happen to alternate, which is why we 
     69  swap coefficients as we find roots.  So the process of finding the 
     70  LSP frequencies is basically finding the roots of 5th order 
     71  polynomials. 
     72 
     73  The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence 
     74  the name Line Spectrum Pairs (LSPs). 
     75 
     76  To convert back to ak we just evaluate (1), "clocking" an impulse 
     77  thru it lpcrdr times gives us the impulse response of A(z) which is 
     78  {ak}. 
     79 
     80\*---------------------------------------------------------------------------*/ 
     81 
    4782#ifdef HAVE_CONFIG_H 
    4883#include "config.h" 
     
    6499#ifdef FIXED_POINT 
    65100 
    66  
    67  
    68101#define FREQ_SCALE 16384 
    69102 
     
    73106/*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/ 
    74107#define X2ANGLE(x) (spx_acos(x)) 
     108 
     109#ifdef BFIN_ASM 
     110#include "lsp_bfin.h" 
     111#endif 
    75112 
    76113#else 
     
    89126/*---------------------------------------------------------------------------*\ 
    90127 
    91         FUNCTION....: cheb_poly_eva() 
    92  
    93         AUTHOR......: David Rowe 
    94         DATE CREATED: 24/2/93 
    95  
    96     This function evaluates a series of Chebyshev polynomials 
     128   FUNCTION....: cheb_poly_eva() 
     129 
     130   AUTHOR......: David Rowe 
     131   DATE CREATED: 24/2/93 
     132 
     133   This function evaluates a series of Chebyshev polynomials 
    97134 
    98135\*---------------------------------------------------------------------------*/ 
     
    100137#ifdef FIXED_POINT 
    101138 
    102 static inline spx_word32_t cheb_poly_eva(spx_word32_t *coef,spx_word16_t x,int m,char *stack) 
    103 /*  float coef[]        coefficients of the polynomial to be evaluated  */ 
    104 /*  float x             the point where polynomial is to be evaluated   */ 
    105 /*  int m               order of the polynomial                         */ 
     139#ifndef OVERRIDE_CHEB_POLY_EVA 
     140static inline spx_word32_t cheb_poly_eva( 
     141  spx_word16_t *coef, /* P or Q coefs in Q13 format               */ 
     142  spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */ 
     143  int              m, /* LPC order/2                              */ 
     144  char         *stack 
     145) 
    106146{ 
    107147    int i; 
    108     VARDECL(spx_word16_t *T); 
     148    spx_word16_t b0, b1; 
    109149    spx_word32_t sum; 
    110     int m2=m>>1; 
    111     VARDECL(spx_word16_t *coefn); 
    112150 
    113151    /*Prevents overflows*/ 
     
    117155       x = -16383; 
    118156 
    119     /* Allocate memory for Chebyshev series formulation */ 
    120     ALLOC(T, m2+1, spx_word16_t); 
    121     ALLOC(coefn, m2+1, spx_word16_t); 
    122  
    123     for (i=0;i<m2+1;i++) 
     157    /* Initialise values */ 
     158    b1=16384; 
     159    b0=x; 
     160 
     161    /* Evaluate Chebyshev series formulation usin g iterative approach  */ 
     162    sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x))); 
     163    for(i=2;i<=m;i++) 
    124164    { 
    125        coefn[i] = coef[i]; 
    126        /*printf ("%f ", coef[i]);*/ 
    127     } 
    128     /*printf ("\n");*/ 
    129  
    130     /* Initialise values */ 
    131     T[0]=16384; 
    132     T[1]=x; 
    133  
    134     /* Evaluate Chebyshev series formulation using iterative approach  */ 
    135     /* Evaluate polynomial and return value also free memory space */ 
    136     sum = ADD32(EXTEND32(coefn[m2]), EXTEND32(MULT16_16_P14(coefn[m2-1],x))); 
    137     /*x *= 2;*/ 
    138     for(i=2;i<=m2;i++) 
    139     { 
    140        T[i] = SUB16(MULT16_16_Q13(x,T[i-1]), T[i-2]); 
    141        sum = ADD32(sum, EXTEND32(MULT16_16_P14(coefn[m2-i],T[i]))); 
    142        /*printf ("%f ", sum);*/ 
    143     } 
    144      
    145     /*printf ("\n");*/ 
    146     return sum; 
    147 } 
    148 #else 
    149 static float cheb_poly_eva(spx_word32_t *coef,float x,int m,char *stack) 
    150 /*  float coef[]        coefficients of the polynomial to be evaluated  */ 
    151 /*  float x             the point where polynomial is to be evaluated   */ 
    152 /*  int m               order of the polynomial                         */ 
    153 { 
    154     int i; 
    155     VARDECL(float *T); 
    156     float sum; 
    157     int m2=m>>1; 
    158  
    159     /* Allocate memory for Chebyshev series formulation */ 
    160     ALLOC(T, m2+1, float); 
    161  
    162     /* Initialise values */ 
    163     T[0]=1; 
    164     T[1]=x; 
    165  
    166     /* Evaluate Chebyshev series formulation using iterative approach  */ 
    167     /* Evaluate polynomial and return value also free memory space */ 
    168     sum = coef[m2] + coef[m2-1]*x; 
    169     x *= 2; 
    170     for(i=2;i<=m2;i++) 
    171     { 
    172        T[i] = x*T[i-1] - T[i-2]; 
    173        sum += coef[m2-i] * T[i]; 
     165       spx_word16_t tmp=b0; 
     166       b0 = SUB16(MULT16_16_Q13(x,b0), b1); 
     167       b1 = tmp; 
     168       sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0))); 
    174169    } 
    175170     
     
    178173#endif 
    179174 
     175#else 
     176 
     177static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack) 
     178{ 
     179   int k; 
     180   float b0, b1, tmp; 
     181 
     182   /* Initial conditions */ 
     183   b0=0; /* b_(m+1) */ 
     184   b1=0; /* b_(m+2) */ 
     185 
     186   x*=2; 
     187 
     188   /* Calculate the b_(k) */ 
     189   for(k=m;k>0;k--) 
     190   { 
     191      tmp=b0;                           /* tmp holds the previous value of b0 */ 
     192      b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */ 
     193      b1=tmp;                           /* b1 holds the previous value of b0 */ 
     194   } 
     195 
     196   return(-b1+.5*x*b0+coef[m]); 
     197} 
     198#endif 
     199 
    180200/*---------------------------------------------------------------------------*\ 
    181201 
    182         FUNCTION....: lpc_to_lsp() 
    183  
    184         AUTHOR......: David Rowe 
    185         DATE CREATED: 24/2/93 
     202    FUNCTION....: lpc_to_lsp() 
     203 
     204    AUTHOR......: David Rowe 
     205    DATE CREATED: 24/2/93 
    186206 
    187207    This function converts LPC coefficients to LSP 
     
    211231    VARDECL(spx_word32_t *Q);                   /* ptrs for memory allocation           */ 
    212232    VARDECL(spx_word32_t *P); 
     233    VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation           */ 
     234    VARDECL(spx_word16_t *P16); 
    213235    spx_word32_t *px;                   /* ptrs of respective P'(z) & Q'(z)     */ 
    214236    spx_word32_t *qx; 
    215237    spx_word32_t *p; 
    216238    spx_word32_t *q; 
    217     spx_word32_t *pt;                   /* ptr used for cheb_poly_eval() 
     239    spx_word16_t *pt;                   /* ptr used for cheb_poly_eval() 
    218240                                whether P' or Q'                        */ 
    219241    int roots=0;                /* DR 8/2/94: number of roots found     */ 
     
    277299    qx = Q; 
    278300 
     301    /* now that we have computed P and Q convert to 16 bits to 
     302       speed up cheb_poly_eval */ 
     303 
     304    ALLOC(P16, m+1, spx_word16_t); 
     305    ALLOC(Q16, m+1, spx_word16_t); 
     306 
     307    for (i=0;i<m+1;i++) 
     308    { 
     309       P16[i] = P[i]; 
     310       Q16[i] = Q[i]; 
     311    } 
     312 
    279313    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z). 
    280314    Keep alternating between the two polynomials as each zero is found  */ 
     
    283317    xl = FREQ_SCALE;                    /* start at point xl = 1                */ 
    284318 
    285  
    286319    for(j=0;j<lpcrdr;j++){ 
    287320        if(j&1)                 /* determines whether P' or Q' is eval. */ 
    288             pt = qx; 
     321            pt = Q16; 
    289322        else 
    290             pt = px; 
    291  
    292         psuml = cheb_poly_eva(pt,xl,lpcrdr,stack);      /* evals poly. at xl    */ 
     323            pt = P16; 
     324 
     325        psuml = cheb_poly_eva(pt,xl,m,stack);   /* evals poly. at xl    */ 
    293326        flag = 1; 
    294327        while(flag && (xr >= -FREQ_SCALE)){ 
     
    305338#endif 
    306339           xr = SUB16(xl, dd);                          /* interval spacing     */ 
    307             psumr = cheb_poly_eva(pt,xr,lpcrdr,stack);/* poly(xl-delta_x)       */ 
     340            psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x)    */ 
    308341            temp_psumr = psumr; 
    309342            temp_xr = xr; 
     
    329362                    xm = .5*(xl+xr);            /* bisect the interval  */ 
    330363#endif 
    331                     psumm=cheb_poly_eva(pt,xm,lpcrdr,stack); 
     364                    psumm=cheb_poly_eva(pt,xm,m,stack); 
    332365                    /*if(psumm*psuml>0.)*/ 
    333366                    if(!SIGN_CHANGE(psumm,psuml)) 
     
    355388} 
    356389 
    357  
    358390/*---------------------------------------------------------------------------*\ 
    359391 
     
    363395        DATE CREATED: 24/2/93 
    364396 
    365     lsp_to_lpc: This function converts LSP coefficients to LPC 
    366     coefficients. 
     397        Converts LSP coefficients to LPC coefficients. 
    367398 
    368399\*---------------------------------------------------------------------------*/ 
     
    374405/*  float *ak           array of LPC coefficients                       */ 
    375406/*  int lpcrdr          order of LPC coefficients                       */ 
    376  
    377  
    378407{ 
    379408    int i,j; 
    380     spx_word32_t xout1,xout2,xin1,xin2; 
    381     VARDECL(spx_word32_t *Wp); 
    382     spx_word32_t *pw,*n1,*n2,*n3,*n4=NULL; 
     409    spx_word32_t xout1,xout2,xin; 
     410    spx_word32_t mult, a; 
    383411    VARDECL(spx_word16_t *freqn); 
     412    VARDECL(spx_word32_t **xp); 
     413    VARDECL(spx_word32_t *xpmem); 
     414    VARDECL(spx_word32_t **xq); 
     415    VARDECL(spx_word32_t *xqmem); 
    384416    int m = lpcrdr>>1; 
     417 
     418    /*  
    385419     
     420       Reconstruct P(z) and Q(z) by cascading second order polynomials 
     421       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency. 
     422       In the time domain this is: 
     423 
     424       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2) 
     425     
     426       This is what the ALLOCS below are trying to do: 
     427 
     428         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP 
     429         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP 
     430 
     431       These matrices store the output of each stage on each row.  The 
     432       final (m-th) row has the output of the final (m-th) cascaded 
     433       2nd order filter.  The first row is the impulse input to the 
     434       system (not written as it is known). 
     435 
     436       The version below takes advantage of the fact that a lot of the 
     437       outputs are zero or known, for example if we put an inpulse 
     438       into the first section the "clock" it 10 times only the first 3 
     439       outputs samples are non-zero (it's an FIR filter). 
     440    */ 
     441 
     442    ALLOC(xp, (m+1), spx_word32_t*); 
     443    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t); 
     444 
     445    ALLOC(xq, (m+1), spx_word32_t*); 
     446    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t); 
     447     
     448    for(i=0; i<=m; i++) { 
     449      xp[i] = xpmem + i*(lpcrdr+1+2); 
     450      xq[i] = xqmem + i*(lpcrdr+1+2); 
     451    } 
     452 
     453    /* work out 2cos terms in Q14 */ 
     454 
    386455    ALLOC(freqn, lpcrdr, spx_word16_t); 
    387     for (i=0;i<lpcrdr;i++) 
     456    for (i=0;i<lpcrdr;i++)  
    388457       freqn[i] = ANGLE2X(freq[i]); 
    389458 
    390     ALLOC(Wp, 4*m+2, spx_word32_t); 
    391     pw = Wp; 
    392  
    393  
    394     /* initialise contents of array */ 
    395  
    396     for(i=0;i<=4*m+1;i++){              /* set contents of buffer to 0 */ 
    397         *pw++ = 0; 
    398     } 
    399  
    400     /* Set pointers up */ 
    401  
    402     pw = Wp; 
    403     xin1 = 1048576; 
    404     xin2 = 1048576; 
    405  
    406     /* reconstruct P(z) and Q(z) by  cascading second order 
    407       polynomials in form 1 - 2xz(-1) +z(-2), where x is the 
    408       LSP coefficient */ 
    409  
    410     for(j=0;j<=lpcrdr;j++){ 
    411        spx_word16_t *fr=freqn; 
    412         for(i=0;i<m;i++){ 
    413             n1 = pw+(i<<2); 
    414             n2 = n1 + 1; 
    415             n3 = n2 + 1; 
    416             n4 = n3 + 1; 
    417             xout1 = ADD32(SUB32(xin1, MULT16_32_Q14(*fr,*n1)), *n2); 
    418             fr++; 
    419             xout2 = ADD32(SUB32(xin2, MULT16_32_Q14(*fr,*n3)), *n4); 
    420             fr++; 
    421             *n2 = *n1; 
    422             *n4 = *n3; 
    423             *n1 = xin1; 
    424             *n3 = xin2; 
    425             xin1 = xout1; 
    426             xin2 = xout2; 
    427         } 
    428         xout1 = xin1 + *(n4+1); 
    429         xout2 = xin2 - *(n4+2); 
    430         /* FIXME: perhaps apply bandwidth expansion in case of overflow? */ 
    431         if (j>0) 
    432         { 
    433         if (xout1 + xout2>SHL32(EXTEND32(32766),8)) 
    434            ak[j-1] = 32767; 
    435         else if (xout1 + xout2 < -SHL32(EXTEND32(32766),8)) 
    436            ak[j-1] = -32767; 
    437         else 
    438            ak[j-1] = EXTRACT16(PSHR32(ADD32(xout1,xout2),8)); 
    439         } else {/*speex_warning_int("ak[0] = ", EXTRACT16(PSHR32(ADD32(xout1,xout2),8)));*/} 
    440         *(n4+1) = xin1; 
    441         *(n4+2) = xin2; 
    442  
    443         xin1 = 0; 
    444         xin2 = 0; 
    445     } 
    446 } 
     459    #define QIMP  21   /* scaling for impulse */ 
     460 
     461    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */ 
     462    
     463    /* first col and last non-zero values of each row are trivial */ 
     464     
     465    for(i=0;i<=m;i++) { 
     466     xp[i][1] = 0; 
     467     xp[i][2] = xin; 
     468     xp[i][2+2*i] = xin; 
     469     xq[i][1] = 0; 
     470     xq[i][2] = xin; 
     471     xq[i][2+2*i] = xin; 
     472    } 
     473 
     474    /* 2nd row (first output row) is trivial */ 
     475 
     476    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]); 
     477    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]); 
     478 
     479    xout1 = xout2 = 0; 
     480 
     481    /* now generate remaining rows */ 
     482 
     483    for(i=1;i<m;i++) { 
     484 
     485      for(j=1;j<2*(i+1)-1;j++) { 
     486        mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]); 
     487        xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]); 
     488        mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]); 
     489        xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]); 
     490      } 
     491 
     492      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */ 
     493 
     494      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]); 
     495      xp[i+1][j+2] = SUB32(xp[i][j], mult); 
     496      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]); 
     497      xq[i+1][j+2] = SUB32(xq[i][j], mult); 
     498    } 
     499 
     500    /* process last row to extra a{k} */ 
     501 
     502    for(j=1;j<=lpcrdr;j++) { 
     503      int shift = QIMP-13; 
     504 
     505      /* final filter sections */ 
     506      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);  
     507      xout1 = xp[m][j+2]; 
     508      xout2 = xq[m][j+2]; 
     509       
     510      /* hard limit ak's to +/- 32767 */ 
     511 
     512      if (a < -32767) a = 32767; 
     513      if (a > 32767) a = 32767; 
     514      ak[j-1] = (short)a; 
     515      
     516    } 
     517 
     518} 
     519 
    447520#else 
    448521 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/ltp.c

    r516 r628  
    5656 
    5757#ifndef OVERRIDE_INNER_PROD 
    58 static spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
     58spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
    5959{ 
    6060   spx_word32_t sum=0; 
     
    7676#ifndef OVERRIDE_PITCH_XCORR 
    7777#if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */ 
    78 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
     78void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
    7979{ 
    8080   int i,j; 
     
    139139} 
    140140#else 
    141 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
     141void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
    142142{ 
    143143   int i; 
     
    153153 
    154154#ifndef OVERRIDE_COMPUTE_PITCH_ERROR 
    155 static inline spx_word32_t compute_pitch_error(spx_word32_t *C, spx_word16_t *g, spx_word16_t pitch_control) 
     155static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control) 
    156156{ 
    157157   spx_word32_t sum = 0; 
    158    sum = ADD32(sum,MULT16_32_Q15(MULT16_16_16(g[0],pitch_control),C[0])); 
    159    sum = ADD32(sum,MULT16_32_Q15(MULT16_16_16(g[1],pitch_control),C[1])); 
    160    sum = ADD32(sum,MULT16_32_Q15(MULT16_16_16(g[2],pitch_control),C[2])); 
    161    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[0],g[1]),C[3])); 
    162    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[2],g[1]),C[4])); 
    163    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[2],g[0]),C[5])); 
    164    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[0],g[0]),C[6])); 
    165    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[1],g[1]),C[7])); 
    166    sum = SUB32(sum,MULT16_32_Q15(MULT16_16_16(g[2],g[2]),C[8])); 
     158   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[0],pitch_control),C[0])); 
     159   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[1],pitch_control),C[1])); 
     160   sum = ADD32(sum,MULT16_16(MULT16_16_16(g[2],pitch_control),C[2])); 
     161   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[1]),C[3])); 
     162   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[1]),C[4])); 
     163   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[0]),C[5])); 
     164   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[0]),C[6])); 
     165   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[1],g[1]),C[7])); 
     166   sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[2]),C[8])); 
    167167   return sum; 
    168168} 
    169169#endif 
    170170 
    171 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack) 
     171#ifndef OVERRIDE_OPEN_LOOP_NBEST_PITCH 
     172void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack) 
    172173{ 
    173174   int i,j,k; 
    174175   VARDECL(spx_word32_t *best_score); 
     176   VARDECL(spx_word32_t *best_ener); 
    175177   spx_word32_t e0; 
    176178   VARDECL(spx_word32_t *corr); 
    177179   VARDECL(spx_word32_t *energy); 
    178    VARDECL(spx_word32_t *score); 
    179    VARDECL(spx_word16_t *swn2); 
    180    spx_word16_t *swn; 
    181180 
    182181   ALLOC(best_score, N, spx_word32_t); 
     182   ALLOC(best_ener, N, spx_word32_t); 
    183183   ALLOC(corr, end-start+1, spx_word32_t); 
    184184   ALLOC(energy, end-start+2, spx_word32_t); 
    185    ALLOC(score, end-start+1, spx_word32_t); 
    186  
    187 #ifdef FIXED_POINT 
    188    ALLOC(swn2, end+len, spx_word16_t); 
    189    normalize16(sw-end, swn2, 16384, end+len); 
    190    swn = swn2 + end; 
    191 #else 
    192    swn = sw; 
    193 #endif 
    194185 
    195186   for (i=0;i<N;i++) 
    196187   { 
    197188        best_score[i]=-1; 
     189        best_ener[i]=0; 
    198190        pitch[i]=start; 
    199191   } 
    200192 
    201  
    202    energy[0]=inner_prod(swn-start, swn-start, len); 
    203    e0=inner_prod(swn, swn, len); 
    204    for (i=start;i<=end;i++) 
     193   energy[0]=inner_prod(sw-start, sw-start, len); 
     194   e0=inner_prod(sw, sw, len); 
     195   for (i=start;i<end;i++) 
    205196   { 
    206197      /* Update energy for next pitch*/ 
    207       energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(swn[-i-1],swn[-i-1]),6)), SHR32(MULT16_16(swn[-i+len-1],swn[-i+len-1]),6)); 
     198      energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6)); 
    208199      if (energy[i-start+1] < 0) 
    209200         energy[i-start+1] = 0; 
    210201   } 
    211202 
    212    pitch_xcorr(swn, swn-end, corr, len, end-start+1, stack); 
    213  
     203   pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack); 
     204 
     205   /* FIXME: Fixed-point and floating-point code should be merged */ 
    214206#ifdef FIXED_POINT 
    215207   { 
     
    218210      ALLOC(corr16, end-start+1, spx_word16_t); 
    219211      ALLOC(ener16, end-start+1, spx_word16_t); 
    220       normalize16(corr, corr16, 16384, end-start+1); 
    221       normalize16(energy, ener16, 16384, end-start+1); 
     212      /* Normalize to 180 so we can square it and it still fits in 16 bits */ 
     213      normalize16(corr, corr16, 180, end-start+1); 
     214      normalize16(energy, ener16, 180, end-start+1); 
    222215 
    223216      for (i=start;i<=end;i++) 
    224217      { 
    225          spx_word16_t g; 
    226          spx_word32_t tmp; 
    227          tmp = corr16[i-start]; 
    228          if (tmp>0) 
     218         spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]); 
     219         /* Instead of dividing the tmp by the energy, we multiply on the other side */ 
     220         if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start]))) 
    229221         { 
    230             if (SHR16(corr16[i-start],4)>ener16[i-start]) 
    231                tmp = SHL32(EXTEND32(ener16[i-start]),14); 
    232             else if (-SHR16(corr16[i-start],4)>ener16[i-start]) 
    233                tmp = -SHL32(EXTEND32(ener16[i-start]),14); 
    234             else 
    235                tmp = SHL32(tmp,10); 
    236             g = DIV32_16(tmp, 8+ener16[i-start]); 
    237             score[i-start] = MULT16_16(corr16[i-start],g); 
    238          } else 
    239          { 
    240             score[i-start] = 1; 
     222            /* We can safely put it last and then check */ 
     223            best_score[N-1]=tmp; 
     224            best_ener[N-1]=ener16[i-start]+1; 
     225            pitch[N-1]=i; 
     226            /* Check if it comes in front of others */ 
     227            for (j=0;j<N-1;j++) 
     228            { 
     229               if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start]))) 
     230               { 
     231                  for (k=N-1;k>j;k--) 
     232                  { 
     233                     best_score[k]=best_score[k-1]; 
     234                     best_ener[k]=best_ener[k-1]; 
     235                     pitch[k]=pitch[k-1]; 
     236                  } 
     237                  best_score[j]=tmp; 
     238                  best_ener[j]=ener16[i-start]+1; 
     239                  pitch[j]=i; 
     240                  break; 
     241               } 
     242            } 
    241243         } 
    242244      } 
     
    245247   for (i=start;i<=end;i++) 
    246248   { 
    247       float g = corr[i-start]/(1+energy[i-start]); 
    248       if (g>16) 
    249          g = 16; 
    250       else if (g<-16) 
    251          g = -16; 
    252       score[i-start] = g*corr[i-start]; 
    253    } 
    254 #endif 
    255  
    256    /* Extract best scores */ 
    257    for (i=start;i<=end;i++) 
    258    { 
    259       if (score[i-start]>best_score[N-1]) 
     249      float tmp = corr[i-start]*corr[i-start]; 
     250      if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start])) 
    260251      { 
    261252         for (j=0;j<N;j++) 
    262253         { 
    263             if (score[i-start] > best_score[j]) 
     254            if (tmp*best_ener[j]>best_score[j]*(1+energy[i-start])) 
    264255            { 
    265256               for (k=N-1;k>j;k--) 
    266257               { 
    267258                  best_score[k]=best_score[k-1]; 
     259                  best_ener[k]=best_ener[k-1]; 
    268260                  pitch[k]=pitch[k-1]; 
    269261               } 
    270                best_score[j]=score[i-start]; 
     262               best_score[j]=tmp; 
     263               best_ener[j]=energy[i-start]+1; 
    271264               pitch[j]=i; 
    272265               break; 
     
    275268      } 
    276269   } 
     270#endif 
    277271 
    278272   /* Compute open-loop gain */ 
     
    291285   } 
    292286} 
    293  
     287#endif 
     288 
     289#ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ 
     290static int pitch_gain_search_3tap_vq( 
     291  const signed char *gain_cdbk, 
     292  int                gain_cdbk_size, 
     293  spx_word16_t      *C16, 
     294  spx_word16_t       max_gain 
     295) 
     296{ 
     297  const signed char *ptr=gain_cdbk; 
     298  int                best_cdbk=0; 
     299  spx_word32_t       best_sum=-VERY_LARGE32; 
     300  spx_word32_t       sum=0; 
     301  spx_word16_t       g[3]; 
     302  spx_word16_t       pitch_control=64; 
     303  spx_word16_t       gain_sum; 
     304  int                i; 
     305 
     306  for (i=0;i<gain_cdbk_size;i++) { 
     307          
     308    ptr = gain_cdbk+4*i; 
     309    g[0]=ADD16((spx_word16_t)ptr[0],32); 
     310    g[1]=ADD16((spx_word16_t)ptr[1],32); 
     311    g[2]=ADD16((spx_word16_t)ptr[2],32); 
     312    gain_sum = (spx_word16_t)ptr[3]; 
     313          
     314    sum = compute_pitch_error(C16, g, pitch_control); 
     315          
     316    if (sum>best_sum && gain_sum<=max_gain) { 
     317      best_sum=sum; 
     318      best_cdbk=i; 
     319    } 
     320  } 
     321 
     322  return best_cdbk; 
     323} 
     324#endif 
    294325 
    295326/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ 
    296 static spx_word64_t pitch_gain_search_3tap( 
    297 const spx_sig_t target[],       /* Target vector */ 
     327static spx_word32_t pitch_gain_search_3tap( 
     328const spx_word16_t target[],       /* Target vector */ 
    298329const spx_coef_t ak[],          /* LPCs for this subframe */ 
    299330const spx_coef_t awk1[],        /* Weighted LPCs #1 for this subframe */ 
    300331const spx_coef_t awk2[],        /* Weighted LPCs #2 for this subframe */ 
    301332spx_sig_t exc[],                /* Excitation */ 
    302 const void *par, 
     333const signed char *gain_cdbk, 
     334int gain_cdbk_size, 
    303335int   pitch,                    /* Pitch value */ 
    304336int   p,                        /* Number of LPC coeffs */ 
     
    306338SpeexBits *bits, 
    307339char *stack, 
    308 const spx_sig_t *exc2, 
     340const spx_word16_t *exc2, 
    309341const spx_word16_t *r, 
    310 spx_sig_t *new_target, 
     342spx_word16_t *new_target, 
    311343int  *cdbk_index, 
    312 int cdbk_offset, 
    313 int plc_tuning 
     344int plc_tuning, 
     345spx_word32_t cumul_gain 
    314346) 
    315347{ 
    316348   int i,j; 
    317    VARDECL(spx_sig_t *tmp1); 
    318    VARDECL(spx_sig_t *tmp2); 
    319    spx_sig_t *x[3]; 
    320    spx_sig_t *e[3]; 
     349   VARDECL(spx_word16_t *tmp1); 
     350   VARDECL(spx_word16_t *e); 
     351   spx_word16_t *x[3]; 
    321352   spx_word32_t corr[3]; 
    322353   spx_word32_t A[3][3]; 
    323    int   gain_cdbk_size; 
    324    const signed char *gain_cdbk; 
    325354   spx_word16_t gain[3]; 
    326    spx_word64_t err; 
    327  
    328    const ltp_params *params; 
    329    params = (const ltp_params*) par; 
    330    gain_cdbk_size = 1<<params->gain_bits; 
    331    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset; 
    332    ALLOC(tmp1, 3*nsf, spx_sig_t); 
    333    ALLOC(tmp2, 3*nsf, spx_sig_t); 
    334  
     355   spx_word32_t err; 
     356   spx_word16_t max_gain=128; 
     357   int          best_cdbk=0; 
     358 
     359   ALLOC(tmp1, 3*nsf, spx_word16_t); 
     360   ALLOC(e, nsf, spx_word16_t); 
     361 
     362   if (cumul_gain > 262144) 
     363      max_gain = 31; 
     364    
    335365   x[0]=tmp1; 
    336366   x[1]=tmp1+nsf; 
    337367   x[2]=tmp1+2*nsf; 
    338368    
    339    e[0]=tmp2; 
    340    e[1]=tmp2+nsf; 
    341    e[2]=tmp2+2*nsf; 
    342    for (i=2;i>=0;i--) 
    343    { 
    344       int pp=pitch+1-i; 
     369   { 
     370      VARDECL(spx_mem_t *mm); 
     371      int pp=pitch-1; 
     372      ALLOC(mm, p, spx_mem_t); 
    345373      for (j=0;j<nsf;j++) 
    346374      { 
    347375         if (j-pp<0) 
    348             e[i][j]=exc2[j-pp]; 
     376            e[j]=exc2[j-pp]; 
    349377         else if (j-pp-pitch<0) 
    350             e[i][j]=exc2[j-pp-pitch]; 
     378            e[j]=exc2[j-pp-pitch]; 
    351379         else 
    352             e[i][j]=0; 
     380            e[j]=0; 
    353381      } 
    354  
    355       if (i==2) 
    356          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack); 
    357       else { 
    358          for (j=0;j<nsf-1;j++) 
    359             x[i][j+1]=x[i+1][j]; 
    360          x[i][0]=0; 
    361          for (j=0;j<nsf;j++) 
    362          { 
    363             x[i][j]=ADD32(x[i][j],SHL32(MULT16_32_Q15(r[j], e[i][0]),1)); 
    364          } 
    365       } 
    366    } 
    367  
    368 #ifdef FIXED_POINT 
    369    { 
    370       /* If using fixed-point, we need to normalize the signals first */ 
    371       spx_word16_t *y[3]; 
    372       VARDECL(spx_word16_t *ytmp); 
    373       VARDECL(spx_word16_t *t); 
    374  
    375       spx_sig_t max_val=1; 
    376       int sig_shift; 
    377        
    378       ALLOC(ytmp, 3*nsf, spx_word16_t); 
    379 #if 0 
    380       ALLOC(y[0], nsf, spx_word16_t); 
    381       ALLOC(y[1], nsf, spx_word16_t); 
    382       ALLOC(y[2], nsf, spx_word16_t); 
    383 #else 
    384       y[0] = ytmp; 
    385       y[1] = ytmp+nsf; 
    386       y[2] = ytmp+2*nsf; 
    387 #endif 
    388       ALLOC(t, nsf, spx_word16_t); 
    389       for (j=0;j<3;j++) 
    390       { 
    391          for (i=0;i<nsf;i++) 
    392          { 
    393             spx_sig_t tmp = x[j][i]; 
    394             if (tmp<0) 
    395                tmp = -tmp; 
    396             if (tmp > max_val) 
    397                max_val = tmp; 
    398          } 
    399       } 
    400       for (i=0;i<nsf;i++) 
    401       { 
    402          spx_sig_t tmp = target[i]; 
    403          if (tmp<0) 
    404             tmp = -tmp; 
    405          if (tmp > max_val) 
    406             max_val = tmp; 
    407       } 
    408  
    409       sig_shift=0; 
    410       while (max_val>16384) 
    411       { 
    412          sig_shift++; 
    413          max_val >>= 1; 
    414       } 
    415  
    416       for (j=0;j<3;j++) 
    417       { 
    418          for (i=0;i<nsf;i++) 
    419          { 
    420             y[j][i] = EXTRACT16(SHR32(x[j][i],sig_shift)); 
    421          } 
    422       } 
    423       for (i=0;i<nsf;i++) 
    424       { 
    425          t[i] = EXTRACT16(SHR32(target[i],sig_shift)); 
    426       } 
    427  
    428       for (i=0;i<3;i++) 
    429          corr[i]=inner_prod(y[i],t,nsf); 
    430        
    431       for (i=0;i<3;i++) 
    432          for (j=0;j<=i;j++) 
    433             A[i][j]=A[j][i]=inner_prod(y[i],y[j],nsf); 
    434    } 
    435 #else 
    436    { 
    437       for (i=0;i<3;i++) 
    438          corr[i]=inner_prod(x[i],target,nsf); 
    439        
    440       for (i=0;i<3;i++) 
    441          for (j=0;j<=i;j++) 
    442             A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf); 
    443    } 
    444 #endif 
     382      for (j=0;j<p;j++) 
     383         mm[j] = 0; 
     384      iir_mem16(e, ak, e, nsf, p, mm, stack); 
     385      for (j=0;j<p;j++) 
     386         mm[j] = 0; 
     387      filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack); 
     388      for (j=0;j<nsf;j++) 
     389         x[2][j] = e[j]; 
     390   } 
     391   for (i=1;i>=0;i--) 
     392   { 
     393      spx_word16_t e0=exc2[-pitch-1+i]; 
     394      x[i][0]=MULT16_16_Q14(r[0], e0); 
     395      for (j=0;j<nsf-1;j++) 
     396         x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0)); 
     397   } 
     398 
     399   for (i=0;i<3;i++) 
     400      corr[i]=inner_prod(x[i],target,nsf); 
     401   for (i=0;i<3;i++) 
     402      for (j=0;j<=i;j++) 
     403         A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf); 
    445404 
    446405   { 
    447406      spx_word32_t C[9]; 
    448       const signed char *ptr=gain_cdbk; 
    449       int best_cdbk=0; 
    450       spx_word32_t best_sum=0; 
     407#ifdef FIXED_POINT 
     408      spx_word16_t C16[9]; 
     409#else 
     410      spx_word16_t *C16=C; 
     411#endif       
    451412      C[0]=corr[2]; 
    452413      C[1]=corr[1]; 
     
    462423      if (plc_tuning<2) 
    463424         plc_tuning=2; 
    464 #ifdef FIXED_POINT 
    465       C[0] = MAC16_32_Q15(C[0],MULT16_16_16(plc_tuning,-327),C[0]); 
    466       C[1] = MAC16_32_Q15(C[1],MULT16_16_16(plc_tuning,-327),C[1]); 
    467       C[2] = MAC16_32_Q15(C[2],MULT16_16_16(plc_tuning,-327),C[2]); 
     425      if (plc_tuning>30) 
     426         plc_tuning=30; 
     427#ifdef FIXED_POINT 
    468428      C[0] = SHL32(C[0],1); 
    469429      C[1] = SHL32(C[1],1); 
     
    472432      C[4] = SHL32(C[4],1); 
    473433      C[5] = SHL32(C[5],1); 
    474 #else 
    475       C[0]*=1-.01*plc_tuning; 
    476       C[1]*=1-.01*plc_tuning; 
    477       C[2]*=1-.01*plc_tuning; 
    478       C[6]*=.5*(1+.01*plc_tuning); 
    479       C[7]*=.5*(1+.01*plc_tuning); 
    480       C[8]*=.5*(1+.01*plc_tuning); 
    481 #endif 
    482       for (i=0;i<gain_cdbk_size;i++) 
    483       { 
    484          spx_word32_t sum=0; 
    485          spx_word16_t g[3]; 
    486          spx_word16_t pitch_control=64; 
    487          spx_word16_t gain_sum; 
    488           
    489          ptr = gain_cdbk+3*i; 
    490          g[0]=ADD16((spx_word16_t)ptr[0],32); 
    491          g[1]=ADD16((spx_word16_t)ptr[1],32); 
    492          g[2]=ADD16((spx_word16_t)ptr[2],32); 
    493  
    494          /* We favor "safe" pitch values to handle packet loss better */ 
    495          gain_sum = ADD16(ADD16(g[1],MAX16(g[0], 0)),MAX16(g[2], 0)); 
    496          if (gain_sum > 64) 
    497          { 
    498             gain_sum = SUB16(gain_sum, 64); 
    499             if (gain_sum > 127) 
    500                gain_sum = 127; 
    501 #ifdef FIXED_POINT 
    502             pitch_control =  SUB16(64,EXTRACT16(PSHR32(MULT16_16(64,MULT16_16_16(plc_tuning, gain_sum)),10))); 
    503 #else 
    504             pitch_control = 64*(1.-.001*plc_tuning*gain_sum); 
    505 #endif 
    506             if (pitch_control < 0) 
    507                pitch_control = 0; 
    508          } 
    509           
    510          sum = compute_pitch_error(C, g, pitch_control); 
    511           
    512          if (sum>best_sum || i==0) 
    513          { 
    514             best_sum=sum; 
    515             best_cdbk=i; 
    516          } 
    517       } 
    518 #ifdef FIXED_POINT 
    519       gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3]); 
    520       gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+1]); 
    521       gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*3+2]); 
     434      C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]); 
     435      C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]); 
     436      C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]); 
     437      normalize16(C, C16, 32767, 9); 
     438#else 
     439      C[6]*=.5*(1+.02*plc_tuning); 
     440      C[7]*=.5*(1+.02*plc_tuning); 
     441      C[8]*=.5*(1+.02*plc_tuning); 
     442#endif 
     443 
     444      best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain); 
     445 
     446#ifdef FIXED_POINT 
     447      gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]); 
     448      gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]); 
     449      gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]); 
    522450      /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/ 
    523451#else 
    524       gain[0] = 0.015625*gain_cdbk[best_cdbk*3]  + .5; 
    525       gain[1] = 0.015625*gain_cdbk[best_cdbk*3+1]+ .5; 
    526       gain[2] = 0.015625*gain_cdbk[best_cdbk*3+2]+ .5; 
     452      gain[0] = 0.015625*gain_cdbk[best_cdbk*4]  + .5; 
     453      gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5; 
     454      gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5; 
    527455#endif 
    528456      *cdbk_index=best_cdbk; 
    529457   } 
    530458 
    531 #ifdef FIXED_POINT 
    532459   for (i=0;i<nsf;i++) 
    533      exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])), 
    534                         MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2); 
    535     
    536    err=0; 
     460      exc[i]=0; 
     461   for (i=0;i<3;i++) 
     462   { 
     463      int j; 
     464      int tmp1, tmp3; 
     465      int pp=pitch+1-i; 
     466      tmp1=nsf; 
     467      if (tmp1>pp) 
     468         tmp1=pp; 
     469      for (j=0;j<tmp1;j++) 
     470         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]); 
     471      tmp3=nsf; 
     472      if (tmp3>pp+pitch) 
     473         tmp3=pp+pitch; 
     474      for (j=tmp1;j<tmp3;j++) 
     475         exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]); 
     476   } 
    537477   for (i=0;i<nsf;i++) 
    538478   { 
    539       spx_word16_t perr2; 
    540       spx_sig_t tmp = SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),x[2][i]),MULT16_32_Q15(SHL16(gain[1],7),x[1][i])), 
    541                                   MULT16_32_Q15(SHL16(gain[2],7),x[0][i])),2); 
    542       spx_sig_t perr=SUB32(target[i],tmp); 
    543       new_target[i] = SUB32(target[i], tmp); 
    544       perr2 = EXTRACT16(PSHR32(perr,15)); 
    545       err = ADD64(err,MULT16_16(perr2,perr2)); 
    546        
    547    } 
    548 #else 
    549    for (i=0;i<nsf;i++) 
    550       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i]; 
    551     
    552    err=0; 
    553    for (i=0;i<nsf;i++) 
    554    { 
    555       spx_sig_t tmp = gain[2]*x[0][i]+gain[1]*x[1][i]+gain[0]*x[2][i]; 
    556       new_target[i] = target[i] - tmp; 
    557       err+=new_target[i]*new_target[i]; 
    558    } 
    559 #endif 
     479      spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])), 
     480                            MULT16_16(gain[2],x[0][i])); 
     481      new_target[i] = SUB16(target[i], EXTRACT16(PSHR32(tmp,6))); 
     482   } 
     483   err = inner_prod(new_target, new_target, nsf); 
    560484 
    561485   return err; 
    562486} 
    563  
    564487 
    565488/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ 
    566489int pitch_search_3tap( 
    567 spx_sig_t target[],                 /* Target vector */ 
    568 spx_sig_t *sw, 
     490spx_word16_t target[],                 /* Target vector */ 
     491spx_word16_t *sw, 
    569492spx_coef_t ak[],                     /* LPCs for this subframe */ 
    570493spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */ 
     
    579502SpeexBits *bits, 
    580503char *stack, 
    581 spx_sig_t *exc2, 
     504spx_word16_t *exc2, 
    582505spx_word16_t *r, 
    583506int complexity, 
    584507int cdbk_offset, 
    585 int plc_tuning 
     508int plc_tuning, 
     509spx_word32_t *cumul_gain 
    586510) 
    587511{ 
     
    589513   int cdbk_index, pitch=0, best_gain_index=0; 
    590514   VARDECL(spx_sig_t *best_exc); 
    591    VARDECL(spx_sig_t *new_target); 
    592    VARDECL(spx_sig_t *best_target); 
     515   VARDECL(spx_word16_t *new_target); 
     516   VARDECL(spx_word16_t *best_target); 
    593517   int best_pitch=0; 
    594    spx_word64_t err, best_err=-1; 
     518   spx_word32_t err, best_err=-1; 
    595519   int N; 
    596520   const ltp_params *params; 
     521   const signed char *gain_cdbk; 
     522   int   gain_cdbk_size; 
     523    
    597524   VARDECL(int *nbest); 
    598  
     525    
     526   params = (const ltp_params*) par; 
     527   gain_cdbk_size = 1<<params->gain_bits; 
     528   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; 
     529    
    599530   N=complexity; 
    600531   if (N>10) 
     
    614545      return start; 
    615546   } 
    616     
    617    ALLOC(best_exc, nsf, spx_sig_t); 
    618    ALLOC(new_target, nsf, spx_sig_t); 
    619    ALLOC(best_target, nsf, spx_sig_t); 
    620547    
    621548   if (N>end-start+1) 
     
    625552   else 
    626553      nbest[0] = start; 
     554    
     555   ALLOC(best_exc, nsf, spx_sig_t); 
     556   ALLOC(new_target, nsf, spx_word16_t); 
     557   ALLOC(best_target, nsf, spx_word16_t); 
     558    
    627559   for (i=0;i<N;i++) 
    628560   { 
     
    630562      for (j=0;j<nsf;j++) 
    631563         exc[j]=0; 
    632       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf, 
    633                                  bits, stack, exc2, r, new_target, &cdbk_index, cdbk_offset, plc_tuning); 
     564      err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf, 
     565                                 bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain); 
    634566      if (err<best_err || best_err<0) 
    635567      { 
     
    643575      } 
    644576   } 
    645     
    646577   /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/ 
    647578   speex_bits_pack(bits, best_pitch-start, params->pitch_bits); 
    648579   speex_bits_pack(bits, best_gain_index, params->gain_bits); 
     580#ifdef FIXED_POINT 
     581   *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain)); 
     582#else 
     583   *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3]; 
     584#endif 
     585   /*printf ("%f\n", cumul_gain);*/ 
    649586   /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/ 
    650587   for (i=0;i<nsf;i++) 
     
    657594 
    658595void pitch_unquant_3tap( 
    659 spx_sig_t exc[],                    /* Excitation */ 
     596spx_word16_t exc[],             /* Input excitation */ 
     597spx_word32_t exc_out[],         /* Output excitation */ 
    660598int   start,                    /* Smallest pitch value allowed */ 
    661599int   end,                      /* Largest pitch value allowed */ 
    662 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */ 
     600spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */ 
    663601const void *par, 
    664602int   nsf,                      /* Number of samples in subframe */ 
     
    683621   params = (const ltp_params*) par; 
    684622   gain_cdbk_size = 1<<params->gain_bits; 
    685    gain_cdbk = params->gain_cdbk + 3*gain_cdbk_size*cdbk_offset; 
     623   gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset; 
    686624 
    687625   pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits); 
     
    690628   /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/ 
    691629#ifdef FIXED_POINT 
    692    gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*3]); 
    693    gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*3+1]); 
    694    gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*3+2]); 
    695 #else 
    696    gain[0] = 0.015625*gain_cdbk[gain_index*3]+.5; 
    697    gain[1] = 0.015625*gain_cdbk[gain_index*3+1]+.5; 
    698    gain[2] = 0.015625*gain_cdbk[gain_index*3+2]+.5; 
     630   gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]); 
     631   gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]); 
     632   gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]); 
     633#else 
     634   gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5; 
     635   gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5; 
     636   gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5; 
    699637#endif 
    700638 
     
    729667   gain_val[1]=gain[1]; 
    730668   gain_val[2]=gain[2]; 
    731  
    732    { 
    733       spx_sig_t *e[3]; 
    734       VARDECL(spx_sig_t *tmp2); 
    735       ALLOC(tmp2, 3*nsf, spx_sig_t); 
    736       e[0]=tmp2; 
    737       e[1]=tmp2+nsf; 
    738       e[2]=tmp2+2*nsf; 
    739        
    740       for (i=0;i<3;i++) 
    741       { 
    742          int j; 
    743          int pp=pitch+1-i; 
    744 #if 0 
    745          for (j=0;j<nsf;j++) 
    746          { 
    747             if (j-pp<0) 
    748                e[i][j]=exc[j-pp]; 
    749             else if (j-pp-pitch<0) 
    750                e[i][j]=exc[j-pp-pitch]; 
    751             else 
    752                e[i][j]=0; 
    753          } 
    754 #else 
    755          { 
    756             int tmp1, tmp3; 
    757             tmp1=nsf; 
    758             if (tmp1>pp) 
    759                tmp1=pp; 
    760             for (j=0;j<tmp1;j++) 
    761                e[i][j]=exc[j-pp]; 
    762             tmp3=nsf; 
    763             if (tmp3>pp+pitch) 
    764                tmp3=pp+pitch; 
    765             for (j=tmp1;j<tmp3;j++) 
    766                e[i][j]=exc[j-pp-pitch]; 
    767             for (j=tmp3;j<nsf;j++) 
    768                e[i][j]=0; 
    769          } 
    770 #endif 
    771       } 
    772  
    773 #ifdef FIXED_POINT 
    774       { 
    775          for (i=0;i<nsf;i++) 
    776             exc[i]=SHL32(ADD32(ADD32(MULT16_32_Q15(SHL16(gain[0],7),e[2][i]), MULT16_32_Q15(SHL16(gain[1],7),e[1][i])), 
    777                                MULT16_32_Q15(SHL16(gain[2],7),e[0][i])), 2); 
    778       } 
    779 #else 
    780       for (i=0;i<nsf;i++) 
    781          exc[i]=VERY_SMALL+gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i]; 
    782 #endif 
    783    } 
     669   gain[0] = SHL16(gain[0],7); 
     670   gain[1] = SHL16(gain[1],7); 
     671   gain[2] = SHL16(gain[2],7); 
     672   for (i=0;i<nsf;i++) 
     673      exc_out[i]=0; 
     674   for (i=0;i<3;i++) 
     675   { 
     676      int j; 
     677      int tmp1, tmp3; 
     678      int pp=pitch+1-i; 
     679      tmp1=nsf; 
     680      if (tmp1>pp) 
     681         tmp1=pp; 
     682      for (j=0;j<tmp1;j++) 
     683         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]); 
     684      tmp3=nsf; 
     685      if (tmp3>pp+pitch) 
     686         tmp3=pp+pitch; 
     687      for (j=tmp1;j<tmp3;j++) 
     688         exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]); 
     689   } 
     690   /*for (i=0;i<nsf;i++) 
     691   exc[i]=PSHR32(exc32[i],13);*/ 
    784692} 
    785693 
     
    787695/** Forced pitch delay and gain */ 
    788696int forced_pitch_quant( 
    789 spx_sig_t target[],                 /* Target vector */ 
    790 spx_sig_t *sw, 
     697spx_word16_t target[],                 /* Target vector */ 
     698spx_word16_t *sw, 
    791699spx_coef_t ak[],                     /* LPCs for this subframe */ 
    792700spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */ 
     
    801709SpeexBits *bits, 
    802710char *stack, 
    803 spx_sig_t *exc2, 
     711spx_word16_t *exc2, 
    804712spx_word16_t *r, 
    805713int complexity, 
    806714int cdbk_offset, 
    807 int plc_tuning 
     715int plc_tuning, 
     716spx_word32_t *cumul_gain 
    808717) 
    809718{ 
    810719   int i; 
    811    float coef = GAIN_SCALING_1*pitch_coef; 
    812    if (coef>.99) 
    813       coef=.99; 
     720   VARDECL(spx_sig_t *res); 
     721   ALLOC(res, nsf, spx_sig_t); 
     722#ifdef FIXED_POINT 
     723   if (pitch_coef>63) 
     724      pitch_coef=63; 
     725#else 
     726   if (pitch_coef>.99) 
     727      pitch_coef=.99; 
     728#endif 
     729   for (i=0;i<nsf&&i<start;i++) 
     730   { 
     731      exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]); 
     732   } 
     733   for (;i<nsf;i++) 
     734   { 
     735      exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]); 
     736   } 
     737   syn_percep_zero(exc, ak, awk1, awk2, res, nsf, p, stack); 
    814738   for (i=0;i<nsf;i++) 
    815    { 
    816       exc[i]=exc[i-start]*coef; 
    817    } 
     739      target[i]=EXTRACT16(SATURATE(SUB32(EXTEND32(target[i]),PSHR32(res[i],SIG_SHIFT-1)),32700)); 
    818740   return start; 
    819741} 
     
    821743/** Unquantize forced pitch delay and gain */ 
    822744void forced_pitch_unquant( 
    823 spx_sig_t exc[],                    /* Excitation */ 
     745spx_word16_t exc[],             /* Input excitation */ 
     746spx_word32_t exc_out[],         /* Output excitation */ 
    824747int   start,                    /* Smallest pitch value allowed */ 
    825748int   end,                      /* Largest pitch value allowed */ 
    826 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */ 
     749spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */ 
    827750const void *par, 
    828751int   nsf,                      /* Number of samples in subframe */ 
     
    838761{ 
    839762   int i; 
    840    float coef = GAIN_SCALING_1*pitch_coef; 
    841    if (coef>.99) 
    842       coef=.99; 
     763#ifdef FIXED_POINT 
     764   if (pitch_coef>63) 
     765      pitch_coef=63; 
     766#else 
     767   if (pitch_coef>.99) 
     768      pitch_coef=.99; 
     769#endif 
    843770   for (i=0;i<nsf;i++) 
    844771   { 
    845       exc[i]=exc[i-start]*coef; 
     772      exc_out[i]=MULT16_16(exc[i-start],SHL16(pitch_coef,7)); 
     773      exc[i] = PSHR(exc_out[i],13); 
    846774   } 
    847775   *pitch_val = start; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/ltp.h

    r278 r628  
    4949#endif 
    5050 
    51 void open_loop_nbest_pitch(spx_sig_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack); 
     51spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len); 
     52void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack); 
     53 
     54void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack); 
    5255 
    5356 
    5457/** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */ 
    5558int pitch_search_3tap( 
    56 spx_sig_t target[],                 /* Target vector */ 
    57 spx_sig_t *sw, 
     59spx_word16_t target[],                 /* Target vector */ 
     60spx_word16_t *sw, 
    5861spx_coef_t ak[],                     /* LPCs for this subframe */ 
    5962spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */ 
     
    6871SpeexBits *bits, 
    6972char *stack, 
    70 spx_sig_t *exc2, 
     73spx_word16_t *exc2, 
    7174spx_word16_t *r, 
    7275int   complexity, 
    7376int   cdbk_offset, 
    74 int plc_tuning 
     77int plc_tuning, 
     78spx_word32_t *cumul_gain 
    7579); 
    7680 
    7781/*Unquantize adaptive codebook and update pitch contribution*/ 
    7882void pitch_unquant_3tap( 
    79 spx_sig_t exc[],                    /* Excitation */ 
     83spx_word16_t exc[],             /* Input excitation */ 
     84spx_word32_t exc_out[],         /* Output excitation */ 
    8085int   start,                    /* Smallest pitch value allowed */ 
    8186int   end,                      /* Largest pitch value allowed */ 
    82 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */ 
     87spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */ 
    8388const void *par, 
    8489int   nsf,                      /* Number of samples in subframe */ 
     
    95100/** Forced pitch delay and gain */ 
    96101int forced_pitch_quant( 
    97 spx_sig_t target[],                 /* Target vector */ 
    98 spx_sig_t *sw, 
     102spx_word16_t target[],                 /* Target vector */ 
     103spx_word16_t *sw, 
    99104spx_coef_t ak[],                     /* LPCs for this subframe */ 
    100105spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */ 
     
    109114SpeexBits *bits, 
    110115char *stack, 
    111 spx_sig_t *exc2, 
     116spx_word16_t *exc2, 
    112117spx_word16_t *r, 
    113118int complexity, 
    114119int cdbk_offset, 
    115 int plc_tuning 
     120int plc_tuning, 
     121spx_word32_t *cumul_gain 
    116122); 
    117123 
    118124/** Unquantize forced pitch delay and gain */ 
    119125void forced_pitch_unquant( 
    120 spx_sig_t exc[],                    /* Excitation */ 
     126spx_word16_t exc[],             /* Input excitation */ 
     127spx_word32_t exc_out[],         /* Output excitation */ 
    121128int   start,                    /* Smallest pitch value allowed */ 
    122129int   end,                      /* Largest pitch value allowed */ 
    123 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */ 
     130spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */ 
    124131const void *par, 
    125132int   nsf,                      /* Number of samples in subframe */ 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/ltp_arm4.h

    r278 r628  
    3434 
    3535#define OVERRIDE_INNER_PROD 
    36 static spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
     36spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
    3737{ 
    3838   spx_word32_t sum1=0,sum2=0; 
     
    8585 
    8686#define OVERRIDE_PITCH_XCORR 
    87 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
     87void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
    8888{ 
    8989   int i,j; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/ltp_bfin.h

    r278 r628  
    3535 
    3636#define OVERRIDE_INNER_PROD 
    37 static spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
     37spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
    3838{ 
    3939   spx_word32_t sum=0; 
     
    6464 
    6565#define OVERRIDE_PITCH_XCORR 
    66 static void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
     66void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack) 
    6767{ 
    6868   corr += nb_pitch - 1; 
     
    110110 
    111111#define OVERRIDE_COMPUTE_PITCH_ERROR 
    112 static inline spx_word32_t compute_pitch_error(spx_word32_t *C, spx_word16_t *g, spx_word16_t pitch_control) 
     112static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control) 
    113113{ 
    114114   spx_word32_t sum; 
    115115   __asm__ __volatile__ 
    116116         ( 
    117          "A1 = A0 = 0;\n\t" 
    118           
    119          "R0 = [%1++];\n\t" 
     117         "A0 = 0;\n\t" 
     118          
     119         "R0 = W[%1++];\n\t" 
    120120         "R1.L = %2.L*%5.L (IS);\n\t" 
    121          "R0 <<= 1;\n\t" 
    122          "A1 += R1.L*R0.L (M), A0 += R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     121         "A0 += R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    123122          
    124123         "R1.L = %3.L*%5.L (IS);\n\t" 
    125          "R0 <<= 1;\n\t" 
    126          "A1 += R1.L*R0.L (M), A0 += R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     124         "A0 += R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    127125          
    128126         "R1.L = %4.L*%5.L (IS);\n\t" 
    129          "R0 <<= 1;\n\t" 
    130          "A1 += R1.L*R0.L (M), A0 += R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     127         "A0 += R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    131128          
    132129         "R1.L = %2.L*%3.L (IS);\n\t" 
    133          "R0 <<= 1;\n\t" 
    134          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     130         "A0 -= R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    135131 
    136132         "R1.L = %4.L*%3.L (IS);\n\t" 
    137          "R0 <<= 1;\n\t" 
    138          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     133         "A0 -= R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    139134          
    140135         "R1.L = %4.L*%2.L (IS);\n\t" 
    141          "R0 <<= 1;\n\t" 
    142          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     136         "A0 -= R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    143137          
    144138         "R1.L = %2.L*%2.L (IS);\n\t" 
    145          "R0 <<= 1;\n\t" 
    146          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     139         "A0 -= R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    147140 
    148141         "R1.L = %3.L*%3.L (IS);\n\t" 
    149          "R0 <<= 1;\n\t" 
    150          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS) || R0 = [%1++];\n\t" 
     142         "A0 -= R1.L*R0.L (IS) || R0 = W[%1++];\n\t" 
    151143          
    152144         "R1.L = %4.L*%4.L (IS);\n\t" 
    153          "R0 <<= 1;\n\t" 
    154          "A1 -= R1.L*R0.L (M), A0 -= R1.L*R0.H (IS);\n\t" 
    155           
    156          "A1 = A1 >>> 16;\n\t" 
    157          "A0 += A1;\n\t" 
     145         "A0 -= R1.L*R0.L (IS);\n\t" 
     146          
    158147         "%0 = A0;\n\t" 
    159148   : "=&D" (sum), "=a" (C) 
     
    164153} 
    165154 
     155#define OVERRIDE_OPEN_LOOP_NBEST_PITCH 
     156#ifdef OVERRIDE_OPEN_LOOP_NBEST_PITCH 
     157void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack) 
     158{ 
     159   int i,j,k; 
     160   VARDECL(spx_word32_t *best_score); 
     161   VARDECL(spx_word32_t *best_ener); 
     162   spx_word32_t e0; 
     163   VARDECL(spx_word32_t *corr); 
     164   VARDECL(spx_word32_t *energy); 
     165 
     166   ALLOC(best_score, N, spx_word32_t); 
     167   ALLOC(best_ener, N, spx_word32_t); 
     168   ALLOC(corr, end-start+1, spx_word32_t); 
     169   ALLOC(energy, end-start+2, spx_word32_t); 
     170 
     171   for (i=0;i<N;i++) 
     172   { 
     173        best_score[i]=-1; 
     174        best_ener[i]=0; 
     175        pitch[i]=start; 
     176   } 
     177 
     178   energy[0]=inner_prod(sw-start, sw-start, len); 
     179   e0=inner_prod(sw, sw, len); 
     180 
     181   /* energy update -------------------------------------*/ 
     182 
     183      __asm__ __volatile__ 
     184      ( 
     185"        P0 = %0;\n\t" 
     186"        I1 = %1;\n\t" 
     187"        L1 = 0;\n\t" 
     188"        I2 = %2;\n\t" 
     189"        L2 = 0;\n\t" 
     190"        R2 = [P0++];\n\t" 
     191"        R3 = 0;\n\t" 
     192"        LSETUP (eu1, eu2) LC1 = %3;\n\t" 
     193"eu1:      R1.L = W [I1--] || R0.L = W [I2--] ;\n\t" 
     194"          R1 = R1.L * R1.L (IS);\n\t" 
     195"          R0 = R0.L * R0.L (IS);\n\t" 
     196"          R1 >>>= 6;\n\t" 
     197"          R1 = R1 + R2;\n\t" 
     198"          R0 >>>= 6;\n\t" 
     199"          R1 = R1 - R0;\n\t" 
     200"          R2 = MAX(R1,R3);\n\t" 
     201"eu2:      [P0++] = R2;\n\t" 
     202       : : "d" (energy), "d" (&sw[-start-1]), "d" (&sw[-start+len-1]), 
     203           "a" (end-start)   
     204       : "P0", "I1", "I2", "R0", "R1", "R2", "R3" 
     205#if (__GNUC__ == 4) 
     206         , "LC1" 
     207#endif 
     208       ); 
     209 
     210   pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack); 
     211 
     212   /* FIXME: Fixed-point and floating-point code should be merged */ 
     213   { 
     214      VARDECL(spx_word16_t *corr16); 
     215      VARDECL(spx_word16_t *ener16); 
     216      ALLOC(corr16, end-start+1, spx_word16_t); 
     217      ALLOC(ener16, end-start+1, spx_word16_t); 
     218      /* Normalize to 180 so we can square it and it still fits in 16 bits */ 
     219      normalize16(corr, corr16, 180, end-start+1); 
     220      normalize16(energy, ener16, 180, end-start+1); 
     221 
     222      if (N == 1) { 
     223        /* optimised asm to handle N==1 case */ 
     224      __asm__ __volatile__ 
     225      ( 
     226"        I0 = %1;\n\t"                     /* I0: corr16[]    */ 
     227"        L0 = 0;\n\t" 
     228"        I1 = %2;\n\t"                     /* I1: energy      */ 
     229"        L1 = 0;\n\t" 
     230"        R2 = -1;\n\t"                     /* R2: best score  */ 
     231"        R3 = 0;\n\t"                      /* R3: best energy */ 
     232"        P0 = %4;\n\t"                     /* P0: best pitch  */ 
     233"        P1 = %4;\n\t"                     /* P1: counter     */ 
     234"        LSETUP (sl1, sl2) LC1 = %3;\n\t" 
     235"sl1:      R0.L = W [I0++] || R1.L = W [I1++];\n\t"          
     236"          R0 = R0.L * R0.L (IS);\n\t" 
     237"          R1   += 1;\n\t" 
     238"          R4   = R0.L * R3.L;\n\t" 
     239"          R5   = R2.L * R1.L;\n\t" 
     240"          cc   = R5 < R4;\n\t" 
     241"          if cc R2 = R0;\n\t" 
     242"          if cc R3 = R1;\n\t" 
     243"          if cc P0 = P1;\n\t" 
     244"sl2:      P1 += 1;\n\t" 
     245"        %0 = P0;\n\t" 
     246       : "=&d" (pitch[0]) 
     247       : "a" (corr16), "a" (ener16), "a" (end+1-start), "d" (start)  
     248       : "P0", "P1", "I0", "I1", "R0", "R1", "R2", "R3", "R4", "R5" 
     249#if (__GNUC__ == 4) 
     250         , "LC1" 
     251#endif 
     252       ); 
     253 
     254      } 
     255      else { 
     256        for (i=start;i<=end;i++) 
     257          { 
     258            spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]); 
     259            /* Instead of dividing the tmp by the energy, we multiply on the other side */ 
     260            if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start]))) 
     261              { 
     262                /* We can safely put it last and then check */ 
     263                best_score[N-1]=tmp; 
     264                best_ener[N-1]=ener16[i-start]+1; 
     265                pitch[N-1]=i; 
     266                /* Check if it comes in front of others */ 
     267                for (j=0;j<N-1;j++) 
     268                  { 
     269                    if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start]))) 
     270                      { 
     271                        for (k=N-1;k>j;k--) 
     272                          { 
     273                            best_score[k]=best_score[k-1]; 
     274                            best_ener[k]=best_ener[k-1]; 
     275                            pitch[k]=pitch[k-1]; 
     276                          } 
     277                        best_score[j]=tmp; 
     278                        best_ener[j]=ener16[i-start]+1; 
     279                        pitch[j]=i; 
     280                        break; 
     281                      } 
     282                  } 
     283              } 
     284          } 
     285      } 
     286   } 
     287 
     288   /* Compute open-loop gain */ 
     289   if (gain) 
     290   { 
     291       for (j=0;j<N;j++) 
     292       { 
     293          spx_word16_t g; 
     294          i=pitch[j]; 
     295          g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6)); 
     296          /* FIXME: g = max(g,corr/energy) */ 
     297                   if (g<0) 
     298                   g = 0; 
     299             gain[j]=g; 
     300       } 
     301   } 
     302} 
     303#endif 
     304 
     305#define OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ 
     306#ifdef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ 
     307static int pitch_gain_search_3tap_vq( 
     308  const signed char *gain_cdbk, 
     309  int                gain_cdbk_size, 
     310  spx_word16_t      *C16, 
     311  spx_word16_t       max_gain 
     312) 
     313{ 
     314  const signed char *ptr=gain_cdbk; 
     315  int                best_cdbk=0; 
     316  spx_word32_t       best_sum=-VERY_LARGE32; 
     317  spx_word32_t       sum=0; 
     318  spx_word16_t       g[3]; 
     319  spx_word16_t       pitch_control=64; 
     320  spx_word16_t       gain_sum; 
     321  int                i; 
     322 
     323      /* fast asm version of VQ codebook search */ 
     324 
     325      __asm__ __volatile__ 
     326      ( 
     327 
     328"        P0 = %2;\n\t"                     /* P0: ptr to gain_cdbk */ 
     329"        L1 = 0;\n\t"                      /* no circ addr for L1  */ 
     330"        %0 = 0;\n\t"                      /* %0: best_sum         */ 
     331"        %1 = 0;\n\t"                      /* %1: best_cbdk        */ 
     332"        P1 = 0;\n\t"                      /* P1: loop counter     */ 
     333"        R5 = 64;\n\t"                     /* R5: pitch_control    */ 
     334 
     335"        LSETUP (pgs1, pgs2) LC1 = %4;\n\t" 
     336"pgs1:     R2  = B [P0++] (X);\n\t"        /* R2: g[0]             */ 
     337"          R3  = B [P0++] (X);\n\t"        /* R3: g[1]             */ 
     338"          R4  = B [P0++] (X);\n\t"        /* R4: g[2]             */ 
     339"          R2 += 32;\n\t" 
     340"          R3 += 32;\n\t" 
     341"          R4 += 32;\n\t" 
     342 
     343"          R0  = B [P0++] (X);\n\t"               
     344"          B0  = R0;\n\t"                  /* BO: gain_sum         */ 
     345           
     346           /* compute_pitch_error() -------------------------------*/ 
     347 
     348"          I1 = %3;\n\t"                   /* I1: ptr to C         */ 
     349"          A0 = 0;\n\t" 
     350          
     351"          R0.L = W[I1++];\n\t" 
     352"          R1.L = R2.L*R5.L (IS);\n\t" 
     353"          A0 += R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     354          
     355"          R1.L = R3.L*R5.L (IS);\n\t" 
     356"          A0 += R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     357          
     358"          R1.L = R4.L*R5.L (IS);\n\t" 
     359"          A0 += R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     360          
     361"          R1.L = R2.L*R3.L (IS);\n\t" 
     362"          A0 -= R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     363 
     364"          R1.L = R4.L*R3.L (IS);\n\t" 
     365"          A0 -= R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     366          
     367"          R1.L = R4.L*R2.L (IS);\n\t" 
     368"          A0 -= R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     369          
     370"          R1.L = R2.L*R2.L (IS);\n\t" 
     371"          A0 -= R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     372 
     373"          R1.L = R3.L*R3.L (IS);\n\t" 
     374"          A0 -= R1.L*R0.L (IS) || R0.L = W[I1++];\n\t" 
     375          
     376"          R1.L = R4.L*R4.L (IS);\n\t" 
     377"          R0 = (A0 -= R1.L*R0.L) (IS);\n\t" 
     378 
     379/* 
     380    Re-arrange the if-then to code efficiently on the Blackfin: 
     381 
     382      if (sum>best_sum && gain_sum<=max_gain)   ------ (1) 
     383 
     384      if (sum>best_sum && !(gain_sum>max_gain)) ------ (2) 
     385 
     386      if (max_gain<=gain_sum) {                 ------ (3) 
     387      sum = -VERY_LARGE32; 
     388      } 
     389      if (best_sum<=sum) 
     390 
     391    The blackin cc instructions are all of the form: 
     392 
     393      cc = x < y (or cc = x <= y) 
     394*/ 
     395"          R1 = B0\n\t" 
     396"          R2 = %5\n\t" 
     397"          R3 = %6\n\t" 
     398"          cc = R2 <= R1;\n\t"  
     399"          if cc R0 = R3;\n\t" 
     400"          cc = %0 <= R0;\n\t" 
     401"          if cc %0 = R0;\n\t" 
     402"          if cc %1 = P1;\n\t" 
     403 
     404"pgs2:     P1 += 1;\n\t" 
     405    
     406       : "=&d" (best_sum), "=&d" (best_cdbk)  
     407       : "a" (gain_cdbk), "a" (C16), "a" (gain_cdbk_size), "a" (max_gain), 
     408         "b" (-VERY_LARGE32) 
     409       : "R0", "R1", "R2", "R3", "R4", "R5", "P0",  
     410         "P1", "I1", "L1", "A0", "B0" 
     411#if (__GNUC__ == 4) 
     412         , "LC1" 
     413#endif 
     414       ); 
     415 
     416  return best_cdbk; 
     417} 
     418#endif 
     419 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/ltp_sse.h

    r278 r628  
    3636 
    3737#define OVERRIDE_INNER_PROD 
    38 static float inner_prod(const float *a, const float *b, int len) 
     38float inner_prod(const float *a, const float *b, int len) 
    3939{ 
    4040   int i; 
     
    5555 
    5656#define OVERRIDE_PITCH_XCORR 
    57 static void pitch_xcorr(const float *_x, const float *_y, float *corr, int len, int nb_pitch, char *stack) 
     57void pitch_xcorr(const float *_x, const float *_y, float *corr, int len, int nb_pitch, char *stack) 
    5858{ 
    5959   int i, offset; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/mdf.c

    r523 r628  
    9191 
    9292#ifdef FIXED_POINT 
    93 static const spx_float_t MIN_LEAK = ((spx_float_t){16777, -24}); 
     93static const spx_float_t MIN_LEAK = {16777, -24}; 
    9494#define TOP16(x) ((x)>>16) 
    9595#else 
     
    141141   spx_word16_t notch_radius; 
    142142   spx_mem_t notch_mem[2]; 
     143 
     144   /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */ 
     145   spx_int16_t *play_buf; 
     146   int play_buf_pos; 
    143147}; 
    144148 
    145 static inline void filter_dc_notch16(spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem) 
     149static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem) 
    146150{ 
    147151   int i; 
     
    167171} 
    168172 
    169 static inline spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
     173static inline spx_word32_t mdf_inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len) 
    170174{ 
    171175   spx_word32_t sum=0; 
    172    len >>= 2; 
     176   len >>= 1; 
    173177   while(len--) 
    174178   { 
    175179      spx_word32_t part=0; 
    176       part = MAC16_16(part,*x++,*y++); 
    177       part = MAC16_16(part,*x++,*y++); 
    178180      part = MAC16_16(part,*x++,*y++); 
    179181      part = MAC16_16(part,*x++,*y++); 
     
    185187 
    186188/** Compute power spectrum of a half-complex (packed) vector */ 
    187 static inline void power_spectrum(spx_word16_t *X, spx_word32_t *ps, int N) 
     189static inline void power_spectrum(const spx_word16_t *X, spx_word32_t *ps, int N) 
    188190{ 
    189191   int i, j; 
     
    198200/** Compute cross-power spectrum of a half-complex (packed) vectors and add to acc */ 
    199201#ifdef FIXED_POINT 
    200 static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M) 
     202static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M) 
    201203{ 
    202204   int i,j; 
     
    226228} 
    227229#else 
    228 static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M) 
     230static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M) 
    229231{ 
    230232   int i,j; 
     
    247249 
    248250/** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */ 
    249 static inline void weighted_spectral_mul_conj(spx_float_t *w, spx_word16_t *X, spx_word16_t *Y, spx_word32_t *prod, int N) 
     251static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N) 
    250252{ 
    251253   int i, j; 
     
    274276   /* FIXME: Make that an init option (new API call?) */ 
    275277   st->sampling_rate = 8000; 
    276    st->spec_average = DIV32_16(SHL32(st->frame_size, 15), st->sampling_rate); 
    277 #ifdef FIXED_POINT 
    278    st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate); 
    279    st->beta_max = DIV32_16(SHL32(st->frame_size, 14), st->sampling_rate); 
     278   st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate); 
     279#ifdef FIXED_POINT 
     280   st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate); 
     281   st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate); 
    280282#else 
    281283   st->beta0 = (2.0f*st->frame_size)/st->sampling_rate; 
     
    333335   st->adapted = 0; 
    334336   st->Pey = st->Pyy = FLOAT_ONE; 
     337    
     338   st->play_buf = (spx_int16_t*)speex_alloc(2*st->frame_size*sizeof(spx_int16_t)); 
     339   st->play_buf_pos = 0; 
     340 
    335341   return st; 
    336342} 
     
    386392   speex_free(st->wtmp2); 
    387393#endif 
     394   speex_free(st->play_buf); 
    388395   speex_free(st); 
    389396} 
    390397 
    391 extern int fixed_point; 
     398void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout) 
     399{ 
     400   int i; 
     401   if (st->play_buf_pos>=st->frame_size) 
     402   { 
     403      speex_echo_cancel(st, rec, st->play_buf, out, Yout); 
     404      st->play_buf_pos -= st->frame_size; 
     405      for (i=0;i<st->frame_size;i++) 
     406         st->play_buf[i] = st->play_buf[i+st->frame_size]; 
     407   } else { 
     408      speex_warning("no playback frame available"); 
     409      if (st->play_buf_pos!=0) 
     410      { 
     411         speex_warning("internal playback buffer corruption?"); 
     412         st->play_buf_pos = 0; 
     413      } 
     414      for (i=0;i<st->frame_size;i++) 
     415         out[i] = rec[i]; 
     416   } 
     417} 
     418 
     419void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play) 
     420{ 
     421   if (st->play_buf_pos<=st->frame_size) 
     422   { 
     423      int i; 
     424      for (i=0;i<st->frame_size;i++) 
     425         st->play_buf[st->play_buf_pos+i] = play[i]; 
     426      st->play_buf_pos += st->frame_size; 
     427   } else { 
     428      speex_warning("had to discard a playback frame"); 
     429   } 
     430} 
     431 
    392432/** Performs echo cancellation on a frame */ 
    393 void speex_echo_cancel(SpeexEchoState *st, short *ref, short *echo, short *out, spx_int32_t *Yout) 
     433void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *ref, const spx_int16_t *echo, spx_int16_t *out, spx_int32_t *Yout) 
    394434{ 
    395435   int i,j; 
     
    403443   spx_word32_t tmp32; 
    404444   spx_word16_t M_1; 
     445   int saturated=0; 
    405446    
    406447   N = st->window_size; 
     
    417458#endif 
    418459 
    419    filter_dc_notch16((spx_int16_t*)ref, st->notch_radius, st->d, st->frame_size, st->notch_mem); 
     460   filter_dc_notch16(ref, st->notch_radius, st->d, st->frame_size, st->notch_mem); 
    420461   /* Copy input data to buffer */ 
    421462   for (i=0;i<st->frame_size;i++) 
    422463   { 
    423464      spx_word16_t tmp; 
     465      spx_word32_t tmp32; 
    424466      st->x[i] = st->x[i+st->frame_size]; 
    425       st->x[i+st->frame_size] = SUB16(echo[i], MULT16_16_P15(st->preemph, st->memX)); 
     467      tmp32 = SUB32(EXTEND32(echo[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX))); 
     468#ifdef FIXED_POINT 
     469      /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */ 
     470      if (tmp32 > 32767) 
     471      { 
     472         tmp32 = 32767; 
     473         saturated = 1; 
     474      }       
     475      if (tmp32 < -32767) 
     476      { 
     477         tmp32 = -32767; 
     478         saturated = 1; 
     479      }       
     480#endif 
     481      st->x[i+st->frame_size] = EXTRACT16(tmp32); 
    426482      st->memX = echo[i]; 
    427483       
    428484      tmp = st->d[i]; 
    429485      st->d[i] = st->d[i+st->frame_size]; 
    430       st->d[i+st->frame_size] = SUB16(tmp, MULT16_16_P15(st->preemph, st->memD)); 
     486      tmp32 = SUB32(EXTEND32(tmp), EXTEND32(MULT16_16_P15(st->preemph, st->memD))); 
     487#ifdef FIXED_POINT 
     488      if (tmp32 > 32767) 
     489      { 
     490         tmp32 = 32767; 
     491         saturated = 1; 
     492      }       
     493      if (tmp32 < -32767) 
     494      { 
     495         tmp32 = -32767; 
     496         saturated = 1; 
     497      } 
     498#endif 
     499      st->d[i+st->frame_size] = tmp32; 
    431500      st->memD = tmp; 
    432501   } 
     
    466535         tmp_out = -32768; 
    467536      tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE))); 
     537      /* This is an arbitrary test for saturation */ 
     538      if (ref[i] <= -32000 || ref[i] >= 32000) 
     539      { 
     540         tmp_out = 0; 
     541         saturated = 1; 
     542      } 
    468543      out[i] = tmp_out; 
    469544      st->memE = tmp_out; 
     
    478553 
    479554   /* Compute a bunch of correlations */ 
    480    See = inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size); 
    481    See = ADD32(See, SHR32(10000,6)); 
    482    Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size); 
     555   See = mdf_inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size); 
     556   See = ADD32(See, SHR32(EXTEND32(10000),6)); 
     557   Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size); 
    483558    
    484559   /* Convert error to frequency domain */ 
     
    545620   if (FLOAT_GT(st->Pey, st->Pyy)) 
    546621      st->Pey = st->Pyy; 
    547    /* leak_estimate is the limear regression result */ 
     622   /* leak_estimate is the linear regression result */ 
    548623   leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14)); 
     624   /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */ 
    549625   if (leak_estimate > 16383) 
    550626      leak_estimate = 32767; 
     
    595671      spx_word16_t adapt_rate=0; 
    596672 
    597       Sxx = inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size); 
     673      Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size); 
    598674      /* Temporary adaption rate if filter is not adapted correctly */ 
    599675 
     
    621697   } 
    622698 
    623    /* Gradient descent */ 
    624    for (i=0;i<M*N;i++) 
    625    { 
    626       st->W[i] += st->PHI[i]; 
    627       /* Old value of W in PHI */ 
    628       st->PHI[i] = st->W[i] - st->PHI[i]; 
     699   if (!saturated) 
     700   { 
     701      /* Gradient descent */ 
     702      for (i=0;i<M*N;i++) 
     703      { 
     704         st->W[i] += st->PHI[i]; 
     705         /* Old value of W in PHI */ 
     706         st->PHI[i] = st->W[i] - st->PHI[i]; 
     707      } 
    629708   } 
    630709    
     
    638717#ifdef FIXED_POINT 
    639718         for (i=0;i<N;i++) 
    640             st->wtmp2[i] = PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16); 
     719            st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16)); 
    641720         spx_ifft(st->fft_table, st->wtmp2, st->wtmp); 
    642721         for (i=0;i<st->frame_size;i++) 
     
    646725         for (i=st->frame_size;i<N;i++) 
    647726         { 
    648             st->wtmp[i]=SHL(st->wtmp[i],NORMALIZE_SCALEUP); 
     727            st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP); 
    649728         } 
    650729         spx_fft(st->fft_table, st->wtmp, st->wtmp2); 
    651730         /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */ 
    652731         for (i=0;i<N;i++) 
    653             st->W[j*N+i] -= SHL32(st->wtmp2[i],16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); 
     732            st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1); 
    654733#else 
    655734         spx_ifft(st->fft_table, &st->W[j*N], st->wtmp); 
     
    716795      case SPEEX_ECHO_SET_SAMPLING_RATE: 
    717796         st->sampling_rate = (*(int*)ptr); 
    718          st->spec_average = DIV32_16(SHL32(st->frame_size, 15), st->sampling_rate); 
    719 #ifdef FIXED_POINT 
    720          st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate); 
    721          st->beta_max = DIV32_16(SHL32(st->frame_size, 14), st->sampling_rate); 
     797         st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate); 
     798#ifdef FIXED_POINT 
     799         st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate); 
     800         st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate); 
    722801#else 
    723802         st->beta0 = (2.0f*st->frame_size)/st->sampling_rate; 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/misc.c

    r278 r628  
    197197 
    198198#ifdef FIXED_POINT 
    199 spx_word32_t speex_rand(spx_word16_t std, spx_int32_t *seed) 
     199spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed) 
    200200{ 
    201201   spx_word32_t res; 
    202202   *seed = 1664525 * *seed + 1013904223; 
    203203   res = MULT16_16(EXTRACT16(SHR32(*seed,16)),std); 
    204    return SUB32(res, SHR(res, 3)); 
     204   return PSHR32(SUB32(res, SHR(res, 3)),14); 
    205205} 
    206206#else 
     
    217217#endif 
    218218 
    219 void speex_rand_vec(float std, spx_sig_t *data, int len) 
    220 { 
    221    int i; 
    222    for (i=0;i<len;i++) 
    223       data[i]+=SIG_SCALING*3*std*((((float)rand())/RAND_MAX)-.5); 
    224 } 
    225  
    226  
    227 /*float speex_rand(float std) 
    228 { 
    229    return 3*std*((((float)rand())/RAND_MAX)-.5); 
    230 }*/ 
    231  
    232219#ifndef OVERRIDE_SPEEX_PUTC 
    233220void _speex_putc(int ch, void *file) 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/misc.h

    r278 r628  
    3939#define SPEEX_MAJOR_VERSION 1         /**< Major Speex version. */ 
    4040#define SPEEX_MINOR_VERSION 1         /**< Minor Speex version. */ 
    41 #define SPEEX_MICRO_VERSION 12        /**< Micro Speex version. */ 
     41#define SPEEX_MICRO_VERSION 13        /**< Micro Speex version. */ 
    4242#define SPEEX_EXTRA_VERSION ""        /**< Extra Speex version. */ 
    43 #define SPEEX_VERSION "speex-1.1.12"  /**< Speex version string. */ 
     43#define SPEEX_VERSION "speex-1.1.13"  /**< Speex version string. */ 
     44#endif 
     45 
     46/* A couple test to catch stupid option combinations */ 
     47#ifdef FIXED_POINT 
     48 
     49#ifdef _USE_SSE 
     50#error SSE is only for floating-point 
     51#endif 
     52#if ((defined (ARM4_ASM)||defined (ARM4_ASM)) && defined(BFIN_ASM)) || (defined (ARM4_ASM)&&defined(ARM5E_ASM)) 
     53#error Make up your mind. What CPU do you have? 
     54#endif 
     55#ifdef VORBIS_PSYCHO 
     56#error Vorbis-psy model currently not implemented in fixed-point 
     57#endif 
     58 
     59#else 
     60 
     61#if defined (ARM4_ASM) || defined(ARM5E_ASM) || defined(BFIN_ASM) 
     62#error I suppose you can have a [ARM4/ARM5E/Blackfin] that has float instructions? 
     63#endif 
     64#ifdef FIXED_POINT_DEBUG 
     65#error Don't you think enabling fixed-point is a good thing to do if you want to debug that? 
     66#endif 
     67 
     68 
    4469#endif 
    4570 
     
    89114void speex_warning_int(const char *str, int val); 
    90115 
    91 /** Generate a vector of random numbers */ 
    92 void speex_rand_vec(float std, spx_sig_t *data, int len); 
    93  
    94116/** Generate a random number */ 
    95 spx_word32_t speex_rand(spx_word16_t std, spx_int32_t *seed); 
     117spx_word16_t speex_rand(spx_word16_t std, spx_int32_t *seed); 
    96118 
    97119/** Speex wrapper for putc */ 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/modes.c

    r278 r628  
    191191   noise_codebook_unquant, 
    192192   NULL, 
    193 #ifdef FIXED_POINT 
    194    22938, 22938, 0, -1, 
    195 #else 
    196    .7, .7, 0, -1, 
    197 #endif 
     193   -1, 
    198194   43 
    199195}; 
     
    216212   split_cb_shape_sign_unquant, 
    217213   &split_cb_nb_ulbr, 
    218 #ifdef FIXED_POINT 
    219    22938, 16384, 11796, 21299, 
    220 #else 
    221    0.7, 0.5, .36, .65, 
    222 #endif 
     214   QCONST16(.65,15), 
    223215   79 
    224216}; 
     
    241233   split_cb_shape_sign_unquant, 
    242234   &split_cb_nb_vlbr, 
    243 #ifdef FIXED_POINT 
    244    22938, 16384, 11796, 18022, 
    245 #else 
    246    0.7, 0.5, .36, .55, 
    247 #endif 
     235   QCONST16(.55,15), 
    248236   119 
    249237}; 
     
    266254   split_cb_shape_sign_unquant, 
    267255   &split_cb_nb_lbr, 
    268 #ifdef FIXED_POINT 
    269    22938, 18022, 9830, 14746, 
    270 #else 
    271    0.7, 0.55, .30, .45, 
    272 #endif 
     256   QCONST16(.45,15), 
    273257   160 
    274258}; 
     
    291275   split_cb_shape_sign_unquant, 
    292276   &split_cb_nb_med, 
    293 #ifdef FIXED_POINT 
    294    22938, 20644, 5243, 11469, 
    295 #else 
    296    0.7, 0.63, .16, .35, 
    297 #endif 
     277   QCONST16(.35,15), 
    298278   220 
    299279}; 
     
    316296   split_cb_shape_sign_unquant, 
    317297   &split_cb_nb, 
    318 #ifdef FIXED_POINT 
    319    22938, 21299, 3932, 8192, 
    320 #else 
    321    0.7, 0.65, .12, .25, 
    322 #endif 
     298   QCONST16(.2,15), 
    323299   300 
    324300}; 
     
    341317   split_cb_shape_sign_unquant, 
    342318   &split_cb_sb, 
    343 #ifdef FIXED_POINT 
    344    22282, 21299, 2294, 3277, 
    345 #else 
    346    0.68, 0.65, .07, .1, 
    347 #endif 
     319   QCONST16(.1,15), 
    348320   364 
    349321}; 
     
    366338   split_cb_shape_sign_unquant, 
    367339   &split_cb_nb, 
    368 #ifdef FIXED_POINT 
    369    21299, 21299, 0, -1, 
    370 #else 
    371    0.65, 0.65, .0, -1, 
    372 #endif 
     340   -1, 
    373341   492 
    374342}; 
     
    435403   NULL, 
    436404   NULL, 
    437 #ifdef FIXED_POINT 
    438    24576, 24576, 0, -1, 
    439 #else 
    440    .75, .75, .0, -1, 
    441 #endif 
     405   -1, 
    442406   36 
    443407}; 
     
    464428   &split_cb_high_lbr, 
    465429#endif 
    466 #ifdef FIXED_POINT 
    467    27853, 19661, 8192, -1, 
    468 #else 
    469    .85, .6, .25, -1, 
    470 #endif 
     430   -1, 
    471431   112 
    472432}; 
     
    493453   &split_cb_high, 
    494454#endif 
    495  
    496 #ifdef FIXED_POINT 
    497    24576, 22938, 1638, -1, 
    498 #else 
    499    .75, .7, .05, -1, 
    500 #endif 
     455   -1, 
    501456   192 
    502457}; 
     
    522477   &split_cb_high, 
    523478#endif 
    524 #ifdef FIXED_POINT 
    525    24576, 24576, 0, -1, 
    526 #else 
    527    .75, .75, .0, -1, 
    528 #endif 
     479   -1, 
    529480   352 
    530481}; 
     
    543494   0.9, 0.6, /* gamma1, gamma2 */ 
    544495#endif 
    545    .001,   /*lag_factor*/ 
    546    QCONST16(.0001,15), /*lpc_floor*/ 
     496   .012,   /*lag_factor*/ 
     497   QCONST16(.0002,15), /*lpc_floor*/ 
    547498   0.9, 
    548499   {NULL, &wb_submode1, &wb_submode2, &wb_submode3, &wb_submode4, NULL, NULL, NULL}, 
     
    589540   0.9, 0.6, /* gamma1, gamma2 */ 
    590541#endif 
    591    .002,   /*lag_factor*/ 
    592    QCONST16(.0001,15), /*lpc_floor*/ 
     542   .012,   /*lag_factor*/ 
     543   QCONST16(.0002,15), /*lpc_floor*/ 
    593544   0.7, 
    594545   {NULL, &wb_submode1, NULL, NULL, NULL, NULL, NULL, NULL}, 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/modes.h

    r278 r628  
    5656 
    5757/** Long-term predictor quantization */ 
    58 typedef int (*ltp_quant_func)(spx_sig_t *, spx_sig_t *, spx_coef_t *, spx_coef_t *,  
     58typedef int (*ltp_quant_func)(spx_word16_t *, spx_word16_t *, spx_coef_t *, spx_coef_t *,  
    5959                              spx_coef_t *, spx_sig_t *, const void *, int, int, spx_word16_t,  
    60                               int, int, SpeexBits*, char *, spx_sig_t *, spx_word16_t *, int, int, int); 
     60                              int, int, SpeexBits*, char *, spx_word16_t *, spx_word16_t *, int, int, int, spx_word32_t *); 
    6161 
    6262/** Long-term un-quantize */ 
    63 typedef void (*ltp_unquant_func)(spx_sig_t *, int, int, spx_word16_t, const void *, int, int *, 
     63typedef void (*ltp_unquant_func)(spx_word16_t *, spx_word32_t *, int, int, spx_word16_t, const void *, int, int *, 
    6464                                 spx_word16_t *, SpeexBits*, char*, int, int, spx_word16_t, int); 
    6565 
    6666 
    6767/** Innovation quantization function */ 
    68 typedef void (*innovation_quant_func)(spx_sig_t *, spx_coef_t *, spx_coef_t *, spx_coef_t *, const void *, int, int,  
     68typedef void (*innovation_quant_func)(spx_word16_t *, spx_coef_t *, spx_coef_t *, spx_coef_t *, const void *, int, int,  
    6969                                      spx_sig_t *, spx_word16_t *, SpeexBits *, char *, int, int); 
    7070 
     
    8585   ltp_quant_func    ltp_quant; /**< Long-term predictor (pitch) quantizer */ 
    8686   ltp_unquant_func  ltp_unquant; /**< Long-term predictor (pitch) un-quantizer */ 
    87    const void             *ltp_params; /**< Pitch parameters (options) */ 
     87   const void       *ltp_params; /**< Pitch parameters (options) */ 
    8888 
    8989   /*Quantization of innovation*/ 
     
    9292   const void             *innovation_params; /**< Innovation quantization parameters*/ 
    9393 
    94    /*Synthesis filter enhancement*/ 
    95    spx_word16_t      lpc_enh_k1; /**< Enhancer constant */ 
    96    spx_word16_t      lpc_enh_k2; /**< Enhancer constant */ 
    97    spx_word16_t      lpc_enh_k3; /**< Enhancer constant */ 
    9894   spx_word16_t      comb_gain;  /**< Gain of enhancer comb filter */ 
    9995 
  • pjproject/trunk/pjmedia/src/pjmedia-codec/speex/nb_celp.c

    r278 r628  
    4747#include "vbr.h" 
    4848#include "misc.h" 
     49#include "math_approx.h" 
    4950#include <speex/speex_callbacks.h> 
    5051 
     
    106107 
    107108#define sqr(x) ((x)*(x)) 
     109 
     110extern const spx_word16_t lpc_window[]; 
    108111 
    109112void *nb_encoder_init(const SpeexMode *m) 
     
    126129 
    127130   st->frameSize = mode->frameSize; 
    128    st->windowSize = st->frameSize*3/2; 
    129131   st->nbSubframes=mode->frameSize/mode->subframeSize; 
    130132   st->subframeSize=mode->subframeSize; 
     133   st->windowSize = st->frameSize+st->subframeSize; 
    131134   st->lpcSize = mode->lpcSize; 
    132135   st->gamma1=mode->gamma1; 
     
    150153   st->curve = speex_alloc(128*sizeof(float)); 
    151154   st->old_curve = speex_alloc(128*sizeof(float)); 
    152 #endif 
     155   st->psy_window = speex_alloc(256*sizeof(float)); 
     156#endif 
     157 
     158   st->cumul_gain = 1024; 
    153159 
    154160   /* Allocating input buffer */ 
    155    st->inBuf = speex_alloc((st->windowSize+EXTRA_BUFFER)*sizeof(spx_sig_t)); 
    156    st->frame = st->inBuf+EXTRA_BUFFER; 
     161   st->winBuf = speex_alloc((st->windowSize-st->frameSize)*sizeof(spx_word16_t)); 
    157162   /* Allocating excitation buffer */ 
    158    st->excBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t)); 
    159    st->exc = st->excBuf + mode->pitchEnd + 1; 
    160    st->swBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t)); 
    161    st->sw = st->swBuf + mode->pitchEnd + 1; 
    162  
    163    st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t)); 
    164  
    165    /* Asymmetric "pseudo-Hamming" window */ 
    166    { 
    167       int part1, part2; 
    168       part1=st->frameSize - (st->subframeSize>>1); 
    169       part2=(st->frameSize>>1) + (st->subframeSize>>1); 
    170       st->window = speex_alloc((st->windowSize)*sizeof(spx_word16_t)); 
    171       for (i=0;i<part1;i++) 
    172          st->window[i]=(spx_word16_t)(SIG_SCALING*(.54-.46*cos(M_PI*i/part1))); 
    173       for (i=0;i<part2;i++) 
    174          st->window[part1+i]=(spx_word16_t)(SIG_SCALING*(.54+.46*cos(M_PI*i/part2))); 
    175    } 
     163   st->excBuf = speex_alloc((mode->frameSize+mode->pitchEnd+2)*sizeof(spx_word16_t)); 
     164   st->exc = st->excBuf + mode->pitchEnd + 2; 
     165   st->swBuf = speex_alloc((mode->frameSize+mode->pitchEnd+2)*sizeof(spx_word16_t)); 
     166   st->sw = st->swBuf + mode->pitchEnd + 2; 
     167 
     168   st->window= lpc_window; 
     169    
    176170   /* Create the window for autocorrelation (lag-windowing) */ 
    177171   st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t)); 
     
    179173      st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i)); 
    180174 
    181    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t)); 
    182  
    183    st->lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t)); 
    184    st->interp_lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t)); 
    185    st->interp_qlpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t)); 
    186    st->bw_lpc1 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t)); 
    187    st->bw_lpc2 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t)); 
    188  
    189    st->lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    190    st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    191175   st->old_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    192176   st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    193    st->interp_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    194    st->interp_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t)); 
    195  
    196177   st->first = 1; 
    197178   for (i=0;i<st->lpcSize;i++) 
    198179   { 
    199       st->lsp[i]=LSP_SCALING*(M_PI*((float)(i+1)))/(st->lpcSize+1); 
     180      st->old_lsp[i]=LSP_SCALING*(M_PI*((float)(i+1)))/(st->lpcSize+1); 
    200181   } 
    201182 
     
    204185   st->mem_sw_whole = speex_alloc((st->lpcSize)*sizeof(spx_mem_t)); 
    205186   st->mem_exc = speex_alloc((st->lpcSize)*sizeof(spx_mem_t)); 
     187   st->mem_exc2 = speex_alloc((st->lpcSize)*sizeof(spx_mem_t)); 
    206188 
    207189   st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t)); 
    208  
     190   st->innov_save = NULL; 
     191    
    209192   st->pitch = speex_alloc((st->nbSubframes)*sizeof(int)); 
    210193 
     
    213196   st->vbr_quality = 8; 
    214197   st->vbr_enabled = 0; 
     198   st->vbr_max = 0; 
    215199   st->vad_enabled = 0; 
    216200   st->dtx_enabled = 0; 
     
    237221#endif 
    238222 
    239    speex_free (st->inBuf); 
     223   speex_free (st->winBuf); 
    240224   speex_free (st->excBuf); 
    241    speex_free (st->innov); 
    242    speex_free (st->interp_qlpc); 
    243    speex_free (st->qlsp); 
    244225   speex_free (st->old_qlsp); 
    245    speex_free (st->interp_qlsp); 
    246226   speex_free (st->swBuf); 
    247227 
    248    speex_free (st->window); 
    249228   speex_free (st->lagWindow); 
    250    speex_free (st->autocorr); 
    251    speex_free (st->lpc); 
    252    speex_free (st->lsp); 
    253  
    254    speex_free (st->interp_lpc); 
    255    speex_free (st->bw_lpc1); 
    256    speex_free (st->bw_lpc2); 
     229 
    257230   speex_free (st->old_lsp); 
    258    speex_free (st->interp_lsp); 
    259231   speex_free (st->mem_sp); 
    260232   speex_free (st->mem_sw); 
    261233   speex_free (st->mem_sw_whole); 
    262234   speex_free (st->mem_exc); 
     235   speex_free (st->mem_exc2); 
    263236   speex_free (st->pi_gain); 
    264237   speex_free (st->pitch); 
     
    271244   speex_free (st->curve); 
    272245   speex_free (st->old_curve); 
     246   speex_free (st->psy_window); 
    273247#endif 
    274248 
     
    284258   spx_word16_t ol_pitch_coef; 
    285259   spx_word32_t ol_gain; 
    286    VARDECL(spx_sig_t *res); 
    287    VARDECL(spx_sig_t *target); 
     260   VARDECL(spx_word16_t *ringing); 
     261   VARDECL(spx_word16_t *target); 
     262   VARDECL(spx_sig_t *innov); 
     263   VARDECL(spx_word32_t *exc32); 
    288264   VARDECL(spx_mem_t *mem); 
     265   VARDECL(spx_coef_t *bw_lpc1); 
     266   VARDECL(spx_coef_t *bw_lpc2); 
     267   VARDECL(spx_coef_t *lpc); 
     268   VARDECL(spx_lsp_t *lsp); 
     269   VARDECL(spx_lsp_t *qlsp); 
     270   VARDECL(spx_lsp_t *interp_lsp); 
     271   VARDECL(spx_lsp_t *interp_qlsp); 
     272   VARDECL(spx_coef_t *interp_lpc); 
     273   VARDECL(spx_coef_t *interp_qlpc); 
    289274   char *stack; 
    290275   VARDECL(spx_word16_t *syn_resp); 
    291    VARDECL(spx_sig_t *real_exc); 
     276   VARDECL(spx_word16_t *real_exc); 
    292277#ifdef EPIC_48K 
    293278   int pitch_half[2]; 
     
    299284   stack=st->stack; 
    300285 
    301    /* Copy new data in input buffer */ 
    302    speex_move(st->inBuf, st->inBuf+st->frameSize, (EXTRA_BUFFER+st->windowSize-st->frameSize)*sizeof(spx_sig_t)); 
    303    for (i=0;i<st->frameSize;i++) 
    304       st->inBuf[st->windowSize-st->frameSize+i+EXTRA_BUFFER] = SHL32(EXTEND32(in[i]), SIG_SHIFT); 
     286   ALLOC(lpc, st->lpcSize, spx_coef_t); 
     287   ALLOC(bw_lpc1, st->lpcSize, spx_coef_t); 
     288   ALLOC(bw_lpc2, st->lpcSize, spx_coef_t); 
     289   ALLOC(lsp, st->lpcSize, spx_lsp_t); 
     290   ALLOC(qlsp, st->lpcSize, spx_lsp_t); 
     291   ALLOC(interp_lsp, st->lpcSize, spx_lsp_t); 
     292   ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t); 
     293   ALLOC(interp_lpc, st->lpcSize, spx_coef_t); 
     294   ALLOC(interp_qlpc, st->lpcSize, spx_coef_t); 
    305295 
    306296   /* Move signals 1 frame towards the past */ 
    307    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t)); 
    308    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t)); 
     297   speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch+2)*sizeof(spx_word16_t)); 
     298   speex_move(st->swBuf, st->swBuf+st->frameSize, (st->max_pitch+2)*sizeof(spx_word16_t)); 
    309299 
    310300   { 
    311301      VARDECL(spx_word16_t *w_sig); 
     302      VARDECL(spx_word16_t *autocorr); 
    312303      ALLOC(w_sig, st->windowSize, spx_word16_t); 
     304      ALLOC(autocorr, st->lpcSize+1, spx_word16_t); 
    313305      /* Window for analysis */ 
    314       for (i=0;i<st->windowSize;i++) 
    315          w_sig[i] = EXTRACT16(SHR32(MULT16_16(EXTRACT16(SHR32(st->frame[i],SIG_SHIFT)),st->window[i]),SIG_SHIFT)); 
    316  
     306      for (i=0;i<st->windowSize-st->frameSize;i++) 
     307         w_sig[i] = EXTRACT16(SHR32(MULT16_16(st->winBuf[i],st->window[i]),SIG_SHIFT)); 
     308      for (;i<st->windowSize;i++) 
     309         w_sig[i] = EXTRACT16(SHR32(MULT16_16(in[i-st->windowSize+st->frameSize],st->window[i]),SIG_SHIFT)); 
    317310      /* Compute auto-correlation */ 
    318       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize); 
    319    } 
    320    st->autocorr[0] = ADD16(st->autocorr[0],MULT16_16_Q15(st->autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */ 
    321  
    322    /* Lag windowing: equivalent to filtering in the power-spectrum domain */ 
    323    for (i=0;i<st->lpcSize+1;i++) 
    324       st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]); 
    325  
    326    /* Levinson-Durbin */ 
    327    _spx_lpc(st->lpc, st->autocorr, st->lpcSize); 
    328  
    329    /* LPC to LSPs (x-domain) transform */ 
    330    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, LSP_DELTA1, stack); 
    331    /* Check if we found all the roots */ 
    332    if (roots!=st->lpcSize) 
    333    { 
    334       /* Search again if we can afford it */ 
    335       if (st->complexity>1) 
    336          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, LSP_DELTA2, stack); 
    337       if (roots!=st->lpcSize)  
     311      _spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize); 
     312      autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */ 
     313 
     314      /* Lag windowing: equivalent to filtering in the power-spectrum domain */ 
     315      for (i=0;i<st->lpcSize+1;i++) 
     316         autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]); 
     317 
     318      /* Levinson-Durbin */ 
     319      _spx_lpc(lpc, autocorr, st->lpcSize); 
     320      /* LPC to LSPs (x-domain) transform */ 
     321      roots=lpc_to_lsp (lpc, st->lpcSize, lsp, 10, LSP_DELTA1, stack); 
     322      /* Check if we found all the roots */ 
     323      if (roots!=st->lpcSize) 
    338324      { 
    339325         /*If we can't find all LSP's, do some damage control and use previous filter*/ 
    340326         for (i=0;i<st->lpcSize;i++) 
    341327         { 
    342             st->lsp[i]=st->old_lsp[i]; 
    343          } 
    344       } 
    345    } 
     328            lsp[i]=st->old_lsp[i]; 
     329         } 
     330      } 
     331   } 
     332 
    346333 
    347334 
     
    351338      if (st->first) 
    352339         for (i=0;i<st->lpcSize;i++) 
    353             st->interp_lsp[i] = st->lsp[i]; 
     340            interp_lsp[i] = lsp[i]; 
    354341      else 
    355          lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1); 
    356  
    357       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN); 
     342         lsp_interpolate(st->old_lsp, lsp, interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1); 
     343 
     344      lsp_enforce_margin(interp_lsp, st->lpcSize, LSP_MARGIN); 
    358345 
    359346      /* Compute interpolated LPCs (unquantized) for whole frame*/ 
    360       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack); 
     347      lsp_to_lpc(interp_lsp, interp_lpc, st->lpcSize,stack); 
    361348 
    362349 
    363350      /*Open-loop pitch*/ 
    364       if (!st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) || 
     351      if (st->complexity>2 || !st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) || 
    365352          SUBMODE(lbr_pitch) != -1) 
    366353      { 
     
    368355         spx_word16_t nol_pitch_coef[6]; 
    369356          
    370          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize); 
    371          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize); 
     357         bw_lpc(st->gamma1, interp_lpc, bw_lpc1, st->lpcSize); 
     358         bw_lpc(st->gamma2, interp_lpc, bw_lpc2, st->lpcSize); 
    372359          
    373          filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole); 
     360         for (i=0;i<st->windowSize-st->frameSize;i++) 
     361            st->sw[i] = st->winBuf[i]; 
     362         for (;i<st->frameSize;i++)