onepole-int.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_INT_H
  20 #define CBOX_ONEPOLE_INT_H
  21
  22 #include "dspmath.h"
  23
  24 struct cbox_onepole_state
  25 {
  26     int32_t x1;
  27     int32_t y1;
  28 };
  29
  30 struct cbox_onepole_coeffs
  31 {
  32     int32_t a0;
  33     int32_t a1;
  34     int32_t b1;
  35     int shift;
  36 };
  37
  38 static inline void cbox_onepole_reset(struct cbox_onepole_state *state)
  39 {
  40     state->x1 = state->y1 = 0;
  41 }
  42
  43 static inline void cbox_onepole_set_lowpass(struct cbox_onepole_coeffs *coeffs, float w)
  44 {
  45     float x = tan (w);
  46     float q = 1 / (1 + x);
  47     float a01 = x*q;
  48     float b1 = a01 - q;
  49     int shift = 28;
  50     float scaler = (1 << shift);
  51
  52     coeffs->a1 = coeffs->a0 = (int32_t)(a01 * scaler);
  53     coeffs->b1 = (int32_t)(b1  * scaler);
  54     coeffs->shift = shift;
  55 }
  56
  57 static inline void cbox_onepole_set_highpass(struct cbox_onepole_coeffs *coeffs, float w)
  58 {
  59     float x = tan (w);
  60     float q = 1 / (1 + x);
  61     float a01 = x*q;
  62     float b1 = a01 - q;
  63     int shift = 28;
  64     float scaler = (1 << shift)-1;
  65
  66     coeffs->a0 = (int32_t)(a01 * scaler);
  67     coeffs->a1 = -coeffs->a0;
  68     coeffs->b1 = (int32_t)(b1  * scaler);
  69     coeffs->shift = shift;
  70 }
  71
  72 static inline void cbox_onepole_process(struct cbox_onepole_state *state, struct cbox_onepole_coeffs *coeffs, int32_t *buffer)
  73 {
  74     int i;
  75     int64_t a0 = coeffs->a0;
  76     int64_t a1 = coeffs->a1;
  77     int64_t b1 = coeffs->b1;
  78     int shift = coeffs->shift;
  79     int64_t maxint = ((int64_t)0x7FFFFFF) << shift;
  80     int32_t round = 1 << (shift - 1);
  81
  82     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
  83     {
  84         int32_t in = buffer[i];
  85         int64_t v = a0 * in + a1 * state->x1 - b1 * state->y1 + round;
  86         int32_t out = (llabs(v) >= maxint) ? (v > 0 ? 0x7FFFFFFF : -0x7FFFFFFF) : (v >> shift);
  87
  88         buffer[i] = out;
  89         state->x1 = in;
  90         state->y1 = out;
  91     }
  92     if (state->y1 > 0 && state->y1 < round)
  93         state->y1--;
  94     if (state->y1 < 0 && state->y1 > -round)
  95         state->y1++;
  96 }
  97
  98 static inline void cbox_onepole_process_to(struct cbox_onepole_state *state, struct cbox_onepole_coeffs *coeffs, int32_t *buffer_in, int32_t *buffer_out)
  99 {
 100     int i;
 101     int64_t a0 = coeffs->a0;
 102     int64_t a1 = coeffs->a1;
 103     int64_t b1 = coeffs->b1;
 104     int shift = coeffs->shift;
 105     int64_t maxint = ((int64_t)0x7FFFFFF) << shift;
 106     int64_t round = 1 << (shift - 1);
 107
 108     for (i = 0; i < CBOX_BLOCK_SIZE; i++)
 109     {
 110         int32_t in = buffer_in[i];
 111         int64_t v = a0 * in + a1 * state->x1 - b1 * state->y1 + round;
 112         int32_t out = (llabs(v) >= maxint) ? (v > 0 ? 0x7FFFFFFF : -0x7FFFFFFF) : (v >> shift);
 113
 114         buffer_out[i] = out;
 115         state->x1 = in;
 116         state->y1 = out;
 117     }
 118 }
 119
 120 #endif