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