libaf/af_sinesuppress.c

   1 /*=============================================================================
   2 //
   3 //  This software has been released under the terms of the GNU General Public
   4 //  license. See http://www.gnu.org/copyleft/gpl.html for details.
   5 //
   6 //  Copyright 2006 Michael Niedermayer
   7 //  Copyright 2004 Alex Beregszaszi & Pierre Lombard (original af_extrastereo.c upon which this is based)
   8 //
   9 //=============================================================================
  10 */
  11
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <string.h>
  15
  16 #include <inttypes.h>
  17 #include <math.h>
  18 #include <limits.h>
  19
  20 #include "af.h"
  21
  22 // Data for specific instances of this filter
  23 typedef struct af_sinesuppress_s
  24 {
  25     double freq;
  26     double decay;
  27     double real;
  28     double imag;
  29     double ref;
  30     double pos;
  31 }af_sinesuppress_t;
  32
  33 static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data);
  34 //static af_data_t* play_float(struct af_instance_s* af, af_data_t* data);
  35
  36 // Initialization and runtime control
  37 static int control(struct af_instance_s* af, int cmd, void* arg)
  38 {
  39   af_sinesuppress_t* s   = (af_sinesuppress_t*)af->setup;
  40
  41   switch(cmd){
  42   case AF_CONTROL_REINIT:{
  43     // Sanity check
  44     if(!arg) return AF_ERROR;
  45
  46     af->data->rate   = ((af_data_t*)arg)->rate;
  47     af->data->nch    = 1;
  48 #if 0
  49     if (((af_data_t*)arg)->format == AF_FORMAT_FLOAT_NE)
  50     {
  51         af->data->format = AF_FORMAT_FLOAT_NE;
  52         af->data->bps = 4;
  53         af->play = play_float;
  54     }// else
  55 #endif
  56     {
  57         af->data->format = AF_FORMAT_S16_NE;
  58         af->data->bps = 2;
  59         af->play = play_s16;
  60     }
  61
  62     return af_test_output(af,(af_data_t*)arg);
  63   }
  64   case AF_CONTROL_COMMAND_LINE:{
  65     float f1,f2;
  66     sscanf((char*)arg,"%f:%f", &f1,&f2);
  67     s->freq = f1;
  68     s->decay = f2;
  69     return AF_OK;
  70   }
  71   case AF_CONTROL_SS_FREQ | AF_CONTROL_SET:
  72     s->freq = *(float*)arg;
  73     return AF_OK;
  74   case AF_CONTROL_SS_FREQ | AF_CONTROL_GET:
  75     *(float*)arg = s->freq;
  76     return AF_OK;
  77   case AF_CONTROL_SS_DECAY | AF_CONTROL_SET:
  78     s->decay = *(float*)arg;
  79     return AF_OK;
  80   case AF_CONTROL_SS_DECAY | AF_CONTROL_GET:
  81     *(float*)arg = s->decay;
  82     return AF_OK;
  83   }
  84   return AF_UNKNOWN;
  85 }
  86
  87 // Deallocate memory
  88 static void uninit(struct af_instance_s* af)
  89 {
  90   if(af->data)
  91     free(af->data);
  92   if(af->setup)
  93     free(af->setup);
  94 }
  95
  96 // Filter data through filter
  97 static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data)
  98 {
  99   af_sinesuppress_t *s = af->setup;
 100   register int i = 0;
 101   int16_t *a = (int16_t*)data->audio;   // Audio data
 102   int len = data->len/2;                // Number of samples
 103
 104   for (i = 0; i < len; i++)
 105   {
 106     double co= cos(s->pos);
 107     double si= sin(s->pos);
 108
 109     s->real += co * a[i];
 110     s->imag += si * a[i];
 111     s->ref  += co * co;
 112
 113     a[i] -= (s->real * co + s->imag * si) / s->ref;
 114
 115     s->real -= s->real * s->decay;
 116     s->imag -= s->imag * s->decay;
 117     s->ref  -= s->ref  * s->decay;
 118
 119     s->pos += 2 * M_PI * s->freq / data->rate;
 120   }
 121
 122    af_msg(AF_MSG_VERBOSE,"[sinesuppress] f:%8.2f: amp:%8.2f\n", s->freq, sqrt(s->real*s->real + s->imag*s->imag) / s->ref);
 123
 124   return data;
 125 }
 126
 127 #if 0
 128 static af_data_t* play_float(struct af_instance_s* af, af_data_t* data)
 129 {
 130   af_sinesuppress_t *s = af->setup;
 131   register int i = 0;
 132   float *a = (float*)data->audio;       // Audio data
 133   int len = data->len/4;                // Number of samples
 134   float avg, l, r;
 135
 136   for (i = 0; i < len; i+=2)
 137   {
 138     avg = (a[i] + a[i + 1]) / 2;
 139
 140 /*    l = avg + (s->mul * (a[i] - avg));
 141     r = avg + (s->mul * (a[i + 1] - avg));*/
 142
 143     a[i] = af_softclip(l);
 144     a[i + 1] = af_softclip(r);
 145   }
 146
 147   return data;
 148 }
 149 #endif
 150
 151 // Allocate memory and set function pointers
 152 static int af_open(af_instance_t* af){
 153   af->control=control;
 154   af->uninit=uninit;
 155   af->play=play_s16;
 156   af->mul=1;
 157   af->data=calloc(1,sizeof(af_data_t));
 158   af->setup=calloc(1,sizeof(af_sinesuppress_t));
 159   if(af->data == NULL || af->setup == NULL)
 160     return AF_ERROR;
 161
 162   ((af_sinesuppress_t*)af->setup)->freq = 50.0;
 163   ((af_sinesuppress_t*)af->setup)->decay = 0.0001;
 164   return AF_OK;
 165 }
 166
 167 // Description of this filter
 168 af_info_t af_info_sinesuppress = {
 169     "Sine Suppress",
 170     "sinesuppress",
 171     "Michael Niedermayer",
 172     "",
 173     0,
 174     af_open
 175 };