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: hp_wsp.c * 19e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Description: * 20e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* 3nd order high pass filter with cut off frequency at 180Hz * 21e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* Algorithm: * 22e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 23e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] + b[3]*x[i-3] * 24e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* + a[1]*y[i-1] + a[2]*y[i-2] + a[3]*y[i-3]; * 25e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 26e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* float a_coef[HP_ORDER]= { * 27e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* -2.64436711600664f, * 28e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* 2.35087386625360f, * 29e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* -0.70001156927424f}; * 30e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 31e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* float b_coef[HP_ORDER+1]= { * 32e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* -0.83787057505665f, * 33e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* 2.50975570071058f, * 34e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* -2.50975570071058f, * 35e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* 0.83787057505665f}; * 36e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard* * 37e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard*************************************************************************/ 38e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 39e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "typedef.h" 40e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "basic_op.h" 41e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "oper_32b.h" 42e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard#include "acelp.h" 43e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 44e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/* filter coefficients in Q12 */ 45e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic Word16 a[4] = {8192, 21663, -19258, 5734}; 46e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardstatic Word16 b[4] = {-3432, +10280, -10280, +3432}; 47e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 48e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard/* Initialization of static values */ 49e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid Init_Hp_wsp(Word16 mem[]) 50e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 515d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Set_zero(mem, 9); 52e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 535d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return; 54e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 55e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 56e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid scale_mem_Hp_wsp(Word16 mem[], Word16 exp) 57e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 585d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 i; 595d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 L_tmp; 605d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 615d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 0; i < 6; i += 2) 625d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 635d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = ((mem[i] << 16) + (mem[i + 1]<<1)); 645d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_shl(L_tmp, exp); 655d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[i] = L_tmp >> 16; 665d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[i + 1] = (L_tmp & 0xffff)>>1; 675d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 685d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 695d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 6; i < 9; i++) 705d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 715d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_deposit_h(mem[i]); /* x[i] */ 725d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_shl(L_tmp, exp); 735d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[i] = vo_round(L_tmp); 745d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 755d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 765d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return; 77e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 78e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 79e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 80e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid Hp_wsp( 815d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 wsp[], /* i : wsp[] signal */ 825d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 hp_wsp[], /* o : hypass wsp[] */ 835d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 lg, /* i : lenght of signal */ 845d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 mem[] /* i/o : filter memory [9] */ 855d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen ) 86e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 875d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 x0, x1, x2, x3; 885d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word16 y3_hi, y3_lo, y2_hi, y2_lo, y1_hi, y1_lo; 895d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen Word32 i, L_tmp; 905d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 915d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y3_hi = mem[0]; 925d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y3_lo = mem[1]; 935d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y2_hi = mem[2]; 945d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y2_lo = mem[3]; 955d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y1_hi = mem[4]; 965d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y1_lo = mem[5]; 975d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x0 = mem[6]; 985d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x1 = mem[7]; 995d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x2 = mem[8]; 1005d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1015d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen for (i = 0; i < lg; i++) 1025d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen { 1035d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x3 = x2; 1045d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x2 = x1; 1055d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x1 = x0; 1065d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen x0 = wsp[i]; 1075d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] + b[3]*x[i-3] */ 1085d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen /* + a[1]*y[i-1] + a[2] * y[i-2] + a[3]*y[i-3] */ 1095d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1105d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = 16384L; /* rounding to maximise precision */ 1115d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y1_lo * a[1])<<1; 1125d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y2_lo * a[2])<<1; 1135d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y3_lo * a[3])<<1; 1145d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_tmp >> 15; 1155d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y1_hi * a[1])<<1; 1165d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y2_hi * a[2])<<1; 1175d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (y3_hi * a[3])<<1; 1185d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (x0 * b[0])<<1; 1195d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (x1 * b[1])<<1; 1205d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (x2 * b[2])<<1; 1215d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp += (x3 * b[3])<<1; 1225d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1235d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen L_tmp = L_tmp << 2; 1245d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1255d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y3_hi = y2_hi; 1265d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y3_lo = y2_lo; 1275d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y2_hi = y1_hi; 1285d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y2_lo = y1_lo; 1295d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y1_hi = L_tmp >> 16; 1305d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen y1_lo = (L_tmp & 0xffff) >>1; 1315d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1325d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen hp_wsp[i] = (L_tmp + 0x4000)>>15; 1335d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen } 1345d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1355d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[0] = y3_hi; 1365d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[1] = y3_lo; 1375d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[2] = y2_hi; 1385d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[3] = y2_lo; 1395d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[4] = y1_hi; 1405d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[5] = y1_lo; 1415d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[6] = x0; 1425d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[7] = x1; 1435d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen mem[8] = x2; 1445d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen 1455d5c3a132bb446ac78a37dfaac24a46cacf0dd73Marco Nelissen return; 146e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 147e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 148e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 149