equalizer.c 9.47 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
/*
mediastreamer2 library - modular sound and video processing and streaming
Copyright (C) 2009  Simon MORLAT (simon.morlat@linphone.org)

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/

smorlat's avatar
smorlat committed
20
#include <mediastreamer2/msequalizer.h>
smorlat's avatar
smorlat committed
21 22
#include <mediastreamer2/dsptools.h>

smorlat's avatar
smorlat committed
23 24
#include <math.h>

25 26 27
#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif
smorlat's avatar
smorlat committed
28 29

#ifdef MS_FIXED_POINT
30
#define GAIN_ZERODB 20000
smorlat's avatar
smorlat committed
31
#else
32
#define GAIN_ZERODB 1.0
smorlat's avatar
smorlat committed
33
#endif
34

35
#define EQUALIZER_DEFAULT_RATE 8000
36 37 38

typedef struct _EqualizerState{
	int rate;
smorlat's avatar
smorlat committed
39
	int nfft; /*number of fft points in time*/
smorlat's avatar
smorlat committed
40
	ms_word16_t *fft_cpx;
41
	int fir_len;
smorlat's avatar
smorlat committed
42 43 44
	ms_word16_t *fir;
	ms_mem_t *mem; /*memories for filtering computations*/
	bool_t needs_update;
smorlat's avatar
smorlat committed
45
	bool_t active;
46 47
} EqualizerState;

smorlat's avatar
smorlat committed
48
static void equalizer_state_flatten(EqualizerState *s){
smorlat's avatar
smorlat committed
49
	int i;
smorlat's avatar
smorlat committed
50 51 52 53
	ms_word16_t val=GAIN_ZERODB/s->nfft;
	s->fft_cpx[0]=val;
	for(i=1;i<s->nfft;i+=2)
		s->fft_cpx[i]=val;
smorlat's avatar
smorlat committed
54 55
}

56 57 58 59 60 61 62 63 64 65 66 67 68 69
static void equalizer_rate_update( EqualizerState* s, int rate ){
    int nFFT;

    if( rate < 16000 ){
        nFFT = 128;
    } else if( rate < 32000){
        nFFT = 256;
    } else {
        nFFT = 512;
    }
    ms_message("Equalizer rate: %d, selecting %d steps for FFT", rate, nFFT);

    s->rate=rate;
	s->nfft=nFFT;
70
	if (s->fft_cpx != NULL) ms_free(s->fft_cpx);
71
	s->fft_cpx=(ms_word16_t*)ms_new0(ms_word16_t,s->nfft);
smorlat's avatar
smorlat committed
72
	equalizer_state_flatten(s);
73
	s->fir_len=s->nfft;
74
	if (s->fir != NULL) ms_free(s->fir);
75
	s->fir=(ms_word16_t*)ms_new0(ms_word16_t,s->fir_len);
76
	if (s->mem != NULL) ms_free(s->mem);
77
	s->mem=(ms_mem_t*)ms_new0(ms_mem_t,s->fir_len);
smorlat's avatar
smorlat committed
78
	s->needs_update=TRUE;
79 80 81 82 83 84
}


static EqualizerState * equalizer_state_new(int rate){
	EqualizerState *s=(EqualizerState *)ms_new0(EqualizerState,1);
	equalizer_rate_update(s,rate);
smorlat's avatar
smorlat committed
85
	s->active=TRUE;
smorlat's avatar
smorlat committed
86 87 88 89 90 91 92 93
	return s;
}

static void equalizer_state_destroy(EqualizerState *s){
	ms_free(s->fft_cpx);
	ms_free(s->fir);
	ms_free(s->mem);
	ms_free(s);
94 95 96 97 98 99 100 101 102 103 104
}

static int equalizer_state_hz_to_index(EqualizerState *s, int hz){
	int ret;
	if (hz<0){
		ms_error("Bad frequency value %i",hz);
		return -1;
	}
	if (hz>(s->rate/2)){
		hz=(s->rate/2);
	}
smorlat's avatar
smorlat committed
105
	/*round to nearest integer*/
106
	ret=((hz*s->nfft)+(s->rate/2))/s->rate;
smorlat's avatar
smorlat committed
107
	if (ret==s->nfft/2) ret=(s->nfft/2)-1;
108 109 110
	return ret;
}

111 112
static int equalizer_state_index2hz(EqualizerState *s, int index){
	return (index * s->rate + s->nfft/2) / s->nfft;
113 114
}

115 116
static float gain_float(ms_word16_t val){
	return (float)val/GAIN_ZERODB;
117 118
}

smorlat's avatar
smorlat committed
119
static float equalizer_state_get(EqualizerState *s, int freqhz){
120
	int idx=equalizer_state_hz_to_index(s,freqhz);
smorlat's avatar
smorlat committed
121
	if (idx>=0) return gain_float(s->fft_cpx[idx*2])*s->nfft;
122 123 124
	return 0;
}

125 126 127 128 129 130 131 132 133 134 135 136
/* The natural peaking equalizer amplitude transfer function is multiplied to the discrete f-points.
 * Note that for PEQ no sqrt is needed for the overall calculation, applying it to gain yields the
 * same response.
 */
static float equalizer_compute_gainpoint(int f, int freq_0, float sqrt_gain, int freq_bw)
{
	float k1, k2;
	k1 = ((float)(f*f)-(float)(freq_0*freq_0));
	k1*= k1;
	k2 = (float)(f*freq_bw);
	k2*= k2;
	return (k1+k2*sqrt_gain)/(k1+k2/sqrt_gain);
137 138
}

139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161
static void equalizer_point_set(EqualizerState *s, int i, int f, float gain){
	ms_message("Setting gain %f for freq_index %i (%i Hz)\n",gain,i,f);
	s->fft_cpx[1+((i-1)*2)] = (s->fft_cpx[1+((i-1)*2)]*(int)(gain*32768))/32768;
}

static void equalizer_state_set(EqualizerState *s, int freq_0, float gain, int freq_bw){
	//int low,high;
	int i, f;
	int delta_f = equalizer_state_index2hz(s, 1);
	float sqrt_gain = sqrt(gain);
	int mid = equalizer_state_hz_to_index(s, freq_0);
	freq_bw-= delta_f/2;   /* subtract a constant - compensates for limited fft steepness at low f */
	if (freq_bw < delta_f/2)
		freq_bw = delta_f/2;
	i = mid;
	f = equalizer_state_index2hz(s, i);
	equalizer_point_set(s, i, f, gain);   /* gain according to argument */
	do {	/* note: to better accomodate limited fft steepness, -delta is applied in f-calc ... */
		i++;
		f = equalizer_state_index2hz(s, i);
		gain = equalizer_compute_gainpoint(f-delta_f, freq_0, sqrt_gain, freq_bw);
		equalizer_point_set(s, i, f, gain);
	}
smorlat's avatar
smorlat committed
162
	while (i < s->nfft/2 && (gain>1.1 || gain<0.9));
163 164 165 166 167 168
	i = mid;
	do {	/* ... and here +delta, as to  */
		i--;
		f = equalizer_state_index2hz(s, i);
		gain = equalizer_compute_gainpoint(f+delta_f, freq_0, sqrt_gain, freq_bw);
		equalizer_point_set(s, i, f, gain);
169
	}
170
	while (i>=0 && (gain>1.1 || gain<0.9));
smorlat's avatar
smorlat committed
171
	s->needs_update=TRUE;
172
}
smorlat's avatar
smorlat committed
173

smorlat's avatar
smorlat committed
174
static void dump_table(ms_word16_t *t, int len){
smorlat's avatar
smorlat committed
175 176
	int i;
	for(i=0;i<len;i++)
smorlat's avatar
smorlat committed
177
#ifdef MS_FIXED_POINT
smorlat's avatar
smorlat committed
178
		ms_message("[%i]\t%i",i,t[i]);
smorlat's avatar
smorlat committed
179 180 181
#else
		ms_message("[%i]\t%f",i,t[i]);
#endif
smorlat's avatar
smorlat committed
182 183
}

smorlat's avatar
smorlat committed
184 185 186 187 188 189 190 191 192 193 194
static void time_shift(ms_word16_t *s, int len){
	int i;
	int half=len/2;
	ms_word16_t tmp;
	for (i=0;i<half;++i){
		tmp=s[i];
		s[i]=s[i+half];
		s[i+half]=tmp;
	}
}

smorlat's avatar
smorlat committed
195 196 197 198 199 200 201 202
/*
 *hamming:
 * 0.54 - 0.46*cos(2*M_PI*t/T)
 *
 * blackman
 * 0.42 - 0.5*cos(2*M_PI*t/T) + 0.08*cos(4*M_PI*t/T)
*/

smorlat's avatar
smorlat committed
203
static void norm_and_apodize(ms_word16_t *s, int len){
smorlat's avatar
smorlat committed
204 205
	int i;
	float x;
smorlat's avatar
smorlat committed
206
	float w;
smorlat's avatar
smorlat committed
207
	for(i=0;i<len;++i){
smorlat's avatar
smorlat committed
208
		x=(float)i*2*M_PI/(float)len;
209 210
		w=0.54 - (0.46*cos(x));
		//w=0.42 - (0.5*cos(x)) + (0.08*cos(2*x));
smorlat's avatar
smorlat committed
211
		s[i]=w*(float)s[i];
212
	}
smorlat's avatar
smorlat committed
213 214
}

smorlat's avatar
smorlat committed
215
static void equalizer_state_compute_impulse_response(EqualizerState *s){
smorlat's avatar
smorlat committed
216
	void *fft_handle=ms_fft_init(s->nfft);
smorlat's avatar
smorlat committed
217 218
	ms_message("Spectral domain:");
	dump_table(s->fft_cpx,s->nfft);
smorlat's avatar
smorlat committed
219
	ms_ifft(fft_handle,s->fft_cpx,s->fir);
smorlat's avatar
smorlat committed
220
	ms_fft_destroy(fft_handle);
221
	/*
smorlat's avatar
smorlat committed
222 223
	ms_message("Inverse fft result:");
	dump_table(s->fir,s->fir_len);
224
	*/
smorlat's avatar
smorlat committed
225
	time_shift(s->fir,s->fir_len);
226
	/*
smorlat's avatar
smorlat committed
227 228
	ms_message("Time shifted:");
	dump_table(s->fir,s->fir_len);
229
	*/
smorlat's avatar
smorlat committed
230
	norm_and_apodize(s->fir,s->fir_len);
smorlat's avatar
smorlat committed
231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260
	ms_message("Apodized impulse response:");
	dump_table(s->fir,s->fir_len);
	s->needs_update=FALSE;
}



#ifdef MS_FIXED_POINT
#define INT16_TO_WORD16(i,w,l) w=(i)
#define WORD16_TO_INT16(i,w,l) i=(w)
#else

static void int16_to_word16(const int16_t *is, ms_word16_t *w, int l){
	int i;
	for(i=0;i<l;++i){
		w[i]=(ms_word16_t)is[i];
	}
}

static void word16_to_int16(const ms_word16_t *w, int16_t *is, int l){
	int i;
	for (i=0;i<l;++i)
		is[i]=(int16_t)w[i];
}

#define INT16_TO_WORD16(i,w,l) w=(ms_word16_t*)alloca(sizeof(ms_word16_t)*(l));int16_to_word16(i,w,l)
#define WORD16_TO_INT16(w,i,l) word16_to_int16(w,i,l)
#endif

static void equalizer_state_run(EqualizerState *s, int16_t *samples, int nsamples){
unknown's avatar
unknown committed
261
	ms_word16_t *w;
smorlat's avatar
smorlat committed
262 263 264 265 266 267 268 269
	if (s->needs_update)
		equalizer_state_compute_impulse_response(s);
	INT16_TO_WORD16(samples,w,nsamples);
	ms_fir_mem16(w,s->fir,w,nsamples,s->fir_len,s->mem);
	WORD16_TO_INT16(w,samples,nsamples);
}


270
static void equalizer_init(MSFilter *f){
271
	f->data=equalizer_state_new(EQUALIZER_DEFAULT_RATE);
smorlat's avatar
smorlat committed
272 273
}

274
static void equalizer_uninit(MSFilter *f){
smorlat's avatar
smorlat committed
275 276 277
	equalizer_state_destroy((EqualizerState*)f->data);
}

278
static void equalizer_process(MSFilter *f){
smorlat's avatar
smorlat committed
279 280 281
	mblk_t *m;
	EqualizerState *s=(EqualizerState*)f->data;
	while((m=ms_queue_get(f->inputs[0]))!=NULL){
smorlat's avatar
smorlat committed
282 283 284
		if (s->active){
			equalizer_state_run(s,(int16_t*)m->b_rptr,(m->b_wptr-m->b_rptr)/2);
		}
smorlat's avatar
smorlat committed
285 286 287 288
		ms_queue_put(f->outputs[0],m);
	}
}

289
static int equalizer_set_gain(MSFilter *f, void *data){
smorlat's avatar
smorlat committed
290 291
	EqualizerState *s=(EqualizerState*)f->data;
	MSEqualizerGain *d=(MSEqualizerGain*)data;
292
	equalizer_state_set(s,d->frequency,d->gain,d->width);
smorlat's avatar
smorlat committed
293 294 295
	return 0;
}

296
static int equalizer_get_gain(MSFilter *f, void *data){
smorlat's avatar
smorlat committed
297 298 299
	EqualizerState *s=(EqualizerState*)f->data;
	MSEqualizerGain *d=(MSEqualizerGain*)data;
	d->gain=equalizer_state_get(s,d->frequency);
300
	d->width=0;
smorlat's avatar
smorlat committed
301 302 303
	return 0;
}

304
static int equalizer_set_rate(MSFilter *f, void *data){
smorlat's avatar
smorlat committed
305
	EqualizerState *s=(EqualizerState*)f->data;
306
	equalizer_rate_update(s,*(int*)data);
smorlat's avatar
smorlat committed
307
	return 0;
smorlat's avatar
smorlat committed
308 309
}

310
static int equalizer_set_active(MSFilter *f, void *data){
smorlat's avatar
smorlat committed
311 312 313 314 315
	EqualizerState *s=(EqualizerState*)f->data;
	s->active=*(int*)data;
	return 0;
}

316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334
static int equalizer_dump(MSFilter *f, void *data){
	EqualizerState *s=(EqualizerState*)f->data;
	float *t=(float*)data;
	int i;
	*t=s->fft_cpx[0];
	t++;
	for (i=1;i<s->nfft;i+=2){
		*t=((float)s->fft_cpx[i]*(float)s->nfft)/(float)GAIN_ZERODB;
		t++;
	}
	return 0;
}

static int equalizer_get_nfreqs(MSFilter *f, void *data){
	EqualizerState *s=(EqualizerState*)f->data;
	*(int*)data=s->nfft/2;
	return 0;
}

smorlat's avatar
smorlat committed
335 336 337
static MSFilterMethod equalizer_methods[]={
	{	MS_EQUALIZER_SET_GAIN		,	equalizer_set_gain	},
	{	MS_EQUALIZER_GET_GAIN		,	equalizer_get_gain	},
smorlat's avatar
smorlat committed
338
	{	MS_EQUALIZER_SET_ACTIVE		,	equalizer_set_active	},
smorlat's avatar
smorlat committed
339
	{	MS_FILTER_SET_SAMPLE_RATE	,	equalizer_set_rate	},
340 341
	{	MS_EQUALIZER_DUMP_STATE		,	equalizer_dump		},
	{	MS_EQUALIZER_GET_NUM_FREQUENCIES,	equalizer_get_nfreqs	},
smorlat's avatar
smorlat committed
342 343 344
	{	0				,	NULL			}
};

345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
#ifdef _MSC_VER

MSFilterDesc ms_equalizer_desc={
	MS_EQUALIZER_ID,
	"MSEqualizer",
	N_("Parametric sound equalizer."),
	MS_FILTER_OTHER,
	NULL,
	1,
	1,
	equalizer_init,
	NULL,
	equalizer_process,
	NULL,
	equalizer_uninit,
	equalizer_methods
};

#else

smorlat's avatar
smorlat committed
365 366 367 368 369 370 371 372 373 374 375 376 377
MSFilterDesc ms_equalizer_desc={
	.id= MS_EQUALIZER_ID,
	.name="MSEqualizer",
	.text=N_("Parametric sound equalizer."),
	.category=MS_FILTER_OTHER,
	.ninputs=1,
	.noutputs=1,
	.init=equalizer_init,
	.process=equalizer_process,
	.uninit=equalizer_uninit,
	.methods=equalizer_methods
};

378 379
#endif

smorlat's avatar
smorlat committed
380
MS_FILTER_DESC_EXPORT(ms_equalizer_desc)