squeezelite-esp32/components/spotify/cspot/bell/libhelix-aac/sbrmath.c

/* ***** BEGIN LICENSE BLOCK *****  
 * Source last modified: $Id: sbrmath.c,v 1.1 2005/02/26 01:47:35 jrecker 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
 *
 * sbrmath.c - fixed-point math functions for SBR
 **************************************************************************************/

#include "sbr.h"
#include "assembly.h"

#define Q28_2	0x20000000	/* Q28: 2.0 */
#define Q28_15	0x30000000	/* Q28: 1.5 */

#define NUM_ITER_IRN		5

/**************************************************************************************
 * Function:    InvRNormalized
 *
 * Description: use Newton's method to solve for x = 1/r
 *
 * Inputs:      r = Q31, range = [0.5, 1) (normalize your inputs to this range)
 *
 * Outputs:     none
 *
 * Return:      x = Q29, range ~= [1.0, 2.0]
 *
 * Notes:       guaranteed to converge and not overflow for any r in [0.5, 1)
 * 
 *              xn+1  = xn - f(xn)/f'(xn)
 *              f(x)  = 1/r - x = 0 (find root)
 *                    = 1/x - r
 *              f'(x) = -1/x^2
 *
 *              so xn+1 = xn - (1/xn - r) / (-1/xn^2)
 *                      = xn * (2 - r*xn)
 *
 *              NUM_ITER_IRN = 2, maxDiff = 6.2500e-02 (precision of about 4 bits)
 *              NUM_ITER_IRN = 3, maxDiff = 3.9063e-03 (precision of about 8 bits)
 *              NUM_ITER_IRN = 4, maxDiff = 1.5288e-05 (precision of about 16 bits)
 *              NUM_ITER_IRN = 5, maxDiff = 3.0034e-08 (precision of about 24 bits)
 **************************************************************************************/
int InvRNormalized(int r)
{
	int i, xn, t;

	/* r =   [0.5, 1.0) 
	 * 1/r = (1.0, 2.0] 
	 *   so use 1.5 as initial guess 
	 */
	xn = Q28_15;

	/* xn = xn*(2.0 - r*xn) */
	for (i = NUM_ITER_IRN; i != 0; i--) {
		t = MULSHIFT32(r, xn);			/* Q31*Q29 = Q28 */
		t = Q28_2 - t;					/* Q28 */
		xn = MULSHIFT32(xn, t) << 4;	/* Q29*Q28 << 4 = Q29 */
	}

	return xn;
}

#define NUM_TERMS_RPI	5
#define LOG2_EXP_INV	0x58b90bfc	/* 1/log2(e), Q31 */

/* invTab[x] = 1/(x+1), format = Q30 */
static const int invTab[NUM_TERMS_RPI] PROGMEM = {0x40000000, 0x20000000, 0x15555555, 0x10000000, 0x0ccccccd};

/**************************************************************************************
 * Function:    RatioPowInv
 *
 * Description: use Taylor (MacLaurin) series expansion to calculate (a/b) ^ (1/c)
 *
 * Inputs:      a = [1, 64], b = [1, 64], c = [1, 64], a >= b
 *
 * Outputs:     none
 *
 * Return:      y = Q24, range ~= [0.015625, 64]
 **************************************************************************************/
int RatioPowInv(int a, int b, int c)
{
	int lna, lnb, i, p, t, y;

	if (a < 1 || b < 1 || c < 1 || a > 64 || b > 64 || c > 64 || a < b)
		return 0;

	lna = MULSHIFT32(log2Tab[a], LOG2_EXP_INV) << 1;	/* ln(a), Q28 */
	lnb = MULSHIFT32(log2Tab[b], LOG2_EXP_INV) << 1;	/* ln(b), Q28 */
	p = (lna - lnb) / c;	/* Q28 */

	/* sum in Q24 */
	y = (1 << 24);
	t = p >> 4;		/* t = p^1 * 1/1! (Q24)*/
	y += t;

	for (i = 2; i <= NUM_TERMS_RPI; i++) {
		t = MULSHIFT32(invTab[i-1], t) << 2;
		t = MULSHIFT32(p, t) << 4;	/* t = p^i * 1/i! (Q24) */
		y += t;
	}

	return y;
}

/**************************************************************************************
 * Function:    SqrtFix
 *
 * Description: use binary search to calculate sqrt(q)
 *
 * Inputs:      q = Q30
 *              number of fraction bits in input
 *
 * Outputs:     number of fraction bits in output
 *
 * Return:      lo = Q(fBitsOut)
 *
 * Notes:       absolute precision varies depending on fBitsIn
 *              normalizes input to range [0x200000000, 0x7fffffff] and takes 
 *                floor(sqrt(input)), and sets fBitsOut appropriately
 **************************************************************************************/
int SqrtFix(int q, int fBitsIn, int *fBitsOut)
{
	int z, lo, hi, mid;

	if (q <= 0) {
		*fBitsOut = fBitsIn;
		return 0;
	}

	/* force even fBitsIn */
	z = fBitsIn & 0x01;
	q >>= z;
	fBitsIn -= z;

	/* for max precision, normalize to [0x20000000, 0x7fffffff] */
	z = (CLZ(q) - 1);
	z >>= 1;
	q <<= (2*z);

	/* choose initial bounds */
	lo = 1;
	if (q >= 0x10000000)
		lo = 16384;	/* (int)sqrt(0x10000000) */
	hi = 46340;		/* (int)sqrt(0x7fffffff) */

	/* do binary search with 32x32->32 multiply test */
	do {
		mid = (lo + hi) >> 1;
		if (mid*mid > q)
			hi = mid - 1;
		else
			lo = mid + 1;
	} while (hi >= lo);
	lo--;

	*fBitsOut = ((fBitsIn + 2*z) >> 1);
	return lo;
}