onepole-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_ONEPOLE_FLOAT_H
  20 #define CBOX_ONEPOLE_FLOAT_H
  21
  22 #include "dspmath.h"
  23
  24 struct cbox_onepolef_state
  25 {
  26     float x1;
  27     float y1;
  28 };
  29
  30 struct cbox_onepolef_coeffs
  31 {
  32     float a0;
  33     float a1;
  34     float b1;
  35 };
  36
  37 static inline void cbox_onepolef_reset(struct cbox_onepolef_state *state)
  38 {
  39     state->x1 = state->y1 = 0.f;
  40 }
  41
  42 static inline void cbox_onepolef_set_lowpass(struct cbox_onepolef_coeffs *coeffs, float w)
  43 {
  44     float x = tan (w * 0.5f);
  45     float q = 1 / (1 + x);
  46     float a01 = x*q;
  47     float b1 = a01 - q;
  48
  49     coeffs->a0 = a01;
  50     coeffs->a1 = a01;
  51     coeffs->b1 = b1;
  52 }
  53
  54 static inline void cbox_onepolef_set_highpass(struct cbox_onepolef_coeffs *coeffs, float w)
  55 {
  56     float x = tan (w * 0.5f);
  57     float q = 1 / (1 + x);
  58     float a01 = x*q;
  59     float b1 = a01 - q;
  60
  61     coeffs->a0 = q;
  62     coeffs->a1 = -q;
  63     coeffs->b1 = b1;
  64 }
  65
  66 static inline void cbox_onepolef_set_highshelf_tonectl(struct cbox_onepolef_coeffs *coeffs, float w, float g0)
  67 {
  68     float x = tan (w * 0.5f);
  69     float q = 1 / (1 + x);
  70     float b1 = x * q - q;
  71
  72     coeffs->a0 = 0.5 * (1 + b1 + g0 - b1 * g0);
  73     coeffs->a1 = 0.5 * (1 + b1 - g0 + b1 * g0);
  74     coeffs->b1 = b1;
  75 }
  76
  77 static inline void cbox_onepolef_set_highshelf_setgain(struct cbox_onepolef_coeffs *coeffs, float g0)
  78 {
  79     coeffs->a0 = 0.5 * (1 + coeffs->b1 + g0 - coeffs->b1 * g0);
  80     coeffs->a1 = 0.5 * (1 + coeffs->b1 - g0 + coeffs->b1 * g0);
  81 }
  82
  83 static inline void cbox_onepolef_set_allpass(struct cbox_onepolef_coeffs *coeffs, float w)
  84 {
  85     float x = tan (w * 0.5f);
  86     float q = 1 / (1 + x);
  87     float a01 = x*q;
  88     float b1 = a01 - q;
  89
  90     coeffs->a0 = b1;
  91     coeffs->a1 = 1;
  92     coeffs->b1 = b1;
  93 }
  94
  95 static inline float cbox_onepolef_process_sample(struct cbox_onepolef_state *state, struct cbox_onepolef_coeffs *coeffs, float in)
  96 {
  97     float out = sanef(coeffs->a0 * in + coeffs->a1 * state->x1 - coeffs->b1 * state->y1);
  98
  99     state->x1 = in;
 100     state->y1 = out;
 101     return out;
 102 }
 103
 104 #if USE_NEON_NOTREALLYFASTER
 105
 106 #include <arm_neon.h>
 107
 108 static inline void cbox_onepolef_process(struct cbox_onepolef_state *state, struct cbox_onepolef_coeffs *coeffs, float *buffer)
 109 {
 110     int i;
 111     float a0 = coeffs->a0;
 112     float a1 = coeffs->a1;
 113     float b1 = coeffs->b1;
 114     float32x2_t a00 = {1, a0};
 115     float32x2_t ab1 = {a1, -b1};
 116     float32x2_t xy = {state->x1, state->y1};
 117     float32x2_t zero = {0, 0};
 118
 119     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 120     {
 121         float32x2_t inin = vdup_n_f32(buffer[i]); // {in, in}
 122         float32x2_t xymul = vmul_f32(ab1, xy); // {x1 * a1, y1 * b1}
 123         xymul = vpadd_f32(zero, xymul); // {0, x1 * a1 + y1 * b1}
 124         xy = vmla_f32(xymul, inin, a00); // {in, a0 * in + a1 * x1 + b1 * y1}
 125
 126         buffer[i] = xy[1];
 127     }
 128     state->x1 = xy[0];
 129     state->y1 = sanef(xy[1]);
 130 }
 131
 132 #else
 133 static inline void cbox_onepolef_process(struct cbox_onepolef_state *state, struct cbox_onepolef_coeffs *coeffs, float *buffer)
 134 {
 135     int i;
 136     float a0 = coeffs->a0;
 137     float a1 = coeffs->a1;
 138     float b1 = coeffs->b1;
 139     float x1 = state->x1;
 140     float y1 = state->y1;
 141
 142     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 143     {
 144         float in = buffer[i];
 145         double out = a0 * in + a1 * x1 - b1 * y1;
 146
 147         buffer[i] = out;
 148         x1 = in;
 149         y1 = out;
 150     }
 151     state->x1 = x1;
 152     state->y1 = sanef(y1);
 153 }
 154 #endif
 155
 156 static inline void cbox_onepolef_process_stereo(struct cbox_onepolef_state *lstate, struct cbox_onepolef_state *rstate, struct cbox_onepolef_coeffs *coeffs, float *buffer)
 157 {
 158     int i;
 159     float a0 = coeffs->a0;
 160     float a1 = coeffs->a1;
 161     float b1 = coeffs->b1;
 162     float lx1 = lstate->x1;
 163     float ly1 = lstate->y1;
 164     float rx1 = rstate->x1;
 165     float ry1 = rstate->y1;
 166
 167     for (i = 0; i < 2 * CBOX_BLOCK_SIZE; i += 2)
 168     {
 169         float inl = buffer[i], inr = buffer[i + 1];
 170         double outl = a0 * inl + a1 * lx1 - b1 * ly1;
 171         double outr = a0 * inr + a1 * rx1 - b1 * ry1;
 172
 173         buffer[i] = outl;
 174         buffer[i + 1] = outr;
 175         lx1 = inl;
 176         ly1 = outl;
 177         rx1 = inr;
 178         ry1 = outr;
 179     }
 180     lstate->x1 = lx1;
 181     lstate->y1 = sanef(ly1);
 182     rstate->x1 = rx1;
 183     rstate->y1 = sanef(ry1);
 184 }
 185
 186 static inline void cbox_onepolef_process_to(struct cbox_onepolef_state *state, struct cbox_onepolef_coeffs *coeffs, float *buffer_in, float *buffer_out)
 187 {
 188     int i;
 189     float a0 = coeffs->a0;
 190     float a1 = coeffs->a1;
 191     float b1 = coeffs->b1;
 192
 193     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 194     {
 195         float in = buffer_in[i];
 196         double out = a0 * in + a1 * state->x1 - b1 * state->y1;
 197
 198         buffer_out[i] = out;
 199         state->x1 = in;
 200         state->y1 = out;
 201     }
 202     state->y1 = sanef(state->y1);
 203 }
 204
 205 #endif