C++ EXTEND32函数代码示例

static inline spx_word32_t cheb_poly_eva(
  spx_word16_t *coef, /* P or Q coefs in Q13 format               */
  spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
  int              m, /* LPC order/2                              */
  char         *stack
)
{
    int i;
    spx_word16_t b0, b1;
    spx_word32_t sum;

    /*Prevents overflows*/
    if (x>16383)
       x = 16383;
    if (x<-16383)
       x = -16383;

    /* Initialise values */
    b1=16384;
    b0=x;

    /* Evaluate Chebyshev series formulation usin g iterative approach  */
    sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
    for(i=2;i<=m;i++)
    {
       spx_word16_t tmp=b0;
       b0 = SUB16(MULT16_16_Q13(x,b0), b1);
       b1 = tmp;
       sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
    }

    return sum;
}

/* Computes a rough approximation of log2(2^a + 2^b) */
static opus_val16 logSum(opus_val16 a, opus_val16 b)
{
   opus_val16 max;
   opus_val32 diff;
   opus_val16 frac;
   static const opus_val16 diff_table[17] = {
         QCONST16(0.5000000f, DB_SHIFT), QCONST16(0.2924813f, DB_SHIFT), QCONST16(0.1609640f, DB_SHIFT), QCONST16(0.0849625f, DB_SHIFT),
         QCONST16(0.0437314f, DB_SHIFT), QCONST16(0.0221971f, DB_SHIFT), QCONST16(0.0111839f, DB_SHIFT), QCONST16(0.0056136f, DB_SHIFT),
         QCONST16(0.0028123f, DB_SHIFT)
   };
   int low;
   if (a>b)
   {
      max = a;
      diff = SUB32(EXTEND32(a),EXTEND32(b));
   } else {
      max = b;
      diff = SUB32(EXTEND32(b),EXTEND32(a));
   }
   if (!(diff < QCONST16(8.f, DB_SHIFT)))  /* inverted to catch NaNs */
      return max;
#ifdef FIXED_POINT
   low = SHR32(diff, DB_SHIFT-1);
   frac = SHL16(diff - SHL16(low, DB_SHIFT-1), 16-DB_SHIFT);
#else
   low = (int)floor(2*diff);
   frac = 2*diff - low;
#endif
   return max + diff_table[low] + MULT16_16_Q15(frac, SUB16(diff_table[low+1], diff_table[low]));
}

static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K, int spread)
{
    static const int SPREAD_FACTOR[3]= {15,10,5};
    int i;
    opus_val16 c, s;
    opus_val16 gain, theta;
    int stride2=0;
    int factor;

    if (2*K>=len || spread==SPREAD_NONE)
        return;
    factor = SPREAD_FACTOR[spread-1];

    gain = celt_div((opus_val32)MULT16_16(Q15_ONE,len),(opus_val32)(len+factor*K));
    theta = HALF16(MULT16_16_Q15(gain,gain));

    c = celt_cos_norm(EXTEND32(theta));
    s = celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */

    if (len>=8*stride)
    {
        stride2 = 1;
        /* This is just a simple (equivalent) way of computing sqrt(len/stride) with rounding.
           It's basically incrementing long as (stride2+0.5)^2 < len/stride. */
        while ((stride2*stride2+stride2)*stride + (stride>>2) < len)
            stride2++;
    }

void split_cb_shape_sign_unquant(
spx_sig_t *exc,
const void *par,                      /* non-overlapping codebook */
int   nsf,                      /* number of samples in subframe */
SpeexBits *bits,
char *stack,
spx_int32_t *seed
)
{
   int i,j;
   VARDECL(int *ind);
   VARDECL(int *signs);
   const signed char *shape_cb;
   //int shape_cb_size;
   int subvect_size, nb_subvect;
   const split_cb_params *params;
   int have_sign;

   params = (const split_cb_params *) par;
   subvect_size = params->subvect_size;
   nb_subvect = params->nb_subvect;
   //shape_cb_size = 1<<params->shape_bits;
   shape_cb = params->shape_cb;
   have_sign = params->have_sign;

   ALLOC(ind, nb_subvect, int);
   ALLOC(signs, nb_subvect, int);

   /* Decode codewords and gains */
   for (i=0;i<nb_subvect;i++)
   {
      if (have_sign)
         signs[i] = speex_bits_unpack_unsigned(bits, 1);
      else
         signs[i] = 0;
      ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
   }
   /* Compute decoded excitation */
   for (i=0;i<nb_subvect;i++)
   {
      spx_word16_t s=1;
      if (signs[i])
         s=-1;
#ifdef FIXED_POINT
      if (s==1)
      {
         for (j=0;j<subvect_size;j++)
            exc[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5);
      } else {
         for (j=0;j<subvect_size;j++)
            exc[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5));
      }
#else
      for (j=0;j<subvect_size;j++)
         exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];      
#endif
   }
}

EXPORT int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
{
   switch(request)
   {
      
      case SPEEX_ECHO_GET_FRAME_SIZE:
         (*(int*)ptr) = st->frame_size;
         break;
      case SPEEX_ECHO_SET_SAMPLING_RATE:
         st->sampling_rate = (*(int*)ptr);
         st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
#ifdef FIXED_POINT
         st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
         st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
#else
         st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
         st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
#endif
         if (st->sampling_rate<12000)
            st->notch_radius = QCONST16(.9, 15);
         else if (st->sampling_rate<24000)
            st->notch_radius = QCONST16(.982, 15);
         else
            st->notch_radius = QCONST16(.992, 15);
         break;
      case SPEEX_ECHO_GET_SAMPLING_RATE:
         (*(int*)ptr) = st->sampling_rate;
         break;
      case SPEEX_ECHO_GET_IMPULSE_RESPONSE_SIZE:
         /*FIXME: Implement this for multiple channels */
         *((spx_int32_t *)ptr) = st->M * st->frame_size;
         break;
      case SPEEX_ECHO_GET_IMPULSE_RESPONSE:
      {
         int M = st->M, N = st->window_size, n = st->frame_size, i, j;
         spx_int32_t *filt = (spx_int32_t *) ptr;
         for(j=0;j<M;j++)
         {
            /*FIXME: Implement this for multiple channels */
#ifdef FIXED_POINT
            for (i=0;i<N;i++)
               st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],16+NORMALIZE_SCALEDOWN));
            spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
#else
            spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
#endif
            for(i=0;i<n;i++)
               filt[j*n+i] = PSHR32(MULT16_16(32767,st->wtmp[i]), WEIGHT_SHIFT-NORMALIZE_SCALEDOWN);
         }
      }
         break;
      default:
         speex_warning_int("Unknown speex_echo_ctl request: ", request);
         return -1;
   }
   return 0;
}

static void kf_bfly2(
        kiss_fft_cpx * Fout,
        const size_t fstride,
        const kiss_fft_cfg st,
        int m,
        int N,
        int mm
        )
{
    kiss_fft_cpx * Fout2;
    kiss_fft_cpx * tw1;
    kiss_fft_cpx t;
    if (!st->inverse) {
       int i,j;
       kiss_fft_cpx * Fout_beg = Fout;
       for (i=0;i<N;i++)
       {
          Fout = Fout_beg + i*mm;
          Fout2 = Fout + m;
          tw1 = st->twiddles;
          for(j=0;j<m;j++)
          {
             /* Almost the same as the code path below, except that we divide the input by two
              (while keeping the best accuracy possible) */
             ms_word32_t tr, ti;
             tr = SHR32(SUB32(MULT16_16(Fout2->r , tw1->r),MULT16_16(Fout2->i , tw1->i)), 1);
             ti = SHR32(ADD32(MULT16_16(Fout2->i , tw1->r),MULT16_16(Fout2->r , tw1->i)), 1);
             tw1 += fstride;
             Fout2->r = PSHR32(SUB32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
             Fout2->i = PSHR32(SUB32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
             Fout->r = PSHR32(ADD32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
             Fout->i = PSHR32(ADD32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
             ++Fout2;
             ++Fout;
          }
       }
    } else {
       int i,j;
       kiss_fft_cpx * Fout_beg = Fout;
       for (i=0;i<N;i++)
       {
          Fout = Fout_beg + i*mm;
          Fout2 = Fout + m;
          tw1 = st->twiddles;
          for(j=0;j<m;j++)
          {
             C_MUL (t,  *Fout2 , *tw1);
             tw1 += fstride;
             C_SUB( *Fout2 ,  *Fout , t );
             C_ADDTO( *Fout ,  t );
             ++Fout2;
             ++Fout;
          }
       }
    }
}

/** Forced pitch delay and gain */
int forced_pitch_quant(spx_word16_t target[],	/* Target vector */
		       spx_word16_t * sw, spx_coef_t ak[],	/* LPCs for this subframe */
		       spx_coef_t awk1[],	/* Weighted LPCs #1 for this subframe */
		       spx_coef_t awk2[],	/* Weighted LPCs #2 for this subframe */
		       spx_sig_t exc[],	/* Excitation */
		       const void *par, int start,	/* Smallest pitch value allowed */
		       int end,	/* Largest pitch value allowed */
		       spx_word16_t pitch_coef,	/* Voicing (pitch) coefficient */
		       int p,	/* Number of LPC coeffs */
		       int nsf,	/* Number of samples in subframe */
		       SpeexBits * bits,
		       char *stack,
		       spx_word16_t * exc2,
		       spx_word16_t * r,
		       int complexity,
		       int cdbk_offset,
		       int plc_tuning, spx_word32_t * cumul_gain)
{
	(void)sw;
	(void)par;
	(void)end;
	(void)bits;
	(void)r;
	(void)complexity;
	(void)cdbk_offset;
	(void)plc_tuning;
	(void)cumul_gain;
	int i;
	spx_word16_t res[nsf];
#ifdef FIXED_POINT
	if (pitch_coef > 63)
		pitch_coef = 63;
#else
	if (pitch_coef > .99)
		pitch_coef = .99;
#endif
	for (i = 0; i < nsf && i < start; i++) {
		exc[i] = MULT16_16(SHL16(pitch_coef, 7), exc2[i - start]);
	}
	for (; i < nsf; i++) {
		exc[i] = MULT16_32_Q15(SHL16(pitch_coef, 9), exc[i - start]);
	}
	for (i = 0; i < nsf; i++)
		res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT - 1));
	syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
	for (i = 0; i < nsf; i++)
		target[i] =
		    EXTRACT16(SATURATE
			      (SUB32(EXTEND32(target[i]), EXTEND32(res[i])),
			       32700));
	return start;
}

static void exp_rotation(celt_norm *X, int len, int dir, int stride, int K)
{
   int i, k, iter;
   celt_word16 c, s;
   celt_word16 gain, theta;
   celt_norm *Xptr;
   gain = celt_div((celt_word32)MULT16_16(Q15_ONE,len),(celt_word32)(3+len+6*K));
   /* FIXME: Make that HALF16 instead of HALF32 */
   theta = SUB16(Q15ONE, HALF32(MULT16_16_Q15(gain,gain)));
   /*if (len==30)
   {
   for (i=0;i<len;i++)
   X[i] = 0;
   X[14] = 1;
}*/ 
   c = celt_cos_norm(EXTEND32(theta));
   s = dir*celt_cos_norm(EXTEND32(SUB16(Q15ONE,theta))); /*  sin(theta) */
   if (len > 8*stride)
      stride *= len/(8*stride);
   iter = 1;
   for (k=0;k<iter;k++)
   {
      /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
      but at this point, I really don't think it's necessary */
      Xptr = X;
      for (i=0;i<len-stride;i++)
      {
         celt_norm x1, x2;
         x1 = Xptr[0];
         x2 = Xptr[stride];
         Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
         *Xptr++      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
      }
      Xptr = &X[len-2*stride-1];
      for (i=len-2*stride-1;i>=0;i--)
      {
         celt_norm x1, x2;
         x1 = Xptr[0];
         x2 = Xptr[stride];
         Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
         *Xptr--      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
      }
   }
   /*if (len==30)
   {
   for (i=0;i<len;i++)
   printf ("%f ", X[i]);
   printf ("\n");
   exit(0);
}*/
}

static void cubic_coef(spx_word16_t x, spx_word16_t interp[4])
{
   /* Compute interpolation coefficients. I'm not sure whether this corresponds to cubic interpolation
   but I know it's MMSE-optimal on a sinc */
   spx_word16_t x2, x3;
   x2 = MULT16_16_P15(x, x);
   x3 = MULT16_16_P15(x, x2);
   interp[0] = PSHR32(MULT16_16(QCONST16(-0.16667f, 15),x) + MULT16_16(QCONST16(0.16667f, 15),x3),15);
   interp[1] = EXTRACT16(EXTEND32(x) + SHR32(SUB32(EXTEND32(x2),EXTEND32(x3)),1));
   interp[3] = PSHR32(MULT16_16(QCONST16(-0.33333f, 15),x) + MULT16_16(QCONST16(.5f,15),x2) - MULT16_16(QCONST16(0.16667f, 15),x3),15);
   /* Just to make sure we don't have rounding problems */
   interp[2] = Q15_ONE-interp[0]-interp[1]-interp[3];
   if (interp[2]<32767)
      interp[2]+=1;
}

void noise_codebook_quant(
spx_word16_t target[],			/* target vector */
spx_coef_t ak[],			/* LPCs for this subframe */
spx_coef_t awk1[],			/* Weighted LPCs for this subframe */
spx_coef_t awk2[],			/* Weighted LPCs for this subframe */
const void *par,                      /* Codebook/search parameters*/
int   p,                        /* number of LPC coeffs */
int   nsf,                      /* number of samples in subframe */
spx_sig_t *exc,
spx_word16_t *r,
SpeexBits *bits,
char *stack,
int   complexity,
int   update_target
)
{
   int i;
   VARDECL(spx_word16_t *tmp);
   ALLOC(tmp, nsf, spx_word16_t);
   residue_percep_zero16(target, ak, awk1, awk2, tmp, nsf, p, stack);

   for (i=0;i<nsf;i++)
      exc[i]+=SHL32(EXTEND32(tmp[i]),8);
   SPEEX_MEMSET(target, 0, nsf);
}

static void celt_fir5(opus_val16 *x,
         const opus_val16 *num,
         int N)
{
   int i;
   opus_val16 num0, num1, num2, num3, num4;
   opus_val32 mem0, mem1, mem2, mem3, mem4;
   num0=num[0];
   num1=num[1];
   num2=num[2];
   num3=num[3];
   num4=num[4];
   mem0=0;
   mem1=0;
   mem2=0;
   mem3=0;
   mem4=0;
   for (i=0;i<N;i++)
   {
      opus_val32 sum = SHL32(EXTEND32(x[i]), SIG_SHIFT);
      sum = MAC16_16(sum,num0,mem0);
      sum = MAC16_16(sum,num1,mem1);
      sum = MAC16_16(sum,num2,mem2);
      sum = MAC16_16(sum,num3,mem3);
      sum = MAC16_16(sum,num4,mem4);
      mem4 = mem3;
      mem3 = mem2;
      mem2 = mem1;
      mem1 = mem0;
      mem0 = x[i];
      x[i] = ROUND16(sum, SIG_SHIFT);
   }
}

static inline void mdf_adjust_prop(const spx_word32_t *W, int N, int M, int P, spx_word16_t *prop)
{
   int i, j, p;
   spx_word16_t max_sum = 1;
   spx_word32_t prop_sum = 1;
   for (i=0;i<M;i++)
   {
      spx_word32_t tmp = 1;
      for (p=0;p<P;p++)
         for (j=0;j<N;j++)
            tmp += MULT16_16(EXTRACT16(SHR32(W[p*N*M + i*N+j],18)), EXTRACT16(SHR32(W[p*N*M + i*N+j],18)));
#ifdef FIXED_POINT
      /* Just a security in case an overflow were to occur */
      tmp = MIN32(ABS32(tmp), 536870912);
#endif
      prop[i] = spx_sqrt(tmp);
      if (prop[i] > max_sum)
         max_sum = prop[i];
   }
   for (i=0;i<M;i++)
   {
      prop[i] += MULT16_16_Q15(QCONST16(.1f,15),max_sum);
      prop_sum += EXTEND32(prop[i]);
   }
   for (i=0;i<M;i++)
   {
      prop[i] = DIV32(MULT16_16(QCONST16(.99f,15), prop[i]),prop_sum);
      /*printf ("%f ", prop[i]);*/
   }
   /*printf ("\n");*/
}

void clt_mdct_init(mdct_lookup *l,int N)
{
   int i;
   int N2;
   l->n = N;
   N2 = N>>1;
   l->kfft = cpx32_fft_alloc(N>>2);
#ifndef ENABLE_TI_DSPLIB55
   if (l->kfft==NULL)
     return;
#endif
   l->trig = (kiss_twiddle_scalar*)celt_alloc(N2*sizeof(kiss_twiddle_scalar));
   if (l->trig==NULL)
     return;
   /* We have enough points that sine isn't necessary */
#if defined(FIXED_POINT)
#if defined(DOUBLE_PRECISION) & !defined(MIXED_PRECISION)
   for (i=0;i<N2;i++)
      l->trig[i] = SAMP_MAX*cos(2*M_PI*(i+1./8.)/N);
#else
   for (i=0;i<N2;i++)
      l->trig[i] = TRIG_UPSCALE*celt_cos_norm(DIV32(ADD32(SHL32(EXTEND32(i),17),16386),N));
#endif
#else
   for (i=0;i<N2;i++)
      l->trig[i] = cos(2*M_PI*(i+1./8.)/N);
#endif
}

void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
{
#ifndef FIXED_LPC_SIZE
   int i;
#endif

   spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
   spx_word16_t tmp2 = 16384-tmp;

#ifndef FIXED_LPC_SIZE
   for (i=0;i<len;i++)
   {
      interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
   }
#else
   interp_lsp[0] = MULT16_16_P14(tmp2,old_lsp[0]) + MULT16_16_P14(tmp,new_lsp[0]);
   interp_lsp[1] = MULT16_16_P14(tmp2,old_lsp[1]) + MULT16_16_P14(tmp,new_lsp[1]);
   interp_lsp[2] = MULT16_16_P14(tmp2,old_lsp[2]) + MULT16_16_P14(tmp,new_lsp[2]);
   interp_lsp[3] = MULT16_16_P14(tmp2,old_lsp[3]) + MULT16_16_P14(tmp,new_lsp[3]);
   interp_lsp[4] = MULT16_16_P14(tmp2,old_lsp[4]) + MULT16_16_P14(tmp,new_lsp[4]);
   interp_lsp[5] = MULT16_16_P14(tmp2,old_lsp[5]) + MULT16_16_P14(tmp,new_lsp[5]);
   interp_lsp[6] = MULT16_16_P14(tmp2,old_lsp[6]) + MULT16_16_P14(tmp,new_lsp[6]);
   interp_lsp[7] = MULT16_16_P14(tmp2,old_lsp[7]) + MULT16_16_P14(tmp,new_lsp[7]);
   interp_lsp[8] = MULT16_16_P14(tmp2,old_lsp[8]) + MULT16_16_P14(tmp,new_lsp[8]);
   interp_lsp[9] = MULT16_16_P14(tmp2,old_lsp[9]) + MULT16_16_P14(tmp,new_lsp[9]);
#endif
}

/* Compute the amplitude (sqrt energy) in each of the bands */
void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bandE, int end, int C, int M)
{
   int i, c, N;
   const opus_int16 *eBands = m->eBands;
   N = M*m->shortMdctSize;
   c=0; do {
      for (i=0;i<end;i++)
      {
         int j;
         opus_val32 maxval=0;
         opus_val32 sum = 0;

         j=M*eBands[i]; do {
            maxval = MAX32(maxval, X[j+c*N]);
            maxval = MAX32(maxval, -X[j+c*N]);
         } while (++j<M*eBands[i+1]);

         if (maxval > 0)
         {
            int shift = celt_ilog2(maxval)-10;
            j=M*eBands[i]; do {
               sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
                                   EXTRACT16(VSHR32(X[j+c*N],shift)));
            } while (++j<M*eBands[i+1]);
            /* We're adding one here to make damn sure we never end up with a pitch vector that's
               larger than unity norm */
            bandE[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
         } else {
            bandE[i+c*m->nbEBands] = EPSILON;
         }
         /*printf ("%f ", bandE[i+c*m->nbEBands]);*/
      }
   } while (++c<C);
   /*printf ("\n");*/
}