1/*
2 ** Copyright 2003-2010, VisualOn, Inc.
3 **
4 ** Licensed under the Apache License, Version 2.0 (the "License");
5 ** you may not use this file except in compliance with the License.
6 ** You may obtain a copy of the License at
7 **
8 **     http://www.apache.org/licenses/LICENSE-2.0
9 **
10 ** Unless required by applicable law or agreed to in writing, software
11 ** distributed under the License is distributed on an "AS IS" BASIS,
12 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 ** See the License for the specific language governing permissions and
14 ** limitations under the License.
15 */
16
17/***********************************************************************
18*       File: hp_wsp.c                                                  *
19*       Description:                                                    *
20*       3nd order high pass filter with cut off frequency at 180Hz      *
21* Algorithm:                                                            *
22*                                                                       *
23*  y[i] = b[0]*x[i] + b[1]*x[i-1] + b[2]*x[i-2] + b[3]*x[i-3]           *
24*                   + a[1]*y[i-1] + a[2]*y[i-2] + a[3]*y[i-3];          *
25*                                                                       *
26* float a_coef[HP_ORDER]= {                                             *
27*    -2.64436711600664f,                                                *
28*    2.35087386625360f,                                                 *
29*   -0.70001156927424f};                                                *
30*                                                                       *
31* float b_coef[HP_ORDER+1]= {                                           *
32*     -0.83787057505665f,                                               *
33*    2.50975570071058f,                                                 *
34*   -2.50975570071058f,                                                 *
35*    0.83787057505665f};                                                *
36*                                                                       *
37*************************************************************************/
38
39#include "typedef.h"
40#include "basic_op.h"
41#include "oper_32b.h"
42#include "acelp.h"
43
44/* filter coefficients in Q12 */
45static Word16 a[4] = {8192, 21663, -19258, 5734};
46static Word16 b[4] = {-3432, +10280, -10280, +3432};
47
48/* Initialization of static values */
49void Init_Hp_wsp(Word16 mem[])
50{
51    Set_zero(mem, 9);
52
53    return;
54}
55
56void scale_mem_Hp_wsp(Word16 mem[], Word16 exp)
57{
58    Word32 i;
59    Word32 L_tmp;
60
61    for (i = 0; i < 6; i += 2)
62    {
63        L_tmp = ((mem[i] << 16) + (mem[i + 1]<<1));
64        L_tmp = L_shl(L_tmp, exp);
65        mem[i] = L_tmp >> 16;
66        mem[i + 1] = (L_tmp & 0xffff)>>1;
67    }
68
69    for (i = 6; i < 9; i++)
70    {
71        L_tmp = L_deposit_h(mem[i]);       /* x[i] */
72        L_tmp = L_shl(L_tmp, exp);
73        mem[i] = vo_round(L_tmp);
74    }
75
76    return;
77}
78
79
80void Hp_wsp(
81        Word16 wsp[],                         /* i   : wsp[]  signal       */
82        Word16 hp_wsp[],                      /* o   : hypass wsp[]        */
83        Word16 lg,                            /* i   : lenght of signal    */
84        Word16 mem[]                          /* i/o : filter memory [9]   */
85       )
86{
87    Word16 x0, x1, x2, x3;
88    Word16 y3_hi, y3_lo, y2_hi, y2_lo, y1_hi, y1_lo;
89    Word32 i, L_tmp;
90
91    y3_hi = mem[0];
92    y3_lo = mem[1];
93    y2_hi = mem[2];
94    y2_lo = mem[3];
95    y1_hi = mem[4];
96    y1_lo = mem[5];
97    x0 = mem[6];
98    x1 = mem[7];
99    x2 = mem[8];
100
101    for (i = 0; i < lg; i++)
102    {
103        x3 = x2;
104        x2 = x1;
105        x1 = x0;
106        x0 = wsp[i];
107        /* y[i] = b[0]*x[i] + b[1]*x[i-1] + b140[2]*x[i-2] + b[3]*x[i-3]  */
108        /* + a[1]*y[i-1] + a[2] * y[i-2]  + a[3]*y[i-3]  */
109
110        L_tmp = 16384L;                    /* rounding to maximise precision */
111        L_tmp += (y1_lo * a[1])<<1;
112        L_tmp += (y2_lo * a[2])<<1;
113        L_tmp += (y3_lo * a[3])<<1;
114        L_tmp = L_tmp >> 15;
115        L_tmp += (y1_hi * a[1])<<1;
116        L_tmp += (y2_hi * a[2])<<1;
117        L_tmp += (y3_hi * a[3])<<1;
118        L_tmp += (x0 * b[0])<<1;
119        L_tmp += (x1 * b[1])<<1;
120        L_tmp += (x2 * b[2])<<1;
121        L_tmp += (x3 * b[3])<<1;
122
123        L_tmp = L_tmp << 2;
124
125        y3_hi = y2_hi;
126        y3_lo = y2_lo;
127        y2_hi = y1_hi;
128        y2_lo = y1_lo;
129        y1_hi = L_tmp >> 16;
130        y1_lo = (L_tmp & 0xffff) >>1;
131
132        hp_wsp[i] = (L_tmp + 0x4000)>>15;
133    }
134
135    mem[0] = y3_hi;
136    mem[1] = y3_lo;
137    mem[2] = y2_hi;
138    mem[3] = y2_lo;
139    mem[4] = y1_hi;
140    mem[5] = y1_lo;
141    mem[6] = x0;
142    mem[7] = x1;
143    mem[8] = x2;
144
145    return;
146}
147
148
149