Mirror/squeezelite-esp32
1
0
Fork 0
mirror of https://github.com/sle118/squeezelite-esp32.git synced 2025-12-09 13:07:03 +03:00
Code Issues Packages Projects Releases Wiki Activity
Files
3d63670499b38a9b47863c310aa7a516112c4dc3
squeezelite-esp32/components/spotify/cspot/bell/libhelix-aac/sbrhfadj.c
Philippe G 898998efb0 big merge
2021-12-18 21:04:23 -08:00

854 lines
32 KiB
C
Raw Blame History

/* ***** BEGIN LICENSE BLOCK *****
* Source last modified: $Id: sbrhfadj.c,v 1.3 2005/05/24 16:01:55 albertofloyd Exp $
*
* Portions Copyright (c) 1995-2005 RealNetworks, Inc. All Rights Reserved.
*
* The contents of this file, and the files included with this file,
* are subject to the current version of the RealNetworks Public
* Source License (the "RPSL") available at
* http://www.helixcommunity.org/content/rpsl unless you have licensed
* the file under the current version of the RealNetworks Community
* Source License (the "RCSL") available at
* http://www.helixcommunity.org/content/rcsl, in which case the RCSL
* will apply. You may also obtain the license terms directly from
* RealNetworks. You may not use this file except in compliance with
* the RPSL or, if you have a valid RCSL with RealNetworks applicable
* to this file, the RCSL. Please see the applicable RPSL or RCSL for
* the rights, obligations and limitations governing use of the
* contents of the file.
*
* This file is part of the Helix DNA Technology. RealNetworks is the
* developer of the Original Code and owns the copyrights in the
* portions it created.
*
* This file, and the files included with this file, is distributed
* and made available on an 'AS IS' basis, WITHOUT WARRANTY OF ANY
* KIND, EITHER EXPRESS OR IMPLIED, AND REALNETWORKS HEREBY DISCLAIMS
* ALL SUCH WARRANTIES, INCLUDING WITHOUT LIMITATION, ANY WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, QUIET
* ENJOYMENT OR NON-INFRINGEMENT.
*
* Technology Compatibility Kit Test Suite(s) Location:
* http://www.helixcommunity.org/content/tck
*
* Contributor(s):
*
* ***** END LICENSE BLOCK ***** */
/**************************************************************************************
* Fixed-point HE-AAC decoder
* Jon Recker (jrecker@real.com)
* February 2005
*
* sbrhfadj.c - high frequency adjustment for SBR
**************************************************************************************/
#include "sbr.h"
#include "assembly.h"
/* invBandTab[i] = 1.0 / (i + 1), Q31 */
static const int invBandTab[64] PROGMEM = {
0x7fffffff, 0x40000000, 0x2aaaaaab, 0x20000000, 0x1999999a, 0x15555555, 0x12492492, 0x10000000,
0x0e38e38e, 0x0ccccccd, 0x0ba2e8ba, 0x0aaaaaab, 0x09d89d8a, 0x09249249, 0x08888889, 0x08000000,
0x07878788, 0x071c71c7, 0x06bca1af, 0x06666666, 0x06186186, 0x05d1745d, 0x0590b216, 0x05555555,
0x051eb852, 0x04ec4ec5, 0x04bda12f, 0x04924925, 0x0469ee58, 0x04444444, 0x04210842, 0x04000000,
0x03e0f83e, 0x03c3c3c4, 0x03a83a84, 0x038e38e4, 0x03759f23, 0x035e50d8, 0x03483483, 0x03333333,
0x031f3832, 0x030c30c3, 0x02fa0be8, 0x02e8ba2f, 0x02d82d83, 0x02c8590b, 0x02b93105, 0x02aaaaab,
0x029cbc15, 0x028f5c29, 0x02828283, 0x02762762, 0x026a439f, 0x025ed098, 0x0253c825, 0x02492492,
0x023ee090, 0x0234f72c, 0x022b63cc, 0x02222222, 0x02192e2a, 0x02108421, 0x02082082, 0x02000000,
};
/**************************************************************************************
* Function: EstimateEnvelope
*
* Description: estimate power of generated HF QMF bands in one time-domain envelope
* (4.6.18.7.3)
*
* 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
* index of current envelope
*
* Outputs: power of each QMF subband, stored as integer (Q0) * 2^N, N >= 0
*
* Return: none
**************************************************************************************/
static void EstimateEnvelope(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int env)
{
int i, m, iStart, iEnd, xre, xim, nScale, expMax;
int p, n, mStart, mEnd, invFact, t;
int *XBuf;
U64 eCurr;
unsigned char *freqBandTab;
/* estimate current envelope */
iStart = sbrGrid->envTimeBorder[env] + HF_ADJ;
iEnd = sbrGrid->envTimeBorder[env+1] + HF_ADJ;
if (sbrGrid->freqRes[env]) {
n = sbrFreq->nHigh;
freqBandTab = sbrFreq->freqHigh;
} else {
n = sbrFreq->nLow;
freqBandTab = sbrFreq->freqLow;
}
/* ADS should inline MADD64 (smlal) properly, but check to make sure */
expMax = 0;
if (sbrHdr->interpFreq) {
for (m = 0; m < sbrFreq->numQMFBands; m++) {
eCurr.w64 = 0;
XBuf = psi->XBuf[iStart][sbrFreq->kStart + m];
for (i = iStart; i < iEnd; i++) {
/* scale to int before calculating power (precision not critical, and avoids overflow) */
xre = (*XBuf) >> FBITS_OUT_QMFA; XBuf += 1;
xim = (*XBuf) >> FBITS_OUT_QMFA; XBuf += (2*64 - 1);
eCurr.w64 = MADD64(eCurr.w64, xre, xre);
eCurr.w64 = MADD64(eCurr.w64, xim, xim);
}
/* eCurr.w64 is now Q(64 - 2*FBITS_OUT_QMFA) (64-bit word)
* if energy is too big to fit in 32-bit word (> 2^31) scale down by power of 2
*/
nScale = 0;
if (eCurr.r.hi32) {
nScale = (32 - CLZ(eCurr.r.hi32)) + 1;
t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */
t |= eCurr.r.hi32 << (32 - nScale);
} else if (eCurr.r.lo32 >> 31) {
nScale = 1;
t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */
} else {
t = (int)eCurr.r.lo32;
}
invFact = invBandTab[(iEnd - iStart)-1];
psi->eCurr[m] = MULSHIFT32(t, invFact);
psi->eCurrExp[m] = nScale + 1; /* +1 for invFact = Q31 */
if (psi->eCurrExp[m] > expMax)
expMax = psi->eCurrExp[m];
}
} else {
for (p = 0; p < n; p++) {
mStart = freqBandTab[p];
mEnd = freqBandTab[p+1];
eCurr.w64 = 0;
for (i = iStart; i < iEnd; i++) {
XBuf = psi->XBuf[i][mStart];
for (m = mStart; m < mEnd; m++) {
xre = (*XBuf++) >> FBITS_OUT_QMFA;
xim = (*XBuf++) >> FBITS_OUT_QMFA;
eCurr.w64 = MADD64(eCurr.w64, xre, xre);
eCurr.w64 = MADD64(eCurr.w64, xim, xim);
}
}
nScale = 0;
if (eCurr.r.hi32) {
nScale = (32 - CLZ(eCurr.r.hi32)) + 1;
t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */
t |= eCurr.r.hi32 << (32 - nScale);
} else if (eCurr.r.lo32 >> 31) {
nScale = 1;
t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */
} else {
t = (int)eCurr.r.lo32;
}
invFact = invBandTab[(iEnd - iStart)-1];
invFact = MULSHIFT32(invBandTab[(mEnd - mStart)-1], invFact) << 1;
t = MULSHIFT32(t, invFact);
for (m = mStart; m < mEnd; m++) {
psi->eCurr[m - sbrFreq->kStart] = t;
psi->eCurrExp[m - sbrFreq->kStart] = nScale + 1; /* +1 for invFact = Q31 */
}
if (psi->eCurrExp[mStart - sbrFreq->kStart] > expMax)
expMax = psi->eCurrExp[mStart - sbrFreq->kStart];
}
}
psi->eCurrExpMax = expMax;
}
/**************************************************************************************
* Function: GetSMapped
*
* Description: calculate SMapped (4.6.18.7.2)
*
* Inputs: initialized PSInfoSBR struct
* initialized SBRGrid struct for this channel
* initialized SBRFreq struct for this SCE/CPE block
* initialized SBRChan struct for this channel
* index of current envelope
* index of current QMF band
* la flag for this envelope
*
* Outputs: none
*
* Return: 1 if a sinusoid is present in this band, 0 if not
**************************************************************************************/
static int GetSMapped(SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int band, int la)
{
int bandStart, bandEnd, oddFlag, r;
if (sbrGrid->freqRes[env]) {
/* high resolution */
bandStart = band;
bandEnd = band+1;
} else {
/* low resolution (see CalcFreqLow() for mapping) */
oddFlag = sbrFreq->nHigh & 0x01;
bandStart = (band > 0 ? 2*band - oddFlag : 0); /* starting index for freqLow[band] */
bandEnd = 2*(band+1) - oddFlag; /* ending index for freqLow[band+1] */
}
/* sMapped = 1 if sIndexMapped == 1 for any frequency in this band */
for (band = bandStart; band < bandEnd; band++) {
if (sbrChan->addHarmonic[1][band]) {
r = ((sbrFreq->freqHigh[band+1] + sbrFreq->freqHigh[band]) >> 1);
if (env >= la || sbrChan->addHarmonic[0][r] == 1)
return 1;
}
}
return 0;
}
#define GBOOST_MAX 0x2830afd3 /* Q28, 1.584893192 squared */
#define ACC_SCALE 6
/* squared version of table in 4.6.18.7.5 */
static const int limGainTab[4] PROGMEM = {0x20138ca7, 0x40000000, 0x7fb27dce, 0x80000000}; /* Q30 (0x80000000 = sentinel for GMAX) */
/**************************************************************************************
* Function: CalcMaxGain
*
* Description: calculate max gain in one limiter band (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
* index of current channel (0 for SCE, 0 or 1 for CPE)
* index of current envelope
* index of current limiter band
* number of fraction bits in dequantized envelope
* (max = Q(FBITS_OUT_DQ_ENV - 6) = Q23, can go negative)
*
* Outputs: updated gainMax, gainMaxFBits, and sumEOrigMapped in PSInfoSBR struct
*
* Return: none
**************************************************************************************/
static void CalcMaxGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int ch, int env, int lim, int fbitsDQ)
{
int m, mStart, mEnd, q, z, r;
int sumEOrigMapped, sumECurr, gainMax, eOMGainMax, envBand;
unsigned char eCurrExpMax;
unsigned char *freqBandTab;
mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */
mEnd = sbrFreq->freqLimiter[lim + 1];
freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow);
/* calculate max gain to apply to signal in this limiter band */
sumECurr = 0;
sumEOrigMapped = 0;
eCurrExpMax = psi->eCurrExpMax;
eOMGainMax = psi->eOMGainMax;
envBand = psi->envBand;
for (m = mStart; m < mEnd; m++) {
/* map current QMF band to appropriate envelope band */
if (m == freqBandTab[envBand + 1] - sbrFreq->kStart) {
envBand++;
eOMGainMax = psi->envDataDequant[ch][env][envBand] >> ACC_SCALE; /* summing max 48 bands */
}
sumEOrigMapped += eOMGainMax;
/* easy test for overflow on ARM */
sumECurr += (psi->eCurr[m] >> (eCurrExpMax - psi->eCurrExp[m]));
if (sumECurr >> 30) {
sumECurr >>= 1;
eCurrExpMax++;
}
}
psi->eOMGainMax = eOMGainMax;
psi->envBand = envBand;
psi->gainMaxFBits = 30; /* Q30 tables */
if (sumECurr == 0) {
/* any non-zero numerator * 1/EPS_0 is > G_MAX */
gainMax = (sumEOrigMapped == 0 ? (int)limGainTab[sbrHdr->limiterGains] : (int)0x80000000);
} else if (sumEOrigMapped == 0) {
/* 1/(any non-zero denominator) * EPS_0 * limGainTab[x] is appx. 0 */
gainMax = 0;
} else {
/* sumEOrigMapped = Q(fbitsDQ - ACC_SCALE), sumECurr = Q(-eCurrExpMax) */
gainMax = limGainTab[sbrHdr->limiterGains];
if (sbrHdr->limiterGains != 3) {
q = MULSHIFT32(sumEOrigMapped, gainMax); /* Q(fbitsDQ - ACC_SCALE - 2), gainMax = Q30 */
z = CLZ(sumECurr) - 1;
r = InvRNormalized(sumECurr << z); /* in = Q(z - eCurrExpMax), out = Q(29 + 31 - z + eCurrExpMax) */
gainMax = MULSHIFT32(q, r); /* Q(29 + 31 - z + eCurrExpMax + fbitsDQ - ACC_SCALE - 2 - 32) */
psi->gainMaxFBits = 26 - z + eCurrExpMax + fbitsDQ - ACC_SCALE;
}
}
psi->sumEOrigMapped = sumEOrigMapped;
psi->gainMax = gainMax;
}
/**************************************************************************************
* Function: CalcNoiseDivFactors
*
* Description: calculate 1/(1+Q) and Q/(1+Q) (4.6.18.7.4; 4.6.18.7.5)
*
* Inputs: dequantized noise floor scalefactor
*
* Outputs: 1/(1+Q) and Q/(1+Q), format = Q31
*
* Return: none
**************************************************************************************/
static void CalcNoiseDivFactors(int q, int *qp1Inv, int *qqp1Inv)
{
int z, qp1, t, s;
/* 1 + Q_orig */
qp1 = (q >> 1);
qp1 += (1 << (FBITS_OUT_DQ_NOISE - 1)); /* >> 1 to avoid overflow when adding 1.0 */
z = CLZ(qp1) - 1; /* z <= 31 - FBITS_OUT_DQ_NOISE */
qp1 <<= z; /* Q(FBITS_OUT_DQ_NOISE + z) = Q31 * 2^-(31 - (FBITS_OUT_DQ_NOISE + z)) */
t = InvRNormalized(qp1) << 1; /* Q30 * 2^(31 - (FBITS_OUT_DQ_NOISE + z)), guaranteed not to overflow */
/* normalize to Q31 */
s = (31 - (FBITS_OUT_DQ_NOISE - 1) - z - 1); /* clearly z >= 0, z <= (30 - (FBITS_OUT_DQ_NOISE - 1)) */
*qp1Inv = (t >> s); /* s = [0, 31 - FBITS_OUT_DQ_NOISE] */
*qqp1Inv = MULSHIFT32(t, q) << (32 - FBITS_OUT_DQ_NOISE - s);
}
/**************************************************************************************
* Function: CalcComponentGains
*
* Description: calculate gain of envelope, sinusoids, and noise in one limiter band
* (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
* index of current limiter band
* number of fraction bits in dequantized envelope
*
* Outputs: gains for envelope, sinusoids and noise
* number of fraction bits for envelope gain
* sum of the total gain for each component in this band
* other updated state variables
*
* Return: none
**************************************************************************************/
static void CalcComponentGains(PSInfoSBR *psi, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env, int lim, int fbitsDQ)
{
int d, m, mStart, mEnd, q, qm, noiseFloor, sIndexMapped;
int shift, eCurr, maxFlag, gainMax, gainMaxFBits;
int gain, sm, z, r, fbitsGain, gainScale;
unsigned char *freqBandTab;
mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */
mEnd = sbrFreq->freqLimiter[lim + 1];
gainMax = psi->gainMax;
gainMaxFBits = psi->gainMaxFBits;
d = (env == psi->la || env == sbrChan->laPrev ? 0 : 1);
freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow);
/* figure out which noise floor this envelope is in (only 1 or 2 noise floors allowed) */
noiseFloor = 0;
if (sbrGrid->numNoiseFloors == 2 && sbrGrid->noiseTimeBorder[1] <= sbrGrid->envTimeBorder[env])
noiseFloor++;
psi->sumECurrGLim = 0;
psi->sumSM = 0;
psi->sumQM = 0;
/* calculate energy of noise to add in this limiter band */
for (m = mStart; m < mEnd; m++) {
if (m == sbrFreq->freqNoise[psi->noiseFloorBand + 1] - sbrFreq->kStart) {
/* map current QMF band to appropriate noise floor band (NOTE: freqLimiter[0] == freqLow[0] = freqHigh[0]) */
psi->noiseFloorBand++;
CalcNoiseDivFactors(psi->noiseDataDequant[ch][noiseFloor][psi->noiseFloorBand], &(psi->qp1Inv), &(psi->qqp1Inv));
}
if (m == sbrFreq->freqHigh[psi->highBand + 1] - sbrFreq->kStart)
psi->highBand++;
if (m == freqBandTab[psi->sBand + 1] - sbrFreq->kStart) {
psi->sBand++;
psi->sMapped = GetSMapped(sbrGrid, sbrFreq, sbrChan, env, psi->sBand, psi->la);
}
/* get sIndexMapped for this QMF subband */
sIndexMapped = 0;
r = ((sbrFreq->freqHigh[psi->highBand+1] + sbrFreq->freqHigh[psi->highBand]) >> 1);
if (m + sbrFreq->kStart == r) {
/* r = center frequency, deltaStep = (env >= la || sIndexMapped'(r, numEnv'-1) == 1) */
if (env >= psi->la || sbrChan->addHarmonic[0][r] == 1)
sIndexMapped = sbrChan->addHarmonic[1][psi->highBand];
}
/* save sine flags from last envelope in this frame:
* addHarmonic[0][0...63] = saved sine present flag from previous frame, for each QMF subband
* addHarmonic[1][0...nHigh-1] = addHarmonic bit from current frame, for each high-res frequency band
* from MPEG reference code - slightly different from spec
* (sIndexMapped'(m,LE'-1) can still be 0 when numEnv == psi->la)
*/
if (env == sbrGrid->numEnv - 1) {
if (m + sbrFreq->kStart == r)
sbrChan->addHarmonic[0][m + sbrFreq->kStart] = sbrChan->addHarmonic[1][psi->highBand];
else
sbrChan->addHarmonic[0][m + sbrFreq->kStart] = 0;
}
gain = psi->envDataDequant[ch][env][psi->sBand];
qm = MULSHIFT32(gain, psi->qqp1Inv) << 1;
sm = (sIndexMapped ? MULSHIFT32(gain, psi->qp1Inv) << 1 : 0);
/* three cases: (sMapped == 0 && delta == 1), (sMapped == 0 && delta == 0), (sMapped == 1) */
if (d == 1 && psi->sMapped == 0)
gain = MULSHIFT32(psi->qp1Inv, gain) << 1;
else if (psi->sMapped != 0)
gain = MULSHIFT32(psi->qqp1Inv, gain) << 1;
/* gain, qm, sm = Q(fbitsDQ), gainMax = Q(fbitsGainMax) */
eCurr = psi->eCurr[m];
if (eCurr) {
z = CLZ(eCurr) - 1;
r = InvRNormalized(eCurr << z); /* in = Q(z - eCurrExp), out = Q(29 + 31 - z + eCurrExp) */
gainScale = MULSHIFT32(gain, r); /* out = Q(29 + 31 - z + eCurrExp + fbitsDQ - 32) */
fbitsGain = 29 + 31 - z + psi->eCurrExp[m] + fbitsDQ - 32;
} else {
/* if eCurr == 0, then gain is unchanged (divide by EPS = 1) */
gainScale = gain;
fbitsGain = fbitsDQ;
}
/* see if gain for this band exceeds max gain */
maxFlag = 0;
if (gainMax != (int)0x80000000) {
if (fbitsGain >= gainMaxFBits) {
shift = MIN(fbitsGain - gainMaxFBits, 31);
maxFlag = ((gainScale >> shift) > gainMax ? 1 : 0);
} else {
shift = MIN(gainMaxFBits - fbitsGain, 31);
maxFlag = (gainScale > (gainMax >> shift) ? 1 : 0);
}
}
if (maxFlag) {
/* gainScale > gainMax, calculate ratio with 32/16 division */
q = 0;
r = gainScale; /* guaranteed > 0, else maxFlag could not have been set */
z = CLZ(r);
if (z < 16) {
q = 16 - z;
r >>= q; /* out = Q(fbitsGain - q) */
}
z = CLZ(gainMax) - 1;
r = (gainMax << z) / r; /* out = Q((fbitsGainMax + z) - (fbitsGain - q)) */
q = (gainMaxFBits + z) - (fbitsGain - q); /* r = Q(q) */
if (q > 30) {
r >>= MIN(q - 30, 31);
} else {
z = MIN(30 - q, 30);
CLIP_2N_SHIFT30(r, z); /* let r = Q30 since range = [0.0, 1.0) (clip to 0x3fffffff = 0.99999) */
}
qm = MULSHIFT32(qm, r) << 2;
gain = MULSHIFT32(gain, r) << 2;
psi->gLimBuf[m] = gainMax;
psi->gLimFbits[m] = gainMaxFBits;
} else {
psi->gLimBuf[m] = gainScale;
psi->gLimFbits[m] = fbitsGain;
}
/* sumSM, sumQM, sumECurrGLim = Q(fbitsDQ - ACC_SCALE) */
psi->smBuf[m] = sm;
psi->sumSM += (sm >> ACC_SCALE);
psi->qmLimBuf[m] = qm;
if (env != psi->la && env != sbrChan->laPrev && sm == 0)
psi->sumQM += (qm >> ACC_SCALE);
/* eCurr * gain^2 same as gain^2, before division by eCurr
* (but note that gain != 0 even if eCurr == 0, since it's divided by eps)
*/
if (eCurr)
psi->sumECurrGLim += (gain >> ACC_SCALE);
}
}
/**************************************************************************************
* Function: ApplyBoost
*
* Description: calculate and apply boost factor for envelope, sinusoids, and noise
* in this limiter band (4.6.18.7.5)
*
* Inputs: initialized PSInfoSBR struct
* initialized SBRFreq struct for this SCE/CPE block
* index of current limiter band
* number of fraction bits in dequantized envelope
*
* Outputs: envelope gain, sinusoids and noise after scaling by gBoost
* format = Q(FBITS_GLIM_BOOST) for envelope gain,
* = Q(FBITS_QLIM_BOOST) for noise
* = Q(FBITS_OUT_QMFA) for sinusoids
*
* Return: none
*
* Notes: after scaling, each component has at least 1 GB
**************************************************************************************/
static void ApplyBoost(PSInfoSBR *psi, SBRFreq *sbrFreq, int lim, int fbitsDQ)
{
int m, mStart, mEnd, q, z, r;
int sumEOrigMapped, gBoost;
mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */
mEnd = sbrFreq->freqLimiter[lim + 1];
sumEOrigMapped = psi->sumEOrigMapped >> 1;
r = (psi->sumECurrGLim >> 1) + (psi->sumSM >> 1) + (psi->sumQM >> 1); /* 1 GB fine (sm and qm are mutually exclusive in acc) */
if (r < (1 << (31-28))) {
/* any non-zero numerator * 1/EPS_0 is > GBOOST_MAX
* round very small r to zero to avoid scaling problems
*/
gBoost = (sumEOrigMapped == 0 ? (1 << 28) : GBOOST_MAX);
z = 0;
} else if (sumEOrigMapped == 0) {
/* 1/(any non-zero denominator) * EPS_0 is appx. 0 */
gBoost = 0;
z = 0;
} else {
/* numerator (sumEOrigMapped) and denominator (r) have same Q format (before << z) */
z = CLZ(r) - 1; /* z = [0, 27] */
r = InvRNormalized(r << z);
gBoost = MULSHIFT32(sumEOrigMapped, r);
}
/* gBoost = Q(28 - z) */
if (gBoost > (GBOOST_MAX >> z)) {
gBoost = GBOOST_MAX;
z = 0;
}
gBoost <<= z; /* gBoost = Q28, minimum 1 GB */
/* convert gain, noise, sinusoids to fixed Q format, clipping if necessary
* (rare, usually only happens at very low bitrates, introduces slight
* distortion into final HF mapping, but should be inaudible)
*/
for (m = mStart; m < mEnd; m++) {
/* let gLimBoost = Q24, since in practice the max values are usually 16 to 20
* unless limiterGains == 3 (limiter off) and eCurr ~= 0 (i.e. huge gain, but only
* because the envelope has 0 power anyway)
*/
q = MULSHIFT32(psi->gLimBuf[m], gBoost) << 2; /* Q(gLimFbits) * Q(28) --> Q(gLimFbits[m]-2) */
r = SqrtFix(q, psi->gLimFbits[m] - 2, &z);
z -= FBITS_GLIM_BOOST;
if (z >= 0) {
psi->gLimBoost[m] = r >> MIN(z, 31);
} else {
z = MIN(30, -z);
CLIP_2N_SHIFT30(r, z);
psi->gLimBoost[m] = r;
}
q = MULSHIFT32(psi->qmLimBuf[m], gBoost) << 2; /* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */
r = SqrtFix(q, fbitsDQ - 2, &z);
z -= FBITS_QLIM_BOOST; /* << by 14, since integer sqrt of x < 2^16, and we want to leave 1 GB */
if (z >= 0) {
psi->qmLimBoost[m] = r >> MIN(31, z);
} else {
z = MIN(30, -z);
CLIP_2N_SHIFT30(r, z);
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] PROGMEM = {
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;
}
Reference in New Issue View Git Blame Copy Permalink