← Back to branch summary

~ubuntu-branches/ubuntu/quantal/linphone/quantal

« back to all changes in this revision

Viewing changes to mediastreamer2/src/equalizer.c

  • Committer: Bazaar Package Importer
  • Author(s): Mark Purcell
  • Date: 2009-10-14 08:26:02 UTC
  • mfrom: (1.3.1 upstream) (6.1.1 sid)
  • Revision ID: james.westby@ubuntu.com-20091014082602-751618nxdjooja3l
Tags: 3.2.1-1
New upstream release 

Show diffs side-by-side

added added

removed removed

Lines of Context:
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
static EqualizerState * equalizer_state_new(int nfft){
 
62
        EqualizerState *s=(EqualizerState *)ms_new0(EqualizerState,1);
 
63
        s->rate=8000;
 
64
        s->nfft=nfft;
 
65
        s->fft_cpx=(ms_word16_t*)ms_new0(ms_word16_t,s->nfft);
 
66
        equalizer_state_flatten(s);
 
67
        s->fir_len=s->nfft;
 
68
        s->fir=(ms_word16_t*)ms_new(ms_word16_t,s->fir_len);
 
69
        s->mem=(ms_mem_t*)ms_new0(ms_mem_t,s->fir_len);
 
70
        s->needs_update=TRUE;
 
71
        s->active=TRUE;
 
72
        return s;
 
73
}
 
74
 
 
75
static void equalizer_state_destroy(EqualizerState *s){
 
76
        ms_free(s->fft_cpx);
 
77
        ms_free(s->fir);
 
78
        ms_free(s->mem);
 
79
        ms_free(s);
 
80
}
 
81
 
 
82
static int equalizer_state_hz_to_index(EqualizerState *s, int hz){
 
83
        int ret;
 
84
        if (hz<0){
 
85
                ms_error("Bad frequency value %i",hz);
 
86
                return -1;
 
87
        }
 
88
        if (hz>(s->rate/2)){
 
89
                hz=(s->rate/2);
 
90
        }
 
91
        /*round to nearest integer*/
 
92
        ret=((hz*s->nfft)+(s->rate/2))/s->rate;
 
93
        if (ret==s->nfft/2) ret=(s->nfft/2)-1;
 
94
        return ret;
 
95
}
 
96
 
 
97
static int equalizer_state_index2hz(EqualizerState *s, int index){
 
98
        return (index * s->rate + s->nfft/2) / s->nfft;
 
99
}
 
100
 
 
101
static float gain_float(ms_word16_t val){
 
102
        return (float)val/GAIN_ZERODB;
 
103
}
 
104
 
 
105
static float equalizer_state_get(EqualizerState *s, int freqhz){
 
106
        int idx=equalizer_state_hz_to_index(s,freqhz);
 
107
        if (idx>=0) return gain_float(s->fft_cpx[idx*2])*s->nfft;
 
108
        return 0;
 
109
}
 
110
 
 
111
/* The natural peaking equalizer amplitude transfer function is multiplied to the discrete f-points.
 
112
 * Note that for PEQ no sqrt is needed for the overall calculation, applying it to gain yields the
 
113
 * same response.
 
114
 */
 
115
static float equalizer_compute_gainpoint(int f, int freq_0, float sqrt_gain, int freq_bw)
 
116
{
 
117
        float k1, k2;
 
118
        k1 = ((float)(f*f)-(float)(freq_0*freq_0));
 
119
        k1*= k1;
 
120
        k2 = (float)(f*freq_bw);
 
121
        k2*= k2;
 
122
        return (k1+k2*sqrt_gain)/(k1+k2/sqrt_gain);
 
123
}
 
124
 
 
125
static void equalizer_point_set(EqualizerState *s, int i, int f, float gain){
 
126
        ms_message("Setting gain %f for freq_index %i (%i Hz)\n",gain,i,f);
 
127
        s->fft_cpx[1+((i-1)*2)] = (s->fft_cpx[1+((i-1)*2)]*(int)(gain*32768))/32768;
 
128
}
 
129
 
 
130
static void equalizer_state_set(EqualizerState *s, int freq_0, float gain, int freq_bw){
 
131
        //int low,high;
 
132
        int i, f;
 
133
        int delta_f = equalizer_state_index2hz(s, 1);
 
134
        float sqrt_gain = sqrt(gain);
 
135
        int mid = equalizer_state_hz_to_index(s, freq_0);
 
136
        freq_bw-= delta_f/2;   /* subtract a constant - compensates for limited fft steepness at low f */
 
137
        if (freq_bw < delta_f/2)
 
138
                freq_bw = delta_f/2;
 
139
        i = mid;
 
140
        f = equalizer_state_index2hz(s, i);
 
141
        equalizer_point_set(s, i, f, gain);   /* gain according to argument */
 
142
        do {    /* note: to better accomodate limited fft steepness, -delta is applied in f-calc ... */
 
143
                i++;
 
144
                f = equalizer_state_index2hz(s, i);
 
145
                gain = equalizer_compute_gainpoint(f-delta_f, freq_0, sqrt_gain, freq_bw);
 
146
                equalizer_point_set(s, i, f, gain);
 
147
        }
 
148
        while (i<TAPS && (gain>1.1 || gain<0.9));
 
149
        i = mid;
 
150
        do {    /* ... and here +delta, as to  */
 
151
                i--;
 
152
                f = equalizer_state_index2hz(s, i);
 
153
                gain = equalizer_compute_gainpoint(f+delta_f, freq_0, sqrt_gain, freq_bw);
 
154
                equalizer_point_set(s, i, f, gain);
 
155
        }
 
156
        while (i>=0 && (gain>1.1 || gain<0.9));
 
157
        s->needs_update=TRUE;
 
158
}
 
159
 
 
160
static void dump_table(ms_word16_t *t, int len){
 
161
        int i;
 
162
        for(i=0;i<len;i++)
 
163
#ifdef MS_FIXED_POINT
 
164
                ms_message("[%i]\t%i",i,t[i]);
 
165
#else
 
166
                ms_message("[%i]\t%f",i,t[i]);
 
167
#endif
 
168
}
 
169
 
 
170
static void time_shift(ms_word16_t *s, int len){
 
171
        int i;
 
172
        int half=len/2;
 
173
        ms_word16_t tmp;
 
174
        for (i=0;i<half;++i){
 
175
                tmp=s[i];
 
176
                s[i]=s[i+half];
 
177
                s[i+half]=tmp;
 
178
        }
 
179
}
 
180
 
 
181
/*
 
182
 *hamming:
 
183
 * 0.54 - 0.46*cos(2*M_PI*t/T)
 
184
 *
 
185
 * blackman
 
186
 * 0.42 - 0.5*cos(2*M_PI*t/T) + 0.08*cos(4*M_PI*t/T)
 
187
*/
 
188
 
 
189
static void norm_and_apodize(ms_word16_t *s, int len){
 
190
        int i;
 
191
        float x;
 
192
        float w;
 
193
        for(i=0;i<len;++i){
 
194
                x=(float)i*2*M_PI/(float)len;
 
195
                w=0.54 - (0.46*cos(x));
 
196
                //w=0.42 - (0.5*cos(x)) + (0.08*cos(2*x));
 
197
                s[i]=w*(float)s[i];
 
198
        }       
 
199
}
 
200
 
 
201
static void equalizer_state_compute_impulse_response(EqualizerState *s){
 
202
        void *fft_handle=ms_fft_init(s->nfft);
 
203
        ms_message("Spectral domain:");
 
204
        dump_table(s->fft_cpx,s->nfft);
 
205
        ms_ifft(fft_handle,s->fft_cpx,s->fir);
 
206
        ms_fft_destroy(fft_handle);
 
207
        /*
 
208
        ms_message("Inverse fft result:");
 
209
        dump_table(s->fir,s->fir_len);
 
210
        */
 
211
        time_shift(s->fir,s->fir_len);
 
212
        /*
 
213
        ms_message("Time shifted:");
 
214
        dump_table(s->fir,s->fir_len);
 
215
        */
 
216
        norm_and_apodize(s->fir,s->fir_len);
 
217
        ms_message("Apodized impulse response:");
 
218
        dump_table(s->fir,s->fir_len);
 
219
        s->needs_update=FALSE;
 
220
}
 
221
 
 
222
 
 
223
 
 
224
#ifdef MS_FIXED_POINT
 
225
#define INT16_TO_WORD16(i,w,l) w=(i)
 
226
#define WORD16_TO_INT16(i,w,l) i=(w)
 
227
#else
 
228
 
 
229
static void int16_to_word16(const int16_t *is, ms_word16_t *w, int l){
 
230
        int i;
 
231
        for(i=0;i<l;++i){
 
232
                w[i]=(ms_word16_t)is[i];
 
233
        }
 
234
}
 
235
 
 
236
static void word16_to_int16(const ms_word16_t *w, int16_t *is, int l){
 
237
        int i;
 
238
        for (i=0;i<l;++i)
 
239
                is[i]=(int16_t)w[i];
 
240
}
 
241
 
 
242
#define INT16_TO_WORD16(i,w,l) w=(ms_word16_t*)alloca(sizeof(ms_word16_t)*(l));int16_to_word16(i,w,l)
 
243
#define WORD16_TO_INT16(w,i,l) word16_to_int16(w,i,l)
 
244
#endif
 
245
 
 
246
static void equalizer_state_run(EqualizerState *s, int16_t *samples, int nsamples){
 
247
        if (s->needs_update)
 
248
                equalizer_state_compute_impulse_response(s);
 
249
        ms_word16_t *w;
 
250
        INT16_TO_WORD16(samples,w,nsamples);
 
251
        ms_fir_mem16(w,s->fir,w,nsamples,s->fir_len,s->mem);
 
252
        WORD16_TO_INT16(w,samples,nsamples);
 
253
}
 
254
 
 
255
 
 
256
static void equalizer_init(MSFilter *f){
 
257
        f->data=equalizer_state_new(TAPS);
 
258
}
 
259
 
 
260
static void equalizer_uninit(MSFilter *f){
 
261
        equalizer_state_destroy((EqualizerState*)f->data);
 
262
}
 
263
 
 
264
static void equalizer_process(MSFilter *f){
 
265
        mblk_t *m;
 
266
        EqualizerState *s=(EqualizerState*)f->data;
 
267
        while((m=ms_queue_get(f->inputs[0]))!=NULL){
 
268
                if (s->active){
 
269
                        equalizer_state_run(s,(int16_t*)m->b_rptr,(m->b_wptr-m->b_rptr)/2);
 
270
                }
 
271
                ms_queue_put(f->outputs[0],m);
 
272
        }
 
273
}
 
274
 
 
275
static int equalizer_set_gain(MSFilter *f, void *data){
 
276
        EqualizerState *s=(EqualizerState*)f->data;
 
277
        MSEqualizerGain *d=(MSEqualizerGain*)data;
 
278
        equalizer_state_set(s,d->frequency,d->gain,d->width);
 
279
        return 0;
 
280
}
 
281
 
 
282
static int equalizer_get_gain(MSFilter *f, void *data){
 
283
        EqualizerState *s=(EqualizerState*)f->data;
 
284
        MSEqualizerGain *d=(MSEqualizerGain*)data;
 
285
        d->gain=equalizer_state_get(s,d->frequency);
 
286
        d->width=0;
 
287
        return 0;
 
288
}
 
289
 
 
290
static int equalizer_set_rate(MSFilter *f, void *data){
 
291
        EqualizerState *s=(EqualizerState*)f->data;
 
292
        s->rate=*(int*)data;
 
293
        s->needs_update=TRUE;
 
294
        return 0;
 
295
}
 
296
 
 
297
static int equalizer_set_active(MSFilter *f, void *data){
 
298
        EqualizerState *s=(EqualizerState*)f->data;
 
299
        s->active=*(int*)data;
 
300
        return 0;
 
301
}
 
302
 
 
303
static int equalizer_dump(MSFilter *f, void *data){
 
304
        EqualizerState *s=(EqualizerState*)f->data;
 
305
        float *t=(float*)data;
 
306
        int i;
 
307
        *t=s->fft_cpx[0];
 
308
        t++;
 
309
        for (i=1;i<s->nfft;i+=2){
 
310
                *t=((float)s->fft_cpx[i]*(float)s->nfft)/(float)GAIN_ZERODB;
 
311
                t++;
 
312
        }
 
313
        return 0;
 
314
}
 
315
 
 
316
static int equalizer_get_nfreqs(MSFilter *f, void *data){
 
317
        EqualizerState *s=(EqualizerState*)f->data;
 
318
        *(int*)data=s->nfft/2;
 
319
        return 0;
 
320
}
 
321
 
 
322
static MSFilterMethod equalizer_methods[]={
 
323
        {       MS_EQUALIZER_SET_GAIN           ,       equalizer_set_gain      },
 
324
        {       MS_EQUALIZER_GET_GAIN           ,       equalizer_get_gain      },
 
325
        {       MS_EQUALIZER_SET_ACTIVE         ,       equalizer_set_active    },
 
326
        {       MS_FILTER_SET_SAMPLE_RATE       ,       equalizer_set_rate      },
 
327
        {       MS_EQUALIZER_DUMP_STATE         ,       equalizer_dump          },
 
328
        {       MS_EQUALIZER_GET_NUM_FREQUENCIES,       equalizer_get_nfreqs    },
 
329
        {       0                               ,       NULL                    }
 
330
};
 
331
 
 
332
#ifdef _MSC_VER
 
333
 
 
334
MSFilterDesc ms_equalizer_desc={
 
335
        MS_EQUALIZER_ID,
 
336
        "MSEqualizer",
 
337
        N_("Parametric sound equalizer."),
 
338
        MS_FILTER_OTHER,
 
339
        NULL,
 
340
        1,
 
341
        1,
 
342
        equalizer_init,
 
343
        NULL,
 
344
        equalizer_process,
 
345
        NULL,
 
346
        equalizer_uninit,
 
347
        equalizer_methods
 
348
};
 
349
 
 
350
#else
 
351
 
 
352
MSFilterDesc ms_equalizer_desc={
 
353
        .id= MS_EQUALIZER_ID,
 
354
        .name="MSEqualizer",
 
355
        .text=N_("Parametric sound equalizer."),
 
356
        .category=MS_FILTER_OTHER,
 
357
        .ninputs=1,
 
358
        .noutputs=1,
 
359
        .init=equalizer_init,
 
360
        .process=equalizer_process,
 
361
        .uninit=equalizer_uninit,
 
362
        .methods=equalizer_methods
 
363
};
 
364
 
 
365
#endif
 
366
 
 
367
MS_FILTER_DESC_EXPORT(ms_equalizer_desc)