hp_wsp.c revision b676a05348e4c516fa8b57e33b10548e6142c3f8
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{ 51e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Set_zero(mem, 9); 52e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 53e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard return; 54e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 55e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 56e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid scale_mem_Hp_wsp(Word16 mem[], Word16 exp) 57e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 58e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word32 i; 59e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word32 L_tmp; 60e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 61e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard for (i = 0; i < 6; i += 2) 62e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard { 63e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = ((mem[i] << 16) + (mem[i + 1]<<1)); 64e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = L_shl(L_tmp, exp); 65e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard mem[i] = L_tmp >> 16; 66e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard mem[i + 1] = (L_tmp & 0xffff)>>1; 67e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard } 68e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 69e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard for (i = 6; i < 9; i++) 70e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard { 71e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = L_deposit_h(mem[i]); /* x[i] */ 72e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = L_shl(L_tmp, exp); 73e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard mem[i] = vo_round(L_tmp); 74e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard } 75e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 76e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard return; 77e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 78e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 79e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 80e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgardvoid Hp_wsp( 81e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 wsp[], /* i : wsp[] signal */ 82e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 hp_wsp[], /* o : hypass wsp[] */ 83e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 lg, /* i : lenght of signal */ 84e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 mem[] /* i/o : filter memory [9] */ 85e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard ) 86e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard{ 87e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 x0, x1, x2, x3; 88e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word16 y3_hi, y3_lo, y2_hi, y2_lo, y1_hi, y1_lo; 89e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard Word32 i, L_tmp; 90e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 91b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y3_hi = mem[0]; 92b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y3_lo = mem[1]; 93b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y2_hi = mem[2]; 94b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y2_lo = mem[3]; 95b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y1_hi = mem[4]; 96b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y1_lo = mem[5]; 97b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x0 = mem[6]; 98b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x1 = mem[7]; 99b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x2 = mem[8]; 100e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 101e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard for (i = 0; i < lg; i++) 102e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard { 103b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x3 = x2; 104b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x2 = x1; 105b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x1 = x0; 106b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard x0 = wsp[i]; 107e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] + b[3]*x[i-3] */ 108e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard /* + a[1]*y[i-1] + a[2] * y[i-2] + a[3]*y[i-3] */ 109e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 110e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = 16384L; /* rounding to maximise precision */ 111e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y1_lo * a[1])<<1; 112e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y2_lo * a[2])<<1; 113e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y3_lo * a[3])<<1; 114e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = L_tmp >> 15; 115e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y1_hi * a[1])<<1; 116e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y2_hi * a[2])<<1; 117e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (y3_hi * a[3])<<1; 118e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (x0 * b[0])<<1; 119e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (x1 * b[1])<<1; 120e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (x2 * b[2])<<1; 121e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp += (x3 * b[3])<<1; 122e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 123e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard L_tmp = L_tmp << 2; 124e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 125b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y3_hi = y2_hi; 126b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y3_lo = y2_lo; 127b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y2_hi = y1_hi; 128b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard y2_lo = y1_lo; 129e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard y1_hi = L_tmp >> 16; 130e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard y1_lo = (L_tmp & 0xffff) >>1; 131e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 132b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard hp_wsp[i] = (L_tmp + 0x4000)>>15; 133e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard } 134e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 135b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[0] = y3_hi; 136b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[1] = y3_lo; 137b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[2] = y2_hi; 138b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[3] = y2_lo; 139b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[4] = y1_hi; 140b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[5] = y1_lo; 141b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[6] = x0; 142b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[7] = x1; 143b676a05348e4c516fa8b57e33b10548e6142c3f8Mans Rullgard mem[8] = x2; 144e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 145e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard return; 146e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard} 147e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 148e2e838afcf03e603a41a0455846eaf9614537c16Mans Rullgard 149