speexec.c 12.1 KB
Newer Older
aymeric's avatar
aymeric committed
1 2
/*
mediastreamer2 library - modular sound and video processing and streaming
3 4
Copyright (C) 2010  Belledonne Communications SARL 
Author: Simon Morlat <simon.morlat@linphone.org>
aymeric's avatar
aymeric committed
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20

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.
*/

Simon Morlat's avatar
Simon Morlat committed
21 22 23 24
#if defined(HAVE_CONFIG_H)
#include "mediastreamer-config.h"
#endif

25
#include "mediastreamer2/msfilter.h"
26
#include "mediastreamer2/msticker.h"
aymeric's avatar
aymeric committed
27 28
#include <speex/speex_echo.h>
#include <speex/speex_preprocess.h>
29
#include "ortp/b64.h"
aymeric's avatar
aymeric committed
30 31 32 33 34 35 36 37 38

#ifdef HAVE_CONFIG_H
#include "mediastreamer-config.h"
#endif

#ifdef WIN32
#include <malloc.h> /* for alloca */
#endif

39
#include "mediastreamer2/flowcontrol.h"
40 41


42
//#define EC_DUMP 1
43
#ifdef ANDROID
44
#define EC_DUMP_PREFIX "/sdcard"
45 46 47
#else
#define EC_DUMP_PREFIX "/dynamic/tests"
#endif
48

49
static const int framesize=64;
50
static const int flow_control_interval_ms=5000;
51

aymeric's avatar
aymeric committed
52 53 54

typedef struct SpeexECState{
	SpeexEchoState *ecstate;
55 56
	SpeexPreprocessState *den;
	MSBufferizer delayed_ref;
57
	MSBufferizer ref;
58
	MSBufferizer echo;
aymeric's avatar
aymeric committed
59
	int framesize;
60
	int framesize_at_8000;
aymeric's avatar
aymeric committed
61 62
	int filterlength;
	int samplerate;
63 64
	int delay_ms;
	int tail_length_ms;
65 66
	int nominal_ref_samples;
	int min_ref_samples;
67
	MSAudioFlowController afc;
68
	uint64_t flow_control_time;
69
	char *state_str;
70 71 72
#ifdef EC_DUMP
	FILE *echofile;
	FILE *reffile;
73
	FILE *cleanfile;
74
#endif
75
	bool_t echostarted;
jehan's avatar
jehan committed
76
	bool_t bypass_mode;
77
	bool_t using_zeroes;
aymeric's avatar
aymeric committed
78 79 80
}SpeexECState;

static void speex_ec_init(MSFilter *f){
81
	SpeexECState *s=ms_new0(SpeexECState,1);
aymeric's avatar
aymeric committed
82 83

	s->samplerate=8000;
84 85
	ms_bufferizer_init(&s->delayed_ref);
	ms_bufferizer_init(&s->echo);
86
	ms_bufferizer_init(&s->ref);
87
	s->delay_ms=0;
88
	s->tail_length_ms=250;
89
	s->ecstate=NULL;
90
	s->framesize_at_8000=framesize;
91
	s->den = NULL;
92
	s->state_str=NULL;
93
	s->using_zeroes=FALSE;
94
	s->echostarted=FALSE;
jehan's avatar
jehan committed
95
	s->bypass_mode=FALSE;
aymeric's avatar
aymeric committed
96

97 98 99 100 101 102 103 104
#ifdef EC_DUMP
	{
		char *fname=ms_strdup_printf("%s/msspeexec-%p-echo.raw", EC_DUMP_PREFIX,f);
		s->echofile=fopen(fname,"w");
		ms_free(fname);
		fname=ms_strdup_printf("%s/msspeexec-%p-ref.raw", EC_DUMP_PREFIX,f);
		s->reffile=fopen(fname,"w");
		ms_free(fname);
105 106 107
		fname=ms_strdup_printf("%s/msspeexec-%p-clean.raw", EC_DUMP_PREFIX,f);
		s->cleanfile=fopen(fname,"w");
		ms_free(fname);
108 109 110
	}
#endif
	
aymeric's avatar
aymeric committed
111 112 113 114 115
	f->data=s;
}

static void speex_ec_uninit(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
116
	if (s->state_str) ms_free(s->state_str);
117
	ms_bufferizer_uninit(&s->delayed_ref);
118 119 120 121 122 123
#ifdef EC_DUMP
	if (s->echofile)
		fclose(s->echofile);
	if (s->reffile)
		fclose(s->reffile);
#endif
aymeric's avatar
aymeric committed
124 125 126
	ms_free(s);
}

127 128 129 130 131 132 133 134 135 136 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 162 163 164 165 166 167 168 169 170 171 172 173 174 175
#ifdef SPEEX_ECHO_GET_BLOB

static void apply_config(SpeexECState *s){
	if (s->state_str!=NULL){
		size_t buflen=strlen(s->state_str);
		uint8_t *buffer=alloca(buflen);
		SpeexEchoStateBlob *blob;
		if ((buflen=b64_decode(s->state_str,strlen(s->state_str),buffer,buflen))<=0){
			ms_error("Could not decode base64 %s",s->state_str);
			return;
		}
		blob=speex_echo_state_blob_new_from_memory(buffer,buflen);
		if (blob==NULL){
			ms_error("Could not create blob from config string");
			return;
		}
		if (speex_echo_ctl(s->ecstate, SPEEX_ECHO_SET_BLOB, blob)!=0){
			ms_error("Could not apply speex echo blob !");
		}
		speex_echo_state_blob_free(blob);
		ms_message("speex echo state restored.");
	}	
}

static void fetch_config(SpeexECState *s){
	SpeexEchoStateBlob *blob=NULL;
	char *txt;
	size_t txt_len;

	if (s->ecstate==NULL) return;
	
	if (speex_echo_ctl(s->ecstate, SPEEX_ECHO_GET_BLOB, &blob)!=0){
		ms_error("Could not retrieve speex echo blob !");
		return;
	}
	txt_len=(speex_echo_state_blob_get_size(blob)*4)+1;
	txt=ms_malloc0(txt_len);
	if (b64_encode(speex_echo_state_blob_get_data(blob),speex_echo_state_blob_get_size(blob),
			txt,txt_len)==0){
		ms_error("Base64 encoding failed.");
		ms_free(txt);
		return;
	}
	speex_echo_state_blob_free(blob);
	if (s->state_str) ms_free(s->state_str);
	s->state_str=txt;
}

#endif
176

177 178 179 180 181 182 183 184 185 186 187
static int adjust_framesize(int framesize, int samplerate){
	int newsize=(framesize*samplerate)/8000;
	int n=1;
	int next;
	
	while((next=n<<1)<=newsize){
		n=next;
	}
	return n;
}

188 189
static void speex_ec_preprocess(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
190 191
	int delay_samples=0;
	mblk_t *m;
192

193
	s->echostarted=FALSE;
194
	s->filterlength=(s->tail_length_ms*s->samplerate)/1000;
195
	s->framesize=adjust_framesize(s->framesize_at_8000,s->samplerate);
196 197 198
	delay_samples=s->delay_ms*s->samplerate/1000;
	ms_message("Initializing speex echo canceler with framesize=%i, filterlength=%i, delay_samples=%i",
		s->framesize,s->filterlength,delay_samples);
199
	
200 201 202 203
	s->ecstate=speex_echo_state_init(s->framesize,s->filterlength);
	s->den = speex_preprocess_state_init(s->framesize, s->samplerate);
	speex_echo_ctl(s->ecstate, SPEEX_ECHO_SET_SAMPLING_RATE, &s->samplerate);
	speex_preprocess_ctl(s->den, SPEEX_PREPROCESS_SET_ECHO_STATE, s->ecstate);
204 205
	/* fill with zeroes for the time of the delay*/
	m=allocb(delay_samples*2,0);
206
	m->b_wptr+=delay_samples*2;
207
	ms_bufferizer_put (&s->delayed_ref,m);
208
	s->min_ref_samples=-1;
209
	s->nominal_ref_samples=delay_samples;
210
	ms_audio_flow_controller_init(&s->afc);
211
	s->flow_control_time = f->ticker->time;
212 213 214 215 216
#ifdef SPEEX_ECHO_GET_BLOB
	apply_config(s);
#else
	if (s->state_str) ms_warning("This version of speex doesn't support echo canceller restoration state. Rebuild speex and mediatreamer2 if you want to use this feature.");
#endif
217 218
}

219
/*	inputs[0]= reference signal from far end (sent to soundcard)
220
 *	inputs[1]= near speech & echo signal	(read from soundcard)
221
 *	outputs[0]=  is a copy of inputs[0] to be sent to soundcard
222
 *	outputs[1]=  near end speech, echo removed - towards far end
aymeric's avatar
aymeric committed
223 224 225 226
*/
static void speex_ec_process(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
	int nbytes=s->framesize*2;
227 228 229
	mblk_t *refm;
	uint8_t *ref,*echo;
	
jehan's avatar
jehan committed
230 231 232 233 234 235 236
	if (s->bypass_mode) {
		while((refm=ms_queue_get(f->inputs[0]))!=NULL){
			ms_queue_put(f->outputs[0],refm);
		}
		while((refm=ms_queue_get(f->inputs[1]))!=NULL){
			ms_queue_put(f->outputs[1],refm);
		}
237
		return;
jehan's avatar
jehan committed
238
	}
239 240
	
	if (f->inputs[0]!=NULL){
Simon Morlat's avatar
Simon Morlat committed
241 242
		if (s->echostarted){
			while((refm=ms_queue_get(f->inputs[0]))!=NULL){
243
				refm=ms_audio_flow_controller_process(&s->afc,refm);
244 245 246 247 248
				if (refm){
					mblk_t *cp=dupmsg(refm);
					ms_bufferizer_put(&s->delayed_ref,cp);
					ms_bufferizer_put(&s->ref,refm);
				}
Simon Morlat's avatar
Simon Morlat committed
249 250 251 252
			}
		}else{
			ms_warning("Getting reference signal but no echo to synchronize on.");
			ms_queue_flush(f->inputs[0]);
253 254
		}
	}
255 256

	ms_bufferizer_put_from_queue(&s->echo,f->inputs[1]);
257
	
258
	ref=(uint8_t*)alloca(nbytes);
259
	echo=(uint8_t*)alloca(nbytes);
260
	while (ms_bufferizer_read(&s->echo,echo,nbytes)==nbytes){
261
		mblk_t *oecho=allocb(nbytes,0);
262
		int avail;
Simon Morlat's avatar
Simon Morlat committed
263
		int avail_samples;
Simon Morlat's avatar
Simon Morlat committed
264 265

		if (!s->echostarted) s->echostarted=TRUE;
Simon Morlat's avatar
Simon Morlat committed
266
		if ((avail=ms_bufferizer_get_avail(&s->delayed_ref))<((s->nominal_ref_samples*2)+nbytes)){
267
			/*we don't have enough to read in a reference signal buffer, inject silence instead*/
268
			avail=nbytes;
269 270 271 272
			refm=allocb(nbytes,0);
			memset(refm->b_wptr,0,nbytes);
			refm->b_wptr+=nbytes;
			ms_bufferizer_put(&s->delayed_ref,refm);
273
			ms_queue_put(f->outputs[0],dupmsg(refm));
274 275 276 277
			if (!s->using_zeroes){
				ms_warning("Not enough ref samples, using zeroes");
				s->using_zeroes=TRUE;
			}
278 279 280 281 282 283 284 285 286 287 288 289
		}else{
			if (s->using_zeroes){
				ms_message("Samples are back.");
				s->using_zeroes=FALSE;
			}
			/* read from our no-delay buffer and output */
			refm=allocb(nbytes,0);
			if (ms_bufferizer_read(&s->ref,refm->b_wptr,nbytes)==0){
				ms_fatal("Should never happen");
			}
			refm->b_wptr+=nbytes;
			ms_queue_put(f->outputs[0],refm);
290 291
		}

292
		/*now read a valid buffer of delayed ref samples*/
293
		if (ms_bufferizer_read(&s->delayed_ref,ref,nbytes)==0){
294 295 296
			ms_fatal("Should never happen");
		}
		avail-=nbytes;
Simon Morlat's avatar
Simon Morlat committed
297
		avail_samples=avail/2;
298
		/*ms_message("avail=%i",avail_samples);*/
Simon Morlat's avatar
Simon Morlat committed
299 300
		if (avail_samples<s->min_ref_samples || s->min_ref_samples==-1){
			s->min_ref_samples=avail_samples;
301
		}
302
		
303 304 305 306 307 308
#ifdef EC_DUMP
		if (s->reffile)
			fwrite(ref,nbytes,1,s->reffile);
		if (s->echofile)
			fwrite(echo,nbytes,1,s->echofile);
#endif
309 310
		speex_echo_cancellation(s->ecstate,(short*)echo,(short*)ref,(short*)oecho->b_wptr);
		speex_preprocess_run(s->den, (short*)oecho->b_wptr);
311 312 313 314
#ifdef EC_DUMP
		if (s->cleanfile)
			fwrite(oecho->b_wptr,nbytes,1,s->cleanfile);
#endif
315 316
		oecho->b_wptr+=nbytes;
		ms_queue_put(f->outputs[1],oecho);
aymeric's avatar
aymeric committed
317
	}
318
	
319
	/*verify our ref buffer does not become too big, meaning that we are receiving more samples than we are sending*/
320
	if ((((uint32_t)(f->ticker->time - s->flow_control_time)) >= flow_control_interval_ms) && (s->min_ref_samples != -1)) {
321
		int diff=s->min_ref_samples-s->nominal_ref_samples;
Simon Morlat's avatar
Simon Morlat committed
322 323
		if (diff>(nbytes/2)){
			int purge=diff-(nbytes/2);
324
			ms_warning("echo canceller: we are accumulating too much reference signal, need to throw out %i samples",purge);
325
			ms_audio_flow_controller_set_target(&s->afc,purge,(flow_control_interval_ms*s->samplerate)/1000);
326 327
		}
		s->min_ref_samples=-1;
328
		s->flow_control_time = f->ticker->time;
aymeric's avatar
aymeric committed
329 330 331
	}
}

aymeric's avatar
aymeric committed
332 333
static void speex_ec_postprocess(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
334

335 336
	ms_bufferizer_flush (&s->delayed_ref);
	ms_bufferizer_flush (&s->echo);
337
	ms_bufferizer_flush (&s->ref);
338
	if (s->ecstate!=NULL){
aymeric's avatar
aymeric committed
339
		speex_echo_state_destroy(s->ecstate);
340 341 342 343 344 345
		s->ecstate=NULL;
	}
	if (s->den!=NULL){
		speex_preprocess_state_destroy(s->den);
		s->den=NULL;
	}
aymeric's avatar
aymeric committed
346 347
}

aymeric's avatar
aymeric committed
348 349 350 351 352 353 354 355
static int speex_ec_set_sr(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
	s->samplerate = *(int*)arg;
	return 0;
}

static int speex_ec_set_framesize(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
356
	s->framesize_at_8000 = *(int*)arg;
aymeric's avatar
aymeric committed
357 358 359
	return 0;
}

360
static int speex_ec_set_delay(MSFilter *f, void *arg){
361 362 363 364 365
	SpeexECState *s=(SpeexECState*)f->data;
	s->delay_ms = *(int*)arg;
	return 0;
}

366
static int speex_ec_set_tail_length(MSFilter *f, void *arg){
367 368 369 370
	SpeexECState *s=(SpeexECState*)f->data;
	s->tail_length_ms=*(int*)arg;
	return 0;
}
jehan's avatar
jehan committed
371 372 373 374 375 376 377 378 379 380 381
static int speex_ec_set_bypass_mode(MSFilter *f, void *arg) {
	SpeexECState *s=(SpeexECState*)f->data;
	s->bypass_mode=*(bool_t*)arg;
	ms_message("set EC bypass mode to [%i]",s->bypass_mode);
	return 0;
}
static int speex_ec_get_bypass_mode(MSFilter *f, void *arg) {
	SpeexECState *s=(SpeexECState*)f->data;
	*(bool_t*)arg=s->bypass_mode;
	return 0;
}
aymeric's avatar
aymeric committed
382

383 384 385 386 387 388 389 390 391 392 393 394 395 396 397
static int speex_ec_set_state(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
	s->state_str=ms_strdup((const char*)arg);
	return 0;
}

static int speex_ec_get_state(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
#ifdef SPEEX_ECHO_GET_BLOB
	fetch_config(s);
#endif
	*(char**)arg=s->state_str;
	return 0;
}

aymeric's avatar
aymeric committed
398
static MSFilterMethod speex_ec_methods[]={
399
	{	MS_FILTER_SET_SAMPLE_RATE		,	speex_ec_set_sr 		},
400 401
	{	MS_ECHO_CANCELLER_SET_TAIL_LENGTH	,	speex_ec_set_tail_length	},
	{	MS_ECHO_CANCELLER_SET_DELAY		,	speex_ec_set_delay		},
402
	{	MS_ECHO_CANCELLER_SET_FRAMESIZE		,	speex_ec_set_framesize		},
403
	{	MS_ECHO_CANCELLER_SET_BYPASS_MODE	,	speex_ec_set_bypass_mode	},
404 405
	{	MS_ECHO_CANCELLER_GET_BYPASS_MODE	,	speex_ec_get_bypass_mode	},
	{	MS_ECHO_CANCELLER_GET_STATE_STRING	,	speex_ec_get_state		},
406 407
	{	MS_ECHO_CANCELLER_SET_STATE_STRING	,	speex_ec_set_state		},
	{	0, 0 }
aymeric's avatar
aymeric committed
408 409 410 411 412 413 414
};

#ifdef _MSC_VER

MSFilterDesc ms_speex_ec_desc={
	MS_SPEEX_EC_ID,
	"MSSpeexEC",
415
	N_("Echo canceller using speex library"),
aymeric's avatar
aymeric committed
416 417 418 419 420
	MS_FILTER_OTHER,
	NULL,
	2,
	2,
	speex_ec_init,
421
	speex_ec_preprocess,
aymeric's avatar
aymeric committed
422
	speex_ec_process,
aymeric's avatar
aymeric committed
423
	speex_ec_postprocess,
aymeric's avatar
aymeric committed
424 425 426 427 428 429 430 431 432
	speex_ec_uninit,
	speex_ec_methods
};

#else

MSFilterDesc ms_speex_ec_desc={
	.id=MS_SPEEX_EC_ID,
	.name="MSSpeexEC",
433
	.text=N_("Echo canceller using speex library"),
aymeric's avatar
aymeric committed
434 435 436 437
	.category=MS_FILTER_OTHER,
	.ninputs=2,
	.noutputs=2,
	.init=speex_ec_init,
438
	.preprocess=speex_ec_preprocess,
aymeric's avatar
aymeric committed
439
	.process=speex_ec_process,
smorlat's avatar
smorlat committed
440
	.postprocess=speex_ec_postprocess,
aymeric's avatar
aymeric committed
441 442 443 444 445 446 447
	.uninit=speex_ec_uninit,
	.methods=speex_ec_methods
};

#endif

MS_FILTER_DESC_EXPORT(ms_speex_ec_desc)
448