src/dft_filter.c

   1 /* Abstract effect: dft filter     Copyright (c) 2008 robs@users.sourceforge.net
   2  *
   3  * This library is free software; you can redistribute it and/or modify it
   4  * under the terms of the GNU Lesser General Public License as published by
   5  * the Free Software Foundation; either version 2.1 of the License, or (at
   6  * your option) any later version.
   7  *
   8  * This library is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU Lesser General Public License
  14  * along with this library; if not, write to the Free Software Foundation,
  15  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  16  */
  17
  18 #include "sox_i.h"
  19 #include "fft4g.h"
  20 #include "dft_filter.h"
  21 #include <string.h>
  22
  23 typedef dft_filter_t filter_t;
  24 typedef dft_filter_priv_t priv_t;
  25
  26 void lsx_set_dft_filter(dft_filter_t *f, double *h, int n, int post_peak)
  27 {
  28   int i;
  29   f->num_taps = n;
  30   f->post_peak = post_peak;
  31   f->dft_length = lsx_set_dft_length(f->num_taps);
  32   f->coefs = lsx_calloc(f->dft_length, sizeof(*f->coefs));
  33   for (i = 0; i < f->num_taps; ++i)
  34     f->coefs[(i + f->dft_length - f->num_taps + 1) & (f->dft_length - 1)] = h[i] / f->dft_length * 2;
  35   lsx_safe_rdft(f->dft_length, 1, f->coefs);
  36   free(h);
  37 }
  38
  39 static int start(sox_effect_t * effp)
  40 {
  41   priv_t * p = (priv_t *) effp->priv;
  42
  43   fifo_create(&p->input_fifo, (int)sizeof(double));
  44   memset(fifo_reserve(&p->input_fifo,
  45         p->filter_ptr->post_peak), 0, sizeof(double) * p->filter_ptr->post_peak);
  46   fifo_create(&p->output_fifo, (int)sizeof(double));
  47   return SOX_SUCCESS;
  48 }
  49
  50 static void filter(priv_t * p)
  51 {
  52   int i, num_in = max(0, fifo_occupancy(&p->input_fifo));
  53   filter_t const * f = p->filter_ptr;
  54   int const overlap = f->num_taps - 1;
  55   double * output;
  56
  57   while (num_in >= f->dft_length) {
  58     double const * input = fifo_read_ptr(&p->input_fifo);
  59     fifo_read(&p->input_fifo, f->dft_length - overlap, NULL);
  60     num_in -= f->dft_length - overlap;
  61
  62     output = fifo_reserve(&p->output_fifo, f->dft_length);
  63     fifo_trim_by(&p->output_fifo, overlap);
  64     memcpy(output, input, f->dft_length * sizeof(*output));
  65
  66     lsx_safe_rdft(f->dft_length, 1, output);
  67     output[0] *= f->coefs[0];
  68     output[1] *= f->coefs[1];
  69     for (i = 2; i < f->dft_length; i += 2) {
  70       double tmp = output[i];
  71       output[i  ] = f->coefs[i  ] * tmp - f->coefs[i+1] * output[i+1];
  72       output[i+1] = f->coefs[i+1] * tmp + f->coefs[i  ] * output[i+1];
  73     }
  74     lsx_safe_rdft(f->dft_length, -1, output);
  75   }
  76 }
  77
  78 static int flow(sox_effect_t * effp, const sox_sample_t * ibuf,
  79                 sox_sample_t * obuf, size_t * isamp, size_t * osamp)
  80 {
  81   priv_t * p = (priv_t *)effp->priv;
  82   size_t odone = min(*osamp, (size_t)fifo_occupancy(&p->output_fifo));
  83
  84   double const * s = fifo_read(&p->output_fifo, (int)odone, NULL);
  85   lsx_save_samples(obuf, s, odone, &effp->clips);
  86   p->samples_out += odone;
  87
  88   if (*isamp && odone < *osamp) {
  89     double * t = fifo_write(&p->input_fifo, (int)*isamp, NULL);
  90     p->samples_in += *isamp;
  91     lsx_load_samples(t, ibuf, *isamp);
  92     filter(p);
  93   }
  94   else *isamp = 0;
  95   *osamp = odone;
  96   return SOX_SUCCESS;
  97 }
  98
  99 static int drain(sox_effect_t * effp, sox_sample_t * obuf, size_t * osamp)
 100 {
 101   priv_t * p = (priv_t *)effp->priv;
 102   static size_t isamp = 0;
 103   size_t remaining = p->samples_in > p->samples_out ?
 104       (size_t)(p->samples_in - p->samples_out) : 0;
 105   double * buff = lsx_calloc(1024, sizeof(*buff));
 106
 107   if (remaining > 0) {
 108     while ((size_t)fifo_occupancy(&p->output_fifo) < remaining) {
 109       fifo_write(&p->input_fifo, 1024, buff);
 110       p->samples_in += 1024;
 111       filter(p);
 112     }
 113     fifo_trim_to(&p->output_fifo, (int)remaining);
 114     p->samples_in = 0;
 115   }
 116   free(buff);
 117   return flow(effp, 0, obuf, &isamp, osamp);
 118 }
 119
 120 static int stop(sox_effect_t * effp)
 121 {
 122   priv_t * p = (priv_t *) effp->priv;
 123
 124   fifo_delete(&p->input_fifo);
 125   fifo_delete(&p->output_fifo);
 126   free(p->filter_ptr->coefs);
 127   memset(p->filter_ptr, 0, sizeof(*p->filter_ptr));
 128   return SOX_SUCCESS;
 129 }
 130
 131 sox_effect_handler_t const * lsx_dft_filter_effect_fn(void)
 132 {
 133   static sox_effect_handler_t handler = {
 134     NULL, NULL, SOX_EFF_GAIN, NULL, start, flow, drain, stop, NULL, 0
 135   };
 136   return &handler;
 137 }