1 /* //device/servers/AudioFlinger/AudioBiquadFilter.cpp
2 **
3 ** Copyright 2009, 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 <string.h>
19 #include <assert.h>
20
21 #include "AudioBiquadFilter.h"
22
23 #define LIKELY( exp ) (__builtin_expect( (exp) != 0, true ))
24 #define UNLIKELY( exp ) (__builtin_expect( (exp) != 0, false ))
25
26 namespace android {
27
28 const audio_coef_t AudioBiquadFilter::IDENTITY_COEFS[AudioBiquadFilter::NUM_COEFS] = { AUDIO_COEF_ONE, 0, 0, 0, 0 };
29
AudioBiquadFilter(int nChannels,int sampleRate)30 AudioBiquadFilter::AudioBiquadFilter(int nChannels, int sampleRate) {
31 configure(nChannels, sampleRate);
32 reset();
33 }
34
configure(int nChannels,int sampleRate)35 void AudioBiquadFilter::configure(int nChannels, int sampleRate) {
36 assert(nChannels > 0 && nChannels <= MAX_CHANNELS);
37 assert(sampleRate > 0);
38 mNumChannels = nChannels;
39 mMaxDelta = static_cast<int64_t>(MAX_DELTA_PER_SEC)
40 * AUDIO_COEF_ONE
41 / sampleRate;
42 clear();
43 }
44
reset()45 void AudioBiquadFilter::reset() {
46 memcpy(mCoefs, IDENTITY_COEFS, sizeof(mCoefs));
47 mCoefDirtyBits = 0;
48 setState(STATE_BYPASS);
49 }
50
clear()51 void AudioBiquadFilter::clear() {
52 memset(mDelays, 0, sizeof(mDelays));
53 }
54
setCoefs(const audio_coef_t coefs[NUM_COEFS],bool immediate)55 void AudioBiquadFilter::setCoefs(const audio_coef_t coefs[NUM_COEFS], bool immediate) {
56 memcpy(mTargetCoefs, coefs, sizeof(mTargetCoefs));
57 if (mState & STATE_ENABLED_MASK) {
58 if (UNLIKELY(immediate)) {
59 memcpy(mCoefs, coefs, sizeof(mCoefs));
60 setState(STATE_NORMAL);
61 } else {
62 setState(STATE_TRANSITION_TO_NORMAL);
63 }
64 }
65 }
66
process(const audio_sample_t in[],audio_sample_t out[],int frameCount)67 void AudioBiquadFilter::process(const audio_sample_t in[], audio_sample_t out[],
68 int frameCount) {
69 (this->*mCurProcessFunc)(in, out, frameCount);
70 }
71
enable(bool immediate)72 void AudioBiquadFilter::enable(bool immediate) {
73 if (UNLIKELY(immediate)) {
74 memcpy(mCoefs, mTargetCoefs, sizeof(mCoefs));
75 setState(STATE_NORMAL);
76 } else {
77 setState(STATE_TRANSITION_TO_NORMAL);
78 }
79 }
80
disable(bool immediate)81 void AudioBiquadFilter::disable(bool immediate) {
82 if (UNLIKELY(immediate)) {
83 memcpy(mCoefs, IDENTITY_COEFS, sizeof(mCoefs));
84 setState(STATE_BYPASS);
85 } else {
86 setState(STATE_TRANSITION_TO_BYPASS);
87 }
88 }
89
setState(state_t state)90 void AudioBiquadFilter::setState(state_t state) {
91 switch (state) {
92 case STATE_BYPASS:
93 mCurProcessFunc = &AudioBiquadFilter::process_bypass;
94 break;
95 case STATE_TRANSITION_TO_BYPASS:
96 if (mNumChannels == 1) {
97 mCurProcessFunc = &AudioBiquadFilter::process_transition_bypass_mono;
98 } else {
99 mCurProcessFunc = &AudioBiquadFilter::process_transition_bypass_multi;
100 }
101 mCoefDirtyBits = (1 << NUM_COEFS) - 1;
102 break;
103 case STATE_TRANSITION_TO_NORMAL:
104 if (mNumChannels == 1) {
105 mCurProcessFunc = &AudioBiquadFilter::process_transition_normal_mono;
106 } else {
107 mCurProcessFunc = &AudioBiquadFilter::process_transition_normal_multi;
108 }
109 mCoefDirtyBits = (1 << NUM_COEFS) - 1;
110 break;
111 case STATE_NORMAL:
112 if (mNumChannels == 1) {
113 mCurProcessFunc = &AudioBiquadFilter::process_normal_mono;
114 } else {
115 mCurProcessFunc = &AudioBiquadFilter::process_normal_multi;
116 }
117 break;
118 }
119 mState = state;
120 }
121
updateCoefs(const audio_coef_t coefs[NUM_COEFS],int frameCount)122 bool AudioBiquadFilter::updateCoefs(const audio_coef_t coefs[NUM_COEFS],
123 int frameCount) {
124 int64_t maxDelta = mMaxDelta * frameCount;
125 for (int i = 0; i < NUM_COEFS; ++i) {
126 if (mCoefDirtyBits & (1<<i)) {
127 audio_coef_t diff = coefs[i] - mCoefs[i];
128 if (diff > maxDelta) {
129 mCoefs[i] += maxDelta;
130 } else if (diff < -maxDelta) {
131 mCoefs[i] -= maxDelta;
132 } else {
133 mCoefs[i] = coefs[i];
134 mCoefDirtyBits ^= (1<<i);
135 }
136 }
137 }
138 return mCoefDirtyBits == 0;
139 }
140
process_bypass(const audio_sample_t * in,audio_sample_t * out,int frameCount)141 void AudioBiquadFilter::process_bypass(const audio_sample_t * in,
142 audio_sample_t * out,
143 int frameCount) {
144 // The common case is in-place processing, because this is what the EQ does.
145 if (UNLIKELY(in != out)) {
146 memcpy(out, in, frameCount * mNumChannels * sizeof(audio_sample_t));
147 }
148 }
149
process_normal_mono(const audio_sample_t * in,audio_sample_t * out,int frameCount)150 void AudioBiquadFilter::process_normal_mono(const audio_sample_t * in,
151 audio_sample_t * out,
152 int frameCount) {
153 size_t nFrames = frameCount;
154 audio_sample_t x1 = mDelays[0][0];
155 audio_sample_t x2 = mDelays[0][1];
156 audio_sample_t y1 = mDelays[0][2];
157 audio_sample_t y2 = mDelays[0][3];
158 const audio_coef_t b0 = mCoefs[0];
159 const audio_coef_t b1 = mCoefs[1];
160 const audio_coef_t b2 = mCoefs[2];
161 const audio_coef_t a1 = mCoefs[3];
162 const audio_coef_t a2 = mCoefs[4];
163 while (nFrames-- > 0) {
164 audio_sample_t x0 = *(in++);
165 audio_coef_sample_acc_t acc;
166 acc = mul_coef_sample(b0, x0);
167 acc = mac_coef_sample(b1, x1, acc);
168 acc = mac_coef_sample(b2, x2, acc);
169 acc = mac_coef_sample(a1, y1, acc);
170 acc = mac_coef_sample(a2, y2, acc);
171 audio_sample_t y0 = coef_sample_acc_to_sample(acc);
172 y2 = y1;
173 y1 = y0;
174 x2 = x1;
175 x1 = x0;
176 (*out++) = y0;
177 }
178 mDelays[0][0] = x1;
179 mDelays[0][1] = x2;
180 mDelays[0][2] = y1;
181 mDelays[0][3] = y2;
182 }
183
process_transition_normal_mono(const audio_sample_t * in,audio_sample_t * out,int frameCount)184 void AudioBiquadFilter::process_transition_normal_mono(const audio_sample_t * in,
185 audio_sample_t * out,
186 int frameCount) {
187 if (updateCoefs(mTargetCoefs, frameCount)) {
188 setState(STATE_NORMAL);
189 }
190 process_normal_mono(in, out, frameCount);
191 }
192
process_transition_bypass_mono(const audio_sample_t * in,audio_sample_t * out,int frameCount)193 void AudioBiquadFilter::process_transition_bypass_mono(const audio_sample_t * in,
194 audio_sample_t * out,
195 int frameCount) {
196 if (updateCoefs(IDENTITY_COEFS, frameCount)) {
197 setState(STATE_NORMAL);
198 }
199 process_normal_mono(in, out, frameCount);
200 }
201
process_normal_multi(const audio_sample_t * in,audio_sample_t * out,int frameCount)202 void AudioBiquadFilter::process_normal_multi(const audio_sample_t * in,
203 audio_sample_t * out,
204 int frameCount) {
205 const audio_coef_t b0 = mCoefs[0];
206 const audio_coef_t b1 = mCoefs[1];
207 const audio_coef_t b2 = mCoefs[2];
208 const audio_coef_t a1 = mCoefs[3];
209 const audio_coef_t a2 = mCoefs[4];
210 for (int ch = 0; ch < mNumChannels; ++ch) {
211 size_t nFrames = frameCount;
212 audio_sample_t x1 = mDelays[ch][0];
213 audio_sample_t x2 = mDelays[ch][1];
214 audio_sample_t y1 = mDelays[ch][2];
215 audio_sample_t y2 = mDelays[ch][3];
216 while (nFrames-- > 0) {
217 audio_sample_t x0 = *in;
218 audio_coef_sample_acc_t acc;
219 acc = mul_coef_sample(b0, x0);
220 acc = mac_coef_sample(b1, x1, acc);
221 acc = mac_coef_sample(b2, x2, acc);
222 acc = mac_coef_sample(a1, y1, acc);
223 acc = mac_coef_sample(a2, y2, acc);
224 audio_sample_t y0 = coef_sample_acc_to_sample(acc);
225 y2 = y1;
226 y1 = y0;
227 x2 = x1;
228 x1 = x0;
229 *out = y0;
230 in += mNumChannels;
231 out += mNumChannels;
232 }
233 mDelays[ch][0] = x1;
234 mDelays[ch][1] = x2;
235 mDelays[ch][2] = y1;
236 mDelays[ch][3] = y2;
237 in -= frameCount * mNumChannels - 1;
238 out -= frameCount * mNumChannels - 1;
239 }
240 }
241
process_transition_normal_multi(const audio_sample_t * in,audio_sample_t * out,int frameCount)242 void AudioBiquadFilter::process_transition_normal_multi(const audio_sample_t * in,
243 audio_sample_t * out,
244 int frameCount) {
245 if (updateCoefs(mTargetCoefs, frameCount)) {
246 setState(STATE_NORMAL);
247 }
248 process_normal_multi(in, out, frameCount);
249 }
250
process_transition_bypass_multi(const audio_sample_t * in,audio_sample_t * out,int frameCount)251 void AudioBiquadFilter::process_transition_bypass_multi(const audio_sample_t * in,
252 audio_sample_t * out,
253 int frameCount) {
254 if (updateCoefs(IDENTITY_COEFS, frameCount)) {
255 setState(STATE_NORMAL);
256 }
257 process_normal_multi(in, out, frameCount);
258 }
259
260 }
261