speexec.c 11.5 KB
Newer Older
aymeric's avatar
aymeric committed
1 2
/*
mediastreamer2 library - modular sound and video processing and streaming
3
Copyright (C) 2010  Belledonne Communications SARL
4
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

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
18
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
aymeric's avatar
aymeric committed
19 20
*/

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

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

35
#ifdef _WIN32
aymeric's avatar
aymeric committed
36 37 38
#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

aymeric's avatar
aymeric committed
51 52 53

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

static void speex_ec_init(MSFilter *f){
77
	SpeexECState *s=ms_new0(SpeexECState,1);
aymeric's avatar
aymeric committed
78 79

	s->samplerate=8000;
80 81
	ms_bufferizer_init(&s->delayed_ref);
	ms_bufferizer_init(&s->echo);
82
	ms_flow_controlled_bufferizer_init(&s->ref, f, s->samplerate, 1);
83
	s->delay_ms=0;
84
	s->tail_length_ms=250;
85
	s->ecstate=NULL;
86
	s->framesize_at_8000=framesize;
87
	s->den = NULL;
88
	s->state_str=NULL;
89
	s->using_zeroes=FALSE;
90
	s->echostarted=FALSE;
jehan's avatar
jehan committed
91
	s->bypass_mode=FALSE;
aymeric's avatar
aymeric committed
92

93 94 95 96 97 98 99 100
#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);
101 102 103
		fname=ms_strdup_printf("%s/msspeexec-%p-clean.raw", EC_DUMP_PREFIX,f);
		s->cleanfile=fopen(fname,"w");
		ms_free(fname);
104 105
	}
#endif
106

aymeric's avatar
aymeric committed
107 108 109 110 111
	f->data=s;
}

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

123 124 125 126 127 128 129 130 131 132 133
#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;
		}
134
		blob=speex_echo_state_blob_new_from_memory(buffer,(int)buflen);
135 136 137 138 139 140 141 142 143
		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.");
144
	}
145 146 147 148 149 150 151 152
}

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

	if (s->ecstate==NULL) return;
153

154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
	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
172

173 174 175 176
static int adjust_framesize(int framesize, int samplerate){
	int newsize=(framesize*samplerate)/8000;
	int n=1;
	int next;
177

178 179 180 181 182 183
	while((next=n<<1)<=newsize){
		n=next;
	}
	return n;
}

184 185
static void configure_flow_controlled_bufferizer(SpeexECState *s) {
	ms_flow_controlled_bufferizer_set_samplerate(&s->ref, s->samplerate);
186 187
	ms_flow_controlled_bufferizer_set_max_size_ms(&s->ref, s->delay_ms);
	ms_flow_controlled_bufferizer_set_granularity_ms(&s->ref, (s->framesize * 1000) / s->samplerate);
188 189
}

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

195
	s->echostarted=FALSE;
196
	s->filterlength=(s->tail_length_ms*s->samplerate)/1000;
197
	s->framesize=adjust_framesize(s->framesize_at_8000,s->samplerate);
198 199 200
	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);
201

202 203 204 205
	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);
206 207
	/* fill with zeroes for the time of the delay*/
	m=allocb(delay_samples*2,0);
208
	m->b_wptr+=delay_samples*2;
209
	ms_bufferizer_put (&s->delayed_ref,m);
210
	s->nominal_ref_samples=delay_samples;
211 212 213 214 215
#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
216 217
}

218
/*	inputs[0]= reference signal from far end (sent to soundcard)
219
 *	inputs[1]= near speech & echo signal	(read from soundcard)
220
 *	outputs[0]=  is a copy of inputs[0] to be sent to soundcard
221
 *	outputs[1]=  near end speech, echo removed - towards far end
aymeric's avatar
aymeric committed
222 223 224 225
*/
static void speex_ec_process(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
	int nbytes=s->framesize*2;
226 227
	mblk_t *refm;
	uint8_t *ref,*echo;
228

jehan's avatar
jehan committed
229 230 231 232 233 234 235
	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);
		}
236
		return;
jehan's avatar
jehan committed
237
	}
238

239
	if (f->inputs[0]!=NULL){
Simon Morlat's avatar
Simon Morlat committed
240 241
		if (s->echostarted){
			while((refm=ms_queue_get(f->inputs[0]))!=NULL){
242 243 244
				mblk_t *cp=dupmsg(refm);
				ms_bufferizer_put(&s->delayed_ref,cp);
				ms_flow_controlled_bufferizer_put(&s->ref,refm);
Simon Morlat's avatar
Simon Morlat committed
245 246 247 248
			}
		}else{
			ms_warning("Getting reference signal but no echo to synchronize on.");
			ms_queue_flush(f->inputs[0]);
249 250
		}
	}
251 252

	ms_bufferizer_put_from_queue(&s->echo,f->inputs[1]);
253

254
	ref=(uint8_t*)alloca(nbytes);
255
	echo=(uint8_t*)alloca(nbytes);
256
	while ((int)ms_bufferizer_read(&s->echo,echo,nbytes)==nbytes){
257
		mblk_t *oecho=allocb(nbytes,0);
258
		int avail;
Simon Morlat's avatar
Simon Morlat committed
259 260

		if (!s->echostarted) s->echostarted=TRUE;
261
		if ((avail=(int)ms_bufferizer_get_avail(&s->delayed_ref))<((s->nominal_ref_samples*2)+nbytes)){
262
			/*we don't have enough to read in a reference signal buffer, inject silence instead*/
263
			avail=nbytes;
264 265 266 267
			refm=allocb(nbytes,0);
			memset(refm->b_wptr,0,nbytes);
			refm->b_wptr+=nbytes;
			ms_bufferizer_put(&s->delayed_ref,refm);
268
			ms_queue_put(f->outputs[0],dupmsg(refm));
269 270 271 272
			if (!s->using_zeroes){
				ms_warning("Not enough ref samples, using zeroes");
				s->using_zeroes=TRUE;
			}
273 274 275 276 277 278 279
		}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);
280
			if (ms_flow_controlled_bufferizer_read(&s->ref,refm->b_wptr,nbytes)==0){
281 282 283 284
				ms_fatal("Should never happen");
			}
			refm->b_wptr+=nbytes;
			ms_queue_put(f->outputs[0],refm);
285 286
		}

287
		/*now read a valid buffer of delayed ref samples*/
288
		if (ms_bufferizer_read(&s->delayed_ref,ref,nbytes)==0){
289 290 291
			ms_fatal("Should never happen");
		}
		avail-=nbytes;
292

293 294 295 296 297 298
#ifdef EC_DUMP
		if (s->reffile)
			fwrite(ref,nbytes,1,s->reffile);
		if (s->echofile)
			fwrite(echo,nbytes,1,s->echofile);
#endif
299 300
		speex_echo_cancellation(s->ecstate,(short*)echo,(short*)ref,(short*)oecho->b_wptr);
		speex_preprocess_run(s->den, (short*)oecho->b_wptr);
301 302 303 304
#ifdef EC_DUMP
		if (s->cleanfile)
			fwrite(oecho->b_wptr,nbytes,1,s->cleanfile);
#endif
305 306
		oecho->b_wptr+=nbytes;
		ms_queue_put(f->outputs[1],oecho);
aymeric's avatar
aymeric committed
307 308 309
	}
}

aymeric's avatar
aymeric committed
310 311
static void speex_ec_postprocess(MSFilter *f){
	SpeexECState *s=(SpeexECState*)f->data;
312

313 314
	ms_bufferizer_flush (&s->delayed_ref);
	ms_bufferizer_flush (&s->echo);
315
	ms_flow_controlled_bufferizer_flush (&s->ref);
316
	if (s->ecstate!=NULL){
aymeric's avatar
aymeric committed
317
		speex_echo_state_destroy(s->ecstate);
318 319 320 321 322 323
		s->ecstate=NULL;
	}
	if (s->den!=NULL){
		speex_preprocess_state_destroy(s->den);
		s->den=NULL;
	}
aymeric's avatar
aymeric committed
324 325
}

aymeric's avatar
aymeric committed
326 327 328
static int speex_ec_set_sr(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
	s->samplerate = *(int*)arg;
329
	configure_flow_controlled_bufferizer(s);
aymeric's avatar
aymeric committed
330 331 332 333 334
	return 0;
}

static int speex_ec_set_framesize(MSFilter *f, void *arg){
	SpeexECState *s=(SpeexECState*)f->data;
335
	s->framesize_at_8000 = *(int*)arg;
aymeric's avatar
aymeric committed
336 337 338
	return 0;
}

339
static int speex_ec_set_delay(MSFilter *f, void *arg){
340 341
	SpeexECState *s=(SpeexECState*)f->data;
	s->delay_ms = *(int*)arg;
342
	configure_flow_controlled_bufferizer(s);
343 344 345
	return 0;
}

346
static int speex_ec_set_tail_length(MSFilter *f, void *arg){
347 348
	SpeexECState *s=(SpeexECState*)f->data;
	s->tail_length_ms=*(int*)arg;
349
	configure_flow_controlled_bufferizer(s);
350 351
	return 0;
}
jehan's avatar
jehan committed
352 353 354 355 356 357 358 359 360 361 362
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
363

364 365 366 367 368 369 370 371 372 373 374 375 376 377 378
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
379
static MSFilterMethod speex_ec_methods[]={
380
	{	MS_FILTER_SET_SAMPLE_RATE		,	speex_ec_set_sr 		},
381 382
	{	MS_ECHO_CANCELLER_SET_TAIL_LENGTH	,	speex_ec_set_tail_length	},
	{	MS_ECHO_CANCELLER_SET_DELAY		,	speex_ec_set_delay		},
383
	{	MS_ECHO_CANCELLER_SET_FRAMESIZE		,	speex_ec_set_framesize		},
384
	{	MS_ECHO_CANCELLER_SET_BYPASS_MODE	,	speex_ec_set_bypass_mode	},
385 386
	{	MS_ECHO_CANCELLER_GET_BYPASS_MODE	,	speex_ec_get_bypass_mode	},
	{	MS_ECHO_CANCELLER_GET_STATE_STRING	,	speex_ec_get_state		},
387 388
	{	MS_ECHO_CANCELLER_SET_STATE_STRING	,	speex_ec_set_state		},
	{	0, 0 }
aymeric's avatar
aymeric committed
389 390 391 392 393 394 395
};

#ifdef _MSC_VER

MSFilterDesc ms_speex_ec_desc={
	MS_SPEEX_EC_ID,
	"MSSpeexEC",
396
	N_("Echo canceller using speex library"),
aymeric's avatar
aymeric committed
397 398 399 400 401
	MS_FILTER_OTHER,
	NULL,
	2,
	2,
	speex_ec_init,
402
	speex_ec_preprocess,
aymeric's avatar
aymeric committed
403
	speex_ec_process,
aymeric's avatar
aymeric committed
404
	speex_ec_postprocess,
aymeric's avatar
aymeric committed
405 406 407 408 409 410 411 412 413
	speex_ec_uninit,
	speex_ec_methods
};

#else

MSFilterDesc ms_speex_ec_desc={
	.id=MS_SPEEX_EC_ID,
	.name="MSSpeexEC",
414
	.text=N_("Echo canceller using speex library"),
aymeric's avatar
aymeric committed
415 416 417 418
	.category=MS_FILTER_OTHER,
	.ninputs=2,
	.noutputs=2,
	.init=speex_ec_init,
419
	.preprocess=speex_ec_preprocess,
aymeric's avatar
aymeric committed
420
	.process=speex_ec_process,
smorlat's avatar
smorlat committed
421
	.postprocess=speex_ec_postprocess,
aymeric's avatar
aymeric committed
422 423 424 425 426 427 428
	.uninit=speex_ec_uninit,
	.methods=speex_ec_methods
};

#endif

MS_FILTER_DESC_EXPORT(ms_speex_ec_desc)