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     free(af->data);
 102     free(af->setup);
 103 }
 104
 105 // Filter data through filter
 106 static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data)
 107 {
 108   af_sinesuppress_t *s = af->setup;
 109   register int i = 0;
 110   int16_t *a = (int16_t*)data->audio;   // Audio data
 111   int len = data->len/2;                // Number of samples
 112
 113   for (i = 0; i < len; i++)
 114   {
 115     double co= cos(s->pos);
 116     double si= sin(s->pos);
 117
 118     s->real += co * a[i];
 119     s->imag += si * a[i];
 120     s->ref  += co * co;
 121
 122     a[i] -= (s->real * co + s->imag * si) / s->ref;
 123
 124     s->real -= s->real * s->decay;
 125     s->imag -= s->imag * s->decay;
 126     s->ref  -= s->ref  * s->decay;
 127
 128     s->pos += 2 * M_PI * s->freq / data->rate;
 129   }
 130
 131    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);
 132
 133   return data;
 134 }
 135
 136 #if 0
 137 static af_data_t* play_float(struct af_instance_s* af, af_data_t* data)
 138 {
 139   af_sinesuppress_t *s = af->setup;
 140   register int i = 0;
 141   float *a = (float*)data->audio;       // Audio data
 142   int len = data->len/4;                // Number of samples
 143   float avg, l, r;
 144
 145   for (i = 0; i < len; i+=2)
 146   {
 147     avg = (a[i] + a[i + 1]) / 2;
 148
 149 /*    l = avg + (s->mul * (a[i] - avg));
 150     r = avg + (s->mul * (a[i + 1] - avg));*/
 151
 152     a[i] = af_softclip(l);
 153     a[i + 1] = af_softclip(r);
 154   }
 155
 156   return data;
 157 }
 158 #endif
 159
 160 // Allocate memory and set function pointers
 161 static int af_open(af_instance_t* af){
 162   af->control=control;
 163   af->uninit=uninit;
 164   af->play=play_s16;
 165   af->mul=1;
 166   af->data=calloc(1,sizeof(af_data_t));
 167   af->setup=calloc(1,sizeof(af_sinesuppress_t));
 168   if(af->data == NULL || af->setup == NULL)
 169     return AF_ERROR;
 170
 171   ((af_sinesuppress_t*)af->setup)->freq = 50.0;
 172   ((af_sinesuppress_t*)af->setup)->decay = 0.0001;
 173   return AF_OK;
 174 }
 175
 176 // Description of this filter
 177 af_info_t af_info_sinesuppress = {
 178     "Sine Suppress",
 179     "sinesuppress",
 180     "Michael Niedermayer",
 181     "",
 182     0,
 183     af_open
 184 };