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
24namespace android {
25
26const audio_coef_t AudioBiquadFilter::IDENTITY_COEFS[AudioBiquadFilter::NUM_COEFS] = { AUDIO_COEF_ONE, 0, 0, 0, 0 };
27
28AudioBiquadFilter::AudioBiquadFilter(int nChannels, int sampleRate) {
29    configure(nChannels, sampleRate);
30    reset();
31}
32
33void 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
43void AudioBiquadFilter::reset() {
44    memcpy(mCoefs, IDENTITY_COEFS, sizeof(mCoefs));
45    mCoefDirtyBits = 0;
46    setState(STATE_BYPASS);
47}
48
49void AudioBiquadFilter::clear() {
50    memset(mDelays, 0, sizeof(mDelays));
51}
52
53void 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
65void AudioBiquadFilter::process(const audio_sample_t in[], audio_sample_t out[],
66                                int frameCount) {
67    (this->*mCurProcessFunc)(in, out, frameCount);
68}
69
70void 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
79void 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
88void 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
120bool 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
139void 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
148void 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
182void 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
191void 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
200void 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
240void 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
249void 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