deps/libretro-common/audio/dsp_filters/eq.c

   1 /* Copyright  (C) 2010-2020 The RetroArch team
   2  *
   3  * ---------------------------------------------------------------------------------------
   4  * The following license statement only applies to this file (eq.c).
   5  * ---------------------------------------------------------------------------------------
   6  *
   7  * Permission is hereby granted, free of charge,
   8  * to any person obtaining a copy of this software and associated documentation files (the "Software"),
   9  * to deal in the Software without restriction, including without limitation the rights to
  10  * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software,
  11  * and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
  12  *
  13  * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
  14  *
  15  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  16  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
  18  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  19  * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  21  */
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include <retro_inline.h>
  28 #include <retro_miscellaneous.h>
  29 #include <filters.h>
  30 #include <libretro_dspfilter.h>
  31
  32 #include "fft/fft.c"
  33
  34 struct eq_data
  35 {
  36    fft_t *fft;
  37    float *save;
  38    float *block;
  39    fft_complex_t *filter;
  40    fft_complex_t *fftblock;
  41    float buffer[8 * 1024];
  42    unsigned block_size;
  43    unsigned block_ptr;
  44 };
  45
  46 struct eq_gain
  47 {
  48    float freq;
  49    float gain; /* Linear. */
  50 };
  51
  52 static void eq_free(void *data)
  53 {
  54    struct eq_data *eq = (struct eq_data*)data;
  55    if (!eq)
  56       return;
  57
  58    fft_free(eq->fft);
  59    free(eq->save);
  60    free(eq->block);
  61    free(eq->fftblock);
  62    free(eq->filter);
  63    free(eq);
  64 }
  65
  66 static void eq_process(void *data, struct dspfilter_output *output,
  67       const struct dspfilter_input *input)
  68 {
  69    float *out;
  70    const float *in;
  71    unsigned input_frames;
  72    struct eq_data *eq = (struct eq_data*)data;
  73
  74    output->samples    = eq->buffer;
  75    output->frames     = 0;
  76
  77    out                = eq->buffer;
  78    in                 = input->samples;
  79    input_frames       = input->frames;
  80
  81    while (input_frames)
  82    {
  83       unsigned write_avail = eq->block_size - eq->block_ptr;
  84
  85       if (input_frames < write_avail)
  86          write_avail = input_frames;
  87
  88       memcpy(eq->block + eq->block_ptr * 2, in, write_avail * 2 * sizeof(float));
  89
  90       in            += write_avail * 2;
  91       input_frames  -= write_avail;
  92       eq->block_ptr += write_avail;
  93
  94       /* Convolve a new block. */
  95       if (eq->block_ptr == eq->block_size)
  96       {
  97          unsigned i, c;
  98
  99          for (c = 0; c < 2; c++)
 100          {
 101             fft_process_forward(eq->fft, eq->fftblock, eq->block + c, 2);
 102             for (i = 0; i < 2 * eq->block_size; i++)
 103                eq->fftblock[i] = fft_complex_mul(eq->fftblock[i], eq->filter[i]);
 104             fft_process_inverse(eq->fft, out + c, eq->fftblock, 2);
 105          }
 106
 107          /* Overlap add method, so add in saved block now. */
 108          for (i = 0; i < 2 * eq->block_size; i++)
 109             out[i]      += eq->save[i];
 110
 111          /* Save block for later. */
 112          memcpy(eq->save, out + 2 * eq->block_size, 2 * eq->block_size * sizeof(float));
 113
 114          out            += eq->block_size * 2;
 115          output->frames += eq->block_size;
 116          eq->block_ptr   = 0;
 117       }
 118    }
 119 }
 120
 121 static int gains_cmp(const void *a_, const void *b_)
 122 {
 123    const struct eq_gain *a = (const struct eq_gain*)a_;
 124    const struct eq_gain *b = (const struct eq_gain*)b_;
 125    if (a->freq < b->freq)
 126       return -1;
 127    if (a->freq > b->freq)
 128       return 1;
 129    return 0;
 130 }
 131
 132 static void generate_response(fft_complex_t *response,
 133       const struct eq_gain *gains, unsigned num_gains, unsigned samples)
 134 {
 135    unsigned i;
 136
 137    float start_freq = 0.0f;
 138    float start_gain = 1.0f;
 139
 140    float end_freq   = 1.0f;
 141    float end_gain   = 1.0f;
 142
 143    if (num_gains)
 144    {
 145       end_freq = gains->freq;
 146       end_gain = gains->gain;
 147       num_gains--;
 148       gains++;
 149    }
 150
 151    /* Create a response by linear interpolation between
 152     * known frequency sample points. */
 153    for (i = 0; i <= samples; i++)
 154    {
 155       float gain;
 156       float lerp = 0.5f;
 157       float freq = (float)i / samples;
 158
 159       while (freq >= end_freq)
 160       {
 161          if (num_gains)
 162          {
 163             start_freq = end_freq;
 164             start_gain = end_gain;
 165             end_freq = gains->freq;
 166             end_gain = gains->gain;
 167
 168             gains++;
 169             num_gains--;
 170          }
 171          else
 172          {
 173             start_freq = end_freq;
 174             start_gain = end_gain;
 175             end_freq = 1.0f;
 176             end_gain = 1.0f;
 177             break;
 178          }
 179       }
 180
 181       /* Edge case where i == samples. */
 182       if (end_freq > start_freq)
 183          lerp = (freq - start_freq) / (end_freq - start_freq);
 184       gain = (1.0f - lerp) * start_gain + lerp * end_gain;
 185
 186       response[i].real               = gain;
 187       response[i].imag               = 0.0f;
 188       response[2 * samples - i].real = gain;
 189       response[2 * samples - i].imag = 0.0f;
 190    }
 191 }
 192
 193 static void create_filter(struct eq_data *eq, unsigned size_log2,
 194       struct eq_gain *gains, unsigned num_gains, double beta, const char *filter_path)
 195 {
 196    int i;
 197    int half_block_size = eq->block_size >> 1;
 198    double window_mod   = 1.0 / kaiser_window_function(0.0, beta);
 199    fft_t *fft          = fft_new(size_log2);
 200    float *time_filter  = (float*)calloc(eq->block_size * 2 + 1, sizeof(*time_filter));
 201    if (!fft || !time_filter)
 202       goto end;
 203
 204    /* Make sure bands are in correct order. */
 205    qsort(gains, num_gains, sizeof(*gains), gains_cmp);
 206
 207    /* Compute desired filter response. */
 208    generate_response(eq->filter, gains, num_gains, half_block_size);
 209
 210    /* Get equivalent time-domain filter. */
 211    fft_process_inverse(fft, time_filter, eq->filter, 1);
 212
 213    /* ifftshift() to create the correct linear phase filter.
 214     * The filter response was designed with zero phase, which
 215     * won't work unless we compensate
 216     * for the repeating property of the FFT here
 217     * by flipping left and right blocks. */
 218    for (i = 0; i < half_block_size; i++)
 219    {
 220       float tmp = time_filter[i + half_block_size];
 221       time_filter[i + half_block_size] = time_filter[i];
 222       time_filter[i] = tmp;
 223    }
 224
 225    /* Apply a window to smooth out the frequency repsonse. */
 226    for (i = 0; i < (int)eq->block_size; i++)
 227    {
 228       /* Kaiser window. */
 229       double phase    = (double)i / eq->block_size;
 230       phase           = 2.0 * (phase - 0.5);
 231       time_filter[i] *= window_mod * kaiser_window_function(phase, beta);
 232    }
 233
 234 #ifdef DEBUG
 235    /* Debugging. */
 236    if (filter_path)
 237    {
 238       FILE *file = fopen(filter_path, "w");
 239       if (file)
 240       {
 241          for (i = 0; i < (int)eq->block_size - 1; i++)
 242             fprintf(file, "%.8f\n", time_filter[i + 1]);
 243          fclose(file);
 244       }
 245    }
 246 #endif
 247
 248    /* Padded FFT to create our FFT filter.
 249     * Make our even-length filter odd by discarding the first coefficient.
 250     * For some interesting reason, this allows us to design an odd-length linear phase filter.
 251     */
 252    fft_process_forward(eq->fft, eq->filter, time_filter + 1, 1);
 253
 254 end:
 255    fft_free(fft);
 256    free(time_filter);
 257 }
 258
 259 static void *eq_init(const struct dspfilter_info *info,
 260       const struct dspfilter_config *config, void *userdata)
 261 {
 262    int size_log2;
 263    float beta;
 264    float *frequencies, *gain;
 265    unsigned num_freq, num_gain, i, size;
 266    struct eq_gain *gains      = NULL;
 267    char *filter_path          = NULL;
 268    const float default_freq[] = { 0.0f, info->input_rate };
 269    const float default_gain[] = { 0.0f, 0.0f };
 270    struct eq_data *eq         = (struct eq_data*)calloc(1, sizeof(*eq));
 271    if (!eq)
 272       return NULL;
 273
 274    config->get_float(userdata, "window_beta", &beta, 4.0f);
 275
 276    config->get_int(userdata, "block_size_log2", &size_log2, 8);
 277    size = 1 << size_log2;
 278
 279    config->get_float_array(userdata, "frequencies", &frequencies, &num_freq, default_freq, 2);
 280    config->get_float_array(userdata, "gains", &gain, &num_gain, default_gain, 2);
 281
 282    if (!config->get_string(userdata, "impulse_response_output", &filter_path, ""))
 283    {
 284       config->free(filter_path);
 285       filter_path = NULL;
 286    }
 287
 288    num_gain = num_freq = MIN(num_gain, num_freq);
 289
 290    if (!(gains = (struct eq_gain*)calloc(num_gain, sizeof(*gains))))
 291       goto error;
 292
 293    for (i = 0; i < num_gain; i++)
 294    {
 295       gains[i].freq = frequencies[i] / (0.5f * info->input_rate);
 296       gains[i].gain = pow(10.0, gain[i] / 20.0);
 297    }
 298    config->free(frequencies);
 299    config->free(gain);
 300
 301    eq->block_size = size;
 302
 303    eq->save       = (float*)calloc(    size, 2 * sizeof(*eq->save));
 304    eq->block      = (float*)calloc(2 * size, 2 * sizeof(*eq->block));
 305    eq->fftblock   = (fft_complex_t*)calloc(2 * size, sizeof(*eq->fftblock));
 306    eq->filter     = (fft_complex_t*)calloc(2 * size, sizeof(*eq->filter));
 307
 308    /* Use an FFT which is twice the block size with zero-padding
 309     * to make circular convolution => proper convolution.
 310     */
 311    eq->fft        = fft_new(size_log2 + 1);
 312
 313    if (!eq->fft || !eq->fftblock || !eq->save || !eq->block || !eq->filter)
 314       goto error;
 315
 316    create_filter(eq, size_log2, gains, num_gain, beta, filter_path);
 317    config->free(filter_path);
 318    filter_path = NULL;
 319
 320    free(gains);
 321    return eq;
 322
 323 error:
 324    free(gains);
 325    eq_free(eq);
 326    return NULL;
 327 }
 328
 329 static const struct dspfilter_implementation eq_plug = {
 330    eq_init,
 331    eq_process,
 332    eq_free,
 333
 334    DSPFILTER_API_VERSION,
 335    "Linear-Phase FFT Equalizer",
 336    "eq",
 337 };
 338
 339 #ifdef HAVE_FILTERS_BUILTIN
 340 #define dspfilter_get_implementation eq_dspfilter_get_implementation
 341 #endif
 342
 343 const struct dspfilter_implementation *dspfilter_get_implementation(dspfilter_simd_mask_t mask)
 344 {
 345    return &eq_plug;
 346 }
 347
 348 #undef dspfilter_get_implementation