biquad-float.h

   1 /*
   2 Calf Box, an open source musical instrument.
   3 Copyright (C) 2010 Krzysztof Foltman
   4
   5 This program is free software: you can redistribute it and/or modify
   6 it under the terms of the GNU General Public License as published by
   7 the Free Software Foundation, either version 3 of the License, or
   8 (at your option) any later version.
   9
  10 This program is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17 */
  18
  19 #ifndef CBOX_BIQUAD_FLOAT_H
  20 #define CBOX_BIQUAD_FLOAT_H
  21
  22 #include "config.h"
  23 #include "dspmath.h"
  24
  25 struct cbox_biquadf_state
  26 {
  27     float x1;
  28     float y1;
  29     float x2;
  30     float y2;
  31 };
  32
  33 struct cbox_biquadf_coeffs
  34 {
  35     float a0;
  36     float a1;
  37     float a2;
  38     float b1;
  39     float b2;
  40 };
  41
  42 static inline void cbox_biquadf_reset(struct cbox_biquadf_state *state)
  43 {
  44     state->x1 = state->y1 = state->x2 = state->y2 = 0.f;
  45 }
  46
  47 static inline float cbox_biquadf_is_audible(struct cbox_biquadf_state *state, float level)
  48 {
  49     return fabs(state->x1) + fabs(state->x2) + fabs(state->y1) + fabs(state->y2) >= level;
  50 }
  51
  52 // Based on filter coefficient equations by Robert Bristow-Johnson
  53 static inline void cbox_biquadf_set_lp_rbj(struct cbox_biquadf_coeffs *coeffs, float fc, float q, float sr)
  54 {
  55     float omega=(float)(2*M_PI*fc/sr);
  56     float sn=sin(omega);
  57     float cs=cos(omega);
  58     float alpha=(float)(sn/(2*q));
  59     float inv=(float)(1.0/(1.0+alpha));
  60
  61     coeffs->a2 = coeffs->a0 = (float)(inv*(1 - cs)*0.5f);
  62     coeffs->a1 = coeffs->a0 + coeffs->a0;
  63     coeffs->b1 =  (float)(-2*cs*inv);
  64     coeffs->b2 =  (float)((1 - alpha)*inv);
  65 }
  66
  67 static inline void cbox_biquadf_set_lp_rbj_lookup(struct cbox_biquadf_coeffs *coeffs, const struct cbox_sincos *sincos, float q)
  68 {
  69     float sn=sincos->sine;
  70     float cs=sincos->cosine;
  71     float alpha=(float)(sn/(2*q));
  72     float inv=(float)(1.0/(1.0+alpha));
  73
  74     coeffs->a2 = coeffs->a0 = (float)(inv*(1 - cs)*0.5f);
  75     coeffs->a1 = coeffs->a0 + coeffs->a0;
  76     coeffs->b1 =  (float)(-2*cs*inv);
  77     coeffs->b2 =  (float)((1 - alpha)*inv);
  78 }
  79
  80 // Based on filter coefficient equations by Robert Bristow-Johnson
  81 static inline void cbox_biquadf_set_hp_rbj(struct cbox_biquadf_coeffs *coeffs, float fc, float q, float sr)
  82 {
  83     float omega=(float)(2*M_PI*fc/sr);
  84     float sn=sin(omega);
  85     float cs=cos(omega);
  86     float alpha=(float)(sn/(2*q));
  87     float inv=(float)(1.0/(1.0+alpha));
  88
  89     coeffs->a2 = coeffs->a0 = (float)(inv*(1 + cs)*0.5f);
  90     coeffs->a1 = -2 * coeffs->a0;
  91     coeffs->b1 =  (float)(-2*cs*inv);
  92     coeffs->b2 =  (float)((1 - alpha)*inv);
  93 }
  94
  95 static inline void cbox_biquadf_set_hp_rbj_lookup(struct cbox_biquadf_coeffs *coeffs, const struct cbox_sincos *sincos, float q)
  96 {
  97     float sn=sincos->sine;
  98     float cs=sincos->cosine;
  99     float alpha=(float)(sn/(2*q));
 100     float inv=(float)(1.0/(1.0+alpha));
 101
 102     coeffs->a2 = coeffs->a0 = (float)(inv*(1 + cs)*0.5f);
 103     coeffs->a1 = -2 * coeffs->a0;
 104     coeffs->b1 =  (float)(-2*cs*inv);
 105     coeffs->b2 =  (float)((1 - alpha)*inv);
 106 }
 107
 108 // Based on filter coefficient equations by Robert Bristow-Johnson
 109 static inline void cbox_biquadf_set_bp_rbj(struct cbox_biquadf_coeffs *coeffs, float fc, float q, float sr)
 110 {
 111     float omega=(float)(2*M_PI*fc/sr);
 112     float sn=sin(omega);
 113     float cs=cos(omega);
 114     float alpha=(float)(sn/(2*q));
 115     float inv=(float)(1.0/(1.0+alpha));
 116
 117     coeffs->a0 = (float)(inv*alpha);
 118     coeffs->a1 = 0.f;
 119     coeffs->a2 = -coeffs->a0;
 120     coeffs->b1 =  (float)(-2*cs*inv);
 121     coeffs->b2 =  (float)((1 - alpha)*inv);
 122 }
 123
 124 static inline void cbox_biquadf_set_bp_rbj_lookup(struct cbox_biquadf_coeffs *coeffs, const struct cbox_sincos *sincos, float q)
 125 {
 126     float sn=sincos->sine;
 127     float cs=sincos->cosine;
 128     float alpha=(float)(sn/(2*q));
 129     float inv=(float)(1.0/(1.0+alpha));
 130
 131     coeffs->a0 = (float)(inv*alpha);
 132     coeffs->a1 = 0.f;
 133     coeffs->a2 = -coeffs->a0;
 134     coeffs->b1 =  (float)(-2*cs*inv);
 135     coeffs->b2 =  (float)((1 - alpha)*inv);
 136 }
 137
 138
 139 // Based on filter coefficient equations by Robert Bristow-Johnson
 140 static inline void cbox_biquadf_set_peakeq_rbj(struct cbox_biquadf_coeffs *coeffs, float freq, float q, float peak, float sr)
 141 {
 142     float A = sqrt(peak);
 143     float w0 = freq * 2 * M_PI * (1.0 / sr);
 144     float alpha = sin(w0) / (2 * q);
 145     float ib0 = 1.0 / (1 + alpha/A);
 146     coeffs->a1 = coeffs->b1 = -2*cos(w0) * ib0;
 147     coeffs->a0 = ib0 * (1 + alpha*A);
 148     coeffs->a2 = ib0 * (1 - alpha*A);
 149     coeffs->b2 = ib0 * (1 - alpha/A);
 150 }
 151
 152 static inline void cbox_biquadf_set_peakeq_rbj_scaled(struct cbox_biquadf_coeffs *coeffs, float freq, float q, float A, float sr)
 153 {
 154     float w0 = freq * 2 * M_PI * (1.0 / sr);
 155     float alpha = sin(w0) / (2 * q);
 156     float ib0 = 1.0 / (1 + alpha/A);
 157     coeffs->a1 = coeffs->b1 = -2*cos(w0) * ib0;
 158     coeffs->a0 = ib0 * (1 + alpha*A);
 159     coeffs->a2 = ib0 * (1 - alpha*A);
 160     coeffs->b2 = ib0 * (1 - alpha/A);
 161 }
 162
 163 // This is my math, and it's rather suspect
 164 static inline void cbox_biquadf_set_1plp(struct cbox_biquadf_coeffs *coeffs, float freq, float sr)
 165 {
 166     float w = hz2w(freq, sr);
 167     float x = tan (w * 0.5f);
 168     float q = 1 / (1 + x);
 169     float a01 = x*q;
 170     float b1 = a01 - q;
 171
 172     coeffs->a0 = a01;
 173     coeffs->a1 = a01;
 174     coeffs->b1 = b1;
 175     coeffs->a2 = 0;
 176     coeffs->b2 = 0;
 177 }
 178
 179 static inline void cbox_biquadf_set_1php(struct cbox_biquadf_coeffs *coeffs, float freq, float sr)
 180 {
 181     float w = hz2w(freq, sr);
 182     float x = tan (w * 0.5f);
 183     float q = 1 / (1 + x);
 184     float a01 = x*q;
 185     float b1 = a01 - q;
 186
 187     coeffs->a0 = q;
 188     coeffs->a1 = -q;
 189     coeffs->b1 = b1;
 190     coeffs->a2 = 0;
 191     coeffs->b2 = 0;
 192 }
 193
 194 static inline void cbox_biquadf_set_1p(struct cbox_biquadf_coeffs *coeffs, float a0, float a1, float b1, int two_copies)
 195 {
 196     if (two_copies)
 197     {
 198         // (a0 + a1z) * (a0 + a1z) = a0^2 + 2*a0*a1*z + a1^2*z^2
 199         // (1 - b1z) * (1 - b1z) = 1 - 2b1*z + b1^2*z^2
 200         coeffs->a0 = a0*a0;
 201         coeffs->a1 = 2*a0*a1;
 202         coeffs->b1 = 2 * b1;
 203         coeffs->a2 = a1*a1;
 204         coeffs->b2 = b1*b1;
 205     }
 206     else
 207     {
 208         coeffs->a0 = a0;
 209         coeffs->a1 = a1;
 210         coeffs->b1 = b1;
 211         coeffs->a2 = 0;
 212         coeffs->b2 = 0;
 213     }
 214 }
 215
 216 static inline void cbox_biquadf_set_1plp_lookup(struct cbox_biquadf_coeffs *coeffs, struct cbox_sincos *sincos, int two_copies)
 217 {
 218     float x = sincos->prewarp;
 219     float q = 1 / (1 + x);
 220     float a01 = x*q;
 221     float b1 = a01 - q;
 222
 223     cbox_biquadf_set_1p(coeffs, a01, a01, b1, two_copies);
 224 }
 225
 226 static inline void cbox_biquadf_set_1php_lookup(struct cbox_biquadf_coeffs *coeffs, struct cbox_sincos *sincos, int two_copies)
 227 {
 228     float x = sincos->prewarp;
 229     float q = 1 / (1 + x);
 230     float a01 = x*q;
 231     float b1 = a01 - q;
 232
 233     cbox_biquadf_set_1p(coeffs, q, -q, b1, two_copies);
 234 }
 235
 236 #if USE_NEON
 237
 238 #include <arm_neon.h>
 239
 240 static inline void cbox_biquadf_process(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer)
 241 {
 242     int i;
 243     float32x2_t c0 = {coeffs->a0, 0};
 244     float32x2_t c1 = {coeffs->a1, -coeffs->b1};
 245     float32x2_t c2 = {coeffs->a2, -coeffs->b2};
 246     float32x2_t s1 = {state->x1, state->y1};
 247     float32x2_t s2 = {state->x2, state->y2};
 248
 249     for (i = 0; i < CBOX_BLOCK_SIZE; i ++)
 250     {
 251         float32x2_t in12 = {buffer[i], 0.f};
 252
 253         float32x2_t out12 = vmla_f32(vmla_f32(vmul_f32(c1, s1), c2, s2), in12, c0); // [a1 * x1 + a2 * x2 + a0 * in, -b1 * y1 - b2 * y2 + 0]
 254         float32x2x2_t trn = vtrn_f32(out12, in12); // [[a1 * x1 + a2 * x2 + a0 * in, in12], [-b1 * y1 - b2 * y2, 0]]
 255         float32x2_t out120 = vadd_f32(trn.val[0], trn.val[1]);
 256
 257         s2 = s1;
 258         s1 = vrev64_f32(out120);
 259         buffer[i] = out120[0];
 260     }
 261     state->x1 = s1[0];
 262     state->y1 = s1[1];
 263     state->x2 = s2[0];
 264     state->y2 = s2[1];
 265 }
 266
 267 #else
 268
 269 static inline void cbox_biquadf_process(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer)
 270 {
 271     int i;
 272     float a0 = coeffs->a0;
 273     float a1 = coeffs->a1;
 274     float a2 = coeffs->a2;
 275     float b1 = coeffs->b1;
 276     float b2 = coeffs->b2;
 277     double y1 = state->y1;
 278     double y2 = state->y2;
 279
 280     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 281     {
 282         float in = buffer[i];
 283         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 284
 285         buffer[i] = out;
 286         state->x2 = state->x1;
 287         state->x1 = in;
 288         y2 = y1;
 289         y1 = out;
 290     }
 291     state->y2 = sanef(y2);
 292     state->y1 = sanef(y1);
 293 }
 294
 295 #endif
 296
 297 static inline void cbox_biquadf_process_stereo(struct cbox_biquadf_state *lstate, struct cbox_biquadf_state *rstate, struct cbox_biquadf_coeffs *coeffs, float *buffer)
 298 {
 299     int i;
 300     float a0 = coeffs->a0;
 301     float a1 = coeffs->a1;
 302     float a2 = coeffs->a2;
 303     float b1 = coeffs->b1;
 304     float b2 = coeffs->b2;
 305     double ly1 = lstate->y1;
 306     double ly2 = lstate->y2;
 307     double ry1 = rstate->y1;
 308     double ry2 = rstate->y2;
 309
 310     for (i = 0; i < 2 * CBOX_BLOCK_SIZE; i += 2)
 311     {
 312         float inl = buffer[i], inr = buffer[i + 1];
 313         float outl = a0 * inl + a1 * lstate->x1 + a2 * lstate->x2 - b1 * ly1 - b2 * ly2;
 314         float outr = a0 * inr + a1 * rstate->x1 + a2 * rstate->x2 - b1 * ry1 - b2 * ry2;
 315
 316         lstate->x2 = lstate->x1;
 317         lstate->x1 = inl;
 318         ly2 = ly1;
 319         ly1 = outl;
 320         buffer[i] = outl;
 321
 322         rstate->x2 = rstate->x1;
 323         rstate->x1 = inr;
 324         ry2 = ry1;
 325         ry1 = outr;
 326         buffer[i + 1] = outr;
 327     }
 328     lstate->y2 = sanef(ly2);
 329     lstate->y1 = sanef(ly1);
 330     rstate->y2 = sanef(ry2);
 331     rstate->y1 = sanef(ry1);
 332 }
 333
 334 static inline double cbox_biquadf_process_sample(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, double in)
 335 {
 336     double out = sanef(coeffs->a0 * sanef(in) + coeffs->a1 * state->x1 + coeffs->a2 * state->x2 - coeffs->b1 * state->y1 - coeffs->b2 * state->y2);
 337
 338     state->x2 = state->x1;
 339     state->x1 = in;
 340     state->y2 = state->y1;
 341     state->y1 = out;
 342
 343     return out;
 344 }
 345
 346 static inline void cbox_biquadf_process_to(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer_in, float *buffer_out)
 347 {
 348     int i;
 349     float a0 = coeffs->a0;
 350     float a1 = coeffs->a1;
 351     float a2 = coeffs->a2;
 352     float b1 = coeffs->b1;
 353     float b2 = coeffs->b2;
 354     double y1 = state->y1;
 355     double y2 = state->y2;
 356
 357     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 358     {
 359         float in = buffer_in[i];
 360         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 361
 362         buffer_out[i] = out;
 363         state->x2 = state->x1;
 364         state->x1 = in;
 365         y2 = y1;
 366         y1 = out;
 367     }
 368     state->y2 = sanef(y2);
 369     state->y1 = sanef(y1);
 370 }
 371
 372 static inline void cbox_biquadf_process_adding(struct cbox_biquadf_state *state, struct cbox_biquadf_coeffs *coeffs, float *buffer_in, float *buffer_out)
 373 {
 374     int i;
 375     float a0 = coeffs->a0;
 376     float a1 = coeffs->a1;
 377     float a2 = coeffs->a2;
 378     float b1 = coeffs->b1;
 379     float b2 = coeffs->b2;
 380     double y1 = state->y1;
 381     double y2 = state->y2;
 382
 383     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 384     {
 385         float in = buffer_in[i];
 386         double out = a0 * in + a1 * state->x1 + a2 * state->x2 - b1 * y1 - b2 * y2;
 387
 388         buffer_out[i] += out;
 389         state->x2 = state->x1;
 390         state->x1 = in;
 391         y2 = y1;
 392         y1 = out;
 393     }
 394     state->y2 = sanef(y2);
 395     state->y1 = sanef(y1);
 396 }
 397
 398 #endif