Add remainder of rtp fixes, iax2 patch (bug #3961)

git-svn-id: https://origsvn.digium.com/svn/asterisk/trunk@5654 65c4cc65-6c06-0410-ace0-fbb531ad65f3

4 files changed, 202 insertions, 162 deletions

diff --git a/channels/chan_iax2.c b/channels/chan_iax2.c
index 29cf3cd05..61e6c3a45 100755
--- a/channels/chan_iax2.c
+++ b/channels/chan_iax2.c
@@ -859,48 +859,6 @@ static struct chan_iax2_pvt *new_iax(struct sockaddr_in *sin, int lockpeer, cons
 	return tmp;
 }
 
-static int get_samples(struct ast_frame *f)
-{
-	int samples=0;
-	switch(f->subclass) {
-	case AST_FORMAT_SPEEX:
-		samples = 160;	/* XXX Not necessarily true XXX */
-		break;
-	case AST_FORMAT_G723_1:
-		samples = 240 /* XXX Not necessarily true XXX */;
-		break;
-	case AST_FORMAT_ILBC:
-		samples = 240 * (f->datalen / 50);
-		break;
-	case AST_FORMAT_GSM:
-		samples = 160 * (f->datalen / 33);
-		break;
-	case AST_FORMAT_G729A:
-		samples = 160 * (f->datalen / 20);
-		break;
-	case AST_FORMAT_SLINEAR:
-		samples = f->datalen / 2;
-		break;
-	case AST_FORMAT_LPC10:
-		samples = 22 * 8;
-		samples += (((char *)(f->data))[7] & 0x1) * 8;
-		break;
-	case AST_FORMAT_ULAW:
-		samples = f->datalen;
-		break;
-	case AST_FORMAT_ALAW:
-		samples = f->datalen;
-		break;
-	case AST_FORMAT_ADPCM:
-	case AST_FORMAT_G726:
-		samples = f->datalen *2;
-		break;
-	default:
-		ast_log(LOG_WARNING, "Don't know how to calculate samples on %d packets\n", f->subclass);
-	}
-	return samples;
-}
-
 static struct iax_frame *iaxfrdup2(struct iax_frame *fr)
 {
 	/* Malloc() a copy of a frame */
@@ -2347,7 +2305,7 @@ static int schedule_delivery(struct iax_frame *fr, int reallydeliver, int update
 
 	if(fr->af.frametype == AST_FRAME_VOICE) {
 		type = JB_TYPE_VOICE;
-		len = get_samples(&fr->af)/8;
+                len = ast_codec_get_samples(&fr->af) / 8;
 	} else if(fr->af.frametype == AST_FRAME_CNG) {
 		type = JB_TYPE_SILENCE;
 	}
@@ -6240,7 +6198,7 @@ static int socket_read(int *id, int fd, short events, void *cbdata)
 									f.mallocd = 0;
 									f.offset = 0;
 									if (f.datalen && (f.frametype == AST_FRAME_VOICE)) 
-										f.samples = get_samples(&f);
+										f.samples = ast_codec_get_samples(&f);
 									else
 										f.samples = 0;
 									fr.outoforder = 0;
@@ -7355,7 +7313,7 @@ retryowner2:
 	f.mallocd = 0;
 	f.offset = 0;
 	if (f.datalen && (f.frametype == AST_FRAME_VOICE)) {
-		f.samples = get_samples(&f);
+		f.samples = ast_codec_get_samples(&f);
 		/* We need to byteswap incoming slinear samples from network byte order */
 		if (f.subclass == AST_FORMAT_SLINEAR)
 			ast_frame_byteswap_be(&f);
diff --git a/frame.c b/frame.c
index 41932f880..2e68ec4ff 100755
--- a/frame.c
+++ b/frame.c
@@ -35,6 +35,12 @@ AST_MUTEX_DEFINE_STATIC(framelock);
 
 #define SMOOTHER_SIZE 8000
 
+#define TYPE_HIGH	 0x0
+#define TYPE_LOW	 0x1
+#define TYPE_SILENCE	 0x2
+#define TYPE_DONTSEND	 0x3
+#define TYPE_MASK	 0x3
+
 struct ast_format_list {
 	int visible; /* Can we see this entry */
 	int bits; /* bitmask value */
@@ -1008,4 +1014,190 @@ void ast_parse_allow_disallow(struct ast_codec_pref *pref, int *mask, char *list
 	}
 }
 
+static int g723_len(unsigned char buf)
+{
+	switch(buf & TYPE_MASK) {
+	case TYPE_DONTSEND:
+		return 0;
+		break;
+	case TYPE_SILENCE:
+		return 4;
+		break;
+	case TYPE_HIGH:
+		return 24;
+		break;
+	case TYPE_LOW:
+		return 20;
+		break;
+	default:
+		ast_log(LOG_WARNING, "Badly encoded frame (%d)\n", buf & TYPE_MASK);
+	}
+	return -1;
+}
+
+static int g723_samples(unsigned char *buf, int maxlen)
+{
+	int pos = 0;
+	int samples = 0;
+	int res;
+	while(pos < maxlen) {
+		res = g723_len(buf[pos]);
+		if (res <= 0)
+			break;
+		samples += 240;
+		pos += res;
+	}
+	return samples;
+}
+
+static unsigned char get_n_bits_at(unsigned char *data, int n, int bit)
+{
+	int byte = bit / 8;       /* byte containing first bit */
+	int rem = 8 - (bit % 8);  /* remaining bits in first byte */
+	unsigned char ret = 0;
+	
+	if (n <= 0 || n > 8)
+		return 0;
+
+	if (rem < n) {
+		ret = (data[byte] << (n - rem));
+		ret |= (data[byte + 1] >> (8 - n + rem));
+	} else {
+		ret = (data[byte] >> (rem - n));
+	}
+
+	return (ret & (0xff >> (8 - n)));
+}
+
+static int speex_get_wb_sz_at(unsigned char *data, int len, int bit)
+{
+	static int SpeexWBSubModeSz[] = {
+		0, 36, 112, 192,
+		352, 0, 0, 0 };
+	int off = bit;
+	unsigned char c;
+
+	/* skip up to two wideband frames */
+	if (((len * 8 - off) >= 5) && 
+		get_n_bits_at(data, 1, off)) {
+		c = get_n_bits_at(data, 3, off + 1);
+		off += SpeexWBSubModeSz[c];
+
+		if (((len * 8 - off) >= 5) && 
+			get_n_bits_at(data, 1, off)) {
+			c = get_n_bits_at(data, 3, off + 1);
+			off += SpeexWBSubModeSz[c];
+
+			if (((len * 8 - off) >= 5) && 
+				get_n_bits_at(data, 1, off)) {
+				ast_log(LOG_WARNING, "Encountered corrupt speex frame; too many wideband frames in a row.\n");
+				return -1;
+			}
+		}
+
+	}
+	return off - bit;
+}
+
+static int speex_samples(unsigned char *data, int len)
+{
+	static int SpeexSubModeSz[] = {
+		0, 43, 119, 160, 
+		220, 300, 364, 492, 
+		79, 0, 0, 0,
+		0, 0, 0, 0 };
+	static int SpeexInBandSz[] = { 
+		1, 1, 4, 4,
+		4, 4, 4, 4,
+		8, 8, 16, 16,
+		32, 32, 64, 64 };
+	int bit = 0;
+	int cnt = 0;
+	int off = 0;
+	unsigned char c;
+
+	while ((len * 8 - bit) >= 5) {
+		/* skip wideband frames */
+		off = speex_get_wb_sz_at(data, len, bit);
+		if (off < 0)  {
+			ast_log(LOG_WARNING, "Had error while reading wideband frames for speex samples\n");
+			break;
+		}
+		bit += off;
+
+		if ((len * 8 - bit) < 5) {
+			ast_log(LOG_WARNING, "Not enough bits remaining after wide band for speex samples.\n");
+			break;
+		}
+
+		/* get control bits */
+		c = get_n_bits_at(data, 5, bit);
+		bit += 5;
+
+		if (c == 15) { 
+			/* terminator */
+			break; 
+		} else if (c == 14) {
+			/* in-band signal; next 4 bits contain signal id */
+			c = get_n_bits_at(data, 4, bit);
+			bit += 4;
+			bit += SpeexInBandSz[c];
+		} else if (c == 13) {
+			/* user in-band; next 5 bits contain msg len */
+			c = get_n_bits_at(data, 5, bit);
+			bit += 5;
+			bit += c * 8;
+		} else if (c > 8) {
+			/* unknown */
+			break;
+		} else {
+			/* skip number bits for submode (less the 5 control bits) */
+			bit += SpeexSubModeSz[c] - 5;
+			cnt += 160; /* new frame */
+		}
+	}
+	return cnt;
+}
+
+
+int ast_codec_get_samples(struct ast_frame *f)
+{
+	int samples=0;
+	switch(f->subclass) {
+	case AST_FORMAT_SPEEX:
+		samples = speex_samples(f->data, f->datalen);
+		break;
+	case AST_FORMAT_G723_1:
+                samples = g723_samples(f->data, f->datalen);
+		break;
+	case AST_FORMAT_ILBC:
+		samples = 240 * (f->datalen / 50);
+		break;
+	case AST_FORMAT_GSM:
+		samples = 160 * (f->datalen / 33);
+		break;
+	case AST_FORMAT_G729A:
+		samples = f->datalen * 8;
+		break;
+	case AST_FORMAT_SLINEAR:
+		samples = f->datalen / 2;
+		break;
+	case AST_FORMAT_LPC10:
+                /* assumes that the RTP packet contains one LPC10 frame */
+		samples = 22 * 8;
+		samples += (((char *)(f->data))[7] & 0x1) * 8;
+		break;
+	case AST_FORMAT_ULAW:
+	case AST_FORMAT_ALAW:
+		samples = f->datalen;
+		break;
+	case AST_FORMAT_ADPCM:
+	case AST_FORMAT_G726:
+		samples = f->datalen * 2;
+		break;
+	default:
+		ast_log(LOG_WARNING, "Unable to calculate samples for format %s\n", ast_getformatname(f->subclass));
+	}
+	return samples;
+}
 
diff --git a/include/asterisk/frame.h b/include/asterisk/frame.h
index 695333514..d8a715a8f 100755
--- a/include/asterisk/frame.h
+++ b/include/asterisk/frame.h
@@ -399,6 +399,9 @@ extern int ast_codec_pref_string(struct ast_codec_pref *pref, char *buf, size_t
 /* Shift a codec preference list up or down 65 bytes so that it becomes an ASCII string */
 extern void ast_codec_pref_convert(struct ast_codec_pref *pref, char *buf, size_t size, int right);
 
+/* Returns the number of samples contained in the frame */
+extern int ast_codec_get_samples(struct ast_frame *f);
+
 /* Gets duration in ms of interpolation frame for a format */
 static inline int ast_codec_interp_len(int format) 
 { 
diff --git a/rtp.c b/rtp.c
index 9fde8b857..b0bf75e42 100755
--- a/rtp.c
+++ b/rtp.c
@@ -43,12 +43,6 @@
 
 #define RTP_MTU		1200
 
-#define TYPE_HIGH	 0x0
-#define TYPE_LOW	 0x1
-#define TYPE_SILENCE	 0x2
-#define TYPE_DONTSEND	 0x3
-#define TYPE_MASK	 0x3
-
 static int dtmftimeout = 3000;	/* 3000 samples */
 
 static int rtpstart = 0;
@@ -128,42 +122,6 @@ int ast_rtcp_fd(struct ast_rtp *rtp)
 	return -1;
 }
 
-static int g723_len(unsigned char buf)
-{
-	switch(buf & TYPE_MASK) {
-	case TYPE_DONTSEND:
-		return 0;
-		break;
-	case TYPE_SILENCE:
-		return 4;
-		break;
-	case TYPE_HIGH:
-		return 24;
-		break;
-	case TYPE_LOW:
-		return 20;
-		break;
-	default:
-		ast_log(LOG_WARNING, "Badly encoded frame (%d)\n", buf & TYPE_MASK);
-	}
-	return -1;
-}
-
-static int g723_samples(unsigned char *buf, int maxlen)
-{
-	int pos = 0;
-	int samples = 0;
-	int res;
-	while(pos < maxlen) {
-		res = g723_len(buf[pos]);
-		if (res <= 0)
-			break;
-		samples += 240;
-		pos += res;
-	}
-	return samples;
-}
-
 void ast_rtp_set_data(struct ast_rtp *rtp, void *data)
 {
 	rtp->data = data;
@@ -594,43 +552,9 @@ struct ast_frame *ast_rtp_read(struct ast_rtp *rtp)
 	rtp->f.data = rtp->rawdata + hdrlen + AST_FRIENDLY_OFFSET;
 	rtp->f.offset = hdrlen + AST_FRIENDLY_OFFSET;
 	if (rtp->f.subclass < AST_FORMAT_MAX_AUDIO) {
-		switch(rtp->f.subclass) {
-		case AST_FORMAT_ULAW:
-		case AST_FORMAT_ALAW:
-			rtp->f.samples = rtp->f.datalen;
-			break;
-		case AST_FORMAT_SLINEAR:
-			rtp->f.samples = rtp->f.datalen / 2;
+		rtp->f.samples = ast_codec_get_samples(&rtp->f);
+		if (rtp->f.subclass == AST_FORMAT_SLINEAR) 
 			ast_frame_byteswap_be(&rtp->f);
-			break;
-		case AST_FORMAT_GSM:
-			rtp->f.samples = 160 * (rtp->f.datalen / 33);
-			break;
-		case AST_FORMAT_ILBC:
-			rtp->f.samples = 240 * (rtp->f.datalen / 50);
-			break;
-		case AST_FORMAT_ADPCM:
-		case AST_FORMAT_G726:
-			rtp->f.samples = rtp->f.datalen * 2;
-			break;
-		case AST_FORMAT_G729A:
-			rtp->f.samples = rtp->f.datalen * 8;
-			break;
-		case AST_FORMAT_G723_1:
-			rtp->f.samples = g723_samples(rtp->f.data, rtp->f.datalen);
-			break;
-		case AST_FORMAT_SPEEX:
-			/* assumes that the RTP packet contained one Speex frame */
-	        	rtp->f.samples = 160;
-			break;
-		case AST_FORMAT_LPC10:
-		    	rtp->f.samples = 22 * 8;
-			rtp->f.samples += (((char *)(rtp->f.data))[7] & 0x1) * 8;
-			break;
-		default:
-			ast_log(LOG_NOTICE, "Unable to calculate samples for format %s\n", ast_getformatname(rtp->f.subclass));
-			break;
-		}
 		calc_rxstamp(&rtp->f.delivery, rtp, timestamp, mark);
 	} else {
 		/* Video -- samples is # of samples vs. 90000 */
@@ -1233,45 +1157,8 @@ static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec
 	ms = calc_txstamp(rtp, &f->delivery);
 	/* Default prediction */
 	if (f->subclass < AST_FORMAT_MAX_AUDIO) {
-		pred = rtp->lastts + ms * 8;
-		
-		switch(f->subclass) {
-		case AST_FORMAT_ULAW:
-		case AST_FORMAT_ALAW:
-			/* If we're within +/- 20ms from when where we
-			   predict we should be, use that */
-			pred = rtp->lastts + f->datalen;
-			break;
-		case AST_FORMAT_ADPCM:
-		case AST_FORMAT_G726:
-			/* If we're within +/- 20ms from when where we
-			   predict we should be, use that */
-			pred = rtp->lastts + f->datalen * 2;
-			break;
-		case AST_FORMAT_G729A:
-			pred = rtp->lastts + f->datalen * 8;
-			break;
-		case AST_FORMAT_GSM:
-			pred = rtp->lastts + (f->datalen * 160 / 33);
-			break;
-		case AST_FORMAT_ILBC:
-			pred = rtp->lastts + (f->datalen * 240 / 50);
-			break;
-		case AST_FORMAT_G723_1:
-			pred = rtp->lastts + g723_samples(f->data, f->datalen);
-			break;
-		case AST_FORMAT_SPEEX:
-		    pred = rtp->lastts + 160;
-			/* assumes that the RTP packet contains one Speex frame */
-			break;
-		case AST_FORMAT_LPC10:
-			/* assumes that the RTP packet contains one LPC10 frame */
-			pred = rtp->lastts + 22 * 8;
-			pred += (((char *)(f->data))[7] & 0x1) * 8;
-			break;
-		default:
-			ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %s\n", ast_getformatname(f->subclass));
-		}
+                pred = rtp->lastts + ast_codec_get_samples(f);
+
 		/* Re-calculate last TS */
 		rtp->lastts = rtp->lastts + ms * 8;
 		if (!f->delivery.tv_sec && !f->delivery.tv_usec) {