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