📄 enc_util.c
字号:
/*
*===================================================================
* 3GPP AMR Wideband Floating-point Speech Codec
*===================================================================
*/
#include <math.h>
#include <memory.h>
#include "typedef.h"
#include "enc_main.h"
#include "enc_lpc.h"
#ifdef WIN32
#pragma warning( disable : 4310)
#endif
#define MAX_16 (Word16)0x7FFF
#define MIN_16 (Word16)0x8000
#define MAX_31 (Word32)0x3FFFFFFF
#define MIN_31 (Word32)0xC0000000
#define L_FRAME16k 320 /* Frame size at 16kHz */
#define L_SUBFR16k 80 /* Subframe size at 16kHz */
#define L_SUBFR 64 /* Subframe size */
#define M16k 20 /* Order of LP filter */
#define L_WINDOW 384 /* window size in LP analysis */
#define PREEMPH_FAC 0.68F /* preemphasis factor */
extern const Word16 E_ROM_pow2[];
extern const Word16 E_ROM_log2[];
extern const Word16 E_ROM_isqrt[];
extern const Float32 E_ROM_fir_6k_7k[];
extern const Float32 E_ROM_hp_gain[];
extern const Float32 E_ROM_fir_ipol[];
extern const Float32 E_ROM_hamming_cos[];
/*
* E_UTIL_random
*
* Parameters:
* seed I/O: seed for random number
*
* Function:
* Signed 16 bits random generator.
*
* Returns:
* random number
*/
Word16 E_UTIL_random(Word16 *seed)
{
/*static Word16 seed = 21845;*/
*seed = (Word16) (*seed * 31821L + 13849L);
return(*seed);
}
/*
* E_UTIL_saturate
*
* Parameters:
* inp I: 32-bit number
*
* Function:
* Saturation to 16-bit number
*
* Returns:
* 16-bit number
*/
Word16 E_UTIL_saturate(Word32 inp)
{
Word16 out;
if ((inp < MAX_16) & (inp > MIN_16))
{
out = (Word16)inp;
}
else
{
if (inp > 0)
{
out = MAX_16;
}
else
{
out = MIN_16;
}
}
return(out);
}
/*
* E_UTIL_saturate_31
*
* Parameters:
* inp I: 32-bit number
*
* Function:
* Saturation to 31-bit number
*
* Returns:
* 31(32)-bit number
*/
Word32 E_UTIL_saturate_31(Word32 inp)
{
Word32 out;
if ((inp < MAX_31) & (inp > MIN_31))
{
out = inp;
}
else
{
if (inp > 0)
{
out = MAX_31;
}
else
{
out = MIN_31;
}
}
return(out);
}
/*
* E_UTIL_norm_s
*
* Parameters:
* L_var1 I: 32 bit Word32 signed integer (Word32) whose value
* falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
*
* Function:
* Produces the number of left shift needed to normalize the 16 bit
* variable var1 for positive values on the interval with minimum
* of 16384 and maximum of 32767, and for negative values on
* the interval with minimum of -32768 and maximum of -16384.
*
* Returns:
* 16 bit Word16 signed integer (Word16) whose value falls in the range
* 0x0000 0000 <= var_out <= 0x0000 000f.
*/
Word16 E_UTIL_norm_s (Word16 var1)
{
Word16 var_out;
if (var1 == 0)
{
var_out = 0;
}
else
{
if (var1 == -1)
{
var_out = 15;
}
else
{
if (var1 < 0)
{
var1 = (Word16)~var1;
}
for (var_out = 0; var1 < 0x4000; var_out++)
{
var1 <<= 1;
}
}
}
return (var_out);
}
/*
* E_UTIL_norm_l
*
* Parameters:
* L_var1 I: 32 bit Word32 signed integer (Word32) whose value
* falls in the range 0x8000 0000 <= var1 <= 0x7fff ffff.
*
* Function:
* Produces the number of left shifts needed to normalize the 32 bit
* variable L_var1 for positive values on the interval with minimum of
* 1073741824 and maximum of 2147483647, and for negative values on
* the interval with minimum of -2147483648 and maximum of -1073741824;
* in order to normalize the result, the following operation must be done:
* norm_L_var1 = L_shl(L_var1,norm_l(L_var1)).
*
* Returns:
* 16 bit Word16 signed integer (Word16) whose value falls in the range
* 0x0000 0000 <= var_out <= 0x0000 001f.
*/
Word16 E_UTIL_norm_l (Word32 L_var1)
{
Word16 var_out;
if (L_var1 == 0)
{
var_out = 0;
}
else
{
if (L_var1 == (Word32) 0xffffffffL)
{
var_out = 31;
}
else
{
if (L_var1 < 0)
{
L_var1 = ~L_var1;
}
for (var_out = 0; L_var1 < (Word32) 0x40000000L; var_out++)
{
L_var1 <<= 1;
}
}
}
return (var_out);
}
/*
* E_UTIL_l_extract
*
* Parameters:
* L_32 I: 32 bit integer.
* hi O: b16 to b31 of L_32
* lo O: (L_32 - hi<<16)>>1
*
* Function:
* Extract from a 32 bit integer two 16 bit DPF.
*
* Returns:
* void
*/
void E_UTIL_l_extract(Word32 L_32, Word16 *hi, Word16 *lo)
{
*hi = (Word16)(L_32 >> 16);
*lo = (Word16)((L_32 >> 1) - ((*hi * 16384) << 1));
return;
}
/*
* E_UTIL_mpy_32_16
*
* Parameters:
* hi I: hi part of 32 bit number
* lo I: lo part of 32 bit number
* n I: 16 bit number
*
* Function:
* Multiply a 16 bit integer by a 32 bit (DPF). The result is divided
* by 2^15.
*
* L_32 = (hi1*lo2)<<1 + ((lo1*lo2)>>15)<<1
*
* Returns:
* 32 bit result
*/
Word32 E_UTIL_mpy_32_16 (Word16 hi, Word16 lo, Word16 n)
{
Word32 L_32;
L_32 = (hi * n) << 1;
L_32 = L_32 + (((lo * n) >> 15) << 1);
return (L_32);
}
/*
* E_UTIL_pow2
*
* Parameters:
* exponant I: (Q0) Integer part. (range: 0 <= val <= 30)
* fraction I: (Q15) Fractionnal part. (range: 0.0 <= val < 1.0)
*
* Function:
* L_x = pow(2.0, exponant.fraction) (exponant = interger part)
* = pow(2.0, 0.fraction) << exponant
*
* Algorithm:
*
* The function Pow2(L_x) is approximated by a table and linear
* interpolation.
*
* 1 - i = bit10 - b15 of fraction, 0 <= i <= 31
* 2 - a = bit0 - b9 of fraction
* 3 - L_x = table[i] << 16 - (table[i] - table[i + 1]) * a * 2
* 4 - L_x = L_x >> (30-exponant) (with rounding)
*
* Returns:
* range 0 <= val <= 0x7fffffff
*/
Word32 E_UTIL_pow2(Word16 exponant, Word16 fraction)
{
Word32 L_x, tmp, i, exp;
Word16 a;
L_x = fraction * 32; /* L_x = fraction<<6 */
i = L_x >> 15; /* Extract b10-b16 of fraction */
a = (Word16)(L_x); /* Extract b0-b9 of fraction */
a = (Word16)(a & (Word16)0x7fff);
L_x = E_ROM_pow2[i] << 16; /* table[i] << 16 */
tmp = E_ROM_pow2[i] - E_ROM_pow2[i + 1]; /* table[i] - table[i+1] */
L_x = L_x - ((tmp * a) << 1); /* L_x -= tmp*a*2 */
exp = 30 - exponant;
L_x = (L_x + (1 << (exp - 1))) >> exp;
return(L_x);
}
/*
* E_UTIL_normalised_log2
*
* Parameters:
* L_x I: input value (normalized)
* exp I: norm_l (L_x)
* exponent O: Integer part of Log2. (range: 0<=val<=30)
* fraction O: Fractional part of Log2. (range: 0<=val<1)
*
* Function:
* Computes log2(L_x, exp), where L_x is positive and
* normalized, and exp is the normalisation exponent
* If L_x is negative or zero, the result is 0.
*
* The function Log2(L_x) is approximated by a table and linear
* interpolation. The following steps are used to compute Log2(L_x)
*
* 1. exponent = 30 - norm_exponent
* 2. i = bit25 - b31 of L_x; 32 <= i <= 63 (because of normalization).
* 3. a = bit10 - b24
* 4. i -= 32
* 5. fraction = table[i] << 16 - (table[i] - table[i + 1]) * a * 2
*
*
* Returns:
* void
*/
static void E_UTIL_normalised_log2(Word32 L_x, Word16 exp, Word16 *exponent,
Word16 *fraction)
{
Word32 i, a, tmp;
Word32 L_y;
if (L_x <= 0)
{
*exponent = 0;
*fraction = 0;
return;
}
*exponent = (Word16)(30 - exp);
L_x = L_x >> 10;
i = L_x >> 15; /* Extract b25-b31 */
a = L_x; /* Extract b10-b24 of fraction */
a = a & 0x00007fff;
i = i - 32;
L_y = E_ROM_log2[i] << 16; /* table[i] << 16 */
tmp = E_ROM_log2[i] - E_ROM_log2[i + 1]; /* table[i] - table[i+1] */
L_y = L_y - ((tmp * a) << 1); /* L_y -= tmp*a*2 */
*fraction = (Word16)(L_y >> 16);
return;
}
/*
* E_UTIL_log2
*
* Parameters:
* L_x I: input value
* exponent O: Integer part of Log2. (range: 0<=val<=30)
* fraction O: Fractional part of Log2. (range: 0<=val<1)
*
* Function:
* Computes log2(L_x), where L_x is positive.
* If L_x is negative or zero, the result is 0.
*
* Returns:
* void
*/
void E_UTIL_log2_32 (Word32 L_x, Word16 *exponent, Word16 *fraction)
{
Word16 exp;
exp = E_UTIL_norm_l(L_x);
E_UTIL_normalised_log2((L_x << exp), exp, exponent, fraction);
}
/*
* E_UTIL_interpol
*
* Parameters:
* x I: input vector
* fir I: filter coefficient
* frac I: fraction (0..resol)
* resol I: resolution
* nb_coef I: number of coefficients
*
* Function:
* Fractional interpolation of signal at position (frac/up_samp)
*
* Returns:
* result of interpolation
*/
static Float32 E_UTIL_interpol(Float32 *x, Word32 frac, Word32 up_samp,
Word32 nb_coef)
{
Word32 i;
Float32 s;
Float32 *x1, *x2;
const Float32 *c1, *c2;
x1 = &x[0];
x2 = &x[1];
c1 = &E_ROM_fir_ipol[frac];
c2 = &E_ROM_fir_ipol[up_samp - frac];
s = 0.0;
for(i = 0; i < nb_coef; i++)
{
s += x1[-i] * c1[up_samp * i] + x2[i] * c2[up_samp * i];
}
return s;
}
/*
* E_UTIL_hp50_12k8
*
* Parameters:
* signal I/O: signal
* lg I: lenght of signal
* mem I/O: filter memory [6]
*
* Function:
* 2nd order high pass filter with cut off frequency at 50 Hz.
*
* Algorithm:
*
* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2]
* + a[1]*y[i-1] + a[2]*y[i-2];
*
* b[3] = {0.989501953f, -1.979003906f, 0.989501953f};
* a[3] = {1.000000000F, 1.978881836f,-0.966308594f};
*
*
* Returns:
* void
*/
void E_UTIL_hp50_12k8(Float32 signal[], Word32 lg, Float32 mem[])
{
Word32 i;
Float32 x0, x1, x2, y0, y1, y2;
y1 = mem[0];
y2 = mem[1];
x0 = mem[2];
x1 = mem[3];
for(i = 0; i < lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
y0 = y1 * 1.978881836F + y2 * -0.979125977F + x0 * 0.989501953F +
x1 * -1.979003906F + x2 * 0.989501953F;
signal[i] = y0;
y2 = y1;
y1 = y0;
}
mem[0] = ((y1 > 1e-10) | (y1 < -1e-10)) ? y1 : 0;
mem[1] = ((y2 > 1e-10) | (y2 < -1e-10)) ? y2 : 0;
mem[2] = ((x0 > 1e-10) | (x0 < -1e-10)) ? x0 : 0;
mem[3] = ((x1 > 1e-10) | (x1 < -1e-10)) ? x1 : 0;
return;
}
/*
* E_UTIL_hp400_12k8
*
* Parameters:
* signal I/O: signal
* lg I: lenght of signal
* mem I/O: filter memory [4]
*
* Function:
* 2nd order high pass filter with cut off frequency at 400 Hz.
*
* Algorithm:
*
* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2]
* + a[1]*y[i-1] + a[2]*y[i-2];
*
* b[3] = {0.893554687, -1.787109375, 0.893554687};
* a[3] = {1.000000000, 1.787109375, -0.864257812};
*
*
* Returns:
* void
*/
static void E_UTIL_hp400_12k8(Float32 signal[], Word32 lg, Float32 mem[])
{
Word32 i;
Float32 x0, x1, x2;
Float32 y0, y1, y2;
y1 = mem[0];
y2 = mem[1];
x0 = mem[2];
x1 = mem[3];
for(i = 0; i < lg; i++)
{
x2 = x1;
x1 = x0;
x0 = signal[i];
y0 = y1 * 1.787109375F + y2 * -0.864257812F + x0 * 0.893554687F + x1 * -
1.787109375F + x2 * 0.893554687F;
signal[i] = y0;
y2 = y1;
y1 = y0;
}
mem[0] = y1;
mem[1] = y2;
mem[2] = x0;
mem[3] = x1;
return;
}
/*
* E_UTIL_bp_6k_7k
*
* Parameters:
* signal I/O: signal
* lg I: lenght of signal
* mem I/O: filter memory [4]
*
* Function:
* 15th order band pass 6kHz to 7kHz FIR filter.
*
* Returns:
* void
*/
static void E_UTIL_bp_6k_7k(Float32 signal[], Word32 lg, Float32 mem[])
{
Float32 x[L_SUBFR16k + 30];
Float32 s0, s1, s2, s3;
Float32 *px;
Word32 i, j;
memcpy(x, mem, 30 * sizeof(Float32));
memcpy(x + 30, signal, lg * sizeof(Float32));
px = x;
for(i = 0; i < lg; i++)
{
s0 = 0;
s1 = px[0] * E_ROM_fir_6k_7k[0];
s2 = px[1] * E_ROM_fir_6k_7k[1];
s3 = px[2] * E_ROM_fir_6k_7k[2];
for(j = 3; j < 31; j += 4)
{
s0 += px[j] * E_ROM_fir_6k_7k[j];
s1 += px[j + 1] * E_ROM_fir_6k_7k[j + 1];
s2 += px[j + 2] * E_ROM_fir_6k_7k[j + 2];
s3 += px[j + 3] * E_ROM_fir_6k_7k[j + 3];
}
px++;
signal[i] = (Float32)((s0 + s1 + s2 + s3) * 0.25F); /* gain of coef = 4.0 */
}
memcpy(mem, x + lg, 30 * sizeof(Float32));
return;
}
/*
* E_UTIL_preemph
*
* Parameters:
* x I/O: signal
* mu I: preemphasis factor
* lg I: vector size
* mem I/O: memory (x[-1])
*
* Function:
* Filtering through 1 - mu z^-1
*
*
* Returns:
* void
*/
void E_UTIL_preemph(Word16 x[], Word16 mu, Word32 lg, Word16 *mem)
{
Word32 i, L_tmp;
Word16 temp;
temp = x[lg - 1];
for (i = lg - 1; i > 0; i--)
{
L_tmp = x[i] << 15;
L_tmp -= x[i - 1] * mu;
x[i] = (Word16)((L_tmp + 0x4000) >> 15);
}
L_tmp = (x[0] << 15);
L_tmp -= *mem * mu;
x[0] = (Word16)((L_tmp + 0x4000) >> 15);
*mem = temp;
return;
}
void E_UTIL_f_preemph(Float32 *signal, Float32 mu, Word32 L, Float32 *mem)
{
Word32 i;
Float32 temp;
temp = signal[L - 1];
for (i = L - 1; i > 0; i--)
{
signal[i] = signal[i] - mu * signal[i - 1];
}
signal[0] -= mu * (*mem);
*mem = temp;
return;
}
/*
* E_UTIL_deemph
*
* Parameters:
* signal I/O: signal
* mu I: deemphasis factor
* L I: vector size
* mem I/O: memory (signal[-1])
*
* Function:
* Filtering through 1/(1-mu z^-1)
* Signal is divided by 2.
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -