sjero.net Git - linphone/blob - linphone/mediastreamer2/src/equalizer.c

   1 /*
   2 mediastreamer2 library - modular sound and video processing and streaming
   3 Copyright (C) 2009  Simon MORLAT (simon.morlat@linphone.org)
   4
   5 This program is free software; you can redistribute it and/or
   6 modify it under the terms of the GNU General Public License
   7 as published by the Free Software Foundation; either version 2
   8 of the License, or (at your option) any later version.
   9
  10 This program is distributed in the hope that it will be useful,
  11 but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 GNU General Public License for more details.
  14
  15 You should have received a copy of the GNU General Public License
  16 along with this program; if not, write to the Free Software
  17 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  18 */
  19
  20 #include <mediastreamer2/msequalizer.h>
  21 #include <mediastreamer2/dsptools.h>
  22
  23 #include <math.h>
  24
  25 #ifdef _MSC_VER
  26 #include <malloc.h>
  27 #define alloca _alloca
  28 #endif
  29
  30 #ifndef M_PI
  31 #define M_PI       3.14159265358979323846
  32 #endif
  33
  34 #ifdef MS_FIXED_POINT
  35 #define GAIN_ZERODB 20000
  36 #else
  37 #define GAIN_ZERODB 1.0
  38 #endif
  39
  40 #define TAPS 128
  41
  42 typedef struct _EqualizerState{
  43         int rate;
  44         int nfft; /*number of fft points in time*/
  45         ms_word16_t *fft_cpx;
  46         int fir_len;
  47         ms_word16_t *fir;
  48         ms_mem_t *mem; /*memories for filtering computations*/
  49         bool_t needs_update;
  50         bool_t active;
  51 } EqualizerState;
  52
  53 static void equalizer_state_flatten(EqualizerState *s){
  54         int i;
  55         ms_word16_t val=GAIN_ZERODB/s->nfft;
  56         s->fft_cpx[0]=val;
  57         for(i=1;i<s->nfft;i+=2)
  58                 s->fft_cpx[i]=val;
  59 }
  60
  61 /* TODO: rate also beyond 8000 */
  62 static EqualizerState * equalizer_state_new(int nfft){
  63         EqualizerState *s=(EqualizerState *)ms_new0(EqualizerState,1);
  64         s->rate=8000;
  65         s->nfft=nfft;
  66         s->fft_cpx=(ms_word16_t*)ms_new0(ms_word16_t,s->nfft);
  67         equalizer_state_flatten(s);
  68         s->fir_len=s->nfft;
  69         s->fir=(ms_word16_t*)ms_new(ms_word16_t,s->fir_len);
  70         s->mem=(ms_mem_t*)ms_new0(ms_mem_t,s->fir_len);
  71         s->needs_update=TRUE;
  72         s->active=TRUE;
  73         return s;
  74 }
  75
  76 static void equalizer_state_destroy(EqualizerState *s){
  77         ms_free(s->fft_cpx);
  78         ms_free(s->fir);
  79         ms_free(s->mem);
  80         ms_free(s);
  81 }
  82
  83 static int equalizer_state_hz_to_index(EqualizerState *s, int hz){
  84         int ret;
  85         if (hz<0){
  86                 ms_error("Bad frequency value %i",hz);
  87                 return -1;
  88         }
  89         if (hz>(s->rate/2)){
  90                 hz=(s->rate/2);
  91         }
  92         /*round to nearest integer*/
  93         ret=((hz*s->nfft)+(s->rate/2))/s->rate;
  94         if (ret==s->nfft/2) ret=(s->nfft/2)-1;
  95         return ret;
  96 }
  97
  98 static int equalizer_state_index2hz(EqualizerState *s, int index){
  99         return (index * s->rate + s->nfft/2) / s->nfft;
 100 }
 101
 102 static float gain_float(ms_word16_t val){
 103         return (float)val/GAIN_ZERODB;
 104 }
 105
 106 static float equalizer_state_get(EqualizerState *s, int freqhz){
 107         int idx=equalizer_state_hz_to_index(s,freqhz);
 108         if (idx>=0) return gain_float(s->fft_cpx[idx*2])*s->nfft;
 109         return 0;
 110 }
 111
 112 /* The natural peaking equalizer amplitude transfer function is multiplied to the discrete f-points.
 113  * Note that for PEQ no sqrt is needed for the overall calculation, applying it to gain yields the
 114  * same response.
 115  */
 116 static float equalizer_compute_gainpoint(int f, int freq_0, float sqrt_gain, int freq_bw)
 117 {
 118         float k1, k2;
 119         k1 = ((float)(f*f)-(float)(freq_0*freq_0));
 120         k1*= k1;
 121         k2 = (float)(f*freq_bw);
 122         k2*= k2;
 123         return (k1+k2*sqrt_gain)/(k1+k2/sqrt_gain);
 124 }
 125
 126 static void equalizer_point_set(EqualizerState *s, int i, int f, float gain){
 127         ms_message("Setting gain %f for freq_index %i (%i Hz)\n",gain,i,f);
 128         s->fft_cpx[1+((i-1)*2)] = (s->fft_cpx[1+((i-1)*2)]*(int)(gain*32768))/32768;
 129 }
 130
 131 static void equalizer_state_set(EqualizerState *s, int freq_0, float gain, int freq_bw){
 132         //int low,high;
 133         int i, f;
 134         int delta_f = equalizer_state_index2hz(s, 1);
 135         float sqrt_gain = sqrt(gain);
 136         int mid = equalizer_state_hz_to_index(s, freq_0);
 137         freq_bw-= delta_f/2;   /* subtract a constant - compensates for limited fft steepness at low f */
 138         if (freq_bw < delta_f/2)
 139                 freq_bw = delta_f/2;
 140         i = mid;
 141         f = equalizer_state_index2hz(s, i);
 142         equalizer_point_set(s, i, f, gain);   /* gain according to argument */
 143         do {    /* note: to better accomodate limited fft steepness, -delta is applied in f-calc ... */
 144                 i++;
 145                 f = equalizer_state_index2hz(s, i);
 146                 gain = equalizer_compute_gainpoint(f-delta_f, freq_0, sqrt_gain, freq_bw);
 147                 equalizer_point_set(s, i, f, gain);
 148         }
 149         while (i < s->nfft/2 && (gain>1.1 || gain<0.9));
 150         i = mid;
 151         do {    /* ... and here +delta, as to  */
 152                 i--;
 153                 f = equalizer_state_index2hz(s, i);
 154                 gain = equalizer_compute_gainpoint(f+delta_f, freq_0, sqrt_gain, freq_bw);
 155                 equalizer_point_set(s, i, f, gain);
 156         }
 157         while (i>=0 && (gain>1.1 || gain<0.9));
 158         s->needs_update=TRUE;
 159 }
 160
 161 static void dump_table(ms_word16_t *t, int len){
 162         int i;
 163         for(i=0;i<len;i++)
 164 #ifdef MS_FIXED_POINT
 165                 ms_message("[%i]\t%i",i,t[i]);
 166 #else
 167                 ms_message("[%i]\t%f",i,t[i]);
 168 #endif
 169 }
 170
 171 static void time_shift(ms_word16_t *s, int len){
 172         int i;
 173         int half=len/2;
 174         ms_word16_t tmp;
 175         for (i=0;i<half;++i){
 176                 tmp=s[i];
 177                 s[i]=s[i+half];
 178                 s[i+half]=tmp;
 179         }
 180 }
 181
 182 /*
 183  *hamming:
 184  * 0.54 - 0.46*cos(2*M_PI*t/T)
 185  *
 186  * blackman
 187  * 0.42 - 0.5*cos(2*M_PI*t/T) + 0.08*cos(4*M_PI*t/T)
 188 */
 189
 190 static void norm_and_apodize(ms_word16_t *s, int len){
 191         int i;
 192         float x;
 193         float w;
 194         for(i=0;i<len;++i){
 195                 x=(float)i*2*M_PI/(float)len;
 196                 w=0.54 - (0.46*cos(x));
 197                 //w=0.42 - (0.5*cos(x)) + (0.08*cos(2*x));
 198                 s[i]=w*(float)s[i];
 199         }
 200 }
 201
 202 static void equalizer_state_compute_impulse_response(EqualizerState *s){
 203         void *fft_handle=ms_fft_init(s->nfft);
 204         ms_message("Spectral domain:");
 205         dump_table(s->fft_cpx,s->nfft);
 206         ms_ifft(fft_handle,s->fft_cpx,s->fir);
 207         ms_fft_destroy(fft_handle);
 208         /*
 209         ms_message("Inverse fft result:");
 210         dump_table(s->fir,s->fir_len);
 211         */
 212         time_shift(s->fir,s->fir_len);
 213         /*
 214         ms_message("Time shifted:");
 215         dump_table(s->fir,s->fir_len);
 216         */
 217         norm_and_apodize(s->fir,s->fir_len);
 218         ms_message("Apodized impulse response:");
 219         dump_table(s->fir,s->fir_len);
 220         s->needs_update=FALSE;
 221 }
 222
 223
 224
 225 #ifdef MS_FIXED_POINT
 226 #define INT16_TO_WORD16(i,w,l) w=(i)
 227 #define WORD16_TO_INT16(i,w,l) i=(w)
 228 #else
 229
 230 static void int16_to_word16(const int16_t *is, ms_word16_t *w, int l){
 231         int i;
 232         for(i=0;i<l;++i){
 233                 w[i]=(ms_word16_t)is[i];
 234         }
 235 }
 236
 237 static void word16_to_int16(const ms_word16_t *w, int16_t *is, int l){
 238         int i;
 239         for (i=0;i<l;++i)
 240                 is[i]=(int16_t)w[i];
 241 }
 242
 243 #define INT16_TO_WORD16(i,w,l) w=(ms_word16_t*)alloca(sizeof(ms_word16_t)*(l));int16_to_word16(i,w,l)
 244 #define WORD16_TO_INT16(w,i,l) word16_to_int16(w,i,l)
 245 #endif
 246
 247 static void equalizer_state_run(EqualizerState *s, int16_t *samples, int nsamples){
 248         if (s->needs_update)
 249                 equalizer_state_compute_impulse_response(s);
 250         ms_word16_t *w;
 251         INT16_TO_WORD16(samples,w,nsamples);
 252         ms_fir_mem16(w,s->fir,w,nsamples,s->fir_len,s->mem);
 253         WORD16_TO_INT16(w,samples,nsamples);
 254 }
 255
 256
 257 static void equalizer_init(MSFilter *f){
 258         f->data=equalizer_state_new(TAPS);
 259 }
 260
 261 static void equalizer_uninit(MSFilter *f){
 262         equalizer_state_destroy((EqualizerState*)f->data);
 263 }
 264
 265 static void equalizer_process(MSFilter *f){
 266         mblk_t *m;
 267         EqualizerState *s=(EqualizerState*)f->data;
 268         while((m=ms_queue_get(f->inputs[0]))!=NULL){
 269                 if (s->active){
 270                         equalizer_state_run(s,(int16_t*)m->b_rptr,(m->b_wptr-m->b_rptr)/2);
 271                 }
 272                 ms_queue_put(f->outputs[0],m);
 273         }
 274 }
 275
 276 static int equalizer_set_gain(MSFilter *f, void *data){
 277         EqualizerState *s=(EqualizerState*)f->data;
 278         MSEqualizerGain *d=(MSEqualizerGain*)data;
 279         equalizer_state_set(s,d->frequency,d->gain,d->width);
 280         return 0;
 281 }
 282
 283 static int equalizer_get_gain(MSFilter *f, void *data){
 284         EqualizerState *s=(EqualizerState*)f->data;
 285         MSEqualizerGain *d=(MSEqualizerGain*)data;
 286         d->gain=equalizer_state_get(s,d->frequency);
 287         d->width=0;
 288         return 0;
 289 }
 290
 291 static int equalizer_set_rate(MSFilter *f, void *data){
 292         EqualizerState *s=(EqualizerState*)f->data;
 293         s->rate=*(int*)data;
 294         s->needs_update=TRUE;
 295         return 0;
 296 }
 297
 298 static int equalizer_set_active(MSFilter *f, void *data){
 299         EqualizerState *s=(EqualizerState*)f->data;
 300         s->active=*(int*)data;
 301         return 0;
 302 }
 303
 304 static int equalizer_dump(MSFilter *f, void *data){
 305         EqualizerState *s=(EqualizerState*)f->data;
 306         float *t=(float*)data;
 307         int i;
 308         *t=s->fft_cpx[0];
 309         t++;
 310         for (i=1;i<s->nfft;i+=2){
 311                 *t=((float)s->fft_cpx[i]*(float)s->nfft)/(float)GAIN_ZERODB;
 312                 t++;
 313         }
 314         return 0;
 315 }
 316
 317 static int equalizer_get_nfreqs(MSFilter *f, void *data){
 318         EqualizerState *s=(EqualizerState*)f->data;
 319         *(int*)data=s->nfft/2;
 320         return 0;
 321 }
 322
 323 static MSFilterMethod equalizer_methods[]={
 324         {       MS_EQUALIZER_SET_GAIN           ,       equalizer_set_gain      },
 325         {       MS_EQUALIZER_GET_GAIN           ,       equalizer_get_gain      },
 326         {       MS_EQUALIZER_SET_ACTIVE         ,       equalizer_set_active    },
 327         {       MS_FILTER_SET_SAMPLE_RATE       ,       equalizer_set_rate      },
 328         {       MS_EQUALIZER_DUMP_STATE         ,       equalizer_dump          },
 329         {       MS_EQUALIZER_GET_NUM_FREQUENCIES,       equalizer_get_nfreqs    },
 330         {       0                               ,       NULL                    }
 331 };
 332
 333 #ifdef _MSC_VER
 334
 335 MSFilterDesc ms_equalizer_desc={
 336         MS_EQUALIZER_ID,
 337         "MSEqualizer",
 338         N_("Parametric sound equalizer."),
 339         MS_FILTER_OTHER,
 340         NULL,
 341         1,
 342         1,
 343         equalizer_init,
 344         NULL,
 345         equalizer_process,
 346         NULL,
 347         equalizer_uninit,
 348         equalizer_methods
 349 };
 350
 351 #else
 352
 353 MSFilterDesc ms_equalizer_desc={
 354         .id= MS_EQUALIZER_ID,
 355         .name="MSEqualizer",
 356         .text=N_("Parametric sound equalizer."),
 357         .category=MS_FILTER_OTHER,
 358         .ninputs=1,
 359         .noutputs=1,
 360         .init=equalizer_init,
 361         .process=equalizer_process,
 362         .uninit=equalizer_uninit,
 363         .methods=equalizer_methods
 364 };
 365
 366 #endif
 367
 368 MS_FILTER_DESC_EXPORT(ms_equalizer_desc)