blob: d0a85ace15be475750b61c6b0f4aebb6976310d0
1 | /* ***** BEGIN LICENSE BLOCK ***** |
2 | * Source last modified: $Id: sbrhfadj.c,v 1.1.2.3 2005/05/24 20:34:40 albertofloyd Exp $ |
3 | * |
4 | * Portions Copyright (c) 1995-2005 RealNetworks, Inc. All Rights Reserved. |
5 | * |
6 | * The contents of this file, and the files included with this file, |
7 | * are subject to the current version of the RealNetworks Public |
8 | * Source License (the "RPSL") available at |
9 | * http://www.helixcommunity.org/content/rpsl unless you have licensed |
10 | * the file under the current version of the RealNetworks Community |
11 | * Source License (the "RCSL") available at |
12 | * http://www.helixcommunity.org/content/rcsl, in which case the RCSL |
13 | * will apply. You may also obtain the license terms directly from |
14 | * RealNetworks. You may not use this file except in compliance with |
15 | * the RPSL or, if you have a valid RCSL with RealNetworks applicable |
16 | * to this file, the RCSL. Please see the applicable RPSL or RCSL for |
17 | * the rights, obligations and limitations governing use of the |
18 | * contents of the file. |
19 | * |
20 | * This file is part of the Helix DNA Technology. RealNetworks is the |
21 | * developer of the Original Code and owns the copyrights in the |
22 | * portions it created. |
23 | * |
24 | * This file, and the files included with this file, is distributed |
25 | * and made available on an 'AS IS' basis, WITHOUT WARRANTY OF ANY |
26 | * KIND, EITHER EXPRESS OR IMPLIED, AND REALNETWORKS HEREBY DISCLAIMS |
27 | * ALL SUCH WARRANTIES, INCLUDING WITHOUT LIMITATION, ANY WARRANTIES |
28 | * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, QUIET |
29 | * ENJOYMENT OR NON-INFRINGEMENT. |
30 | * |
31 | * Technology Compatibility Kit Test Suite(s) Location: |
32 | * http://www.helixcommunity.org/content/tck |
33 | * |
34 | * Contributor(s): |
35 | * |
36 | * ***** END LICENSE BLOCK ***** */ |
37 | |
38 | /************************************************************************************** |
39 | * Fixed-point HE-AAC decoder |
40 | * Jon Recker (jrecker@real.com) |
41 | * February 2005 |
42 | * |
43 | * sbrhfadj.c - high frequency adjustment for SBR |
44 | **************************************************************************************/ |
45 | |
46 | #include "sbr.h" |
47 | #include "assembly.h" |
48 | |
49 | /* invBandTab[i] = 1.0 / (i + 1), Q31 */ |
50 | static const int invBandTab[64] = { |
51 | 0x7fffffff, 0x40000000, 0x2aaaaaab, 0x20000000, 0x1999999a, 0x15555555, 0x12492492, 0x10000000, |
52 | 0x0e38e38e, 0x0ccccccd, 0x0ba2e8ba, 0x0aaaaaab, 0x09d89d8a, 0x09249249, 0x08888889, 0x08000000, |
53 | 0x07878788, 0x071c71c7, 0x06bca1af, 0x06666666, 0x06186186, 0x05d1745d, 0x0590b216, 0x05555555, |
54 | 0x051eb852, 0x04ec4ec5, 0x04bda12f, 0x04924925, 0x0469ee58, 0x04444444, 0x04210842, 0x04000000, |
55 | 0x03e0f83e, 0x03c3c3c4, 0x03a83a84, 0x038e38e4, 0x03759f23, 0x035e50d8, 0x03483483, 0x03333333, |
56 | 0x031f3832, 0x030c30c3, 0x02fa0be8, 0x02e8ba2f, 0x02d82d83, 0x02c8590b, 0x02b93105, 0x02aaaaab, |
57 | 0x029cbc15, 0x028f5c29, 0x02828283, 0x02762762, 0x026a439f, 0x025ed098, 0x0253c825, 0x02492492, |
58 | 0x023ee090, 0x0234f72c, 0x022b63cc, 0x02222222, 0x02192e2a, 0x02108421, 0x02082082, 0x02000000, |
59 | }; |
60 | |
61 | /************************************************************************************** |
62 | * Function: EstimateEnvelope |
63 | * |
64 | * Description: estimate power of generated HF QMF bands in one time-domain envelope |
65 | * (4.6.18.7.3) |
66 | * |
67 | * Inputs: initialized PSInfoSBR struct |
68 | * initialized SBRHeader struct for this SCE/CPE block |
69 | * initialized SBRGrid struct for this channel |
70 | * initialized SBRFreq struct for this SCE/CPE block |
71 | * index of current envelope |
72 | * |
73 | * Outputs: power of each QMF subband, stored as integer (Q0) * 2^N, N >= 0 |
74 | * |
75 | * Return: none |
76 | **************************************************************************************/ |
77 | static void EstimateEnvelope(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int env) |
78 | { |
79 | int i, m, iStart, iEnd, xre, xim, nScale, expMax; |
80 | int p, n, mStart, mEnd, invFact, t; |
81 | int *XBuf; |
82 | U64 eCurr; |
83 | unsigned char *freqBandTab; |
84 | |
85 | /* estimate current envelope */ |
86 | iStart = sbrGrid->envTimeBorder[env] + HF_ADJ; |
87 | iEnd = sbrGrid->envTimeBorder[env + 1] + HF_ADJ; |
88 | if (sbrGrid->freqRes[env]) { |
89 | n = sbrFreq->nHigh; |
90 | freqBandTab = sbrFreq->freqHigh; |
91 | } else { |
92 | n = sbrFreq->nLow; |
93 | freqBandTab = sbrFreq->freqLow; |
94 | } |
95 | |
96 | /* ADS should inline MADD64 (smlal) properly, but check to make sure */ |
97 | expMax = 0; |
98 | if (sbrHdr->interpFreq) { |
99 | for (m = 0; m < sbrFreq->numQMFBands; m++) { |
100 | eCurr.w64 = 0; |
101 | XBuf = psi->XBuf[iStart][sbrFreq->kStart + m]; |
102 | for (i = iStart; i < iEnd; i++) { |
103 | /* scale to int before calculating power (precision not critical, and avoids overflow) */ |
104 | xre = (*XBuf) >> FBITS_OUT_QMFA; |
105 | XBuf += 1; |
106 | xim = (*XBuf) >> FBITS_OUT_QMFA; |
107 | XBuf += (2 * 64 - 1); |
108 | eCurr.w64 = MADD64(eCurr.w64, xre, xre); |
109 | eCurr.w64 = MADD64(eCurr.w64, xim, xim); |
110 | } |
111 | |
112 | /* eCurr.w64 is now Q(64 - 2*FBITS_OUT_QMFA) (64-bit word) |
113 | * if energy is too big to fit in 32-bit word (> 2^31) scale down by power of 2 |
114 | */ |
115 | nScale = 0; |
116 | if (eCurr.r.hi32) { |
117 | nScale = (32 - CLZ(eCurr.r.hi32)) + 1; |
118 | t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */ |
119 | t |= eCurr.r.hi32 << (32 - nScale); |
120 | } else if (eCurr.r.lo32 >> 31) { |
121 | nScale = 1; |
122 | t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */ |
123 | } else { |
124 | t = (int)eCurr.r.lo32; |
125 | } |
126 | |
127 | invFact = invBandTab[(iEnd - iStart) - 1]; |
128 | psi->eCurr[m] = MULSHIFT32(t, invFact); |
129 | psi->eCurrExp[m] = nScale + 1; /* +1 for invFact = Q31 */ |
130 | if (psi->eCurrExp[m] > expMax) { |
131 | expMax = psi->eCurrExp[m]; |
132 | } |
133 | } |
134 | } else { |
135 | for (p = 0; p < n; p++) { |
136 | mStart = freqBandTab[p]; |
137 | mEnd = freqBandTab[p + 1]; |
138 | eCurr.w64 = 0; |
139 | for (i = iStart; i < iEnd; i++) { |
140 | XBuf = psi->XBuf[i][mStart]; |
141 | for (m = mStart; m < mEnd; m++) { |
142 | xre = (*XBuf++) >> FBITS_OUT_QMFA; |
143 | xim = (*XBuf++) >> FBITS_OUT_QMFA; |
144 | eCurr.w64 = MADD64(eCurr.w64, xre, xre); |
145 | eCurr.w64 = MADD64(eCurr.w64, xim, xim); |
146 | } |
147 | } |
148 | |
149 | nScale = 0; |
150 | if (eCurr.r.hi32) { |
151 | nScale = (32 - CLZ(eCurr.r.hi32)) + 1; |
152 | t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */ |
153 | t |= eCurr.r.hi32 << (32 - nScale); |
154 | } else if (eCurr.r.lo32 >> 31) { |
155 | nScale = 1; |
156 | t = (int)(eCurr.r.lo32 >> nScale); /* logical (unsigned) >> */ |
157 | } else { |
158 | t = (int)eCurr.r.lo32; |
159 | } |
160 | |
161 | invFact = invBandTab[(iEnd - iStart) - 1]; |
162 | invFact = MULSHIFT32(invBandTab[(mEnd - mStart) - 1], invFact) << 1; |
163 | t = MULSHIFT32(t, invFact); |
164 | |
165 | for (m = mStart; m < mEnd; m++) { |
166 | psi->eCurr[m - sbrFreq->kStart] = t; |
167 | psi->eCurrExp[m - sbrFreq->kStart] = nScale + 1; /* +1 for invFact = Q31 */ |
168 | } |
169 | if (psi->eCurrExp[mStart - sbrFreq->kStart] > expMax) { |
170 | expMax = psi->eCurrExp[mStart - sbrFreq->kStart]; |
171 | } |
172 | } |
173 | } |
174 | psi->eCurrExpMax = expMax; |
175 | } |
176 | |
177 | /************************************************************************************** |
178 | * Function: GetSMapped |
179 | * |
180 | * Description: calculate SMapped (4.6.18.7.2) |
181 | * |
182 | * Inputs: initialized PSInfoSBR struct |
183 | * initialized SBRGrid struct for this channel |
184 | * initialized SBRFreq struct for this SCE/CPE block |
185 | * initialized SBRChan struct for this channel |
186 | * index of current envelope |
187 | * index of current QMF band |
188 | * la flag for this envelope |
189 | * |
190 | * Outputs: none |
191 | * |
192 | * Return: 1 if a sinusoid is present in this band, 0 if not |
193 | **************************************************************************************/ |
194 | static int GetSMapped(SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int band, int la) |
195 | { |
196 | int bandStart, bandEnd, oddFlag, r; |
197 | |
198 | if (sbrGrid->freqRes[env]) { |
199 | /* high resolution */ |
200 | bandStart = band; |
201 | bandEnd = band + 1; |
202 | } else { |
203 | /* low resolution (see CalcFreqLow() for mapping) */ |
204 | oddFlag = sbrFreq->nHigh & 0x01; |
205 | bandStart = (band > 0 ? 2 * band - oddFlag : 0); /* starting index for freqLow[band] */ |
206 | bandEnd = 2 * (band + 1) - oddFlag; /* ending index for freqLow[band+1] */ |
207 | } |
208 | |
209 | /* sMapped = 1 if sIndexMapped == 1 for any frequency in this band */ |
210 | for (band = bandStart; band < bandEnd; band++) { |
211 | if (sbrChan->addHarmonic[1][band]) { |
212 | r = ((sbrFreq->freqHigh[band + 1] + sbrFreq->freqHigh[band]) >> 1); |
213 | if (env >= la || sbrChan->addHarmonic[0][r] == 1) { |
214 | return 1; |
215 | } |
216 | } |
217 | } |
218 | return 0; |
219 | } |
220 | |
221 | #define GBOOST_MAX 0x2830afd3 /* Q28, 1.584893192 squared */ |
222 | #define ACC_SCALE 6 |
223 | |
224 | /* squared version of table in 4.6.18.7.5 */ |
225 | static const int limGainTab[4] = {0x20138ca7, 0x40000000, 0x7fb27dce, 0x80000000}; /* Q30 (0x80000000 = sentinel for GMAX) */ |
226 | |
227 | /************************************************************************************** |
228 | * Function: CalcMaxGain |
229 | * |
230 | * Description: calculate max gain in one limiter band (4.6.18.7.5) |
231 | * |
232 | * Inputs: initialized PSInfoSBR struct |
233 | * initialized SBRHeader struct for this SCE/CPE block |
234 | * initialized SBRGrid struct for this channel |
235 | * initialized SBRFreq struct for this SCE/CPE block |
236 | * index of current channel (0 for SCE, 0 or 1 for CPE) |
237 | * index of current envelope |
238 | * index of current limiter band |
239 | * number of fraction bits in dequantized envelope |
240 | * (max = Q(FBITS_OUT_DQ_ENV - 6) = Q23, can go negative) |
241 | * |
242 | * Outputs: updated gainMax, gainMaxFBits, and sumEOrigMapped in PSInfoSBR struct |
243 | * |
244 | * Return: none |
245 | **************************************************************************************/ |
246 | static void CalcMaxGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, int ch, int env, int lim, int fbitsDQ) |
247 | { |
248 | int m, mStart, mEnd, q, z, r; |
249 | int sumEOrigMapped, sumECurr, gainMax, eOMGainMax, envBand; |
250 | unsigned char eCurrExpMax; |
251 | unsigned char *freqBandTab; |
252 | |
253 | mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */ |
254 | mEnd = sbrFreq->freqLimiter[lim + 1]; |
255 | freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow); |
256 | |
257 | /* calculate max gain to apply to signal in this limiter band */ |
258 | sumECurr = 0; |
259 | sumEOrigMapped = 0; |
260 | eCurrExpMax = psi->eCurrExpMax; |
261 | eOMGainMax = psi->eOMGainMax; |
262 | envBand = psi->envBand; |
263 | for (m = mStart; m < mEnd; m++) { |
264 | /* map current QMF band to appropriate envelope band */ |
265 | if (m == freqBandTab[envBand + 1] - sbrFreq->kStart) { |
266 | envBand++; |
267 | eOMGainMax = psi->envDataDequant[ch][env][envBand] >> ACC_SCALE; /* summing max 48 bands */ |
268 | } |
269 | sumEOrigMapped += eOMGainMax; |
270 | |
271 | /* easy test for overflow on ARM */ |
272 | sumECurr += (psi->eCurr[m] >> (eCurrExpMax - psi->eCurrExp[m])); |
273 | if (sumECurr >> 30) { |
274 | sumECurr >>= 1; |
275 | eCurrExpMax++; |
276 | } |
277 | } |
278 | psi->eOMGainMax = eOMGainMax; |
279 | psi->envBand = envBand; |
280 | |
281 | psi->gainMaxFBits = 30; /* Q30 tables */ |
282 | if (sumECurr == 0) { |
283 | /* any non-zero numerator * 1/EPS_0 is > G_MAX */ |
284 | gainMax = (sumEOrigMapped == 0 ? limGainTab[sbrHdr->limiterGains] : 0x80000000); |
285 | } else if (sumEOrigMapped == 0) { |
286 | /* 1/(any non-zero denominator) * EPS_0 * limGainTab[x] is appx. 0 */ |
287 | gainMax = 0; |
288 | } else { |
289 | /* sumEOrigMapped = Q(fbitsDQ - ACC_SCALE), sumECurr = Q(-eCurrExpMax) */ |
290 | gainMax = limGainTab[sbrHdr->limiterGains]; |
291 | if (sbrHdr->limiterGains != 3) { |
292 | q = MULSHIFT32(sumEOrigMapped, gainMax); /* Q(fbitsDQ - ACC_SCALE - 2), gainMax = Q30 */ |
293 | z = CLZ(sumECurr) - 1; |
294 | r = InvRNormalized(sumECurr << z); /* in = Q(z - eCurrExpMax), out = Q(29 + 31 - z + eCurrExpMax) */ |
295 | gainMax = MULSHIFT32(q, r); /* Q(29 + 31 - z + eCurrExpMax + fbitsDQ - ACC_SCALE - 2 - 32) */ |
296 | psi->gainMaxFBits = 26 - z + eCurrExpMax + fbitsDQ - ACC_SCALE; |
297 | } |
298 | } |
299 | psi->sumEOrigMapped = sumEOrigMapped; |
300 | psi->gainMax = gainMax; |
301 | } |
302 | |
303 | /************************************************************************************** |
304 | * Function: CalcNoiseDivFactors |
305 | * |
306 | * Description: calculate 1/(1+Q) and Q/(1+Q) (4.6.18.7.4; 4.6.18.7.5) |
307 | * |
308 | * Inputs: dequantized noise floor scalefactor |
309 | * |
310 | * Outputs: 1/(1+Q) and Q/(1+Q), format = Q31 |
311 | * |
312 | * Return: none |
313 | **************************************************************************************/ |
314 | static void CalcNoiseDivFactors(int q, int *qp1Inv, int *qqp1Inv) |
315 | { |
316 | int z, qp1, t, s; |
317 | |
318 | /* 1 + Q_orig */ |
319 | qp1 = (q >> 1); |
320 | qp1 += (1 << (FBITS_OUT_DQ_NOISE - 1)); /* >> 1 to avoid overflow when adding 1.0 */ |
321 | z = CLZ(qp1) - 1; /* z <= 31 - FBITS_OUT_DQ_NOISE */ |
322 | qp1 <<= z; /* Q(FBITS_OUT_DQ_NOISE + z) = Q31 * 2^-(31 - (FBITS_OUT_DQ_NOISE + z)) */ |
323 | t = InvRNormalized(qp1) << 1; /* Q30 * 2^(31 - (FBITS_OUT_DQ_NOISE + z)), guaranteed not to overflow */ |
324 | |
325 | /* normalize to Q31 */ |
326 | s = (31 - (FBITS_OUT_DQ_NOISE - 1) - z - 1); /* clearly z >= 0, z <= (30 - (FBITS_OUT_DQ_NOISE - 1)) */ |
327 | *qp1Inv = (t >> s); /* s = [0, 31 - FBITS_OUT_DQ_NOISE] */ |
328 | *qqp1Inv = MULSHIFT32(t, q) << (32 - FBITS_OUT_DQ_NOISE - s); |
329 | } |
330 | |
331 | /************************************************************************************** |
332 | * Function: CalcComponentGains |
333 | * |
334 | * Description: calculate gain of envelope, sinusoids, and noise in one limiter band |
335 | * (4.6.18.7.5) |
336 | * |
337 | * Inputs: initialized PSInfoSBR struct |
338 | * initialized SBRHeader struct for this SCE/CPE block |
339 | * initialized SBRGrid struct for this channel |
340 | * initialized SBRFreq struct for this SCE/CPE block |
341 | * initialized SBRChan struct for this channel |
342 | * index of current channel (0 for SCE, 0 or 1 for CPE) |
343 | * index of current envelope |
344 | * index of current limiter band |
345 | * number of fraction bits in dequantized envelope |
346 | * |
347 | * Outputs: gains for envelope, sinusoids and noise |
348 | * number of fraction bits for envelope gain |
349 | * sum of the total gain for each component in this band |
350 | * other updated state variables |
351 | * |
352 | * Return: none |
353 | **************************************************************************************/ |
354 | static void CalcComponentGains(PSInfoSBR *psi, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env, int lim, int fbitsDQ) |
355 | { |
356 | int d, m, mStart, mEnd, q, qm, noiseFloor, sIndexMapped; |
357 | int shift, eCurr, maxFlag, gainMax, gainMaxFBits; |
358 | int gain, sm, z, r, fbitsGain, gainScale; |
359 | unsigned char *freqBandTab; |
360 | |
361 | mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */ |
362 | mEnd = sbrFreq->freqLimiter[lim + 1]; |
363 | |
364 | gainMax = psi->gainMax; |
365 | gainMaxFBits = psi->gainMaxFBits; |
366 | |
367 | d = (env == psi->la || env == sbrChan->laPrev ? 0 : 1); |
368 | freqBandTab = (sbrGrid->freqRes[env] ? sbrFreq->freqHigh : sbrFreq->freqLow); |
369 | |
370 | /* figure out which noise floor this envelope is in (only 1 or 2 noise floors allowed) */ |
371 | noiseFloor = 0; |
372 | if (sbrGrid->numNoiseFloors == 2 && sbrGrid->noiseTimeBorder[1] <= sbrGrid->envTimeBorder[env]) { |
373 | noiseFloor++; |
374 | } |
375 | |
376 | psi->sumECurrGLim = 0; |
377 | psi->sumSM = 0; |
378 | psi->sumQM = 0; |
379 | /* calculate energy of noise to add in this limiter band */ |
380 | for (m = mStart; m < mEnd; m++) { |
381 | if (m == sbrFreq->freqNoise[psi->noiseFloorBand + 1] - sbrFreq->kStart) { |
382 | /* map current QMF band to appropriate noise floor band (NOTE: freqLimiter[0] == freqLow[0] = freqHigh[0]) */ |
383 | psi->noiseFloorBand++; |
384 | CalcNoiseDivFactors(psi->noiseDataDequant[ch][noiseFloor][psi->noiseFloorBand], &(psi->qp1Inv), &(psi->qqp1Inv)); |
385 | } |
386 | if (m == sbrFreq->freqHigh[psi->highBand + 1] - sbrFreq->kStart) { |
387 | psi->highBand++; |
388 | } |
389 | if (m == freqBandTab[psi->sBand + 1] - sbrFreq->kStart) { |
390 | psi->sBand++; |
391 | psi->sMapped = GetSMapped(sbrGrid, sbrFreq, sbrChan, env, psi->sBand, psi->la); |
392 | } |
393 | |
394 | /* get sIndexMapped for this QMF subband */ |
395 | sIndexMapped = 0; |
396 | r = ((sbrFreq->freqHigh[psi->highBand + 1] + sbrFreq->freqHigh[psi->highBand]) >> 1); |
397 | if (m + sbrFreq->kStart == r) { |
398 | /* r = center frequency, deltaStep = (env >= la || sIndexMapped'(r, numEnv'-1) == 1) */ |
399 | if (env >= psi->la || sbrChan->addHarmonic[0][r] == 1) { |
400 | sIndexMapped = sbrChan->addHarmonic[1][psi->highBand]; |
401 | } |
402 | } |
403 | |
404 | /* save sine flags from last envelope in this frame: |
405 | * addHarmonic[0][0...63] = saved sine present flag from previous frame, for each QMF subband |
406 | * addHarmonic[1][0...nHigh-1] = addHarmonic bit from current frame, for each high-res frequency band |
407 | * from MPEG reference code - slightly different from spec |
408 | * (sIndexMapped'(m,LE'-1) can still be 0 when numEnv == psi->la) |
409 | */ |
410 | if (env == sbrGrid->numEnv - 1) { |
411 | if (m + sbrFreq->kStart == r) { |
412 | sbrChan->addHarmonic[0][m + sbrFreq->kStart] = sbrChan->addHarmonic[1][psi->highBand]; |
413 | } else { |
414 | sbrChan->addHarmonic[0][m + sbrFreq->kStart] = 0; |
415 | } |
416 | } |
417 | |
418 | gain = psi->envDataDequant[ch][env][psi->sBand]; |
419 | qm = MULSHIFT32(gain, psi->qqp1Inv) << 1; |
420 | sm = (sIndexMapped ? MULSHIFT32(gain, psi->qp1Inv) << 1 : 0); |
421 | |
422 | /* three cases: (sMapped == 0 && delta == 1), (sMapped == 0 && delta == 0), (sMapped == 1) */ |
423 | if (d == 1 && psi->sMapped == 0) { |
424 | gain = MULSHIFT32(psi->qp1Inv, gain) << 1; |
425 | } else if (psi->sMapped != 0) { |
426 | gain = MULSHIFT32(psi->qqp1Inv, gain) << 1; |
427 | } |
428 | |
429 | /* gain, qm, sm = Q(fbitsDQ), gainMax = Q(fbitsGainMax) */ |
430 | eCurr = psi->eCurr[m]; |
431 | if (eCurr) { |
432 | z = CLZ(eCurr) - 1; |
433 | r = InvRNormalized(eCurr << z); /* in = Q(z - eCurrExp), out = Q(29 + 31 - z + eCurrExp) */ |
434 | gainScale = MULSHIFT32(gain, r); /* out = Q(29 + 31 - z + eCurrExp + fbitsDQ - 32) */ |
435 | fbitsGain = 29 + 31 - z + psi->eCurrExp[m] + fbitsDQ - 32; |
436 | } else { |
437 | /* if eCurr == 0, then gain is unchanged (divide by EPS = 1) */ |
438 | gainScale = gain; |
439 | fbitsGain = fbitsDQ; |
440 | } |
441 | |
442 | /* see if gain for this band exceeds max gain */ |
443 | maxFlag = 0; |
444 | if (gainMax != (int)0x80000000) { |
445 | if (fbitsGain >= gainMaxFBits) { |
446 | shift = MIN(fbitsGain - gainMaxFBits, 31); |
447 | maxFlag = ((gainScale >> shift) > gainMax ? 1 : 0); |
448 | } else { |
449 | shift = MIN(gainMaxFBits - fbitsGain, 31); |
450 | maxFlag = (gainScale > (gainMax >> shift) ? 1 : 0); |
451 | } |
452 | } |
453 | |
454 | if (maxFlag) { |
455 | /* gainScale > gainMax, calculate ratio with 32/16 division */ |
456 | q = 0; |
457 | r = gainScale; /* guaranteed > 0, else maxFlag could not have been set */ |
458 | z = CLZ(r); |
459 | if (z < 16) { |
460 | q = 16 - z; |
461 | r >>= q; /* out = Q(fbitsGain - q) */ |
462 | } |
463 | |
464 | z = CLZ(gainMax) - 1; |
465 | r = (gainMax << z) / r; /* out = Q((fbitsGainMax + z) - (fbitsGain - q)) */ |
466 | q = (gainMaxFBits + z) - (fbitsGain - q); /* r = Q(q) */ |
467 | if (q > 30) { |
468 | r >>= MIN(q - 30, 31); |
469 | } else { |
470 | z = MIN(30 - q, 30); |
471 | CLIP_2N_SHIFT30(r, z); /* let r = Q30 since range = [0.0, 1.0) (clip to 0x3fffffff = 0.99999) */ |
472 | } |
473 | |
474 | qm = MULSHIFT32(qm, r) << 2; |
475 | gain = MULSHIFT32(gain, r) << 2; |
476 | psi->gLimBuf[m] = gainMax; |
477 | psi->gLimFbits[m] = gainMaxFBits; |
478 | } else { |
479 | psi->gLimBuf[m] = gainScale; |
480 | psi->gLimFbits[m] = fbitsGain; |
481 | } |
482 | |
483 | /* sumSM, sumQM, sumECurrGLim = Q(fbitsDQ - ACC_SCALE) */ |
484 | psi->smBuf[m] = sm; |
485 | psi->sumSM += (sm >> ACC_SCALE); |
486 | |
487 | psi->qmLimBuf[m] = qm; |
488 | if (env != psi->la && env != sbrChan->laPrev && sm == 0) { |
489 | psi->sumQM += (qm >> ACC_SCALE); |
490 | } |
491 | |
492 | /* eCurr * gain^2 same as gain^2, before division by eCurr |
493 | * (but note that gain != 0 even if eCurr == 0, since it's divided by eps) |
494 | */ |
495 | if (eCurr) { |
496 | psi->sumECurrGLim += (gain >> ACC_SCALE); |
497 | } |
498 | } |
499 | } |
500 | |
501 | /************************************************************************************** |
502 | * Function: ApplyBoost |
503 | * |
504 | * Description: calculate and apply boost factor for envelope, sinusoids, and noise |
505 | * in this limiter band (4.6.18.7.5) |
506 | * |
507 | * Inputs: initialized PSInfoSBR struct |
508 | * initialized SBRFreq struct for this SCE/CPE block |
509 | * index of current limiter band |
510 | * number of fraction bits in dequantized envelope |
511 | * |
512 | * Outputs: envelope gain, sinusoids and noise after scaling by gBoost |
513 | * format = Q(FBITS_GLIM_BOOST) for envelope gain, |
514 | * = Q(FBITS_QLIM_BOOST) for noise |
515 | * = Q(FBITS_OUT_QMFA) for sinusoids |
516 | * |
517 | * Return: none |
518 | * |
519 | * Notes: after scaling, each component has at least 1 GB |
520 | **************************************************************************************/ |
521 | static void ApplyBoost(PSInfoSBR *psi, SBRFreq *sbrFreq, int lim, int fbitsDQ) |
522 | { |
523 | int m, mStart, mEnd, q, z, r; |
524 | int sumEOrigMapped, gBoost; |
525 | |
526 | mStart = sbrFreq->freqLimiter[lim]; /* these are offsets from kStart */ |
527 | mEnd = sbrFreq->freqLimiter[lim + 1]; |
528 | |
529 | sumEOrigMapped = psi->sumEOrigMapped >> 1; |
530 | r = (psi->sumECurrGLim >> 1) + (psi->sumSM >> 1) + (psi->sumQM >> 1); /* 1 GB fine (sm and qm are mutually exclusive in acc) */ |
531 | if (r < (1 << (31 - 28))) { |
532 | /* any non-zero numerator * 1/EPS_0 is > GBOOST_MAX |
533 | * round very small r to zero to avoid scaling problems |
534 | */ |
535 | gBoost = (sumEOrigMapped == 0 ? (1 << 28) : GBOOST_MAX); |
536 | z = 0; |
537 | } else if (sumEOrigMapped == 0) { |
538 | /* 1/(any non-zero denominator) * EPS_0 is appx. 0 */ |
539 | gBoost = 0; |
540 | z = 0; |
541 | } else { |
542 | /* numerator (sumEOrigMapped) and denominator (r) have same Q format (before << z) */ |
543 | z = CLZ(r) - 1; /* z = [0, 27] */ |
544 | r = InvRNormalized(r << z); |
545 | gBoost = MULSHIFT32(sumEOrigMapped, r); |
546 | } |
547 | |
548 | /* gBoost = Q(28 - z) */ |
549 | if (gBoost > (GBOOST_MAX >> z)) { |
550 | gBoost = GBOOST_MAX; |
551 | z = 0; |
552 | } |
553 | gBoost <<= z; /* gBoost = Q28, minimum 1 GB */ |
554 | |
555 | /* convert gain, noise, sinusoids to fixed Q format, clipping if necessary |
556 | * (rare, usually only happens at very low bitrates, introduces slight |
557 | * distortion into final HF mapping, but should be inaudible) |
558 | */ |
559 | for (m = mStart; m < mEnd; m++) { |
560 | /* let gLimBoost = Q24, since in practice the max values are usually 16 to 20 |
561 | * unless limiterGains == 3 (limiter off) and eCurr ~= 0 (i.e. huge gain, but only |
562 | * because the envelope has 0 power anyway) |
563 | */ |
564 | q = MULSHIFT32(psi->gLimBuf[m], gBoost) << 2; /* Q(gLimFbits) * Q(28) --> Q(gLimFbits[m]-2) */ |
565 | r = SqrtFix(q, psi->gLimFbits[m] - 2, &z); |
566 | z -= FBITS_GLIM_BOOST; |
567 | if (z >= 0) { |
568 | psi->gLimBoost[m] = r >> MIN(z, 31); |
569 | } else { |
570 | z = MIN(30, -z); |
571 | CLIP_2N_SHIFT30(r, z); |
572 | psi->gLimBoost[m] = r; |
573 | } |
574 | |
575 | q = MULSHIFT32(psi->qmLimBuf[m], gBoost) << 2; /* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */ |
576 | r = SqrtFix(q, fbitsDQ - 2, &z); |
577 | z -= FBITS_QLIM_BOOST; /* << by 14, since integer sqrt of x < 2^16, and we want to leave 1 GB */ |
578 | if (z >= 0) { |
579 | psi->qmLimBoost[m] = r >> MIN(31, z); |
580 | } else { |
581 | z = MIN(30, -z); |
582 | CLIP_2N_SHIFT30(r, z); |
583 | psi->qmLimBoost[m] = r; |
584 | } |
585 | |
586 | q = MULSHIFT32(psi->smBuf[m], gBoost) << 2; /* Q(fbitsDQ) * Q(28) --> Q(fbitsDQ-2) */ |
587 | r = SqrtFix(q, fbitsDQ - 2, &z); |
588 | z -= FBITS_OUT_QMFA; /* justify for adding to signal (xBuf) later */ |
589 | if (z >= 0) { |
590 | psi->smBoost[m] = r >> MIN(31, z); |
591 | } else { |
592 | z = MIN(30, -z); |
593 | CLIP_2N_SHIFT30(r, z); |
594 | psi->smBoost[m] = r; |
595 | } |
596 | } |
597 | } |
598 | |
599 | /************************************************************************************** |
600 | * Function: CalcGain |
601 | * |
602 | * Description: calculate and apply proper gain to HF components in one envelope |
603 | * (4.6.18.7.5) |
604 | * |
605 | * Inputs: initialized PSInfoSBR struct |
606 | * initialized SBRHeader struct for this SCE/CPE block |
607 | * initialized SBRGrid struct for this channel |
608 | * initialized SBRFreq struct for this SCE/CPE block |
609 | * initialized SBRChan struct for this channel |
610 | * index of current channel (0 for SCE, 0 or 1 for CPE) |
611 | * index of current envelope |
612 | * |
613 | * Outputs: envelope gain, sinusoids and noise after scaling |
614 | * |
615 | * Return: none |
616 | **************************************************************************************/ |
617 | static void CalcGain(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch, int env) |
618 | { |
619 | int lim, fbitsDQ; |
620 | |
621 | /* initialize to -1 so that mapping limiter bands to env/noise bands works right on first pass */ |
622 | psi->envBand = -1; |
623 | psi->noiseFloorBand = -1; |
624 | psi->sBand = -1; |
625 | psi->highBand = -1; |
626 | |
627 | fbitsDQ = (FBITS_OUT_DQ_ENV - psi->envDataDequantScale[ch][env]); /* Q(29 - optional scalefactor) */ |
628 | for (lim = 0; lim < sbrFreq->nLimiter; lim++) { |
629 | /* the QMF bands are divided into lim regions (consecutive, non-overlapping) */ |
630 | CalcMaxGain(psi, sbrHdr, sbrGrid, sbrFreq, ch, env, lim, fbitsDQ); |
631 | CalcComponentGains(psi, sbrGrid, sbrFreq, sbrChan, ch, env, lim, fbitsDQ); |
632 | ApplyBoost(psi, sbrFreq, lim, fbitsDQ); |
633 | } |
634 | } |
635 | |
636 | /* hSmooth table from 4.7.18.7.6, format = Q31 */ |
637 | static const int hSmoothCoef[MAX_NUM_SMOOTH_COEFS] = { |
638 | 0x2aaaaaab, 0x2697a512, 0x1becfa68, 0x0ebdb043, 0x04130598, |
639 | }; |
640 | |
641 | /************************************************************************************** |
642 | * Function: MapHF |
643 | * |
644 | * Description: map HF components to proper QMF bands, with optional gain smoothing |
645 | * filter (4.6.18.7.6) |
646 | * |
647 | * Inputs: initialized PSInfoSBR struct |
648 | * initialized SBRHeader struct for this SCE/CPE block |
649 | * initialized SBRGrid struct for this channel |
650 | * initialized SBRFreq struct for this SCE/CPE block |
651 | * initialized SBRChan struct for this channel |
652 | * index of current envelope |
653 | * reset flag (can be non-zero for first envelope only) |
654 | * |
655 | * Outputs: complete reconstructed subband QMF samples for this envelope |
656 | * |
657 | * Return: none |
658 | * |
659 | * Notes: ensures that output has >= MIN_GBITS_IN_QMFS guard bits, |
660 | * so it's not necessary to check anything in the synth QMF |
661 | **************************************************************************************/ |
662 | static int MapHF(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int env, int hfReset) |
663 | { |
664 | int noiseTabIndex, sinIndex, gainNoiseIndex, hSL; |
665 | int i, iStart, iEnd, m, idx, j, s, n, smre, smim; |
666 | int gFilt, qFilt, xre, xim, gbMask, gbIdx; |
667 | int *XBuf; |
668 | |
669 | noiseTabIndex = sbrChan->noiseTabIndex; |
670 | sinIndex = sbrChan->sinIndex; |
671 | gainNoiseIndex = sbrChan->gainNoiseIndex; /* oldest entries in filter delay buffer */ |
672 | |
673 | if (hfReset) { |
674 | noiseTabIndex = 2; /* starts at 1, double since complex */ |
675 | } |
676 | hSL = (sbrHdr->smoothMode ? 0 : 4); |
677 | |
678 | if (hfReset) { |
679 | for (i = 0; i < hSL; i++) { |
680 | for (m = 0; m < sbrFreq->numQMFBands; m++) { |
681 | sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m]; |
682 | sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m]; |
683 | } |
684 | gainNoiseIndex++; |
685 | if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS) { |
686 | gainNoiseIndex = 0; |
687 | } |
688 | } |
689 | ASSERT(env == 0, ERR_AAC_SBR_BITSTREAM); /* should only be reset when env == 0 */ |
690 | } |
691 | |
692 | iStart = sbrGrid->envTimeBorder[env]; |
693 | iEnd = sbrGrid->envTimeBorder[env + 1]; |
694 | for (i = iStart; i < iEnd; i++) { |
695 | /* save new values in temp buffers (delay) |
696 | * we only store MAX_NUM_SMOOTH_COEFS most recent values, |
697 | * so don't keep storing the same value over and over |
698 | */ |
699 | if (i - iStart < MAX_NUM_SMOOTH_COEFS) { |
700 | for (m = 0; m < sbrFreq->numQMFBands; m++) { |
701 | sbrChan->gTemp[gainNoiseIndex][m] = psi->gLimBoost[m]; |
702 | sbrChan->qTemp[gainNoiseIndex][m] = psi->qmLimBoost[m]; |
703 | } |
704 | } |
705 | |
706 | /* see 4.6.18.7.6 */ |
707 | XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart]; |
708 | gbMask = 0; |
709 | for (m = 0; m < sbrFreq->numQMFBands; m++) { |
710 | if (env == psi->la || env == sbrChan->laPrev) { |
711 | /* no smoothing filter for gain, and qFilt = 0 (only need to do once) */ |
712 | if (i == iStart) { |
713 | psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m]; |
714 | psi->qFiltLast[m] = 0; |
715 | } |
716 | } else if (hSL == 0) { |
717 | /* no smoothing filter for gain, (only need to do once) */ |
718 | if (i == iStart) { |
719 | psi->gFiltLast[m] = sbrChan->gTemp[gainNoiseIndex][m]; |
720 | psi->qFiltLast[m] = sbrChan->qTemp[gainNoiseIndex][m]; |
721 | } |
722 | } else { |
723 | /* apply smoothing filter to gain and noise (after MAX_NUM_SMOOTH_COEFS, it's always the same) */ |
724 | if (i - iStart < MAX_NUM_SMOOTH_COEFS) { |
725 | gFilt = 0; |
726 | qFilt = 0; |
727 | idx = gainNoiseIndex; |
728 | for (j = 0; j < MAX_NUM_SMOOTH_COEFS; j++) { |
729 | /* sum(abs(hSmoothCoef[j])) for all j < 1.0 */ |
730 | gFilt += MULSHIFT32(sbrChan->gTemp[idx][m], hSmoothCoef[j]); |
731 | qFilt += MULSHIFT32(sbrChan->qTemp[idx][m], hSmoothCoef[j]); |
732 | idx--; |
733 | if (idx < 0) { |
734 | idx += MAX_NUM_SMOOTH_COEFS; |
735 | } |
736 | } |
737 | psi->gFiltLast[m] = gFilt << 1; /* restore to Q(FBITS_GLIM_BOOST) (gain of filter < 1.0, so no overflow) */ |
738 | psi->qFiltLast[m] = qFilt << 1; /* restore to Q(FBITS_QLIM_BOOST) */ |
739 | } |
740 | } |
741 | |
742 | if (psi->smBoost[m] != 0) { |
743 | /* add scaled signal and sinusoid, don't add noise (qFilt = 0) */ |
744 | smre = psi->smBoost[m]; |
745 | smim = smre; |
746 | |
747 | /* sinIndex: [0] xre += sm [1] xim += sm*s [2] xre -= sm [3] xim -= sm*s */ |
748 | s = (sinIndex >> 1); /* if 2 or 3, flip sign to subtract sm */ |
749 | s <<= 31; |
750 | smre ^= (s >> 31); |
751 | smre -= (s >> 31); |
752 | s ^= ((m + sbrFreq->kStart) << 31); |
753 | smim ^= (s >> 31); |
754 | smim -= (s >> 31); |
755 | |
756 | /* if sinIndex == 0 or 2, smim = 0; if sinIndex == 1 or 3, smre = 0 */ |
757 | s = sinIndex << 31; |
758 | smim &= (s >> 31); |
759 | s ^= 0x80000000; |
760 | smre &= (s >> 31); |
761 | |
762 | noiseTabIndex += 2; /* noise filtered by 0, but still need to bump index */ |
763 | } else { |
764 | /* add scaled signal and scaled noise */ |
765 | qFilt = psi->qFiltLast[m]; |
766 | n = noiseTab[noiseTabIndex++]; |
767 | smre = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA); |
768 | |
769 | n = noiseTab[noiseTabIndex++]; |
770 | smim = MULSHIFT32(n, qFilt) >> (FBITS_QLIM_BOOST - 1 - FBITS_OUT_QMFA); |
771 | } |
772 | noiseTabIndex &= 1023; /* 512 complex numbers */ |
773 | |
774 | gFilt = psi->gFiltLast[m]; |
775 | xre = MULSHIFT32(gFilt, XBuf[0]); |
776 | xim = MULSHIFT32(gFilt, XBuf[1]); |
777 | CLIP_2N_SHIFT30(xre, 32 - FBITS_GLIM_BOOST); |
778 | CLIP_2N_SHIFT30(xim, 32 - FBITS_GLIM_BOOST); |
779 | |
780 | xre += smre; |
781 | *XBuf++ = xre; |
782 | xim += smim; |
783 | *XBuf++ = xim; |
784 | |
785 | gbMask |= FASTABS(xre); |
786 | gbMask |= FASTABS(xim); |
787 | } |
788 | /* update circular buffer index */ |
789 | gainNoiseIndex++; |
790 | if (gainNoiseIndex == MAX_NUM_SMOOTH_COEFS) { |
791 | gainNoiseIndex = 0; |
792 | } |
793 | |
794 | sinIndex++; |
795 | sinIndex &= 3; |
796 | |
797 | /* ensure MIN_GBITS_IN_QMFS guard bits in output |
798 | * almost never occurs in practice, but checking here makes synth QMF logic very simple |
799 | */ |
800 | if (gbMask >> (31 - MIN_GBITS_IN_QMFS)) { |
801 | XBuf = psi->XBuf[i + HF_ADJ][sbrFreq->kStart]; |
802 | for (m = 0; m < sbrFreq->numQMFBands; m++) { |
803 | xre = XBuf[0]; |
804 | xim = XBuf[1]; |
805 | CLIP_2N(xre, (31 - MIN_GBITS_IN_QMFS)); |
806 | CLIP_2N(xim, (31 - MIN_GBITS_IN_QMFS)); |
807 | *XBuf++ = xre; |
808 | *XBuf++ = xim; |
809 | } |
810 | CLIP_2N(gbMask, (31 - MIN_GBITS_IN_QMFS)); |
811 | } |
812 | gbIdx = ((i + HF_ADJ) >> 5) & 0x01; |
813 | sbrChan->gbMask[gbIdx] |= gbMask; |
814 | } |
815 | sbrChan->noiseTabIndex = noiseTabIndex; |
816 | sbrChan->sinIndex = sinIndex; |
817 | sbrChan->gainNoiseIndex = gainNoiseIndex; |
818 | return ERR_AAC_NONE; |
819 | } |
820 | |
821 | /************************************************************************************** |
822 | * Function: AdjustHighFreq |
823 | * |
824 | * Description: adjust high frequencies and add noise and sinusoids (4.6.18.7) |
825 | * |
826 | * Inputs: initialized PSInfoSBR struct |
827 | * initialized SBRHeader struct for this SCE/CPE block |
828 | * initialized SBRGrid struct for this channel |
829 | * initialized SBRFreq struct for this SCE/CPE block |
830 | * initialized SBRChan struct for this channel |
831 | * index of current channel (0 for SCE, 0 or 1 for CPE) |
832 | * |
833 | * Outputs: complete reconstructed subband QMF samples for this channel |
834 | * |
835 | * Return: none |
836 | **************************************************************************************/ |
837 | int AdjustHighFreq(PSInfoSBR *psi, SBRHeader *sbrHdr, SBRGrid *sbrGrid, SBRFreq *sbrFreq, SBRChan *sbrChan, int ch) |
838 | { |
839 | int i, env, hfReset; |
840 | unsigned char frameClass, pointer; |
841 | int err = ERR_AAC_NONE; |
842 | frameClass = sbrGrid->frameClass; |
843 | pointer = sbrGrid->pointer; |
844 | |
845 | /* derive la from table 4.159 */ |
846 | if ((frameClass == SBR_GRID_FIXVAR || frameClass == SBR_GRID_VARVAR) && pointer > 0) { |
847 | psi->la = sbrGrid->numEnv + 1 - pointer; |
848 | } else if (frameClass == SBR_GRID_VARFIX && pointer > 1) { |
849 | psi->la = pointer - 1; |
850 | } else { |
851 | psi->la = -1; |
852 | } |
853 | |
854 | /* for each envelope, estimate gain and adjust SBR QMF bands */ |
855 | hfReset = sbrChan->reset; |
856 | for (env = 0; env < sbrGrid->numEnv; env++) { |
857 | EstimateEnvelope(psi, sbrHdr, sbrGrid, sbrFreq, env); |
858 | CalcGain(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, ch, env); |
859 | err = MapHF(psi, sbrHdr, sbrGrid, sbrFreq, sbrChan, env, hfReset); |
860 | if (err) { |
861 | return err; |
862 | } |
863 | hfReset = 0; /* only set for first envelope after header reset */ |
864 | } |
865 | |
866 | /* set saved sine flags to 0 for QMF bands outside of current frequency range */ |
867 | for (i = 0; i < sbrFreq->freqLimiter[0] + sbrFreq->kStart; i++) { |
868 | sbrChan->addHarmonic[0][i] = 0; |
869 | } |
870 | for (i = sbrFreq->freqLimiter[sbrFreq->nLimiter] + sbrFreq->kStart; i < 64; i++) { |
871 | sbrChan->addHarmonic[0][i] = 0; |
872 | } |
873 | sbrChan->addHarmonicFlag[0] = sbrChan->addHarmonicFlag[1]; |
874 | |
875 | /* save la for next frame */ |
876 | if (psi->la == sbrGrid->numEnv) { |
877 | sbrChan->laPrev = 0; |
878 | } else { |
879 | sbrChan->laPrev = -1; |
880 | } |
881 | return ERR_AAC_NONE; |
882 | } |
883 |