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