1 /*
   2  * Copyright (c) 2014, 2015, Oracle and/or its affiliates. All rights reserved.
   3  * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
   4  *
   5  * This code is free software; you can redistribute it and/or modify it
   6  * under the terms of the GNU General Public License version 2 only, as
   7  * published by the Free Software Foundation.  Oracle designates this
   8  * particular file as subject to the "Classpath" exception as provided
   9  * by Oracle in the LICENSE file that accompanied this code.
  10  *
  11  * This code is distributed in the hope that it will be useful, but WITHOUT
  12  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  14  * version 2 for more details (a copy is included in the LICENSE file that
  15  * accompanied this code).
  16  *
  17  * You should have received a copy of the GNU General Public License version
  18  * 2 along with this work; if not, write to the Free Software Foundation,
  19  * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
  20  *
  21  * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
  22  * or visit www.oracle.com if you need additional information or have any
  23  * questions.
  24  */
  25 
  26 #include "AVFAudioEqualizer.h"
  27 #include <Accelerate/Accelerate.h>
  28 
  29 #define kAVFMinimumQFactor 1e-9
  30 
  31 #define IND_A0 0
  32 #define IND_A1 1
  33 #define IND_A2 2
  34 #define IND_B1 3
  35 #define IND_B2 4
  36 
  37 class AVFEqualizerKernel : public AUKernelBase {
  38 public:
  39     AVFEqualizerKernel(AVFAudioEqualizer *eq, AUEffectBase *inAudioUnit) :
  40         AUKernelBase(dynamic_cast<AUEffectBase*>(inAudioUnit)),
  41         mEQ(eq)
  42     {}
  43 
  44     virtual ~AVFEqualizerKernel() {}
  45 
  46     virtual void Process(const Float32 *inSourceP,
  47                          Float32 *inDestP,
  48                          UInt32 inFramesToProcess,
  49                          UInt32 inNumChannels,
  50                          bool& ioSilence) {
  51         if (ioSilence) {
  52             return;
  53         }
  54 
  55         mEQ->RunFilter(inSourceP, inDestP, inFramesToProcess, mChannelNum);
  56     }
  57 
  58 private:
  59     AVFAudioEqualizer *mEQ;
  60 };
  61 
  62 #pragma mark -
  63 
  64 AVFEqualizerBand::AVFEqualizerBand(AVFAudioEqualizer *eq, double frequency, double bandwidth, double gain) :
  65     CEqualizerBand(bandwidth, gain),
  66     mEQ(eq),
  67     mBypass(true),
  68     mChannels(0),
  69     mHistory(NULL),
  70     mFrequency(frequency),
  71     mFilterType(Peak)  // set later by the EQ, can be changed if band moves
  72 {
  73     // we may not have an audio unit yet
  74     int channels = mEQ->GetChannelCount();
  75     if (channels > 0) {
  76         SetChannelCount(channels);
  77     }
  78     RecalculateParams();
  79 }
  80 
  81 AVFEqualizerBand::~AVFEqualizerBand() {
  82     if (mHistory) {
  83         delete[] mHistory;
  84         mHistory = NULL;
  85     }
  86 }
  87 
  88 void AVFEqualizerBand::SetFilterType(AVFEqualizerBand::AVFEqualizerFilterType type) {
  89     mFilterType = type;
  90     RecalculateParams();
  91 }
  92 
  93 void AVFEqualizerBand::SetCenterFrequency(double centerFrequency) {
  94     mEQ->MoveBand(mFrequency, centerFrequency);
  95     mFrequency = centerFrequency;
  96     RecalculateParams();
  97 }
  98 
  99 void AVFEqualizerBand::SetChannelCount(int newCount) {
 100     if (newCount == mChannels) {
 101         return;
 102     }
 103 
 104     if (mHistory) {
 105         delete[] mHistory;
 106         mHistory = NULL;
 107     }
 108 
 109     mChannels = newCount;
 110     if (mChannels > 0) {
 111         mHistory = new AVFEQBandHistory[mChannels];
 112     }
 113 }
 114 
 115 // These calculations are based on the GStreamer equalizer, so we can produce
 116 // basically the same results
 117 static inline double calculate_omega(double centerFreq, double sampleRate) {
 118     if (centerFreq / sampleRate >= 0.5) {
 119         return M_PI;
 120     }
 121     if (centerFreq < 0.0) {
 122         return 0.0;
 123     }
 124     return 2.0 * M_PI * (centerFreq / sampleRate);
 125 }
 126 
 127 static inline double calculate_bandwidth(double bw, double rate) {
 128     if (bw / rate >= 0.5) {
 129         // tan(M_PI/2) fails, so set to slightly less than pi
 130         return M_PI - 0.00000001;
 131     }
 132     if (bw <= 0.0) {
 133         // this effectively disables the filter
 134         return 0.0;
 135     }
 136     return 2.0 * M_PI * (bw / rate);
 137 }
 138 
 139 /*
 140  * vDSP_deq22:
 141  * https://developer.apple.com/library/mac/documentation/Accelerate/Reference/vDSPRef/Reference/reference.html#//apple_ref/c/func/vDSP_deq22
 142  *
 143  * The biquadratic filter equation for the nth sample is:
 144  * D[n] = S[n] * a0 + S[n-1] * a1 + S[n-2] * a2 - D[n-1] * b1 - D[n-2] * b2
 145  *
 146  * vDSP_deq22 stuffs all coefficients in vector B and uses:
 147  * for p in [0,2]:
 148  *     A(n-p)i * B(p) -> A(n)*B[0] + A(n-1)*B[1] + A(n-2)*B[2]
 149  *
 150  * for p in [3,4]:
 151  *     C(n-p+2)k * B(p) -> C(n-1)*B[3] + C(n-2)*B[4]
 152  *
 153  * where A and C are vectors of at least size N+2
 154  * so B[0..2] is a0 to a2 respectively and B[3..4] is b1 and b2 respectively
 155  *
 156  * The formulae used to calculate the coefficients are taken from GStreamer so
 157  * we can match the behavior of the GStreamer pipeline (and they work well enough)
 158  * though modified for SIMD operations using vDSP_deq22.
 159  *
 160  * Note that the GStreamer coefficient names (a0-a2,b0-b2) are swapped from other
 161  * examples, but the use is the same.
 162  */
 163 void AVFEqualizerBand::SetupPeakFilter(double omega, double bw, double gain) {
 164     double cosF = cos(omega);
 165     double alpha =  tan(bw / 2.0);
 166     double alpha1 = alpha * gain;
 167     double alpha2 = alpha / gain;
 168 
 169     // set up peak filter coefficients
 170     mCoefficients[IND_A0] = 1.0 + alpha1;
 171     mCoefficients[IND_A1] = -2.0 * cosF;
 172     mCoefficients[IND_A2] = 1.0 - alpha1;
 173     double b0 = 1.0 + alpha2;
 174     mCoefficients[IND_B1] = -2.0 * cosF;
 175     mCoefficients[IND_B2] = 1.0 - alpha2;
 176 
 177     // pre-scale coefficients
 178     vDSP_vsdivD(mCoefficients, 1, &b0, mCoefficients, 1, 5);
 179 }
 180 
 181 void AVFEqualizerBand::SetupLowShelfFilter(double omega, double bw, double gain) {
 182     double egm = gain - 1.0;
 183     double egp = gain + 1.0;
 184     double alpha = tan(bw / 2.0);
 185     double delta = 2.0 * sqrt(gain) * alpha;
 186     double cosF = cos(omega);
 187 
 188     mCoefficients[IND_A0] = (egp - egm * cosF + delta) * gain;
 189     mCoefficients[IND_A1] = (egm - egp * cosF) * 2.0 * gain;
 190     mCoefficients[IND_A2] = (egp - egm * cosF - delta) * gain;
 191     double b0 = egp + egm * cosF + delta;
 192     mCoefficients[IND_B1] = (egm + egp * cosF) * -2.0;
 193     mCoefficients[IND_B2] = egp + egm * cosF - delta;
 194 
 195     // pre-scale coefficients
 196     vDSP_vsdivD(mCoefficients, 1, &b0, mCoefficients, 1, 5);
 197 }
 198 
 199 void AVFEqualizerBand::SetupHighShelfFilter(double omega, double bw, double gain) {
 200     double egm = gain - 1.0;
 201     double egp = gain + 1.0;
 202     double alpha = tan(bw / 2.0);
 203     double delta = 2.0 * sqrt(gain) * alpha;
 204     double cosF = cos(omega);
 205 
 206     mCoefficients[IND_A0] = (egp + egm * cosF + delta) * gain;
 207     mCoefficients[IND_A1] = (egm + egp * cosF) * -2.0 * gain;
 208     mCoefficients[IND_A2] = (egp + egm * cosF - delta) * gain;
 209     double b0 = egp - egm * cosF + delta;
 210     mCoefficients[IND_B1] = (egm - egp * cosF) * 2.0;
 211     mCoefficients[IND_B2] = egp - egm * cosF - delta;
 212 
 213     // pre-scale coefficients
 214     vDSP_vsdivD(mCoefficients, 1, &b0, mCoefficients, 1, 5);
 215 }
 216 
 217 void AVFEqualizerBand::RecalculateParams() {
 218     double rate = mEQ->GetSampleRate();
 219 
 220     mBypass = (rate == 0.0);
 221     if (mBypass) {
 222         // can't calculate until we have a sample rate
 223         return;
 224     }
 225 
 226     // recalculate coefficients based on new parameters
 227     double bw = calculate_bandwidth(m_Bandwidth, rate);
 228     if (bw <= 0.0) {
 229         // no bandwidth, no filter...
 230         mBypass = true;
 231         return;
 232     }
 233     double absGain = pow(10, m_Gain / 40);      // convert dB to scale
 234     double omega = calculate_omega(mFrequency, rate);
 235 
 236     switch (mFilterType) {
 237         case Peak:
 238             SetupPeakFilter(omega, bw, absGain);
 239             break;
 240         case LowShelf:
 241             SetupLowShelfFilter(omega, bw, absGain);
 242             break;
 243         case HighShelf:
 244             SetupHighShelfFilter(omega, bw, absGain);
 245             break;
 246     }
 247 }
 248 
 249 void AVFEqualizerBand::ApplyFilter(double *inSource, double *inDest, int frameCount, int channel) {
 250     if (mBypass && mEQ->GetSampleRate() > 0.0) {
 251         // Have a sample rate now, can recalculate
 252         RecalculateParams();
 253     }
 254 
 255     if (mBypass || channel < 0) {
 256         return;
 257     }
 258 
 259     // We may have more channels now than when we were initialized
 260     if (channel > mChannels) {
 261         mChannels = mEQ->GetChannelCount();
 262         SetChannelCount(mChannels);
 263     }
 264 
 265     if (mChannels > 0 && mHistory != NULL) {
 266         // copy source and dest history
 267         inSource[1] = mHistory[channel].x1;
 268         inSource[0] = mHistory[channel].x2;
 269         inDest[1] = mHistory[channel].y1;
 270         inDest[0] = mHistory[channel].y2;
 271         
 272         vDSP_deq22D(inSource, 1, mCoefficients, inDest, 1, frameCount);
 273 
 274         // update history
 275         mHistory[channel].x1 = inSource[frameCount+1];
 276         mHistory[channel].x2 = inSource[frameCount];
 277         mHistory[channel].y1 = inDest[frameCount+1];
 278         mHistory[channel].y2 = inDest[frameCount];
 279     }
 280 }
 281 
 282 #pragma mark -
 283 
 284 AVFAudioEqualizer::~AVFAudioEqualizer() {
 285     mEQBufferA.free();
 286     mEQBufferB.free();
 287 
 288     // Free the EQ bands, otherwise they'll leak
 289     for (AVFEQBandIterator iter = mEQBands.begin(); iter != mEQBands.end(); iter++) {
 290         if (iter->second) {
 291             delete iter->second;
 292         }
 293     }
 294     mEQBands.clear();
 295 }
 296 
 297 AUKernelBase *AVFAudioEqualizer::NewKernel() {
 298     return new AVFEqualizerKernel(this, mAudioUnit);
 299 }
 300 
 301 bool AVFAudioEqualizer::IsEnabled() {
 302     return mEnabled;
 303 }
 304 
 305 void AVFAudioEqualizer::SetEnabled(bool isEnabled) {
 306     mEnabled = isEnabled;
 307 }
 308 
 309 int AVFAudioEqualizer::GetNumBands() {
 310     return (int)mEQBands.size();
 311 }
 312 
 313 CEqualizerBand *AVFAudioEqualizer::AddBand(double frequency, double bandwidth, double gain) {
 314     if (!mEQBands[frequency]) {
 315         mEQBands[frequency] = new AVFEqualizerBand(this, frequency, bandwidth, gain);
 316     } else {
 317         mEQBands[frequency]->SetBandwidth(bandwidth);
 318         mEQBands[frequency]->SetGain(gain);
 319     }
 320     ResetBandParameters();
 321     return mEQBands[frequency];
 322 }
 323 
 324 bool AVFAudioEqualizer::RemoveBand(double frequency) {
 325     AVFEqualizerBand *band = mEQBands[frequency];
 326     if (band) {
 327         mEQBands.erase(frequency);
 328         delete band;
 329         ResetBandParameters();
 330         return true;
 331     }
 332     return false;
 333 }
 334 
 335 void AVFAudioEqualizer::MoveBand(double oldFrequency, double newFrequency) {
 336     // only if freq actually changes
 337     if (oldFrequency != newFrequency) {
 338         AVFEqualizerBand *band = mEQBands[oldFrequency];
 339         if (band) {
 340             RemoveBand(newFrequency);
 341             mEQBands[newFrequency] = band;
 342             mEQBands.erase(oldFrequency);
 343         }
 344         ResetBandParameters();
 345     }
 346 }
 347 
 348 void AVFAudioEqualizer::ResetBandParameters() {
 349     // Update channel counts, recalculate params if necessary
 350     // bands are automatically sorted by the map from low to high
 351     for (AVFEQBandIterator iter = mEQBands.begin(); iter != mEQBands.end();) {
 352         if (!iter->second) {
 353             // NULL pointer protection, just remove the offending band
 354             mEQBands.erase(iter++);
 355             if (!mEQBands.empty() && (iter == mEQBands.end())) {
 356                 // re-process the last valid band, otherwise it won't be set to
 357                 // HighShelf filter type
 358                 --iter;
 359             } else {
 360                 continue;
 361             }
 362         }
 363         AVFEqualizerBand *band = iter->second;
 364         // middle bands are peak/notch filters
 365         AVFEqualizerBand::AVFEqualizerFilterType type = AVFEqualizerBand::Peak;
 366 
 367         if (iter == mEQBands.begin()) {
 368             type = AVFEqualizerBand::LowShelf;
 369         } else if (iter == --(mEQBands.end())) {
 370             type = AVFEqualizerBand::HighShelf;
 371         }
 372 
 373         band->SetFilterType(type);
 374         band->SetChannelCount(GetChannelCount());
 375         band->RecalculateParams();
 376         iter++; // here due to NULL ptr protection, otherwise we double increment
 377     }
 378 }
 379 
 380 void AVFAudioEqualizer::SetAudioUnit(AUEffectBase *unit) {
 381     this->AVFKernelProcessor::SetAudioUnit(unit);
 382     ResetBandParameters();
 383 }
 384 
 385 void AVFAudioEqualizer::RunFilter(const Float32 *inSourceP,
 386                                   Float32 *inDestP,
 387                                   UInt32 inFramesToProcess,
 388                                   UInt32 channel) {
 389     if (mEnabled && !mEQBands.empty()) {
 390         if (inFramesToProcess + 2 > mEQBufferSize) {
 391             mEQBufferSize = inFramesToProcess + 2;
 392             mEQBufferA.free();
 393             mEQBufferA.alloc(mEQBufferSize);
 394             mEQBufferB.free();
 395             mEQBufferB.alloc(mEQBufferSize);
 396         }
 397 
 398         // start processing with A buffer first
 399         bool srcA = true;
 400 
 401         // The first two elements are copied each time we call a band to process
 402             // float* cast is needed for Xcode 4.5
 403         vDSP_vspdp((float*)inSourceP, 1, mEQBufferA.get() + 2, 1, inFramesToProcess);
 404 
 405         // Run each band in sequence
 406         for (AVFEQBandIterator iter = mEQBands.begin(); iter != mEQBands.end(); iter++) {
 407             if (iter->second) {
 408                 if (srcA) {
 409                     iter->second->ApplyFilter(mEQBufferA, mEQBufferB, inFramesToProcess, channel);
 410                 } else {
 411                     iter->second->ApplyFilter(mEQBufferB, mEQBufferA, inFramesToProcess, channel);
 412                 }
 413                 srcA = !srcA;
 414             }
 415         }
 416 
 417         // Copy back to dest stream
 418         vDSP_vdpsp((srcA ? mEQBufferA : mEQBufferB)+2, 1, inDestP, 1, inFramesToProcess);
 419     }
 420 }