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 30 AudioBiquadFilter::AudioBiquadFilter(int nChannels, int sampleRate) { 31 configure(nChannels, sampleRate); 32 reset(); 33 } 34 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 45 void AudioBiquadFilter::reset() { 46 memcpy(mCoefs, IDENTITY_COEFS, sizeof(mCoefs)); 47 mCoefDirtyBits = 0; 48 setState(STATE_BYPASS); 49 } 50 51 void AudioBiquadFilter::clear() { 52 memset(mDelays, 0, sizeof(mDelays)); 53 } 54 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 67 void AudioBiquadFilter::process(const audio_sample_t in[], audio_sample_t out[], 68 int frameCount) { 69 (this->*mCurProcessFunc)(in, out, frameCount); 70 } 71 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 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 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 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 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 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 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 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 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 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 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
