You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

203 lines
5.2 KiB
C++

/*
* Project 25 IMBE Encoder/Decoder Fixed-Point implementation
* Developed by Pavel Yazev E-mail: pyazev@gmail.com
* Version 1.0 (c) Copyright 2009
*
* This is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3, or (at your option)
* any later version.
*
* The software is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this; see the file COPYING. If not, write to the Free
* Software Foundation, Inc., 51 Franklin Street, Boston, MA
* 02110-1301, USA.
*/
#include "typedef.h"
#include "basic_op.h"
#include "imbe.h"
#include "qnt_sub.h"
#include "aux_sub.h"
#include "math_sub.h"
#include "sa_enh.h"
//-----------------------------------------------------------------------------
// PURPOSE:
// Perform Spectral Amplitude Enhancement
//
//
// INPUT:
// IMBE_PARAM *imbe_param - pointer to IMBE_PARAM structure with
// valid num_harms, sa and fund_freq items
//
// OUTPUT:
// None
//
// RETURN:
// Enhanced Spectral Amplitudes
//
//-----------------------------------------------------------------------------
void sa_enh(IMBE_PARAM *imbe_param)
{
Word16 *sa, num_harm, sa_tmp[NUM_HARMS_MAX], nm;
Word16 cos_w[NUM_HARMS_MAX], i, tmp;
Word32 L_tmp, Rm0, Rm1;
Word32 w0, cos_acc;
Word32 L_den, L_num, L_Rm0_2, L_Rm1_2, L_sum_Rm02_Rm12, L_sum_mod;
Word16 Rm0Rm1, nm1, nm2, tot_nm;
Word16 Rm0_s, Rm1_s;
sa = imbe_param->sa;
num_harm = imbe_param->num_harms;
v_equ(sa_tmp, sa, num_harm);
Rm0 = L_v_magsq(sa, num_harm);
if(Rm0 == 0)
return;
nm = norm_l (Rm0);
if(Rm0 == MAX_32)
{
nm = 1;
v_equ_shr(sa_tmp, sa, nm, num_harm);
Rm0 = L_v_magsq(sa_tmp, num_harm);
}
else
{
if(nm > 2)
{
nm = -(nm >> 1);
v_equ_shr(sa_tmp, sa, nm, num_harm);
Rm0 = L_v_magsq(sa_tmp, num_harm);
}
}
w0 = imbe_param->fund_freq;
cos_acc = 0; Rm1 = 0;
for(i = 0; i < num_harm; i++)
{
cos_acc = L_add(cos_acc, w0);
cos_w[i] = cos_fxp(extract_h(cos_acc));
Rm1 = L_add(Rm1, L_mpy_ls(L_mult(sa_tmp[i], sa_tmp[i]), cos_w[i]));
}
Rm0_s = extract_h(Rm0);
Rm1_s = extract_h(Rm1);
L_Rm0_2 = L_mult(Rm0_s, Rm0_s);
L_Rm1_2 = L_mult(Rm1_s, Rm1_s);
L_den = L_sub(L_Rm0_2, L_Rm1_2);
L_den = L_mult(extract_h(L_den), Rm0_s);
nm1 = norm_l(L_den);
L_den = L_shl(L_den, nm1);
nm2 = norm_l(w0);
L_den = L_mpy_ls(L_den, extract_h(L_shl(w0, nm2))); // Calculate w0 * Rm0 * (Rm0^2 - Rm1^2)
nm1 += nm2; // total denominator shift
if (L_den < 1) return; // fix bug infinite loop due to invalid input
L_sum_Rm02_Rm12 = L_add(L_shr(L_Rm0_2, 2), L_shr(L_Rm1_2, 2));
Rm0Rm1 = shr(mult_r(Rm0_s, Rm1_s), 1);
for(i = 0; i < num_harm; i++)
{
if((((i + 1) << 3) > num_harm) && (sa_tmp[i] != 0x0000))
{
L_num = L_sub(L_sum_Rm02_Rm12, L_mult(Rm0Rm1, cos_w[i]));
tot_nm = norm_l(L_num);
L_num = L_shl(L_num, tot_nm);
while(L_num >= L_den)
{
L_num = L_shr(L_num, 1);
tot_nm -= 1;
}
tmp = div_s(extract_h(L_num), extract_h(L_den));
tot_nm -= nm1;
L_tmp = L_mult(sa_tmp[i], sa_tmp[i]);
nm2 = norm_l(L_tmp);
L_tmp = L_shl(L_tmp, nm2);
L_tmp = L_mult(extract_h(L_tmp), tmp);
tot_nm += nm2;
tot_nm -= 2;
if(tot_nm <= 0)
{
L_tmp = L_shr(L_tmp, add(8, tot_nm));
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm);
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm);
L_tmp = L_mult(extract_h(L_tmp), CNST_0_9898_Q1_15);
tmp = extract_h(L_shl(L_tmp, 1));
}
else
{
if(tot_nm <= 8)
{
L_tmp = L_shr(L_tmp, tot_nm);
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm);
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm + 1);
tmp = mult(extract_h(L_tmp), CNST_0_9898_Q1_15);
}
else
{
nm1 = tot_nm & 0xFFFE;
L_tmp = L_shr(L_tmp, tot_nm - nm1);
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm);
tot_nm = nm1 >> 1;
nm1 = tot_nm & 0xFFFE;
L_tmp = L_shr(L_tmp, tot_nm - nm1);
L_tmp = sqrt_l_exp(L_tmp, &tot_nm);
L_tmp = L_shr(L_tmp, tot_nm);
L_tmp = L_mult(extract_h(L_tmp), CNST_0_9898_Q1_15);
tot_nm = nm1 >> 1;
tmp = extract_h(L_shr(L_tmp, tot_nm + 1));
}
}
if(tmp > CNST_1_2_Q2_14)
sa[i] = extract_h(L_shl(L_mult(sa[i], CNST_1_2_Q2_14), 1));
else if(tmp < CNST_0_5_Q2_14)
sa[i] = shr(sa[i], 1);
else
sa[i] = extract_h(L_shl(L_mult(sa[i], tmp), 1));
}
}
// Compute the correct scale factor
v_equ_shr(sa_tmp, sa, nm, num_harm);
L_sum_mod = L_v_magsq(sa_tmp, num_harm);
if(L_sum_mod > Rm0)
{
tmp = div_s(extract_h(Rm0), extract_h(L_sum_mod));
L_tmp = sqrt_l_exp(L_deposit_h(tmp), &tot_nm);
tmp = shr(extract_h(L_tmp), tot_nm);
for(i = 0; i < num_harm; i++)
sa[i] = mult_r(sa[i], tmp);
}
}