1e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/* 2e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** Copyright 2003-2010, VisualOn, Inc. 3e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** 4e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** Licensed under the Apache License, Version 2.0 (the "License"); 5e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** you may not use this file except in compliance with the License. 6e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** You may obtain a copy of the License at 7e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** 8e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** http://www.apache.org/licenses/LICENSE-2.0 9e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** 10e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** Unless required by applicable law or agreed to in writing, software 11e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** distributed under the License is distributed on an "AS IS" BASIS, 12e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** See the License for the specific language governing permissions and 14e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ** limitations under the License. 15e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard */ 16e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 17e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/*********************************************************************** 18e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* File: pitch_f4.c * 19e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 20e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Description: Find the closed loop pitch period with * 215d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen* 1/4 subsample resolution. * 22e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 23e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard************************************************************************/ 24e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 25e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "typedef.h" 26e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "basic_op.h" 27e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "math_op.h" 28e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "acelp.h" 29e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "cnst.h" 30e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 31e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#define UP_SAMP 4 32e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#define L_INTERPOL1 4 33e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 34442cc6dc48f8188e097617f7f804c314a9dacd0eAndreas Huber#define UNUSED(x) (void)(x) 35442cc6dc48f8188e097617f7f804c314a9dacd0eAndreas Huber 36e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/* Local functions */ 37e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 38e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#ifdef ASM_OPT 39e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid Norm_corr_asm( 405d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 exc[], /* (i) : excitation buffer */ 415d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 xn[], /* (i) : target vector */ 425d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ 435d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 L_subfr, 445d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_min, /* (i) : minimum value of pitch lag. */ 455d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_max, /* (i) : maximum value of pitch lag. */ 465d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 corr_norm[] /* (o) Q15 : normalized correlation */ 475d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ); 48e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#else 49e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic void Norm_Corr( 505d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 exc[], /* (i) : excitation buffer */ 515d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 xn[], /* (i) : target vector */ 525d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ 535d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 L_subfr, 545d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_min, /* (i) : minimum value of pitch lag. */ 555d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_max, /* (i) : maximum value of pitch lag. */ 565d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 corr_norm[] /* (o) Q15 : normalized correlation */ 575d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ); 58e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#endif 59e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 60e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic Word16 Interpol_4( /* (o) : interpolated value */ 615d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 * x, /* (i) : input vector */ 625d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 frac /* (i) : fraction (-4..+3) */ 635d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ); 64e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 65e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 66e2e838afcf03e603a41a0455846eaf9614537c16Mans RullgardWord16 Pitch_fr4( /* (o) : pitch period. */ 675d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 exc[], /* (i) : excitation buffer */ 685d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 xn[], /* (i) : target vector */ 695d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ 705d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t0_min, /* (i) : minimum value in the searched range. */ 715d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t0_max, /* (i) : maximum value in the searched range. */ 725d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 * pit_frac, /* (o) : chosen fraction (0, 1, 2 or 3). */ 735d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 i_subfr, /* (i) : indicator for first subframe. */ 745d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t0_fr2, /* (i) : minimum value for resolution 1/2 */ 755d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t0_fr1, /* (i) : minimum value for resolution 1 */ 765d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 L_subfr /* (i) : Length of subframe */ 775d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ) 78e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 795d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 fraction, i; 805d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_min, t_max; 815d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 max, t0, step, temp; 825d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 *corr; 835d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 corr_v[40]; /* Total length = t0_max-t0_min+1+2*L_inter */ 845d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 855d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Find interval to compute normalized correlation */ 865d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 875d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen t_min = L_sub(t0_min, L_INTERPOL1); 885d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen t_max = L_add(t0_max, L_INTERPOL1); 895d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen corr = &corr_v[-t_min]; 905d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Compute normalized correlation between target and filtered excitation */ 91e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#ifdef ASM_OPT /* asm optimization branch */ 92e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Norm_corr_asm(exc, xn, h, L_subfr, t_min, t_max, corr); 93e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#else 945d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Norm_Corr(exc, xn, h, L_subfr, t_min, t_max, corr); 95b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard#endif 96e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 975d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Find integer pitch */ 985d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 995d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen max = corr[t0_min]; 1005d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen t0 = t0_min; 1015d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = t0_min + 1; i <= t0_max; i++) 1025d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1035d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if (corr[i] >= max) 1045d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1055d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen max = corr[i]; 1065d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen t0 = i; 1075d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1085d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1095d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* If first subframe and t0 >= t0_fr1, do not search fractionnal pitch */ 1105d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if ((i_subfr == 0) && (t0 >= t0_fr1)) 1115d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1125d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen *pit_frac = 0; 1135d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return (t0); 1145d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1155d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /*------------------------------------------------------------------* 1165d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen * Search fractionnal pitch with 1/4 subsample resolution. * 1175d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen * Test the fractions around t0 and choose the one which maximizes * 1185d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen * the interpolated normalized correlation. * 1195d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen *------------------------------------------------------------------*/ 1205d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1215d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen step = 1; /* 1/4 subsample resolution */ 1225d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen fraction = -3; 1235d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if ((t0_fr2 == PIT_MIN)||((i_subfr == 0) && (t0 >= t0_fr2))) 1245d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1255d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen step = 2; /* 1/2 subsample resolution */ 1265d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen fraction = -2; 1275d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1285d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if(t0 == t0_min) 1295d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1305d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen fraction = 0; 1315d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1325d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen max = Interpol_4(&corr[t0], fraction); 1335d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1345d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = fraction + step; i <= 3; i += step) 1355d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1365d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen temp = Interpol_4(&corr[t0], i); 1375d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if(temp > max) 1385d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1395d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen max = temp; 1405d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen fraction = i; 1415d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1425d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1435d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* limit the fraction value in the interval [0,1,2,3] */ 1445d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if (fraction < 0) 1455d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1465d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen fraction += UP_SAMP; 1475d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen t0 -= 1; 1485d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1495d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen *pit_frac = fraction; 1505d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return (t0); 151e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 152e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 153e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 154e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/*********************************************************************************** 155e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Function: Norm_Corr() * 156e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 157e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Description: Find the normalized correlation between the target vector and the * 158e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* filtered past excitation. * 159e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* (correlation between target and filtered excitation divided by the * 160e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* square root of energy of target and filtered excitation). * 161e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard************************************************************************************/ 162e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#ifndef ASM_OPT 163e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic void Norm_Corr( 1645d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 exc[], /* (i) : excitation buffer */ 1655d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 xn[], /* (i) : target vector */ 1665d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 h[], /* (i) Q15 : impulse response of synth/wgt filters */ 1675d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 L_subfr, 1685d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_min, /* (i) : minimum value of pitch lag. */ 1695d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 t_max, /* (i) : maximum value of pitch lag. */ 1705d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 corr_norm[]) /* (o) Q15 : normalized correlation */ 171e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 1725d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 i, k, t; 1735d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 corr, exp_corr, norm, exp, scale; 1745d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 exp_norm, excf[L_SUBFR], tmp; 1755d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 L_tmp, L_tmp1, L_tmp2; 176442cc6dc48f8188e097617f7f804c314a9dacd0eAndreas Huber UNUSED(L_subfr); 177e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 1785d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* compute the filtered excitation for the first delay t_min */ 1795d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen k = -t_min; 180e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 181e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#ifdef ASM_OPT /* asm optimization branch */ 1825d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Convolve_asm(&exc[k], h, excf, 64); 183e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#else 1845d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Convolve(&exc[k], h, excf, 64); 185e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#endif 186e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 1875d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Compute rounded down 1/sqrt(energy of xn[]) */ 1885d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = 0; 1895d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 0; i < 64; i+=4) 1905d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1915d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i] * xn[i])); 1925d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+1] * xn[i+1])); 1935d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+2] * xn[i+2])); 1945d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+3] * xn[i+3])); 1955d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1965d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1975d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_shl(L_tmp, 1), 1); 1985d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen exp = norm_l(L_tmp); 1995d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen exp = L_sub(32, exp); 2005d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen //exp = exp + 2; /* energy of xn[] x 2 + rounded up */ 2015d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen scale = -(exp >> 1); /* (1<<scale) < 1/sqrt(energy rounded) */ 2025d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2035d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* loop for every possible period */ 2045d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2055d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (t = t_min; t <= t_max; t++) 2065d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2075d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Compute correlation between xn[] and excf[] */ 2085d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = 0; 2095d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp1 = 0; 2105d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 0; i < 64; i+=4) 2115d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2125d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i] * excf[i])); 2135d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp1 = L_add(L_tmp1, (excf[i] * excf[i])); 2145d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+1] * excf[i+1])); 2155d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp1 = L_add(L_tmp1, (excf[i+1] * excf[i+1])); 2165d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+2] * excf[i+2])); 2175d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp1 = L_add(L_tmp1, (excf[i+2] * excf[i+2])); 2185d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_tmp, (xn[i+3] * excf[i+3])); 2195d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp1 = L_add(L_tmp1, (excf[i+3] * excf[i+3])); 2205d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2215d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2225d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_add(L_shl(L_tmp, 1), 1); 2235d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp1 = L_add(L_shl(L_tmp1, 1), 1); 2245d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2255d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen exp = norm_l(L_tmp); 2265d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_shl(L_tmp, exp); 2275d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen exp_corr = L_sub(30, exp); 2285d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen corr = extract_h(L_tmp); 2295d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2305d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen exp = norm_l(L_tmp1); 2315d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_shl(L_tmp1, exp); 2325d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen exp_norm = L_sub(30, exp); 2335d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2345d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Isqrt_n(&L_tmp, &exp_norm); 2355d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen norm = extract_h(L_tmp); 2365d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2375d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* Normalize correlation = correlation * (1/sqrt(energy)) */ 2385d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2395d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_mult(corr, norm); 2405d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2415d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp2 = L_add(exp_corr, exp_norm + scale); 2425d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if(L_tmp2 < 0) 2435d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2445d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp2 = -L_tmp2; 2455d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_tmp >> L_tmp2; 2465d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2475d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen else 2485d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2495d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_tmp = L_shl(L_tmp, L_tmp2); 2505d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2515d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2525d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen corr_norm[t] = voround(L_tmp); 2535d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* modify the filtered excitation excf[] for the next iteration */ 2545d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 2555d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if(t != t_max) 2565d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2575d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen k = -(t + 1); 2585d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen tmp = exc[k]; 2595d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 63; i > 0; i--) 2605d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 2615d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen excf[i] = add1(vo_mult(tmp, h[i]), excf[i - 1]); 2625d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2635d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen excf[0] = vo_mult(tmp, h[0]); 2645d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2655d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 2665d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return; 267e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 268e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 269e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#endif 270e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/************************************************************************************ 271e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Function: Interpol_4() * 272e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 273e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Description: For interpolating the normalized correlation with 1/4 resolution. * 274e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard**************************************************************************************/ 275e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 276e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/* 1/4 resolution interpolation filter (-3 dB at 0.791*fs/2) in Q14 */ 277e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic Word16 inter4_1[4][8] = 278e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 2795d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen {-12, 420, -1732, 5429, 13418, -1242, 73, 32}, 2805d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen {-26, 455, -2142, 9910, 9910, -2142, 455, -26}, 2815d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen {32, 73, -1242, 13418, 5429, -1732, 420, -12}, 2825d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen {206, -766, 1376, 14746, 1376, -766, 206, 0} 283e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard}; 284e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 285e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/*** Coefficients in floating point 286e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic float inter4_1[UP_SAMP*L_INTERPOL1+1] = { 287e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard0.900000, 288e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard0.818959, 0.604850, 0.331379, 0.083958, 289e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard-0.075795, -0.130717, -0.105685, -0.046774, 290e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard0.004467, 0.027789, 0.025642, 0.012571, 291e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard0.001927, -0.001571, -0.000753, 0.000000}; 292e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard***/ 293e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 294e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic Word16 Interpol_4( /* (o) : interpolated value */ 2955d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 * x, /* (i) : input vector */ 2965d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 frac /* (i) : fraction (-4..+3) */ 2975d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ) 298e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 2995d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 sum; 3005d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 k, L_sum; 3015d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 *ptr; 3025d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 3035d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen if (frac < 0) 3045d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 3055d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen frac += UP_SAMP; 3065d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x--; 3075d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 3085d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x = x - L_INTERPOL1 + 1; 3095d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen k = UP_SAMP - 1 - frac; 3105d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ptr = &(inter4_1[k][0]); 3115d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 3125d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_sum = vo_mult32(x[0], (*ptr++)); 3135d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[1], (*ptr++))); 3145d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[2], (*ptr++))); 3155d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[3], (*ptr++))); 3165d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[4], (*ptr++))); 3175d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[5], (*ptr++))); 3185d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[6], (*ptr++))); 3195d453222ae6dcc10efedb1e4805247d7c22a4168Marco Nelissen L_sum = L_add(L_sum, vo_mult32(x[7], (*ptr++))); 3205d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 3215d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen sum = extract_h(L_add(L_shl2(L_sum, 2), 0x8000)); 3225d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return (sum); 323e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 324e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 325e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 326e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 327e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 328