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 /**************************************************************************
23 ASSUMPTIONS:
24 COEFS-
25 pBiquadState->coefs[0] is A1,
26 pBiquadState->coefs[1] is A0,
27 pBiquadState->coefs[2] is -B1, these are in Q15 format
28 pBiquadState->Shift is Shift value
29 DELAYS-
30 pBiquadState->pDelays[0] is x(n-1)L in Q15 format
31 pBiquadState->pDelays[1] is y(n-1)L in Q30 format
32 pBiquadState->pDelays[2] is x(n-1)R in Q15 format
33 pBiquadState->pDelays[3] is y(n-1)R in Q30 format
34 ***************************************************************************/
35
FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_Instance_t * pInstance,LVM_INT16 * pDataIn,LVM_INT16 * pDataOut,LVM_INT16 NrSamples)36 void 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