filter.c

   1 /* -*- Mode: C ; c-basic-offset: 2 -*- */
   2 /*
   3   Copyright (C) 2008 Nedko Arnaudov <nedko@arnaudov.name>
   4   The DSP code is based on ladspa:1970 by Fons Adriaensen
   5
   6   This program is free software; you can redistribute it and/or modify
   7   it under the terms of the GNU General Public License as published by
   8   the Free Software Foundation; either version 2 of the License, or
   9   (at your option) any later version.
  10
  11   This program is distributed in the hope that it will be useful,
  12   but WITHOUT ANY WARRANTY; without even the implied warranty of
  13   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14   GNU General Public License for more details.
  15
  16   You should have received a copy of the GNU General Public License
  17   along with this program; if not, write to the Free Software
  18   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  19 */
  20
  21 /* if NDEBUG is defined, assert checks are disabled */
  22 //#define NDEBUG
  23
  24 #include <stdbool.h>
  25 #include <stdlib.h>
  26 #include <string.h>
  27 #include <math.h>
  28 #include <assert.h>
  29
  30 #include "filter.h"
  31
  32 static float exp2ap(float x)
  33 {
  34     int i;
  35
  36     i = (int)(floor(x));
  37     x -= i;
  38 //    return ldexp(1 + x * (0.66 + 0.34 * x), i);
  39     return ldexp(1 + x * (0.6930 + x * (0.2416 + x * (0.0517 + x * 0.0137))), i);
  40 }
  41
  42 struct param_sect
  43 {
  44   float f, b, g;
  45   float s1, s2, a;
  46   float z1, z2;
  47 };
  48
  49 inline
  50 void
  51 param_sect_init(
  52   struct param_sect * sect_ptr)
  53 {
  54   sect_ptr->f = 0.25f;
  55   sect_ptr->b = sect_ptr->g = 1.0f;
  56   sect_ptr->a = sect_ptr->s1 = sect_ptr->s2 = sect_ptr->z1 = sect_ptr->z2 = 0.0f;
  57 }
  58
  59 inline
  60 void
  61 param_sect_proc(
  62   struct param_sect * sect_ptr,
  63   int k,
  64   float * sig,
  65   float f,
  66   float b,
  67   float g)
  68 {
  69   float s1, s2, d1, d2, a, da, x, y;
  70   bool  u2 = false;
  71
  72   s1 = sect_ptr->s1;
  73   s2 = sect_ptr->s2;
  74   a = sect_ptr->a;
  75   d1 = 0;
  76   d2 = 0;
  77   da = 0;
  78
  79   if (f != sect_ptr->f)
  80   {
  81     if      (f < 0.5f * sect_ptr->f) f = 0.5f * sect_ptr->f;
  82     else if (f > 2.0f * sect_ptr->f) f = 2.0f * sect_ptr->f;
  83     sect_ptr->f = f;
  84     sect_ptr->s1 = -cosf(6.283185f * f);
  85     d1 = (sect_ptr->s1 - s1) / k;
  86     u2 = true;
  87   }
  88
  89   if (g != sect_ptr->g)
  90   {
  91     if      (g < 0.5f * sect_ptr->g) g = 0.5f * sect_ptr->g;
  92     else if (g > 2.0f * sect_ptr->g) g = 2.0f * sect_ptr->g;
  93     sect_ptr->g = g;
  94     sect_ptr->a = 0.5f * (g - 1.0f);
  95     da = (sect_ptr->a - a) / k;
  96     u2 = true;
  97   }
  98
  99   if (b != sect_ptr->b)
 100   {
 101     if      (b < 0.5f * sect_ptr->b) b = 0.5f * sect_ptr->b;
 102     else if (b > 2.0f * sect_ptr->b) b = 2.0f * sect_ptr->b;
 103     sect_ptr->b = b;
 104     u2 = true;
 105   }
 106
 107   if (u2)
 108   {
 109     b *= 7 * f / sqrtf(g);
 110     sect_ptr->s2 = (1 - b) / (1 + b);
 111     d2 = (sect_ptr->s2 - s2) / k;
 112   }
 113
 114   while (k--)
 115   {
 116     s1 += d1;
 117     s2 += d2;
 118     a += da;
 119     x = *sig;
 120     y = x - s2 * sect_ptr->z2;
 121     *sig++ -= a * (sect_ptr->z2 + s2 * y - x);
 122     y -= s1 * sect_ptr->z1;
 123     sect_ptr->z2 = sect_ptr->z1 + s1 * y;
 124     sect_ptr->z1 = y + 1e-10f;
 125   }
 126 }
 127
 128 struct filter
 129 {
 130   float sample_rate;
 131
 132   const float * global_parameters[GLOBAL_PARAMETERS_COUNT];
 133
 134   unsigned int bands_count;
 135   const float ** band_parameters; /* [band_index * BAND_PARAMETERS_COUNT + parameter_index] */
 136
 137   float gain;
 138   int fade;
 139   struct param_sect * sect;     /* [band_index] */
 140 };
 141
 142 bool
 143 filter_create(
 144   float sample_rate,
 145   unsigned int bands_count,
 146   filter_handle * handle_ptr)
 147 {
 148   struct filter * filter_ptr;
 149   int j;
 150
 151   assert(bands_count > 0);
 152
 153   filter_ptr = calloc(1, sizeof(struct filter));
 154   if (filter_ptr == NULL)
 155   {
 156     goto fail;
 157   }
 158
 159   filter_ptr->band_parameters = calloc(bands_count, sizeof(float *) * BAND_PARAMETERS_COUNT);
 160   if (filter_ptr->band_parameters == NULL)
 161   {
 162     goto free_filter;
 163   }
 164
 165   filter_ptr->sect = malloc(sizeof(struct param_sect) * bands_count);
 166   if (filter_ptr->sect == NULL)
 167   {
 168     goto free_band_params;
 169   }
 170
 171   filter_ptr->sample_rate = sample_rate;
 172   filter_ptr->bands_count = bands_count;
 173   filter_ptr->fade = 0;
 174   filter_ptr->gain = 1.0;
 175
 176   for (j = 0; j < bands_count; j++)
 177   {
 178     param_sect_init(filter_ptr->sect + j);
 179   }
 180
 181   *handle_ptr = (filter_handle)filter_ptr;
 182
 183   return true;
 184
 185 free_band_params:
 186   free(filter_ptr->band_parameters);
 187
 188 free_filter:
 189   free(filter_ptr);
 190
 191 fail:
 192   return false;
 193 }
 194
 195 #define filter_ptr ((struct filter *)handle)
 196
 197 void
 198 filter_destroy(
 199   filter_handle handle)
 200 {
 201   free(filter_ptr->sect);
 202   free(filter_ptr->band_parameters);
 203   free(filter_ptr);
 204 }
 205
 206 void
 207 filter_connect_global_parameter(
 208   filter_handle handle,
 209   unsigned int global_parameter,
 210   const float * value_ptr)
 211 {
 212   assert(global_parameter >= 0);
 213   assert(global_parameter < GLOBAL_PARAMETERS_COUNT);
 214
 215   filter_ptr->global_parameters[global_parameter] = value_ptr;
 216 }
 217
 218 void
 219 filter_connect_band_parameter(
 220   filter_handle handle,
 221   unsigned int band_index,
 222   unsigned int band_parameter,
 223   const float * value_ptr)
 224 {
 225   assert(band_index >= 0);
 226   assert(band_index < filter_ptr->bands_count);
 227   assert(band_parameter >= 0);
 228   assert(band_parameter < BAND_PARAMETERS_COUNT);
 229
 230   filter_ptr->band_parameters[band_index * BAND_PARAMETERS_COUNT + band_parameter] = value_ptr;
 231 }
 232
 233 void
 234 filter_run(
 235   filter_handle handle,
 236   const float * input_buffer,
 237   float * output_buffer,
 238   unsigned long samples_count)
 239 {
 240   int i, j, k;
 241   const float * p;
 242   float sig[48];
 243   float t, g, d;
 244   float fgain;
 245   float sfreq[filter_ptr->bands_count];
 246   float sband[filter_ptr->bands_count];
 247   float sgain[filter_ptr->bands_count];
 248   float bands_count;
 249
 250   bands_count = filter_ptr->bands_count;
 251
 252   fgain = exp2ap(0.1661 * *filter_ptr->global_parameters[GLOBAL_PARAMETER_GAIN]);
 253
 254   for (j = 0; j < bands_count; j++)
 255   {
 256     t = *filter_ptr->band_parameters[BAND_PARAMETERS_COUNT * j + BAND_PARAMETER_FREQUENCY] / filter_ptr->sample_rate;
 257     if (t < 0.0002)
 258     {
 259       t = 0.0002;
 260     }
 261     else if (t > 0.4998)
 262     {
 263       t = 0.4998;
 264     }
 265     sfreq[j] = t;
 266
 267     sband[j] = *filter_ptr->band_parameters[BAND_PARAMETERS_COUNT * j + BAND_PARAMETER_BANDWIDTH];
 268
 269     if (*filter_ptr->band_parameters[BAND_PARAMETERS_COUNT * j + BAND_PARAMETER_ACTIVE] > 0.0)
 270     {
 271       sgain[j] = exp2ap(0.1661 * *filter_ptr->band_parameters[BAND_PARAMETERS_COUNT * j + BAND_PARAMETER_GAIN]);
 272     }
 273     else
 274     {
 275       sgain[j] = 1.0;
 276     }
 277   }
 278
 279   while (samples_count)
 280   {
 281     k = (samples_count > 48) ? 32 : samples_count;
 282
 283     t = fgain;
 284     g = filter_ptr->gain;
 285
 286     if (t > 1.25 * g)
 287     {
 288       t = 1.25 * g;
 289     }
 290     else if (t < 0.80 * g)
 291     {
 292       t = 0.80 * g;
 293     }
 294
 295     filter_ptr->gain = t;
 296     d = (t - g) / k;
 297     for (i = 0; i < k; i++)
 298     {
 299       g += d;
 300       sig[i] = g * input_buffer[i];
 301     }
 302
 303     for (j = 0; j < bands_count; j++)
 304     {
 305       param_sect_proc(filter_ptr->sect + j, k, sig, sfreq[j], sband[j], sgain[j]);
 306     }
 307
 308     j = filter_ptr->fade;
 309     g = j / 16.0;
 310     p = 0;
 311
 312     if (*filter_ptr->global_parameters[GLOBAL_PARAMETER_ACTIVE] > 0.0)
 313     {
 314       if (j == 16)
 315       {
 316         p = sig;
 317       }
 318       else
 319       {
 320         ++j;
 321       }
 322     }
 323     else
 324     {
 325       if (j == 0)
 326       {
 327         p = input_buffer;
 328       }
 329       else
 330       {
 331         --j;
 332       }
 333     }
 334
 335     filter_ptr->fade = j;
 336
 337     if (p)
 338     {
 339       memcpy(output_buffer, p, k * sizeof(float));
 340     }
 341     else
 342     {
 343       d = (j / 16.0 - g) / k;
 344       for (i = 0; i < k; i++)
 345       {
 346         g += d;
 347         output_buffer[i] = g * sig[i] + (1 - g) * input_buffer[i];
 348       }
 349     }
 350
 351     input_buffer += k;
 352     output_buffer += k;
 353     samples_count -= k;
 354   }
 355 }