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 ***************************************************************************/
FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrSamples)35 void FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t *pInstance,
36 LVM_FLOAT *pDataIn,
37 LVM_FLOAT *pDataOut,
38 LVM_INT16 NrSamples)
39 {
40 LVM_FLOAT ynL,ynR;
41 LVM_FLOAT Temp;
42 LVM_FLOAT NegSatValue;
43 LVM_INT16 ii;
44
45 PFilter_Float_State pBiquadState = (PFilter_Float_State) pInstance;
46
47 NegSatValue = -1.0f;
48
49 for (ii = NrSamples; ii != 0; ii--)
50 {
51
52 /**************************************************************************
53 PROCESSING OF THE LEFT CHANNEL
54 ***************************************************************************/
55
56 // ynL =A1 * x(n-1)L
57 ynL = (LVM_FLOAT)pBiquadState->coefs[0] * pBiquadState->pDelays[0];
58 // ynR =A1 * x(n-1)R
59 ynR = (LVM_FLOAT)pBiquadState->coefs[0] * pBiquadState->pDelays[2];
60
61 // ynL+=A0 * x(n)L
62 ynL += (LVM_FLOAT)pBiquadState->coefs[1] * (*pDataIn);
63 // ynR+=A0 * x(n)L
64 ynR += (LVM_FLOAT)pBiquadState->coefs[1] * (*(pDataIn+1));
65
66 // ynL += (-B1 * y(n-1)L )
67 Temp = pBiquadState->pDelays[1] * pBiquadState->coefs[2];
68 ynL += Temp;
69 // ynR += (-B1 * y(n-1)R ) )
70 Temp = pBiquadState->pDelays[3] * pBiquadState->coefs[2];
71 ynR += Temp;
72
73 /**************************************************************************
74 UPDATING THE DELAYS
75 ***************************************************************************/
76 pBiquadState->pDelays[1] = ynL; // Update y(n-1)L
77 pBiquadState->pDelays[0] = (*pDataIn++); // Update x(n-1)L
78
79 pBiquadState->pDelays[3] = ynR; // Update y(n-1)R
80 pBiquadState->pDelays[2] = (*pDataIn++); // Update x(n-1)R
81
82 /**************************************************************************
83 WRITING THE OUTPUT
84 ***************************************************************************/
85
86 /*Saturate results*/
87 if(ynL > 1.0f)
88 {
89 ynL = 1.0f;
90 }
91 else
92 {
93 if(ynL < NegSatValue)
94 {
95 ynL = NegSatValue;
96 }
97 }
98
99 if(ynR > 1.0f)
100 {
101 ynR = 1.0f;
102 }
103 else
104 {
105 if(ynR < NegSatValue)
106 {
107 ynR = NegSatValue;
108 }
109 }
110
111 *pDataOut++ = (LVM_FLOAT)ynL;
112 *pDataOut++ = (LVM_FLOAT)ynR;
113 }
114
115 }
116 #ifdef SUPPORT_MC
117 /**************************************************************************
118 ASSUMPTIONS:
119 COEFS-
120 pBiquadState->coefs[0] is A1,
121 pBiquadState->coefs[1] is A0,
122 pBiquadState->coefs[2] is -B1,
123 DELAYS-
124 pBiquadState->pDelays[2*ch + 0] is x(n-1) of the 'ch' - channel
125 pBiquadState->pDelays[2*ch + 1] is y(n-1) of the 'ch' - channel
126 The index 'ch' runs from 0 to (NrChannels - 1)
127
128 PARAMETERS:
129 pInstance Pointer Instance
130 pDataIn Input/Source
131 pDataOut Output/Destination
132 NrFrames Number of frames
133 NrChannels Number of channels
134
135 RETURNS:
136 void
137 ***************************************************************************/
FO_Mc_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrFrames,LVM_INT16 NrChannels)138 void FO_Mc_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t *pInstance,
139 LVM_FLOAT *pDataIn,
140 LVM_FLOAT *pDataOut,
141 LVM_INT16 NrFrames,
142 LVM_INT16 NrChannels)
143 {
144 LVM_FLOAT yn;
145 LVM_FLOAT Temp;
146 LVM_INT16 ii;
147 LVM_INT16 ch;
148 PFilter_Float_State pBiquadState = (PFilter_Float_State) pInstance;
149
150 LVM_FLOAT *pDelays = pBiquadState->pDelays;
151 LVM_FLOAT *pCoefs = &pBiquadState->coefs[0];
152 LVM_FLOAT A0 = pCoefs[1];
153 LVM_FLOAT A1 = pCoefs[0];
154 LVM_FLOAT B1 = pCoefs[2];
155
156 for (ii = NrFrames; ii != 0; ii--)
157 {
158
159 /**************************************************************************
160 PROCESSING OF THE CHANNELS
161 ***************************************************************************/
162 for (ch = 0; ch < NrChannels; ch++)
163 {
164 // yn =A1 * x(n-1)
165 yn = (LVM_FLOAT)A1 * pDelays[0];
166
167 // yn+=A0 * x(n)
168 yn += (LVM_FLOAT)A0 * (*pDataIn);
169
170 // yn += (-B1 * y(n-1))
171 Temp = B1 * pDelays[1];
172 yn += Temp;
173
174 /**************************************************************************
175 UPDATING THE DELAYS
176 ***************************************************************************/
177 pDelays[1] = yn; // Update y(n-1)
178 pDelays[0] = (*pDataIn++); // Update x(n-1)
179
180 /**************************************************************************
181 WRITING THE OUTPUT
182 ***************************************************************************/
183
184 /*Saturate results*/
185 if (yn > 1.0f)
186 {
187 yn = 1.0f;
188 } else if (yn < -1.0f) {
189 yn = -1.0f;
190 }
191
192 *pDataOut++ = (LVM_FLOAT)yn;
193 pDelays += 2;
194 }
195 pDelays -= NrChannels * 2;
196 }
197 }
198 #endif
199