Merged with master and added proper RTP time stamp handling

5 files changed, 109 insertions, 86 deletions

diff --git a/modules/mpa/decode.c b/modules/mpa/decode.c
index 43adfaa..b71a335 100644
--- a/modules/mpa/decode.c
+++ b/modules/mpa/decode.c
@@ -24,10 +24,13 @@ static void destructor(void *arg)
 {
 	struct audec_state *ads = arg;
 
+	if (ads->resampler)
+		speex_resampler_destroy(ads->resampler);
+
 	mpg123_close(ads->dec);
 	mpg123_delete(ads->dec);
 #ifdef DEBUG
-	debug("mpa: decoder destroyed\n");
+	debug("MPA dec destroyed\n");
 #endif
 }
 
@@ -45,7 +48,7 @@ int mpa_decode_update(struct audec_state **adsp, const struct aucodec *ac,
 	ads = *adsp;
 
 #ifdef DEBUG
-	debug("mpa: decoder created %s\n",fmtp);
+	debug("MPA dec created %s\n",fmtp);
 #endif
 
 	if (ads)
@@ -60,7 +63,7 @@ int mpa_decode_update(struct audec_state **adsp, const struct aucodec *ac,
 
 	ads->dec = mpg123_new(NULL,&result);
 	if (!ads->dec) {
-		error("mpa: decoder create: %s\n",
+		error("MPA dec create: %s\n",
 			mpg123_plain_strerror(result));
 		err = ENOMEM;
 		goto out;
@@ -72,7 +75,7 @@ int mpa_decode_update(struct audec_state **adsp, const struct aucodec *ac,
 	result = mpg123_param(ads->dec, MPG123_VERBOSE, 0, 0.);
 #endif
 	if (result != MPG123_OK) {
-		error("MPA libmpg123 param error %s",
+		error("MPA dec param error %s\n",
 			mpg123_plain_strerror(result));
 		err = EINVAL;
 		goto out;
@@ -81,7 +84,7 @@ int mpa_decode_update(struct audec_state **adsp, const struct aucodec *ac,
 
 	result = mpg123_format_all(ads->dec);
 	if (result != MPG123_OK) {
-		error("MPA libmpg123 format error %s",
+		error("MPA dec format error %s\n",
 			mpg123_plain_strerror(result));
 		err = EINVAL;
 		goto out;
@@ -89,7 +92,7 @@ int mpa_decode_update(struct audec_state **adsp, const struct aucodec *ac,
 
 	result = mpg123_open_feed(ads->dec);
 	if (result != MPG123_OK) {
-		error("MPA libmpg123 open feed error %s",
+		error("MPA dec open feed error %s\n",
 			mpg123_plain_strerror(result));
 		err = EINVAL;
 		goto out;
@@ -114,72 +117,44 @@ int mpa_decode_frm(struct audec_state *ads, int16_t *sampv, size_t *sampc,
 	size_t n;
 	spx_uint32_t intermediate_len;
 	spx_uint32_t out_len;
-	uint32_t header;
+
+#ifdef DEBUG
+	debug("MPA dec start %d %ld\n",len, *sampc);
+#endif
 
 	if (!ads || !sampv || !sampc || !buf || len<=4)
 		return EINVAL;
 
-	header = *(uint32_t*)(void *)buf;
-	if (header != 0) {
-		error("MPA header is not zero %08X, not supported yet\n",
-		      header);
+	if (*(uint32_t*)buf != 0) {
+		error("MPA dec header is not zero %08X, not supported yet\n",
+			*(uint32_t*)buf);
 		return EPROTO;
 	}
 
-	if (ads->resampler)  {
-		result = mpg123_decode(ads->dec, buf+4, len-4,
+	n = 0;
+	result = mpg123_decode(ads->dec, buf+4, len-4,
 				(unsigned char*)ads->intermediate_buffer,
 				sizeof(ads->intermediate_buffer), &n);
 				/* n counts bytes */
-		intermediate_len = (uint32_t)(n / 2 / ads->channels);
-			/* intermediate_len counts samples per channel */
-		out_len = (uint32_t)*sampc;
-		result=speex_resampler_process_interleaved_int(
-			ads->resampler, ads->intermediate_buffer,
-			&intermediate_len, sampv, &out_len);
-		if (result!=RESAMPLER_ERR_SUCCESS) {
-			error("mpa: upsample error: %s %d %d\n",
-				strerror(result), out_len, *sampc/2);
-			return EPROTO;
-		}
-#ifdef DEBUG
-		info("mpa decode %d %d %d %d\n",intermediate_len,*sampc,
-			out_len,n);
-#endif
-		*sampc = out_len * ads->channels;
-	}
-	else {
-		result = mpg123_decode(ads->dec, buf+4, len-4,
-				(unsigned char*)sampv, *sampc*2, &n);
 #ifdef DEBUG
-		info("mpa decode %d %d\n",*sampc,n);
+	debug("MPA dec %d %d %d %d\n",result, len-4, n, ads->channels);
 #endif
-		*sampc = n / 2;
-	}
-
-	if (ads->start<100) {	/* mpg123 needs some to sync */
-		ads->start++;
-		*sampc=0;
-	}
-	if (ads->channels==1) {
-		for (i=(int)(*sampc-1); i>=0; i--)
-			sampv[i+i+1]=sampv[i+i]=sampv[i];
-		*sampc *= 2;
-	}
 
 	if (result == MPG123_NEW_FORMAT) {
 		mpg123_getformat(ads->dec, &samplerate, &channels, &encoding);
-		info("MPA libmpg123 format change %d %d %04X\n",samplerate
+		info("MPA dec format change %d %d %04X\n",samplerate
 			,channels,encoding);
 
 		ads->channels = channels;
 		ads->start = 0;
-		if (samplerate != 48000) {
+		if (ads->resampler)
+			speex_resampler_destroy(ads->resampler);
+		if (samplerate != MPA_IORATE) {
 			ads->resampler = speex_resampler_init(channels,
-				      (uint32_t)samplerate, 48000, 3, &result);
+				      (uint32_t)samplerate, MPA_IORATE, 3, &result);
 			if (result!=RESAMPLER_ERR_SUCCESS
 				|| ads->resampler==NULL) {
-				error("mpa: upsampler failed %d\n",result);
+				error("MPA dec upsampler failed %d\n",result);
 				return EINVAL;
 			}
 		}
@@ -187,16 +162,53 @@ int mpa_decode_frm(struct audec_state *ads, int16_t *sampv, size_t *sampc,
 			ads->resampler = NULL;
 	}
 	else if (result == MPG123_NEED_MORE)
-		return 0;
+		;			/* workaround: do nothing */
 	else if (result != MPG123_OK) {
-		error("MPA libmpg123 feed error %d %s", result,
+		error("MPA dec feed error %d %s\n", result,
 			mpg123_plain_strerror(result));
 		return EPROTO;
 	}
 
+	if (ads->resampler)  {
+		intermediate_len = n / 2 / ads->channels;
+			/* intermediate_len counts samples per channel */
+		out_len = *sampc / 2;
+
+		result=speex_resampler_process_interleaved_int(
+			ads->resampler, ads->intermediate_buffer,
+			&intermediate_len, sampv, &out_len);
+		if (result!=RESAMPLER_ERR_SUCCESS) {
+			error("MPA dec upsample error: %s %d %d\n",
+				strerror(result), out_len, *sampc/2);
+			return EPROTO;
+		}
+		if (ads->channels==1) {
+			for (i=out_len-1;i>=0;i--)
+				sampv[i+i+1]=sampv[i+i]=sampv[i];
+			*sampc = out_len * 2;
+		}
+		else
+			*sampc = out_len * ads->channels;
+	}
+	else {
+		n /= 2;
+		if (ads->channels!=1) {
+			for (i=0;(unsigned)i<n;i++)
+				sampv[i]=ads->intermediate_buffer[i];
+			*sampc = n;
+		}
+		else {
+			for (i=0;(unsigned)i<n;i++)
+				sampv[i*2]=sampv[i*2+1]=
+					ads->intermediate_buffer[i];
+			*sampc = n * 2;
+	}
+
 #ifdef DEBUG
-	debug("mpa decode %d %d %d\n",*sampc,len,n);
+	debug("MPA dec done %d\n",*sampc);
 #endif
+	}
+
 	return 0;
 }
 
diff --git a/modules/mpa/encode.c b/modules/mpa/encode.c
index 85e8dd9..0715d67 100644
--- a/modules/mpa/encode.c
+++ b/modules/mpa/encode.c
@@ -13,9 +13,10 @@
 
 struct auenc_state {
 	twolame_options *enc;
-	int channels;
+	int channels, samplerate;
 	SpeexResamplerState *resampler;
 	int16_t intermediate_buffer[BARESIP_FRAMESIZE];
+	uint32_t timestamp;
 };
 
 
@@ -23,10 +24,15 @@ static void destructor(void *arg)
 {
 	struct auenc_state *aes = arg;
 
+	if (aes->resampler) {
+		speex_resampler_destroy(aes->resampler);
+		aes->resampler = NULL;
+	}
+
 	if (aes->enc)
 		twolame_close(&aes->enc);
 #ifdef DEBUG
-	debug("mpa: encoder destroyed\n");
+	debug("MPA enc destroyed\n");
 #endif
 }
 
@@ -44,27 +50,28 @@ int mpa_encode_update(struct auenc_state **aesp, const struct aucodec *ac,
 
 	aes = *aesp;
 	if (aes) {
-		info("ever?");
 		mem_deref(aes);
 	}
 
 	aes = mem_zalloc(sizeof(*aes), destructor);
 	aes->enc = twolame_init();
 	if (!aes->enc) {
-		error("mpa: encoder create failed");
+		error("MPA enc create failed\n");
 		mem_deref(aes);
 		return ENOMEM;
 	}
-	aes->channels = ac->ch;
 #ifdef DEBUG
-	debug("mpa: encoder created %s\n",fmtp);
+	debug("MPA enc created %s\n",fmtp);
 #endif
+	aes->channels = ac->ch;
+	aes->timestamp = rand_u32();
 
-	prm.samplerate = 32000;
+	prm.samplerate = 48000;
 	prm.bitrate    = 128000;
 	prm.layer      = 2;
 	prm.mode       = SINGLE_CHANNEL;
 	mpa_decode_fmtp(&prm, fmtp);
+	aes->samplerate = prm.samplerate;
 
 	result = 0;
 #ifdef DEBUG
@@ -85,25 +92,25 @@ int mpa_encode_update(struct auenc_state **aesp, const struct aucodec *ac,
 	result |= twolame_set_out_samplerate(aes->enc, prm.samplerate);
 	result |= twolame_set_num_channels(aes->enc, 2);
 	if (result!=0) {
-		error("mpa: encoder set failed\n");
+		error("MPA enc set failed\n");
 		err=EINVAL;
 		goto out;
 	}
 
 	result = twolame_init_params(aes->enc);
 	if (result!=0) {
-		error("mpa: encoder init params failed\n");
+		error("MPA enc init params failed\n");
 		err=EINVAL;
 		goto out;
 	}
-
+#ifdef DEBUG
 	twolame_print_config(aes->enc);
-
-	if (prm.samplerate != 48000) {
-		aes->resampler = speex_resampler_init(2, 48000,
+#endif
+	if (prm.samplerate != MPA_IORATE) {
+		aes->resampler = speex_resampler_init(2, MPA_IORATE,
 			prm.samplerate, 3, &result);
 		if (result!=RESAMPLER_ERR_SUCCESS) {
-			error("mpa: resampler init failed %d\n",result);
+			error("MPA enc resampler init failed %d\n",result);
 			err=EINVAL;
 			goto out;
 		}
@@ -139,7 +146,7 @@ int mpa_encode_frm(struct auenc_state *aes, uint8_t *buf, size_t *len,
 			sampv, &in_len, aes->intermediate_buffer,
 			&intermediate_len);
 		if (n!=RESAMPLER_ERR_SUCCESS || in_len != sampc/2) {
-			warning("mpa: downsample error: %s %d %d\n",
+			error("MPA enc downsample error: %s %d %d\n",
 				strerror(n), in_len, sampc/2);
 			return EPROTO;
 		}
@@ -147,7 +154,7 @@ int mpa_encode_frm(struct auenc_state *aes, uint8_t *buf, size_t *len,
 			aes->intermediate_buffer, intermediate_len,
 			buf+4, (int)(*len)-4);
 #ifdef DEBUG
-		debug("mpa encode %d %d %d %d %d\n",intermediate_len,sampc,
+		debug("MPA enc %d %d %d %d %d\n",intermediate_len,sampc,
 			aes->channels,*len,n);
 #endif
 	}
@@ -156,7 +163,7 @@ int mpa_encode_frm(struct auenc_state *aes, uint8_t *buf, size_t *len,
 				      (int)(sampc/2), buf+4, (int)(*len)-4);
 
 	if (n < 0) {
-		error("mpa: encode error: %s\n", strerror((int)n));
+		error("MPA enc error %s\n", strerror((int)n));
 		return EPROTO;
 	}
 
@@ -168,8 +175,10 @@ int mpa_encode_frm(struct auenc_state *aes, uint8_t *buf, size_t *len,
 		*len = 0;
 
 #ifdef DEBUG
-	debug("mpa encode %d %d %d %d\n",sampc,aes->channels,*len,n);
+	debug("MPA enc done %d %d %d %d\n",sampc,aes->channels,*len,n);
 #endif
-	return 0;
+	aes->timestamp += ((MPA_FRAMESIZE*MPA_RTPRATE)<<4) / aes->samplerate;
+
+	return 0x00010000 | ((aes->timestamp>>4) & 0x0000ffff);
 }
 
diff --git a/modules/mpa/mpa.c b/modules/mpa/mpa.c
index 6c2081b..2c75625 100644
--- a/modules/mpa/mpa.c
+++ b/modules/mpa/mpa.c
@@ -79,13 +79,13 @@
 
 
 static struct aucodec mpa = {
-	.pt       = NULL,	/* for the time being, to cope
-with AVT AC1 interop problems, we do not use "14" here */
+	.pt       = "14",
 	.name      = "MPA",
-	.srate     = 48000,
-	.crate     = 90000,
+	.srate     = MPA_IORATE,
+	.crate     = MPA_RTPRATE,
 	.ch       = 1,
-	.fmtp      = "",
+/* MPA does not expect channels count, even those it is stereo */
+	.fmtp      = "layer=2",
 	.encupdh   = mpa_encode_update,
 	.ench      = mpa_encode_frm,
 	.decupdh   = mpa_decode_update,
@@ -108,7 +108,7 @@ static int module_init(void)
 	if (0 == conf_get_u32(conf, "mpa_bitrate", &value)) {
 		if (value<8000 || value>384000) {
 			error("MPA bitrate between 8000 and "
-				"384000 are allowed.");
+				"384000 are allowed.\n");
 			return -1;
 		}
 
@@ -136,7 +136,7 @@ static int module_init(void)
 			break;
 		default:
 			error("MPA samplerates of 16, 22.05, 24, 32, "
-				"44.1, and 48 kHz are allowed.");
+				"44.1, and 48 kHz are allowed.\n");
 			return -1;
 		}
 		(void)re_snprintf(fmtp+strlen(fmtp),
@@ -155,7 +155,7 @@ static int module_init(void)
 			&& strcmp(mode,"single_channel")
 			&& strcmp(mode,"dual_channel")) {
 			error("MPA mode: Permissible values are stereo, "
-			    "joint_stereo, single_channel, dual_channel");
+			    "joint_stereo, single_channel, dual_channel.\n");
 			return -1;
 		}
 
@@ -172,7 +172,7 @@ static int module_init(void)
 	/* init decoder library */
 	res = mpg123_init();
 	if (res != MPG123_OK) {
-		error("MPA libmpg123 init error %s",
+		error("MPA libmpg123 init error %s\n",
 			mpg123_plain_strerror(res));
 		return -1;
 	}
diff --git a/modules/mpa/mpa.h b/modules/mpa/mpa.h
index 35055b6..0db2528 100644
--- a/modules/mpa/mpa.h
+++ b/modules/mpa/mpa.h
@@ -5,7 +5,9 @@
  */
 
 #define MPA_FRAMESIZE 1152
-#define BARESIP_FRAMESIZE (48000/50*2)
+#define MPA_IORATE 48000
+#define MPA_RTPRATE 90000
+#define BARESIP_FRAMESIZE (MPA_IORATE/50*2)
 
 #undef DEBUG
 
diff --git a/modules/mpa/sdp.c b/modules/mpa/sdp.c
index 363b72c..a4e432c 100644
--- a/modules/mpa/sdp.c
+++ b/modules/mpa/sdp.c
@@ -42,13 +42,13 @@ void mpa_decode_fmtp(struct mpa_param *prm, const char *fmtp)
 
 	if (fmt_param_get(&pl, "mode", &val)) {
 
-		if (!strncmp("stereo",pl.p,pl.l))
+		if (!strncmp("stereo",val.p,val.l))
 			prm->mode = STEREO;
-		else if (!strncmp("joint_stereo",pl.p,pl.l))
+		else if (!strncmp("joint_stereo",val.p,val.l))
 			prm->mode = JOINT_STEREO;
-		else if (!strncmp("single_channel",pl.p,pl.l))
+		else if (!strncmp("single_channel",val.p,val.l))
 			prm->mode = SINGLE_CHANNEL;
-		else if (!strncmp("dual_channel",pl.p,pl.l))
+		else if (!strncmp("dual_channel",val.p,val.l))
 			prm->mode = DUAL_CHANNEL;
 	}
 }