engines/common/BiquadFilter.h

/***************************************************************************
 *                                                                         *
 *   LinuxSampler - modular, streaming capable sampler                     *
 *                                                                         *
 *   Copyright (C) 2003, 2004 by Benno Senoner and Christian Schoenebeck   *
 *   Copyright (C) 2005 Christian Schoenebeck                              *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the Free Software           *
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston,                 *
 *   MA  02111-1307  USA                                                   *
 ***************************************************************************/

#ifndef __LS_BIQUADFILTER_H__
#define __LS_BIQUADFILTER_H__

#include <math.h>

#include "../../common/global.h"

/// ln(2) / 2
#define LN_2_2                  0.34657359f

#ifndef LIMIT
# define LIMIT(v,l,u)           (v < l ? l : (v > u ? u : v))
#endif

namespace LinuxSampler {

    typedef float bq_t;

    /**
     * Internal parameters of the biquad filter, which are actually the
     * final parameters of the filter's transfer function. This strucure is
     * only needed when these parameters should stored outside the
     * BiquadFilter class, e.g. to save calculation time by sharing them
     * between multiple filters.
     */
    struct biquad_param_t {
        bq_t b0;
        bq_t b1;
        bq_t b2;
        bq_t a1;
        bq_t a2;
    };

    /**
     * Bi-quadratic filter
     * (adapted from lisp code by Eli Brandt, http://www.cs.cmu.edu/~eli/)
     */
    class BiquadFilter {
        protected:
            // following five variables are only used if no external biquad_param_t reference is used
            bq_t b0;
            bq_t b1;
            bq_t b2;
            bq_t a1;
            bq_t a2;
            // following four variables are used to buffer the feedback
            bq_t x1;
            bq_t x2;
            bq_t y1;
            bq_t y2;

#if __GNUC__ >= 4
            float fbc;
#else
            const static float fbc = 0.98;
#endif

            /**
             * Prevent \a f from going into denormal mode which would slow down
             * subsequent floating point calculations, we achieve that by setting
             * \a f to zero when it falls under the denormal threshold value.
             */
            inline void KillDenormal(bq_t& f) {
                // TODO: this is a generic solution for 32bit floats, should be replaced by CPU specific asm code
                f += 1e-18f;
                f -= 1e-18f;
            }
        public:
            BiquadFilter() {
                Reset();
#if __GNUC__ >= 4
                fbc = 0.98f;
#endif
            }

            void Reset() {
                x1 = 0.0f;
                x2 = 0.0f;
                y1 = 0.0f;
                y2 = 0.0f;
            }

            inline bq_t Apply(const bq_t x) {
                bq_t y;

                y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
                    this->a1 * this->y1 + this->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            inline bq_t Apply(biquad_param_t* param, const bq_t x) {
                bq_t y;

                y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
                    param->a1 * this->y1 + param->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

#if CONFIG_ASM && ARCH_X86
            // expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm6
            inline void Apply4StepsSSE(biquad_param_t* param) {
                __asm__ __volatile__ (
                    "movss (%2),%%xmm4                # b0\n\t"
                    "shufps   $0x00,%%xmm4,%%xmm4     # copy b0 to other cells\n\t"
                    "mulps  %%xmm0,%%xmm4             # xmm4 = x*b0\n\t"
                    "movups (%0),%%xmm2               # load b1,b2,a1,a2\n\t"
                    "movups (%1),%%xmm5               # load x1,x2,y1,y2\n\t"
                    /* sample 0 */
                    "movaps %%xmm5,%%xmm3\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "movss  %%xmm4,%%xmm6\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 1 */
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "movaps %%xmm3,%%xmm5\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "movss  %%xmm5,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 2 */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "movaps %%xmm3,%%xmm5\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "mulps  %%xmm2,%%xmm5             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "movss  %%xmm5,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6\n\t"
                    "shufps $0x39,%%xmm5,%%xmm5\n\t"
                    "addss  %%xmm5,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* sample 3 */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "shufps $0x39,%%xmm4,%%xmm4       # rotate xmm4 down 1 cell\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x93,%%xmm6,%%xmm6       # rotate output up 1 cell\n\t"
                    "mulps  %%xmm3,%%xmm2             # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell\n\t"
                    "movss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "shufps $0x39,%%xmm2,%%xmm2\n\t"
                    "addss  %%xmm2,%%xmm6\n\t"
                    "addss  %%xmm4,%%xmm6             # xmm6 = b0*x + b1*x1 + b2*x2 + a1*y1 + a2*y2\n\t"
                    /* done */
                    "shufps $0x0a,%%xmm3,%%xmm3       # x2 = x1, y2 = y1\n\t"
                    "movss  %%xmm6,%%xmm3             # y1 = y\n\t"
                    "shufps $0x4e,%%xmm3,%%xmm3       # rotate 2 cells\n\t"
                    "movss  %%xmm0,%%xmm3             # x1 = x\n\t"
                    "shufps $0x1b,%%xmm6,%%xmm6       # swap output to correct order\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0       # rotate input down 1 cell, to restore original input\n\t"
                    "movups %%xmm3,(%1)               # store x1,x2,y1,y2\n\t"
                    : /* no output */
                    : "r" (&param->b1), /* %0 - [b1,b2,a1,a2] */
                      "r" (&x1),        /* %1 - [x1,x2,y1,y2] */
                      "r" (&param->b0)  /* %2 */
                );
            }
#endif // CONFIG_ASM && ARCH_X86

            inline bq_t ApplyFB(bq_t x, const bq_t fb) {
                bq_t y;

                x += this->y1 * fb * 0.98;
                y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
                    this->a1 * this->y1 + this->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

            inline bq_t ApplyFB(biquad_param_t* param, bq_t x, const bq_t fb) {
                bq_t y;

                x += this->y1 * fb * 0.98;
                y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
                    param->a1 * this->y1 + param->a2 * this->y2;
                KillDenormal(y);
                this->x2 = this->x1;
                this->x1 = x;
                this->y2 = this->y1;
                this->y1 = y;

                return y;
            }

#if CONFIG_ASM && ARCH_X86
            // expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm7
            inline void ApplyFB4StepsSSE(biquad_param_t* param, const bq_t &fb) {
                float xs, ys;
                float t0, t1, t2, t3, t4, t5, t6, t7, t8; // temporary stack space
                __asm__ __volatile__ (
                    /* prepare input */
                    "movss  %15,%%xmm5\n\t"
                    "movss  %%xmm0,(%14)\n\t"
                    /* sample 0 */
                    "movss   %0, %%xmm3\n\t"
                    "movss   %1, %%xmm4\n\t"
                    "mulss   %%xmm4, %%xmm5\n\t"
                    "movss   %%xmm3, %2\n\t"
                    "movss   %%xmm5, %16\n\t"
                    "mulss   %%xmm3, %%xmm5\n\t"
                    "movss   %19, %%xmm2\n\t"
                    "movss   %3, %%xmm6\n\t"
                    "movss   %21, %%xmm3\n\t"
                    "addss   %%xmm5, %%xmm6\n\t"
                    "movss  %%xmm2, %%xmm5\n\t"
                    "movss   %20, %%xmm4\n\t"
                    "movss   %%xmm6, %4\n\t"
                    "mulss   %%xmm6, %%xmm5\n\t"
                    "movss   %5, %%xmm6\n\t"
                    "movss   %%xmm2, %6\n\t"
                    "movss   %%xmm4, %7\n\t"
                    "movss   %%xmm3, %%xmm2\n\t"
                    "mulss   %%xmm6, %%xmm4\n\t"
                    "mulss   %8, %%xmm2\n\t"
                    "movss   %%xmm3, %9\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %18, %%xmm3\n\t"
                    "movss   %17, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    "movss   %%xmm4, %10\n\t"
                    "movss   %%xmm3, %%xmm2\n\t"
                    "mulss   %11, %%xmm4\n\t"
                    "mulss   %12, %%xmm2\n\t"
                    "movss   %%xmm3, %13\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %11, %%xmm3\n\t"
                    "movss   %4, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    :: "m" (y1),  /* %0 */
                       "m" (fbc), /* %1 */
                       "m" (t0),  /* %2 */
                       "m" (xs),  /* %3 */
                       "m" (t7),  /* %4 */
                       "m" (x1),  /* %5 */
                       "m" (t1),  /* %6 */
                       "m" (t2),  /* %7 */
                       "m" (x2),  /* %8 */
                       "m" (t3),  /* %9 */
                       "m" (t4),  /* %10 */
                       "m" (t0),  /* %11 */
                       "m" (y2),  /* %12 */
                       "m" (t5),  /* %13 */
                       "r" (&xs),  /* %14 */
                       "m" (fb),   /* %15 */
                       "m" (ys),   /* %16 */
                       "m" (param->a1), /* %17 */
                       "m" (param->a2), /* %18 */
                       "m" (param->b0), /* %19 */
                       "m" (param->b1), /* %20 */
                       "m" (param->b2) /* %21 */
                );
                __asm__ __volatile__ (
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "movss  %%xmm5,%%xmm7\n\t"
                    ::
                );
                /* sample 1 */
                __asm__ __volatile__ (
                    "movss   %0, %%xmm4\n\t"
                    "movss   %%xmm0, %%xmm3\n\t"
                    "mulss   %%xmm5, %%xmm4\n\t"
                    "mulss   %3, %%xmm6\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "addss   %%xmm4, %%xmm3\n\t"
                    "mulss   %7, %%xmm2\n\t"
                    "movss   %6, %%xmm4\n\t"
                    "movss   %%xmm3, %8\n\t"
                    "mulss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm4\n\t"
                    "movss   %9, %%xmm3\n\t"
                    "mulss   %%xmm5, %%xmm3\n\t"
                    "movss   %10, %%xmm2\n\t"
                    "addss   %%xmm6, %%xmm4\n\t"
                    "mulss   %11, %%xmm2\n\t"
                    "addss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm2, %%xmm4\n\t"
                    :: "m" (ys),  /* %0 */
                       "m" (fbc), /* %1 */
                       "m" (xs),  /* %2 */
                       "m" (t3),  /* %3 */
                       "m" (y2),  /* %4 */
                       "m" (t2),  /* %5 */
                       "m" (t1),  /* %6 */
                       "m" (t7),  /* %7 */
                       "m" (t8),  /* %8 */
                       "m" (t4),  /* %9 */
                       "m" (t5),  /* %10 */
                       "m" (t0),  /* %11 */
                       "m" (x2),  /* %12 */
                       "m" (x1),  /* %13 */
                       "m" (y1)   /* %14 */
                );
                __asm__ __volatile__ (
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "movss  %%xmm4,%%xmm7\n\t"
                    ::
                );
                /* sample 2 */
                __asm__ __volatile__ (
                    "movss   %2, %%xmm6\n\t"
                    "movss   %3, %%xmm3\n\t"
                    "mulss   %%xmm4, %%xmm6\n\t"
                    "movss   %4, %%xmm2\n\t"
                    "mulss   %9, %%xmm2\n\t"
                    "addss   %%xmm0, %%xmm6\n\t"
                    "mulss   %7, %%xmm5\n\t"
                    "mulss   %%xmm6, %%xmm3\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "mulss   %8, %%xmm2\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    "movss   %6, %%xmm2\n\t"
                    "mulss   %%xmm4, %%xmm2\n\t"
                    "addss   %%xmm5, %%xmm2\n\t"
                    "addss   %%xmm2, %%xmm3\n\t"
                    :: "m" (xs),  /* %0 */
                       "m" (fb),  /* %1 */
                       "m" (ys), /* %2 */
                       "m" (t1),  /* %3 */
                       "m" (t2),  /* %4 */
                       "m" (t3),  /* %5 */
                       "m" (t4),  /* %6 */
                       "m" (t5),  /* %7 */
                       "m" (t7),  /* %8 */
                       "m" (t8),  /* %9 */
                       "m" (x1),  /* %10 */
                       "m" (x2),  /* %11 */
                       "m" (y1),  /* %12 */
                       "m" (y2)   /* %13 */
                );
                __asm__ __volatile__ (
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell\n\t"
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "movss  %%xmm3,%%xmm7\n\t"
                    ::
                );
                /* sample 3 */
                __asm__ __volatile__ (
                    "movss   %1, %%xmm2\n\t"
                    "mulss   %7, %%xmm4\n\t"
                    "mulss   %%xmm3, %%xmm2\n\t"
                    "movss   %3, %%xmm5\n\t"
                    "movss   %%xmm6, %11\n\t"
                    "addss   %%xmm0, %%xmm2\n\t"
                    "movss   %%xmm3, %13\n\t"
                    "mulss   %%xmm2, %%xmm5\n\t"
                    "mulss   %4, %%xmm6\n\t"
                    "movss   %%xmm2, %10\n\t"
                    "addss   %%xmm6, %%xmm5\n\t"
                    "movss   %5, %%xmm2\n\t"
                    "mulss   %9, %%xmm2\n\t"
                    "mulss   %6, %%xmm3\n\t"
                    "addss   %%xmm2, %%xmm5\n\t"
                    "addss   %%xmm3, %%xmm4\n\t"
                    "addss   %%xmm4, %%xmm5\n\t"
                    "movss   %%xmm5, %12\n\t"
                    :: "m" (xs),  /* %0 */
                       "m" (ys),  /* %1 */
                       "m" (fbc), /* %2 */
                       "m" (t1),  /* %3 */
                       "m" (t2),  /* %4 */
                       "m" (t3),  /* %5 */
                       "m" (t4),  /* %6 */
                       "m" (t5),  /* %7 */
                       "m" (t6),  /* %8 */
                       "m" (t8),  /* %9 */
                       "m" (x1),  /* %10 */
                       "m" (x2),  /* %11 */
                       "m" (y1),  /* %12 */
                       "m" (y2)   /* %13 */
                );
                __asm__ __volatile__ (
                    "shufps $0x93,%%xmm7,%%xmm7  # rotate up one cell\n\t"
                    "shufps $0x39,%%xmm0,%%xmm0  # rotate down one cell to restore original input\n\t"
                    "movss  %%xmm5,%%xmm7\n\t"
                    "shufps $0x1b,%%xmm7,%%xmm7  # swap output to correct order\n\t"
                    ::
                );
            }
#endif // CONFIG_ASM && ARCH_X86
    };

    /** @brief Lowpass Filter
     *
     * Lowpass filter based on biquad filter implementation.
     */
    class LowpassFilter : public BiquadFilter {
        public:
            inline LowpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * (1.0 - cs) * 0.5;
                this->b1 = a0r * (1.0 - cs);
                this->b2 = a0r * (1.0 - cs) * 0.5;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * (1.0 - cs) * 0.5;
                param->b1 = a0r * (1.0 - cs);
                param->b2 = a0r * (1.0 - cs) * 0.5;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

    /** @brief Bandpass Filter
     *
     * Bandpass filter based on biquad filter implementation.
     */
    class BandpassFilter : public BiquadFilter {
        public:
            inline BandpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * sn * 0.71;
                this->b1 = 0.0;
                this->b2 = a0r * -sn * 0.71;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * sn * 0.71;
                param->b1 = 0.0;
                param->b2 = a0r * -sn * 0.71;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

    /** @brief Highpass Filter
     *
     * Highpass filter based on biquad filter implementation.
     */
    class HighpassFilter : public BiquadFilter {
        public:
            inline HighpassFilter() : BiquadFilter() {}

            inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                this->b0 = a0r * (1.0 + cs) * 0.5;
                this->b1 = a0r * -(1.0 + cs);
                this->b2 = a0r * (1.0 + cs) * 0.5;
                this->a1 = a0r * (2.0 * cs);
                this->a2 = a0r * (alpha - 1.0);
            }

            inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
                bq_t omega = 2.0 * M_PI * fc / fs;
                bq_t sn    = sin(omega);
                bq_t cs    = cos(omega);
                bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);

                const float a0r = 1.0 / (1.0 + alpha);
                param->b0 = a0r * (1.0 + cs) * 0.5;
                param->b1 = a0r * -(1.0 + cs);
                param->b2 = a0r * (1.0 + cs) * 0.5;
                param->a1 = a0r * (2.0 * cs);
                param->a2 = a0r * (alpha - 1.0);
            }
    };

} // namespace LinuxSampler

#endif // __LS_BIQUADFILTER_H__
1	/***************************************************************************
2	* *
3	* LinuxSampler - modular, streaming capable sampler *
4	* *
5	* Copyright (C) 2003, 2004 by Benno Senoner and Christian Schoenebeck *
6	* Copyright (C) 2005 Christian Schoenebeck *
7	* *
8	* This program is free software; you can redistribute it and/or modify *
9	* it under the terms of the GNU General Public License as published by *
10	* the Free Software Foundation; either version 2 of the License, or *
11	* (at your option) any later version. *
12	* *
13	* This program is distributed in the hope that it will be useful, *
14	* but WITHOUT ANY WARRANTY; without even the implied warranty of *
15	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
16	* GNU General Public License for more details. *
17	* *
18	* You should have received a copy of the GNU General Public License *
19	* along with this program; if not, write to the Free Software *
20	* Foundation, Inc., 59 Temple Place, Suite 330, Boston, *
21	* MA 02111-1307 USA *
22	***************************************************************************/
23
24	#ifndef __LS_BIQUADFILTER_H__
25	#define __LS_BIQUADFILTER_H__
26
27	#include <math.h>
28
29	#include "../../common/global.h"
30
31	/// ln(2) / 2
32	#define LN_2_2 0.34657359f
33
34	#ifndef LIMIT
35	# define LIMIT(v,l,u) (v < l ? l : (v > u ? u : v))
36	#endif
37
38	namespace LinuxSampler {
39
40	typedef float bq_t;
41
42	/**
43	* Internal parameters of the biquad filter, which are actually the
44	* final parameters of the filter's transfer function. This strucure is
45	* only needed when these parameters should stored outside the
46	* BiquadFilter class, e.g. to save calculation time by sharing them
47	* between multiple filters.
48	*/
49	struct biquad_param_t {
50	bq_t b0;
51	bq_t b1;
52	bq_t b2;
53	bq_t a1;
54	bq_t a2;
55	};
56
57	/**
58	* Bi-quadratic filter
59	* (adapted from lisp code by Eli Brandt, http://www.cs.cmu.edu/~eli/)
60	*/
61	class BiquadFilter {
62	protected:
63	// following five variables are only used if no external biquad_param_t reference is used
64	bq_t b0;
65	bq_t b1;
66	bq_t b2;
67	bq_t a1;
68	bq_t a2;
69	// following four variables are used to buffer the feedback
70	bq_t x1;
71	bq_t x2;
72	bq_t y1;
73	bq_t y2;
74
75	#if __GNUC__ >= 4
76	float fbc;
77	#else
78	const static float fbc = 0.98;
79	#endif
80
81	/**
82	* Prevent \a f from going into denormal mode which would slow down
83	* subsequent floating point calculations, we achieve that by setting
84	* \a f to zero when it falls under the denormal threshold value.
85	*/
86	inline void KillDenormal(bq_t& f) {
87	// TODO: this is a generic solution for 32bit floats, should be replaced by CPU specific asm code
88	f += 1e-18f;
89	f -= 1e-18f;
90	}
91	public:
92	BiquadFilter() {
93	Reset();
94	#if __GNUC__ >= 4
95	fbc = 0.98f;
96	#endif
97	}
98
99	void Reset() {
100	x1 = 0.0f;
101	x2 = 0.0f;
102	y1 = 0.0f;
103	y2 = 0.0f;
104	}
105
106	inline bq_t Apply(const bq_t x) {
107	bq_t y;
108
109	y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
110	this->a1 * this->y1 + this->a2 * this->y2;
111	KillDenormal(y);
112	this->x2 = this->x1;
113	this->x1 = x;
114	this->y2 = this->y1;
115	this->y1 = y;
116
117	return y;
118	}
119
120	inline bq_t Apply(biquad_param_t* param, const bq_t x) {
121	bq_t y;
122
123	y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
124	param->a1 * this->y1 + param->a2 * this->y2;
125	KillDenormal(y);
126	this->x2 = this->x1;
127	this->x1 = x;
128	this->y2 = this->y1;
129	this->y1 = y;
130
131	return y;
132	}
133
134	#if CONFIG_ASM && ARCH_X86
135	// expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm6
136	inline void Apply4StepsSSE(biquad_param_t* param) {
137	__asm__ __volatile__ (
138	"movss (%2),%%xmm4 # b0\n\t"
139	"shufps $0x00,%%xmm4,%%xmm4 # copy b0 to other cells\n\t"
140	"mulps %%xmm0,%%xmm4 # xmm4 = x*b0\n\t"
141	"movups (%0),%%xmm2 # load b1,b2,a1,a2\n\t"
142	"movups (%1),%%xmm5 # load x1,x2,y1,y2\n\t"
143	/* sample 0 */
144	"movaps %%xmm5,%%xmm3\n\t"
145	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
146	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
147	"movss %%xmm4,%%xmm6\n\t"
148	"addss %%xmm5,%%xmm6\n\t"
149	"shufps $0x39,%%xmm5,%%xmm5\n\t"
150	"addss %%xmm5,%%xmm6\n\t"
151	"shufps $0x39,%%xmm5,%%xmm5\n\t"
152	"addss %%xmm5,%%xmm6\n\t"
153	"shufps $0x39,%%xmm5,%%xmm5\n\t"
154	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
155	/* sample 1 */
156	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
157	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
158	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
159	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
160	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
161	"movaps %%xmm3,%%xmm5\n\t"
162	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
163	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
164	"movss %%xmm5,%%xmm6\n\t"
165	"addss %%xmm4,%%xmm6\n\t"
166	"shufps $0x39,%%xmm5,%%xmm5\n\t"
167	"addss %%xmm5,%%xmm6\n\t"
168	"shufps $0x39,%%xmm5,%%xmm5\n\t"
169	"addss %%xmm5,%%xmm6\n\t"
170	"shufps $0x39,%%xmm5,%%xmm5\n\t"
171	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
172	/* sample 2 */
173	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
174	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
175	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
176	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
177	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
178	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
179	"movaps %%xmm3,%%xmm5\n\t"
180	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
181	"mulps %%xmm2,%%xmm5 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
182	"movss %%xmm5,%%xmm6\n\t"
183	"addss %%xmm4,%%xmm6\n\t"
184	"shufps $0x39,%%xmm5,%%xmm5\n\t"
185	"addss %%xmm5,%%xmm6\n\t"
186	"shufps $0x39,%%xmm5,%%xmm5\n\t"
187	"addss %%xmm5,%%xmm6\n\t"
188	"shufps $0x39,%%xmm5,%%xmm5\n\t"
189	"addss %%xmm5,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
190	/* sample 3 */
191	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
192	"shufps $0x39,%%xmm4,%%xmm4 # rotate xmm4 down 1 cell\n\t"
193	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
194	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
195	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
196	"shufps $0x93,%%xmm6,%%xmm6 # rotate output up 1 cell\n\t"
197	"mulps %%xmm3,%%xmm2 # xmm5 = [b1,b2,a1,a2] * [x1,x2,y1,y2]\n\t"
198	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell\n\t"
199	"movss %%xmm2,%%xmm6\n\t"
200	"shufps $0x39,%%xmm2,%%xmm2\n\t"
201	"addss %%xmm2,%%xmm6\n\t"
202	"shufps $0x39,%%xmm2,%%xmm2\n\t"
203	"addss %%xmm2,%%xmm6\n\t"
204	"shufps $0x39,%%xmm2,%%xmm2\n\t"
205	"addss %%xmm2,%%xmm6\n\t"
206	"addss %%xmm4,%%xmm6 # xmm6 = b0x + b1x1 + b2x2 + a1y1 + a2*y2\n\t"
207	/* done */
208	"shufps $0x0a,%%xmm3,%%xmm3 # x2 = x1, y2 = y1\n\t"
209	"movss %%xmm6,%%xmm3 # y1 = y\n\t"
210	"shufps $0x4e,%%xmm3,%%xmm3 # rotate 2 cells\n\t"
211	"movss %%xmm0,%%xmm3 # x1 = x\n\t"
212	"shufps $0x1b,%%xmm6,%%xmm6 # swap output to correct order\n\t"
213	"shufps $0x39,%%xmm0,%%xmm0 # rotate input down 1 cell, to restore original input\n\t"
214	"movups %%xmm3,(%1) # store x1,x2,y1,y2\n\t"
215	: /* no output */
216	: "r" (&param->b1), /* %0 - [b1,b2,a1,a2] */
217	"r" (&x1), /* %1 - [x1,x2,y1,y2] */
218	"r" (&param->b0) /* %2 */
219	);
220	}
221	#endif // CONFIG_ASM && ARCH_X86
222
223	inline bq_t ApplyFB(bq_t x, const bq_t fb) {
224	bq_t y;
225
226	x += this->y1 * fb * 0.98;
227	y = this->b0 * x + this->b1 * this->x1 + this->b2 * this->x2 +
228	this->a1 * this->y1 + this->a2 * this->y2;
229	KillDenormal(y);
230	this->x2 = this->x1;
231	this->x1 = x;
232	this->y2 = this->y1;
233	this->y1 = y;
234
235	return y;
236	}
237
238	inline bq_t ApplyFB(biquad_param_t* param, bq_t x, const bq_t fb) {
239	bq_t y;
240
241	x += this->y1 * fb * 0.98;
242	y = param->b0 * x + param->b1 * this->x1 + param->b2 * this->x2 +
243	param->a1 * this->y1 + param->a2 * this->y2;
244	KillDenormal(y);
245	this->x2 = this->x1;
246	this->x1 = x;
247	this->y2 = this->y1;
248	this->y1 = y;
249
250	return y;
251	}
252
253	#if CONFIG_ASM && ARCH_X86
254	// expects to find input in xmm0 (xmm0 stays unmodified) and finally leaves output in xmm7
255	inline void ApplyFB4StepsSSE(biquad_param_t* param, const bq_t &fb) {
256	float xs, ys;
257	float t0, t1, t2, t3, t4, t5, t6, t7, t8; // temporary stack space
258	__asm__ __volatile__ (
259	/* prepare input */
260	"movss %15,%%xmm5\n\t"
261	"movss %%xmm0,(%14)\n\t"
262	/* sample 0 */
263	"movss %0, %%xmm3\n\t"
264	"movss %1, %%xmm4\n\t"
265	"mulss %%xmm4, %%xmm5\n\t"
266	"movss %%xmm3, %2\n\t"
267	"movss %%xmm5, %16\n\t"
268	"mulss %%xmm3, %%xmm5\n\t"
269	"movss %19, %%xmm2\n\t"
270	"movss %3, %%xmm6\n\t"
271	"movss %21, %%xmm3\n\t"
272	"addss %%xmm5, %%xmm6\n\t"
273	"movss %%xmm2, %%xmm5\n\t"
274	"movss %20, %%xmm4\n\t"
275	"movss %%xmm6, %4\n\t"
276	"mulss %%xmm6, %%xmm5\n\t"
277	"movss %5, %%xmm6\n\t"
278	"movss %%xmm2, %6\n\t"
279	"movss %%xmm4, %7\n\t"
280	"movss %%xmm3, %%xmm2\n\t"
281	"mulss %%xmm6, %%xmm4\n\t"
282	"mulss %8, %%xmm2\n\t"
283	"movss %%xmm3, %9\n\t"
284	"addss %%xmm4, %%xmm5\n\t"
285	"movss %18, %%xmm3\n\t"
286	"movss %17, %%xmm4\n\t"
287	"addss %%xmm2, %%xmm5\n\t"
288	"movss %%xmm4, %10\n\t"
289	"movss %%xmm3, %%xmm2\n\t"
290	"mulss %11, %%xmm4\n\t"
291	"mulss %12, %%xmm2\n\t"
292	"movss %%xmm3, %13\n\t"
293	"addss %%xmm4, %%xmm5\n\t"
294	"movss %11, %%xmm3\n\t"
295	"movss %4, %%xmm4\n\t"
296	"addss %%xmm2, %%xmm5\n\t"
297	:: "m" (y1), /* %0 */
298	"m" (fbc), /* %1 */
299	"m" (t0), /* %2 */
300	"m" (xs), /* %3 */
301	"m" (t7), /* %4 */
302	"m" (x1), /* %5 */
303	"m" (t1), /* %6 */
304	"m" (t2), /* %7 */
305	"m" (x2), /* %8 */
306	"m" (t3), /* %9 */
307	"m" (t4), /* %10 */
308	"m" (t0), /* %11 */
309	"m" (y2), /* %12 */
310	"m" (t5), /* %13 */
311	"r" (&xs), /* %14 */
312	"m" (fb), /* %15 */
313	"m" (ys), /* %16 */
314	"m" (param->a1), /* %17 */
315	"m" (param->a2), /* %18 */
316	"m" (param->b0), /* %19 */
317	"m" (param->b1), /* %20 */
318	"m" (param->b2) /* %21 */
319	);
320	__asm__ __volatile__ (
321	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
322	"movss %%xmm5,%%xmm7\n\t"
323	::
324	);
325	/* sample 1 */
326	__asm__ __volatile__ (
327	"movss %0, %%xmm4\n\t"
328	"movss %%xmm0, %%xmm3\n\t"
329	"mulss %%xmm5, %%xmm4\n\t"
330	"mulss %3, %%xmm6\n\t"
331	"movss %5, %%xmm2\n\t"
332	"addss %%xmm4, %%xmm3\n\t"
333	"mulss %7, %%xmm2\n\t"
334	"movss %6, %%xmm4\n\t"
335	"movss %%xmm3, %8\n\t"
336	"mulss %%xmm3, %%xmm4\n\t"
337	"addss %%xmm2, %%xmm4\n\t"
338	"movss %9, %%xmm3\n\t"
339	"mulss %%xmm5, %%xmm3\n\t"
340	"movss %10, %%xmm2\n\t"
341	"addss %%xmm6, %%xmm4\n\t"
342	"mulss %11, %%xmm2\n\t"
343	"addss %%xmm3, %%xmm4\n\t"
344	"addss %%xmm2, %%xmm4\n\t"
345	:: "m" (ys), /* %0 */
346	"m" (fbc), /* %1 */
347	"m" (xs), /* %2 */
348	"m" (t3), /* %3 */
349	"m" (y2), /* %4 */
350	"m" (t2), /* %5 */
351	"m" (t1), /* %6 */
352	"m" (t7), /* %7 */
353	"m" (t8), /* %8 */
354	"m" (t4), /* %9 */
355	"m" (t5), /* %10 */
356	"m" (t0), /* %11 */
357	"m" (x2), /* %12 */
358	"m" (x1), /* %13 */
359	"m" (y1) /* %14 */
360	);
361	__asm__ __volatile__ (
362	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
363	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
364	"movss %%xmm4,%%xmm7\n\t"
365	::
366	);
367	/* sample 2 */
368	__asm__ __volatile__ (
369	"movss %2, %%xmm6\n\t"
370	"movss %3, %%xmm3\n\t"
371	"mulss %%xmm4, %%xmm6\n\t"
372	"movss %4, %%xmm2\n\t"
373	"mulss %9, %%xmm2\n\t"
374	"addss %%xmm0, %%xmm6\n\t"
375	"mulss %7, %%xmm5\n\t"
376	"mulss %%xmm6, %%xmm3\n\t"
377	"addss %%xmm2, %%xmm3\n\t"
378	"movss %5, %%xmm2\n\t"
379	"mulss %8, %%xmm2\n\t"
380	"addss %%xmm2, %%xmm3\n\t"
381	"movss %6, %%xmm2\n\t"
382	"mulss %%xmm4, %%xmm2\n\t"
383	"addss %%xmm5, %%xmm2\n\t"
384	"addss %%xmm2, %%xmm3\n\t"
385	:: "m" (xs), /* %0 */
386	"m" (fb), /* %1 */
387	"m" (ys), /* %2 */
388	"m" (t1), /* %3 */
389	"m" (t2), /* %4 */
390	"m" (t3), /* %5 */
391	"m" (t4), /* %6 */
392	"m" (t5), /* %7 */
393	"m" (t7), /* %8 */
394	"m" (t8), /* %9 */
395	"m" (x1), /* %10 */
396	"m" (x2), /* %11 */
397	"m" (y1), /* %12 */
398	"m" (y2) /* %13 */
399	);
400	__asm__ __volatile__ (
401	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell\n\t"
402	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
403	"movss %%xmm3,%%xmm7\n\t"
404	::
405	);
406	/* sample 3 */
407	__asm__ __volatile__ (
408	"movss %1, %%xmm2\n\t"
409	"mulss %7, %%xmm4\n\t"
410	"mulss %%xmm3, %%xmm2\n\t"
411	"movss %3, %%xmm5\n\t"
412	"movss %%xmm6, %11\n\t"
413	"addss %%xmm0, %%xmm2\n\t"
414	"movss %%xmm3, %13\n\t"
415	"mulss %%xmm2, %%xmm5\n\t"
416	"mulss %4, %%xmm6\n\t"
417	"movss %%xmm2, %10\n\t"
418	"addss %%xmm6, %%xmm5\n\t"
419	"movss %5, %%xmm2\n\t"
420	"mulss %9, %%xmm2\n\t"
421	"mulss %6, %%xmm3\n\t"
422	"addss %%xmm2, %%xmm5\n\t"
423	"addss %%xmm3, %%xmm4\n\t"
424	"addss %%xmm4, %%xmm5\n\t"
425	"movss %%xmm5, %12\n\t"
426	:: "m" (xs), /* %0 */
427	"m" (ys), /* %1 */
428	"m" (fbc), /* %2 */
429	"m" (t1), /* %3 */
430	"m" (t2), /* %4 */
431	"m" (t3), /* %5 */
432	"m" (t4), /* %6 */
433	"m" (t5), /* %7 */
434	"m" (t6), /* %8 */
435	"m" (t8), /* %9 */
436	"m" (x1), /* %10 */
437	"m" (x2), /* %11 */
438	"m" (y1), /* %12 */
439	"m" (y2) /* %13 */
440	);
441	__asm__ __volatile__ (
442	"shufps $0x93,%%xmm7,%%xmm7 # rotate up one cell\n\t"
443	"shufps $0x39,%%xmm0,%%xmm0 # rotate down one cell to restore original input\n\t"
444	"movss %%xmm5,%%xmm7\n\t"
445	"shufps $0x1b,%%xmm7,%%xmm7 # swap output to correct order\n\t"
446	::
447	);
448	}
449	#endif // CONFIG_ASM && ARCH_X86
450	};
451
452	/** @brief Lowpass Filter
453	*
454	* Lowpass filter based on biquad filter implementation.
455	*/
456	class LowpassFilter : public BiquadFilter {
457	public:
458	inline LowpassFilter() : BiquadFilter() {}
459
460	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
461	bq_t omega = 2.0 * M_PI * fc / fs;
462	bq_t sn = sin(omega);
463	bq_t cs = cos(omega);
464	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
465
466	const float a0r = 1.0 / (1.0 + alpha);
467	this->b0 = a0r * (1.0 - cs) * 0.5;
468	this->b1 = a0r * (1.0 - cs);
469	this->b2 = a0r * (1.0 - cs) * 0.5;
470	this->a1 = a0r * (2.0 * cs);
471	this->a2 = a0r * (alpha - 1.0);
472	}
473
474	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
475	bq_t omega = 2.0 * M_PI * fc / fs;
476	bq_t sn = sin(omega);
477	bq_t cs = cos(omega);
478	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
479
480	const float a0r = 1.0 / (1.0 + alpha);
481	param->b0 = a0r * (1.0 - cs) * 0.5;
482	param->b1 = a0r * (1.0 - cs);
483	param->b2 = a0r * (1.0 - cs) * 0.5;
484	param->a1 = a0r * (2.0 * cs);
485	param->a2 = a0r * (alpha - 1.0);
486	}
487	};
488
489	/** @brief Bandpass Filter
490	*
491	* Bandpass filter based on biquad filter implementation.
492	*/
493	class BandpassFilter : public BiquadFilter {
494	public:
495	inline BandpassFilter() : BiquadFilter() {}
496
497	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
498	bq_t omega = 2.0 * M_PI * fc / fs;
499	bq_t sn = sin(omega);
500	bq_t cs = cos(omega);
501	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
502
503	const float a0r = 1.0 / (1.0 + alpha);
504	this->b0 = a0r * sn * 0.71;
505	this->b1 = 0.0;
506	this->b2 = a0r * -sn * 0.71;
507	this->a1 = a0r * (2.0 * cs);
508	this->a2 = a0r * (alpha - 1.0);
509	}
510
511	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
512	bq_t omega = 2.0 * M_PI * fc / fs;
513	bq_t sn = sin(omega);
514	bq_t cs = cos(omega);
515	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
516
517	const float a0r = 1.0 / (1.0 + alpha);
518	param->b0 = a0r * sn * 0.71;
519	param->b1 = 0.0;
520	param->b2 = a0r * -sn * 0.71;
521	param->a1 = a0r * (2.0 * cs);
522	param->a2 = a0r * (alpha - 1.0);
523	}
524	};
525
526	/** @brief Highpass Filter
527	*
528	* Highpass filter based on biquad filter implementation.
529	*/
530	class HighpassFilter : public BiquadFilter {
531	public:
532	inline HighpassFilter() : BiquadFilter() {}
533
534	inline void SetParameters(bq_t fc, bq_t bw, bq_t fs) {
535	bq_t omega = 2.0 * M_PI * fc / fs;
536	bq_t sn = sin(omega);
537	bq_t cs = cos(omega);
538	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
539
540	const float a0r = 1.0 / (1.0 + alpha);
541	this->b0 = a0r * (1.0 + cs) * 0.5;
542	this->b1 = a0r * -(1.0 + cs);
543	this->b2 = a0r * (1.0 + cs) * 0.5;
544	this->a1 = a0r * (2.0 * cs);
545	this->a2 = a0r * (alpha - 1.0);
546	}
547
548	inline void SetParameters(biquad_param_t* param, bq_t fc, bq_t bw, bq_t fs) {
549	bq_t omega = 2.0 * M_PI * fc / fs;
550	bq_t sn = sin(omega);
551	bq_t cs = cos(omega);
552	bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
553
554	const float a0r = 1.0 / (1.0 + alpha);
555	param->b0 = a0r * (1.0 + cs) * 0.5;
556	param->b1 = a0r * -(1.0 + cs);
557	param->b2 = a0r * (1.0 + cs) * 0.5;
558	param->a1 = a0r * (2.0 * cs);
559	param->a2 = a0r * (alpha - 1.0);
560	}
561	};
562
563	} // namespace LinuxSampler
564
565	#endif // __LS_BIQUADFILTER_H__