libaf/af_sinesuppress.c

   1 /*
   2  * Copyright (C) 2006 Michael Niedermayer
   3  * Copyright (C) 2004 Alex Beregszaszi
   4  * based upon af_extrastereo.c by Pierre Lombard
   5  *
   6  * This file is part of MPlayer.
   7  *
   8  * MPlayer is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * MPlayer is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License along
  19  * with MPlayer; if not, write to the Free Software Foundation, Inc.,
  20  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  21  */
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include <inttypes.h>
  28 #include <math.h>
  29 #include <limits.h>
  30
  31 #include "af.h"
  32
  33 // Data for specific instances of this filter
  34 typedef struct af_sinesuppress_s
  35 {
  36     double freq;
  37     double decay;
  38     double real;
  39     double imag;
  40     double ref;
  41     double pos;
  42 }af_sinesuppress_t;
  43
  44 static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data);
  45 //static af_data_t* play_float(struct af_instance_s* af, af_data_t* data);
  46
  47 // Initialization and runtime control
  48 static int control(struct af_instance_s* af, int cmd, void* arg)
  49 {
  50   af_sinesuppress_t* s   = (af_sinesuppress_t*)af->setup;
  51
  52   switch(cmd){
  53   case AF_CONTROL_REINIT:{
  54     // Sanity check
  55     if(!arg) return AF_ERROR;
  56
  57     af->data->rate   = ((af_data_t*)arg)->rate;
  58     af->data->nch    = 1;
  59 #if 0
  60     if (((af_data_t*)arg)->format == AF_FORMAT_FLOAT_NE)
  61     {
  62         af->data->format = AF_FORMAT_FLOAT_NE;
  63         af->data->bps = 4;
  64         af->play = play_float;
  65     }// else
  66 #endif
  67     {
  68         af->data->format = AF_FORMAT_S16_NE;
  69         af->data->bps = 2;
  70         af->play = play_s16;
  71     }
  72
  73     return af_test_output(af,(af_data_t*)arg);
  74   }
  75   case AF_CONTROL_COMMAND_LINE:{
  76     float f1,f2;
  77     sscanf((char*)arg,"%f:%f", &f1,&f2);
  78     s->freq = f1;
  79     s->decay = f2;
  80     return AF_OK;
  81   }
  82   case AF_CONTROL_SS_FREQ | AF_CONTROL_SET:
  83     s->freq = *(float*)arg;
  84     return AF_OK;
  85   case AF_CONTROL_SS_FREQ | AF_CONTROL_GET:
  86     *(float*)arg = s->freq;
  87     return AF_OK;
  88   case AF_CONTROL_SS_DECAY | AF_CONTROL_SET:
  89     s->decay = *(float*)arg;
  90     return AF_OK;
  91   case AF_CONTROL_SS_DECAY | AF_CONTROL_GET:
  92     *(float*)arg = s->decay;
  93     return AF_OK;
  94   }
  95   return AF_UNKNOWN;
  96 }
  97
  98 // Deallocate memory
  99 static void uninit(struct af_instance_s* af)
 100 {
 101   if(af->data)
 102     free(af->data);
 103   if(af->setup)
 104     free(af->setup);
 105 }
 106
 107 // Filter data through filter
 108 static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data)
 109 {
 110   af_sinesuppress_t *s = af->setup;
 111   register int i = 0;
 112   int16_t *a = (int16_t*)data->audio;   // Audio data
 113   int len = data->len/2;                // Number of samples
 114
 115   for (i = 0; i < len; i++)
 116   {
 117     double co= cos(s->pos);
 118     double si= sin(s->pos);
 119
 120     s->real += co * a[i];
 121     s->imag += si * a[i];
 122     s->ref  += co * co;
 123
 124     a[i] -= (s->real * co + s->imag * si) / s->ref;
 125
 126     s->real -= s->real * s->decay;
 127     s->imag -= s->imag * s->decay;
 128     s->ref  -= s->ref  * s->decay;
 129
 130     s->pos += 2 * M_PI * s->freq / data->rate;
 131   }
 132
 133    mp_msg(MSGT_AFILTER, MSGL_V, "[sinesuppress] f:%8.2f: amp:%8.2f\n", s->freq, sqrt(s->real*s->real + s->imag*s->imag) / s->ref);
 134
 135   return data;
 136 }
 137
 138 #if 0
 139 static af_data_t* play_float(struct af_instance_s* af, af_data_t* data)
 140 {
 141   af_sinesuppress_t *s = af->setup;
 142   register int i = 0;
 143   float *a = (float*)data->audio;       // Audio data
 144   int len = data->len/4;                // Number of samples
 145   float avg, l, r;
 146
 147   for (i = 0; i < len; i+=2)
 148   {
 149     avg = (a[i] + a[i + 1]) / 2;
 150
 151 /*    l = avg + (s->mul * (a[i] - avg));
 152     r = avg + (s->mul * (a[i + 1] - avg));*/
 153
 154     a[i] = af_softclip(l);
 155     a[i + 1] = af_softclip(r);
 156   }
 157
 158   return data;
 159 }
 160 #endif
 161
 162 // Allocate memory and set function pointers
 163 static int af_open(af_instance_t* af){
 164   af->control=control;
 165   af->uninit=uninit;
 166   af->play=play_s16;
 167   af->mul=1;
 168   af->data=calloc(1,sizeof(af_data_t));
 169   af->setup=calloc(1,sizeof(af_sinesuppress_t));
 170   if(af->data == NULL || af->setup == NULL)
 171     return AF_ERROR;
 172
 173   ((af_sinesuppress_t*)af->setup)->freq = 50.0;
 174   ((af_sinesuppress_t*)af->setup)->decay = 0.0001;
 175   return AF_OK;
 176 }
 177
 178 // Description of this filter
 179 af_info_t af_info_sinesuppress = {
 180     "Sine Suppress",
 181     "sinesuppress",
 182     "Michael Niedermayer",
 183     "",
 184     0,
 185     af_open
 186 };