C++ MAC16_16函数代码示例

static inline void spectral_mul_accum16(const spx_word16_t *X, const spx_word16_t *Y, spx_word16_t *acc, int N, int M)
{
   int i,j;
   spx_word32_t tmp1=0,tmp2=0;
   for (j=0;j<M;j++)
   {
      tmp1 = MAC16_16(tmp1, X[j*N],Y[j*N]);
   }
   acc[0] = PSHR32(tmp1,WEIGHT_SHIFT);
   for (i=1;i<N-1;i+=2)
   {
      tmp1 = tmp2 = 0;
      for (j=0;j<M;j++)
      {
         tmp1 = SUB32(MAC16_16(tmp1, X[j*N+i],Y[j*N+i]), MULT16_16(X[j*N+i+1],Y[j*N+i+1]));
         tmp2 = MAC16_16(MAC16_16(tmp2, X[j*N+i+1],Y[j*N+i]), X[j*N+i], Y[j*N+i+1]);
      }
      acc[i] = PSHR32(tmp1,WEIGHT_SHIFT);
      acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT);
   }
   tmp1 = tmp2 = 0;
   for (j=0;j<M;j++)
   {
      tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],Y[(j+1)*N-1]);
   }
   acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT);
}

static void compute_weighted_codebook(const signed char *shape_cb, const spx_word16_t *r, spx_word16_t *resp, spx_word16_t *resp2, spx_word32_t *E, int shape_cb_size, int subvect_size, char *stack)
{
   int i, j, k;
   VARDECL(spx_word16_t *shape);
   ALLOC(shape, subvect_size, spx_word16_t);
   for (i=0;i<shape_cb_size;i++)
   {
      spx_word16_t *res;
      
      res = resp+i*subvect_size;
      for (k=0;k<subvect_size;k++)
         shape[k] = (spx_word16_t)shape_cb[i*subvect_size+k];
      E[i]=0;

      /* Compute codeword response using convolution with impulse response */
      for(j=0;j<subvect_size;j++)
      {
         spx_word32_t resj=0;
         spx_word16_t res16;
         for (k=0;k<=j;k++)
            resj = MAC16_16(resj,shape[k],r[j-k]);
#ifdef FIXED_POINT
         res16 = EXTRACT16(SHR32(resj, 13));
#else
         res16 = 0.03125f*resj;
#endif
         /* Compute codeword energy */
         E[i]=MAC16_16(E[i],res16,res16);
         res[j] = res16;
         /*printf ("%d\n", (int)res[j]);*/
      }
   }

}

void speex_decode_stereo_int(spx_int16_t *data, int frame_size, SpeexStereoState *_stereo)
{
   int i;
   spx_word32_t balance;
   spx_word16_t e_left, e_right, e_ratio;
   RealSpeexStereoState *stereo = (RealSpeexStereoState*)_stereo;

   /* COMPATIBILITY_HACK(stereo); */
   
   balance=stereo->balance;
   e_ratio=stereo->e_ratio;
   
   /* These two are Q14, with max value just below 2. */
   e_right = DIV32(QCONST32(1., 22), spx_sqrt(MULT16_32_Q15(e_ratio, ADD32(QCONST32(1., 16), balance))));
   e_left = SHR32(MULT16_16(spx_sqrt(balance), e_right), 8);

   for (i=frame_size-1;i>=0;i--)
   {
      spx_int16_t tmp=data[i];
      stereo->smooth_left = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_left, QCONST16(0.98, 15)), e_left, QCONST16(0.02, 15)), 15));
      stereo->smooth_right = EXTRACT16(PSHR32(MAC16_16(MULT16_16(stereo->smooth_right, QCONST16(0.98, 15)), e_right, QCONST16(0.02, 15)), 15));
      data[2*i] = (spx_int16_t)MULT16_16_P14(stereo->smooth_left, tmp);
      data[2*i+1] = (spx_int16_t)MULT16_16_P14(stereo->smooth_right, tmp);
   }
}

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);
   }
}

void dual_inner_prod_sse(const opus_val16 *x, const opus_val16 *y01, const opus_val16 *y02,
      int N, opus_val32 *xy1, opus_val32 *xy2)
{
   int i;
   __m128 xsum1, xsum2;
   xsum1 = _mm_setzero_ps();
   xsum2 = _mm_setzero_ps();
   for (i=0;i<N-3;i+=4)
   {
      __m128 xi = _mm_loadu_ps(x+i);
      __m128 y1i = _mm_loadu_ps(y01+i);
      __m128 y2i = _mm_loadu_ps(y02+i);
      xsum1 = _mm_add_ps(xsum1,_mm_mul_ps(xi, y1i));
      xsum2 = _mm_add_ps(xsum2,_mm_mul_ps(xi, y2i));
   }
   /* Horizontal sum */
   xsum1 = _mm_add_ps(xsum1, _mm_movehl_ps(xsum1, xsum1));
   xsum1 = _mm_add_ss(xsum1, _mm_shuffle_ps(xsum1, xsum1, 0x55));
   _mm_store_ss(xy1, xsum1);
   xsum2 = _mm_add_ps(xsum2, _mm_movehl_ps(xsum2, xsum2));
   xsum2 = _mm_add_ss(xsum2, _mm_shuffle_ps(xsum2, xsum2, 0x55));
   _mm_store_ss(xy2, xsum2);
   for (;i<N;i++)
   {
      *xy1 = MAC16_16(*xy1, x[i], y01[i]);
      *xy2 = MAC16_16(*xy2, x[i], y02[i]);
   }
}

void preProcessing(bcg729EncoderChannelContextStruct *encoderChannelContext, word16_t signal[], word16_t preProcessedSignal[]) {
	int i;
	word16_t inputX2;
	word32_t acc; /* in Q12 */

	for(i=0; i<L_FRAME; i++) {
		inputX2 = encoderChannelContext->inputX1;
		encoderChannelContext->inputX1 = encoderChannelContext->inputX0;
		encoderChannelContext->inputX0 = signal[i];
	
		/* compute with acc and coefficients in Q12 */
		acc = MULT16_32_Q12(A1, encoderChannelContext->outputY1); /* Y1 in Q15.12 * A1 in Q1.12 -> acc in Q17.12*/
		acc = MAC16_32_Q12(acc, A2, encoderChannelContext->outputY2); /* Y2 in Q15.12 * A2 in Q0.12 -> Q15.12 + acc in Q17.12 -> acc in Q18.12 */
		/* 3*(Xi in Q15.0 * Bi in Q0.12)->Q17.12 + acc in Q18.12 -> acc in 19.12 */
		acc = MAC16_16(acc, encoderChannelContext->inputX0, B0);
		acc = MAC16_16(acc, encoderChannelContext->inputX1, B1);
		acc = MAC16_16(acc, inputX2, B2);
		/*  acc in Q19.12 : We must check it won't overflow 
			- the Q15.12 of Y
			- the Q15.0 extracted from it by shifting 12 right
		 -> saturate to 28 bits -> acc in Q15.12 */
		acc = SATURATE(acc, MAXINT28);
		
		preProcessedSignal[i] = PSHR(acc,12); /* extract integer value of the Q15.12 representation */
		encoderChannelContext->outputY2 = encoderChannelContext->outputY1;
		encoderChannelContext->outputY1 = acc;
	}
	return;
}

static void exp_rotation1(celt_norm *X, int len, int stride, opus_val16 c, opus_val16 s)
{
    int i;
    opus_val16 ms;
    celt_norm *Xptr;
    Xptr = X;
    ms = NEG16(s);
    for (i=0; i<len-stride; i++)
    {
        celt_norm x1, x2;
        x1 = Xptr[0];
        x2 = Xptr[stride];
        Xptr[stride] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
        *Xptr++      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
    }
    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] = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x2),  s, x1), 15));
        *Xptr--      = EXTRACT16(PSHR32(MAC16_16(MULT16_16(c, x1), ms, x2), 15));
    }
}

void generateAdaptativeCodebookVector(word16_t excitationVector[], int16_t intPitchDelay, int16_t fracPitchDelay)
{
	int n,i,j;
	word16_t *delayedExcitationVector;
	word16_t *b30Increased;
	word16_t *b30Decreased;

	/* fracPitchDelay is in range [-1, 1], convert it to [0,2] needed by eqA.8 */
	fracPitchDelay = -fracPitchDelay;
	if (fracPitchDelay <0) { /* if fracPitchDelay is 1 -> pitchDelay of int+(1/3) -> int+1-(2/3)*/
		intPitchDelay++;
		fracPitchDelay = 2;
	}
	
	/**/
	delayedExcitationVector = &(excitationVector[-intPitchDelay]); /* delayedExcitationVector is used to address the excitation vector at index -intPitchDelay (-k in eq40) */
	b30Increased = &(b30[fracPitchDelay]); /* b30 increased points to b30[fracPitchDelay] : b30[t] in eq40. b30 in Q15 */
	b30Decreased = &(b30[3-fracPitchDelay]); /* b30 decreased points to b30[-fracPitchDelay] : b30[3-t] in eq40. b30 in Q15 */


	for (n=0; n<L_SUBFRAME; n++) {
		word32_t acc = 0; /* acc in Q15 */
		for (i=0, j=0; i<10; i++, j+=3) { /* j is used as a 3*i index */
			acc = MAC16_16(acc, delayedExcitationVector[n-i], b30Increased[j]); /* WARNING: spec 3.7.1 and A.8 give an equation leading to  delayedExcitationVector[n+i] but ITU code uses delayedExcitationVector[n-i], implemented as code */
			acc = MAC16_16(acc, delayedExcitationVector[n+1+i], b30Decreased[j]);
		}
		excitationVector[n] = SATURATE(PSHR(acc, 15), MAXINT16); /* acc in Q15, shift/round to unscaled value and check overflow on 16 bits */
	}
}

void celt_fir_c(
         const opus_val16 *_x,
         const opus_val16 *num,
         opus_val16 *_y,
         int N,
         int ord,
         opus_val16 *mem,
         int arch)
{
   int i,j;
   VARDECL(opus_val16, rnum);
   VARDECL(opus_val16, x);
   SAVE_STACK;

   ALLOC(rnum, ord, opus_val16);
   ALLOC(x, N+ord, opus_val16);
   for(i=0;i<ord;i++)
      rnum[i] = num[ord-i-1];
   for(i=0;i<ord;i++)
      x[i] = mem[ord-i-1];
   for (i=0;i<N;i++)
      x[i+ord]=_x[i];
   for(i=0;i<ord;i++)
      mem[i] = _x[N-i-1];
#ifdef SMALL_FOOTPRINT
   (void)arch;
   for (i=0;i<N;i++)
   {
      opus_val32 sum = SHL32(EXTEND32(_x[i]), SIG_SHIFT);
      for (j=0;j<ord;j++)
      {
         sum = MAC16_16(sum,rnum[j],x[i+j]);
      }
      _y[i] = SATURATE16(PSHR32(sum, SIG_SHIFT));
   }
#else
   for (i=0;i<N-3;i+=4)
   {
      opus_val32 sum[4]={0,0,0,0};
      xcorr_kernel(rnum, x+i, sum, ord, arch);
      _y[i  ] = SATURATE16(ADD32(EXTEND32(_x[i  ]), PSHR32(sum[0], SIG_SHIFT)));
      _y[i+1] = SATURATE16(ADD32(EXTEND32(_x[i+1]), PSHR32(sum[1], SIG_SHIFT)));
      _y[i+2] = SATURATE16(ADD32(EXTEND32(_x[i+2]), PSHR32(sum[2], SIG_SHIFT)));
      _y[i+3] = SATURATE16(ADD32(EXTEND32(_x[i+3]), PSHR32(sum[3], SIG_SHIFT)));
   }
   for (;i<N;i++)
   {
      opus_val32 sum = 0;
      for (j=0;j<ord;j++)
         sum = MAC16_16(sum,rnum[j],x[i+j]);
      _y[i] = SATURATE16(ADD32(EXTEND32(_x[i]), PSHR32(sum, SIG_SHIFT)));
   }
#endif
   RESTORE_STACK;
}

/** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */
static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_float_t p, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
{
   int i, j;
   spx_float_t W;
   W = FLOAT_AMULT(p, w[0]);
   prod[0] = FLOAT_MUL32(W,MULT16_16(X[0],Y[0]));
   for (i=1,j=1;i<N-1;i+=2,j++)
   {
      W = FLOAT_AMULT(p, w[j]);
      prod[i] = FLOAT_MUL32(W,MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1]));
      prod[i+1] = FLOAT_MUL32(W,MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1]));
   }
   W = FLOAT_AMULT(p, w[j]);
   prod[i] = FLOAT_MUL32(W,MULT16_16(X[i],Y[i]));
}

static int lsp_quant(spx_word16_t *x, const signed char *cdbk, int nbVec, int nbDim)
{
   int i,j;
   spx_word32_t dist;
   spx_word16_t tmp;
   spx_word32_t best_dist=VERY_LARGE32;
   int best_id=0;
   const signed char *ptr=cdbk;
   for (i=0;i<nbVec;i++)
   {
      dist=0;
      for (j=0;j<nbDim;j++)
      {
         tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
         dist=MAC16_16(dist,tmp,tmp);
      } 
      if (dist<best_dist)
      {
         best_dist=dist;
         best_id=i;
      }
   }

   for (j=0;j<nbDim;j++)
      x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
    
   return best_id;
}

/*Finds the indices of the n-best entries in a codebook*/
void vq_nbest(spx_word16_t *in, const spx_word16_t *codebook, int len, int entries, spx_word32_t *E, int N, int *nbest, spx_word32_t *best_dist, char *stack)
{
   int i,j,k,used;
   used = 0;
   for (i=0;i<entries;i++)
   {
      spx_word32_t dist=0;
      for (j=0;j<len;j++)
         dist = MAC16_16(dist,in[j],*codebook++);
#ifdef FIXED_POINT
      dist=SUB32(SHR32(E[i],1),dist);
#else
      dist=.5f*E[i]-dist;
#endif
      if (i<N || dist<best_dist[N-1])
      {
         for (k=N-1; (k >= 1) && (k > used || dist < best_dist[k-1]); k--)
         {
            best_dist[k]=best_dist[k-1];
            nbest[k] = nbest[k-1];
         }
         best_dist[k]=dist;
         nbest[k]=i;
         used++;
      }
   }
}

/* 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");*/
}

static void find_best_pitch(opus_val32 *xcorr, opus_val16 *y, int len,
                            int max_pitch, int *best_pitch
#ifdef FIXED_POINT
                            , int yshift, opus_val32 maxcorr
#endif
                            )
{
   int i, j;
   opus_val32 Syy=1;
   opus_val16 best_num[2];
   opus_val32 best_den[2];
#ifdef FIXED_POINT
   int xshift;

   xshift = celt_ilog2(maxcorr)-14;
#endif

   best_num[0] = -1;
   best_num[1] = -1;
   best_den[0] = 0;
   best_den[1] = 0;
   best_pitch[0] = 0;
   best_pitch[1] = 1;
   for (j=0;j<len;j++)
      Syy = MAC16_16(Syy, y[j],y[j]);
   for (i=0;i<max_pitch;i++)
   {
      if (xcorr[i]>0)
      {
         opus_val16 num;
         opus_val32 xcorr16;
         xcorr16 = EXTRACT16(VSHR32(xcorr[i], xshift));
         num = MULT16_16_Q15(xcorr16,xcorr16);
         if (MULT16_32_Q15(num,best_den[1]) > MULT16_32_Q15(best_num[1],Syy))
         {
            if (MULT16_32_Q15(num,best_den[0]) > MULT16_32_Q15(best_num[0],Syy))
            {
               best_num[1] = best_num[0];
               best_den[1] = best_den[0];
               best_pitch[1] = best_pitch[0];
               best_num[0] = num;
               best_den[0] = Syy;
               best_pitch[0] = i;
            } else {
               best_num[1] = num;
               best_den[1] = Syy;
               best_pitch[1] = i;
            }
         }
      }
      Syy += SHR32(MULT16_16(y[i+len],y[i+len]),yshift) - SHR32(MULT16_16(y[i],y[i]),yshift);
      Syy = MAX32(1, Syy);
   }
}

void mlp_process(const MLP * m, const opus_val16 * in, opus_val16 * out)
{
	int j;
	opus_val16 hidden[MAX_NEURONS];
	const opus_val16 *W = m->weights;
	/* Copy to tmp_in */
	for (j = 0; j < m->topo[1]; j++) {
		int k;
		opus_val32 sum = SHL32(EXTEND32(*W++), 8);
		for (k = 0; k < m->topo[0]; k++)
			sum = MAC16_16(sum, in[k], *W++);
		hidden[j] = tansig_approx(sum);
	}
	for (j = 0; j < m->topo[2]; j++) {
		int k;
		opus_val32 sum = SHL32(EXTEND32(*W++), 14);
		for (k = 0; k < m->topo[1]; k++)
			sum = MAC16_16(sum, hidden[k], *W++);
		out[j] = tansig_approx(EXTRACT16(PSHR32(sum, 17)));
	}
}