sbrhfadj.c.svn-base

MP3 for ARM codec. [asm+C]

			psi->qmLimBoost[m] = r;
		}

		q = MULSHIFT32(psi->smBuf[m], gBoost) << 2;		/* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */
		r = SqrtFix(q, fbitsDQ - 2, &z);
		z -= FBITS_OUT_QMFA;		/* justify for adding to signal (xBuf) later */
		if (z >= 0) {
			psi->smBoost[m] = r >> MIN(31, z);
		} else {
			z = MIN(30, -z);
			CLIP_2N_SHIFT30(r, z);
			psi->smBoost[m] = r;
		}
	}
}

/**************************************************************************************
 * Function:    CalcGain
 *
 * Description: calculate and apply proper gain to HF components in one envelope 
 *                (4.6.18.7.5)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *              index of current envelope
 *
 * Outputs:     envelope gain, sinusoids and noise after scaling
 *
 * Return:      none
 **************************************************************************************/
static void CalcGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env)
{
	int lim, fbitsDQ;

	/* initialize to -1 so that mapping limiter bands to env/noise bands works right on first pass */
	psi->envBand        = -1;
	psi->noiseFloorBand = -1;
	psi->sBand          = -1;
	psi->highBand       = -1;

	fbitsDQ = (FBITS_OUT_DQ_ENV - psi->envDataDequantScale[ch][env]);	/* Q(29 - optional scalefactor) */
	for (lim = 0; lim < sbrFreq->nLimiter; lim++) {
		/* the QMF bands are divided into lim regions (consecutive, non-overlapping) */
		CalcMaxGain(psi, sbrHdr, sbrGrid, sbrFreq, ch, env, lim, fbitsDQ);
		CalcComponentGains(psi, sbrGrid, sbrFreq, sbrChan, ch, env, lim, fbitsDQ);
		ApplyBoost(psi, sbrFreq, lim, fbitsDQ);
	}
}

/* hSmooth table from 4.7.18.7.6, format = Q31 */
static const int hSmoothCoef[MAX_NUM_SMOOTH_COEFS] = {
	0x2aaaaaab, 0x2697a512, 0x1becfa68, 0x0ebdb043, 0x04130598, 
};

/**************************************************************************************
 * Function:    MapHF
 *
 * Description: map HF components to proper QMF bands, with optional gain smoothing
 *                filter (4.6.18.7.6)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current envelope
 *              reset flag (can be non-zero for first envelope only)
 *
 * Outputs:     complete reconstructed subband QMF samples for this envelope
 *
 * Return:      none
 * 
 * Notes:       ensures that output has >= MIN_GBITS_IN_QMFS guard bits,
 *                so it's not necessary to check anything in the synth QMF
 **************************************************************************************/
static void MapHF(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int hfReset)
{
	int noiseTabIndex, sinIndex, gainNoiseIndex, hSL;
	int i, iStart, iEnd, m, idx, j, s, n, smre, smim;
	int gFilt, qFilt, xre, xim, gbMask, gbIdx;
	int *XBuf;

	noiseTabIndex =   sbrChan->noiseTabIndex;
	sinIndex =        sbrChan->sinIndex;
	gainNoiseIndex =  sbrChan->gainNoiseIndex;	/* oldest entries in filter delay buffer */

	if (hfReset)
		noiseTabIndex = 2;	/* starts at 1, double since complex */
	hSL = (sbrHdr->smoothMode ? 0 : 4); 

	if (hfReset) {
		for (i = 0; i < hSL; i++) {
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m];
				sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m];
			}
			gainNoiseIndex++;
			if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS)
				gainNoiseIndex = 0;
		}
		ASSERT(env == 0);	/* should only be reset when env == 0 */
	}

	iStart = sbrGrid->envTimeBorder[env];
	iEnd =   sbrGrid->envTimeBorder[env+1];
	for (i = iStart; i < iEnd; i++) {
		/* save new values in temp buffers (delay)
		 * we only store MAX_NUM_SMOOTH_COEFS most recent values, 
		 *   so don't keep storing the same value over and over
		 */
		if (i - iStart < MAX_NUM_SMOOTH_COEFS) {
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m];
				sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m];
			}
		}

		/* see 4.6.18.7.6 */
		XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart];
		gbMask = 0;
		for (m = 0; m < sbrFreq->numQMFBands; m++) {
			if (env == psi->la || env == sbrChan->laPrev) {
				/* no smoothing filter for gain, and qFilt = 0 (only need to do once) */
				if (i == iStart) {
					psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m];
					psi->qFiltLast[m] = 0;
				}
			} else if (hSL == 0) {
				/* no smoothing filter for gain, (only need to do once) */
				if (i == iStart) {
					psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m];
					psi->qFiltLast[m] = sbrChan->qTemp[gainNoiseIndex][m];
				}
			} else {
				/* apply smoothing filter to gain and noise (after MAX_NUM_SMOOTH_COEFS, it's always the same) */
				if (i - iStart < MAX_NUM_SMOOTH_COEFS) {
					gFilt = 0;
					qFilt = 0;
					idx = gainNoiseIndex;
					for (j = 0; j < MAX_NUM_SMOOTH_COEFS; j++) {
						/* sum(abs(hSmoothCoef[j])) for all j < 1.0 */
						gFilt += MULSHIFT32(sbrChan->gTemp[idx][m], hSmoothCoef[j]);
						qFilt += MULSHIFT32(sbrChan->qTemp[idx][m], hSmoothCoef[j]);
						idx--;
						if (idx < 0)
							idx += MAX_NUM_SMOOTH_COEFS;
					}
					psi->gFiltLast[m] = gFilt << 1;	/* restore to Q(FBITS_GLIM_BOOST) (gain of filter < 1.0, so no overflow) */
					psi->qFiltLast[m] = qFilt << 1;	/* restore to Q(FBITS_QLIM_BOOST) */
				}
			}

			if (psi->smBoost[m] != 0) {
				/* add scaled signal and sinusoid, don't add noise (qFilt = 0) */
				smre = psi->smBoost[m];
				smim = smre;

				/* sinIndex:  [0] xre += sm   [1] xim += sm*s   [2] xre -= sm   [3] xim -= sm*s  */
				s = (sinIndex >> 1);	/* if 2 or 3, flip sign to subtract sm */
				s <<= 31;
				smre ^= (s >> 31);
				smre -= (s >> 31);
				s ^= ((m + sbrFreq->kStart) << 31);
				smim ^= (s >> 31);
				smim -= (s >> 31);

				/* if sinIndex == 0 or 2, smim = 0; if sinIndex == 1 or 3, smre = 0 */
				s = sinIndex << 31;
				smim &= (s >> 31);
				s ^= 0x80000000;
				smre &= (s >> 31);

				noiseTabIndex += 2;		/* noise filtered by 0, but still need to bump index */
			} else {
				/* add scaled signal and scaled noise */
				qFilt = psi->qFiltLast[m];	
				n = noiseTab[noiseTabIndex++];
				smre = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA);

				n = noiseTab[noiseTabIndex++];
				smim = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA);
			}
			noiseTabIndex &= 1023;	/* 512 complex numbers */

			gFilt = psi->gFiltLast[m];
			xre = MULSHIFT32(gFilt, XBuf[0]);
			xim = MULSHIFT32(gFilt, XBuf[1]);
			CLIP_2N_SHIFT30(xre, 32 - FBITS_GLIM_BOOST);
			CLIP_2N_SHIFT30(xim, 32 - FBITS_GLIM_BOOST);

			xre += smre;	*XBuf++ = xre;
			xim += smim;	*XBuf++ = xim;

			gbMask |= FASTABS(xre);
			gbMask |= FASTABS(xim);
		}
		/* update circular buffer index */
		gainNoiseIndex++;
		if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS)
			gainNoiseIndex = 0;

		sinIndex++;
		sinIndex &= 3;

		/* ensure MIN_GBITS_IN_QMFS guard bits in output
		 * almost never occurs in practice, but checking here makes synth QMF logic very simple
		 */
		if (gbMask >> (31 - MIN_GBITS_IN_QMFS)) {
			XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart];
			for (m = 0; m < sbrFreq->numQMFBands; m++) {
				xre = XBuf[0];	xim = XBuf[1];	
				CLIP_2N(xre, (31 - MIN_GBITS_IN_QMFS));	
				CLIP_2N(xim, (31 - MIN_GBITS_IN_QMFS));	
				*XBuf++ = xre;	*XBuf++ = xim;
			}	
			CLIP_2N(gbMask, (31 - MIN_GBITS_IN_QMFS));	
		}
		gbIdx = ((i + HF_ADJ) >> 5) & 0x01;
		sbrChan->gbMask[gbIdx] |= gbMask;
	}
	sbrChan->noiseTabIndex =  noiseTabIndex;
	sbrChan->sinIndex =       sinIndex;
	sbrChan->gainNoiseIndex = gainNoiseIndex;
}

/**************************************************************************************
 * Function:    AdjustHighFreq
 *
 * Description: adjust high frequencies and add noise and sinusoids (4.6.18.7)
 *
 * Inputs:      initialized PSInfoSBR struct
 *              initialized SBRHeader struct for this SCE/CPE block
 *              initialized SBRGrid struct for this channel
 *              initialized SBRFreq struct for this SCE/CPE block
 *              initialized SBRChan struct for this channel
 *              index of current channel (0 for SCE, 0 or 1 for CPE)
 *
 * Outputs:     complete reconstructed subband QMF samples for this channel
 *
 * Return:      none
 **************************************************************************************/
void AdjustHighFreq(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch)
{
	int i, env, hfReset;
	unsigned char frameClass, pointer;

	frameClass = sbrGrid->frameClass;
	pointer  = sbrGrid->pointer;

	/* derive la from table 4.159 */
	if ((frameClass == SBR_GRID_FIXVAR || frameClass == SBR_GRID_VARVAR) && pointer > 0)
		psi->la = sbrGrid->numEnv + 1 - pointer;
	else if (frameClass == SBR_GRID_VARFIX && pointer > 1)
		psi->la = pointer - 1;
	else
		psi->la = -1;

	/* for each envelope, estimate gain and adjust SBR QMF bands */
	hfReset = sbrChan->reset;
	for (env = 0; env < sbrGrid->numEnv; env++) {
		EstimateEnvelope(psi, sbrHdr, sbrGrid, sbrFreq, env);
		CalcGain(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, ch, env);
		MapHF(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, env, hfReset);
		hfReset = 0;	/* only set for first envelope after header reset */
	}

	/* set saved sine flags to 0 for QMF bands outside of current frequency range */
	for (i = 0; i < sbrFreq->freqLimiter[0] + sbrFreq->kStart; i++)
		sbrChan->addHarmonic[0][i] = 0;
	for (i = sbrFreq->freqLimiter[sbrFreq->nLimiter] + sbrFreq->kStart; i < 64; i++)
		sbrChan->addHarmonic[0][i] = 0;
	sbrChan->addHarmonicFlag[0] = sbrChan->addHarmonicFlag[1];

	/* save la for next frame */
	if (psi->la == sbrGrid->numEnv)
		sbrChan->laPrev = 0;
	else
		sbrChan->laPrev = -1;
}