Eric Laurent | 5fe37c6 | 2010-05-21 06:05:13 -0700 | [diff] [blame] | 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 | } |