1/*
2 * Copyright (C) 2004-2010 NXP Software
3 * Copyright (C) 2010 The Android Open Source Project
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 *      http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18#include "BIQUAD.h"
19#include "FO_2I_D16F32Css_LShx_TRC_WRA_01_Private.h"
20#include "LVM_Macros.h"
21
22/**************************************************************************
23ASSUMPTIONS:
24COEFS-
25pBiquadState->coefs[0] is A1,
26pBiquadState->coefs[1] is A0,
27pBiquadState->coefs[2] is -B1, these are in Q15 format
28pBiquadState->Shift    is Shift value
29DELAYS-
30pBiquadState->pDelays[0] is x(n-1)L in Q15 format
31pBiquadState->pDelays[1] is y(n-1)L in Q30 format
32pBiquadState->pDelays[2] is x(n-1)R in Q15 format
33pBiquadState->pDelays[3] is y(n-1)R in Q30 format
34***************************************************************************/
35
36void FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_Instance_t       *pInstance,
37                                     LVM_INT16               *pDataIn,
38                                     LVM_INT16               *pDataOut,
39                                     LVM_INT16               NrSamples)
40    {
41        LVM_INT32   ynL,ynR;
42        LVM_INT32   Temp;
43        LVM_INT32   NegSatValue;
44        LVM_INT16   ii;
45        LVM_INT16   Shift;
46        PFilter_State pBiquadState = (PFilter_State) pInstance;
47
48        NegSatValue = LVM_MAXINT_16 +1;
49        NegSatValue = -NegSatValue;
50
51        Shift = pBiquadState->Shift;
52
53
54        for (ii = NrSamples; ii != 0; ii--)
55        {
56
57            /**************************************************************************
58                            PROCESSING OF THE LEFT CHANNEL
59            ***************************************************************************/
60
61            // ynL =A1 (Q15) * x(n-1)L (Q15) in Q30
62            ynL=(LVM_INT32)pBiquadState->coefs[0]* pBiquadState->pDelays[0];
63            // ynR =A1 (Q15) * x(n-1)R (Q15) in Q30
64            ynR=(LVM_INT32)pBiquadState->coefs[0]* pBiquadState->pDelays[2];
65
66
67            // ynL+=A0 (Q15) * x(n)L (Q15) in Q30
68            ynL+=(LVM_INT32)pBiquadState->coefs[1]* (*pDataIn);
69            // ynR+=A0 (Q15) * x(n)L (Q15) in Q30
70            ynR+=(LVM_INT32)pBiquadState->coefs[1]* (*(pDataIn+1));
71
72
73            // ynL +=  (-B1 (Q15) * y(n-1)L (Q30) ) in Q30
74            MUL32x16INTO32(pBiquadState->pDelays[1],pBiquadState->coefs[2],Temp,15);
75            ynL +=Temp;
76            // ynR +=  (-B1 (Q15) * y(n-1)R (Q30) ) in Q30
77            MUL32x16INTO32(pBiquadState->pDelays[3],pBiquadState->coefs[2],Temp,15);
78            ynR +=Temp;
79
80
81            /**************************************************************************
82                            UPDATING THE DELAYS
83            ***************************************************************************/
84            pBiquadState->pDelays[1]=ynL; // Update y(n-1)L in Q30
85            pBiquadState->pDelays[0]=(*pDataIn++); // Update x(n-1)L in Q15
86
87            pBiquadState->pDelays[3]=ynR; // Update y(n-1)R in Q30
88            pBiquadState->pDelays[2]=(*pDataIn++); // Update x(n-1)R in Q15
89
90            /**************************************************************************
91                            WRITING THE OUTPUT
92            ***************************************************************************/
93            /*Apply shift: Instead of left shift on 16-bit result, right shift of (15-shift) is applied
94              for better SNR*/
95            ynL = ynL>>(15-Shift);
96            ynR = ynR>>(15-Shift);
97
98            /*Saturate results*/
99            if(ynL > LVM_MAXINT_16)
100            {
101                ynL = LVM_MAXINT_16;
102            }
103            else
104            {
105                if(ynL < NegSatValue)
106                {
107                    ynL = NegSatValue;
108                }
109            }
110
111            if(ynR > LVM_MAXINT_16)
112            {
113                ynR = LVM_MAXINT_16;
114            }
115            else
116            {
117                if(ynR < NegSatValue)
118                {
119                    ynR = NegSatValue;
120                }
121            }
122
123            *pDataOut++=(LVM_INT16)ynL;
124            *pDataOut++=(LVM_INT16)ynR;
125        }
126
127    }
128
129