Ticket #497: WSOLA implementation

git-svn-id: http://svn.pjsip.org/repos/pjproject/trunk@1824 74dad513-b988-da41-8d7b-12977e46ad98

1 files changed, 535 insertions, 0 deletions

diff --git a/pjmedia/src/pjmedia/wsola.c b/pjmedia/src/pjmedia/wsola.c
new file mode 100644
index 00000000..408dd340
--- /dev/null
+++ b/pjmedia/src/pjmedia/wsola.c
@@ -0,0 +1,535 @@
+/* $Id$ */
+/* 
+ * Copyright (C) 2003-2007 Benny Prijono <benny@prijono.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 
+ */
+#include <pjmedia/wsola.h>
+#include <pjmedia/errno.h>
+#include <pj/assert.h>
+#include <pj/log.h>
+#include <pj/pool.h>
+
+#define THIS_FILE   "wsola.c"
+
+
+/* History size, in percentage of samples_per_frame */
+#define HISTSZ		(1.5)
+
+/* Number of frames in buffer (excluding history) */
+#define FRAME_CNT	4
+
+/* Template size in msec */
+#define TEMPLATE_PTIME	(5)
+
+/* Generate extra samples, in msec */
+#define GEN_EXTRA_PTIME	(0.0)
+
+
+#ifndef M_PI
+#   define  M_PI	3.14159265358979323846
+#endif
+
+#if 0
+#   define TRACE_(x)	PJ_LOG(4,x)
+#else
+#   define TRACE_(x)
+#endif
+
+
+struct pjmedia_wsola
+{
+    pj_uint16_t clock_rate;	/* Sampling rate.			*/
+    pj_uint16_t samples_per_frame;/* Samples per frame.			*/
+    pj_uint16_t options;	/* Options.				*/
+    pj_uint16_t hist_cnt;       /* # of history samples.		*/
+    pj_uint16_t buf_cnt;        /* Total buffer capacity		*/
+    pj_uint16_t cur_cnt;        /* Cur # of samples, inc. history	*/
+    pj_uint16_t template_size;	/* Template size.			*/
+    pj_uint16_t min_extra;	/* Min extra samples for merging.	*/
+    pj_uint16_t gen_extra;	/* Generate extra samples.		*/
+    pj_uint16_t expand_cnt;     /* Number of expansion currently done   */
+
+    short    *buf;		/* The buffer.				*/
+    short    *frm;	        /* Pointer to next frame to play in buf */
+    short    *mergebuf;	        /* Temporary merge buffer.		*/
+#if defined(PJ_HAS_FLOATING_POINT) && PJ_HAS_FLOATING_POINT!=0
+    float    *hanning;		/* Hanning window.			*/
+#else
+    pj_uint16_t *hanning;	/* Hanning window.			*/
+#endif
+
+    pj_timestamp ts;	        /* Running timestamp.			*/
+};
+
+
+#if defined(PJ_HAS_FLOATING_POINT) && PJ_HAS_FLOATING_POINT!=0
+/*
+ * Floating point version.
+ */
+#include <math.h>
+
+static short *find_pitch(short *frm, short *beg, short *end, 
+			 unsigned template_cnt, int first)
+{
+    short *sr, *best=beg;
+    double best_corr = 0;
+
+    for (sr=beg; sr!=end; ++sr) {
+	double corr = 0;
+	unsigned i;
+
+	for (i=0; i<template_cnt; ++i) {
+	    corr += ((float)frm[i]) * ((float)sr[i]);
+	}
+
+	if (first) {
+	    if (corr > best_corr) {
+		best_corr = corr;
+		best = sr;
+	    }
+	} else {
+	    if (corr >= best_corr) {
+		best_corr = corr;
+		best = sr;
+	    }
+	}
+    }
+
+    TRACE_((THIS_FILE, "found pitch at %u", best-beg));
+    return best;
+}
+
+static void overlapp_add(short dst[], unsigned count,
+			 short l[], short r[],
+			 float w[])
+{
+    unsigned i;
+
+    for (i=0; i<count; ++i) {
+	dst[i] = (short)(l[i] * w[count-1-i] + r[i] * w[i]);
+    }
+}
+
+static void overlapp_add_simple(short dst[], unsigned count,
+				short l[], short r[])
+{
+    float step = (float)(1.0 / count), stepdown = 1.0;
+    unsigned i;
+
+    for (i=0; i<count; ++i) {
+	dst[i] = (short)(l[i] * stepdown + r[i] * (1-stepdown));
+	stepdown -= step;
+    }
+}
+
+static void create_win(pj_pool_t *pool, float **pw, unsigned count)
+{
+    unsigned i;
+    float *w = (float*)pj_pool_calloc(pool, count, sizeof(float));
+
+    *pw = w;
+
+    for (i=0;i<count; i++) {
+	w[i] = (float)(0.5 - 0.5 * cos(2.0 * M_PI * i / (count*2-1)) );
+    }
+}
+
+#else	/* PJ_HAS_FLOATING_POINT */
+/*
+ * Fixed point version.
+ */
+#define WINDOW_BITS	15
+enum { WINDOW_MAX_VAL = (1 << WINDOW_BITS)-1 };
+
+static short *find_pitch(short *frm, short *beg, short *end, 
+			 unsigned template_cnt, int first)
+{
+    short *sr, *best=beg;
+    pj_int64_t best_corr = 0;
+
+    for (sr=beg; sr!=end; ++sr) {
+	pj_int64_t corr = 0;
+	unsigned i;
+
+	for (i=0; i<template_cnt; ++i) {
+	    corr += ((int)frm[i]) * ((int)sr[i]);
+	}
+
+	if (first) {
+	    if (corr > best_corr) {
+		best_corr = corr;
+		best = sr;
+	    }
+	} else {
+	    if (corr >= best_corr) {
+		best_corr = corr;
+		best = sr;
+	    }
+	}
+    }
+
+    TRACE_((THIS_FILE, "found pitch at %u", best-beg));
+    return best;
+}
+
+static void overlapp_add(short dst[], unsigned count,
+			 short l[], short r[],
+			 pj_uint16_t w[])
+{
+    unsigned i;
+
+    for (i=0; i<count; ++i) {
+	int val;
+	val = ((int)(l[i]) * (int)(w[count-1-i]) + 
+	       (int)(r[i]) * (int)(w[i]));
+	dst[i] = (short)(val >> WINDOW_BITS);
+
+	assert((val>=0 && dst[i]>=0) ||
+	       (val<0 && dst[i]<0));
+    }
+}
+
+static void overlapp_add_simple(short dst[], unsigned count,
+				short l[], short r[])
+{
+    int step = ((WINDOW_MAX_VAL+1) / count), 
+	stepdown = WINDOW_MAX_VAL;
+    unsigned i;
+
+    for (i=0; i<count; ++i) {
+	int val;
+
+	assert(stepdown >= 0);
+
+	val = (l[i] * stepdown + r[i] * (1-stepdown));
+	dst[i] = (short)(val >> WINDOW_BITS);
+	stepdown -= step;
+
+	assert((val>=0 && dst[i]>=0) ||
+	       (val<0 && dst[i]<0));
+    }
+}
+
+#if PJ_HAS_INT64
+/* approx_cos():
+ *   see: http://www.audiomulch.com/~rossb/code/sinusoids/ 
+ */
+static pj_uint32_t approx_cos( pj_uint32_t x )
+{
+    pj_uint32_t i,j,k;
+
+    if( x == 0 )
+	return 0xFFFFFFFF;
+
+    i = x << 1;
+    k = ((x + 0xBFFFFFFD) & 0x80000000) >> 30;
+    j = i - i * ((i & 0x80000000)>>30);
+    j = j >> 15;
+    j = (j * j + j) >> 1;
+    j = j - j * k;
+
+    return j;
+}
+#endif	/* PJ_HAS_INT64 */
+
+static void create_win(pj_pool_t *pool, pj_uint16_t **pw, unsigned count)
+{
+    
+    unsigned i;
+    pj_uint16_t *w = (pj_uint16_t*)pj_pool_calloc(pool, count, 
+						  sizeof(pj_uint16_t));
+
+    *pw = w;
+
+    for (i=0; i<count; i++) {
+#if PJ_HAS_INT64
+	pj_uint32_t phase;
+	pj_uint64_t cos_val;
+
+	/* w[i] = (float)(0.5 - 0.5 * cos(2.0 * M_PI * i / (count*2-1)) ); */
+
+	phase = (pj_uint32_t)(PJ_INT64(0xFFFFFFFF) * i / (count*2-1));
+	cos_val = approx_cos(phase);
+
+	w[i] = (pj_uint16_t)(WINDOW_MAX_VAL - 
+			      (WINDOW_MAX_VAL * cos_val) / 0xFFFFFFFF);
+#else
+	/* Revert to linear */
+	w[i] = i * WINDOW_MAX_VAL / count;
+#endif
+    }
+}
+
+#endif	/* PJ_HAS_FLOATING_POINT */
+
+
+PJ_DEF(pj_status_t) pjmedia_wsola_create( pj_pool_t *pool, 
+					  unsigned clock_rate,
+					  unsigned samples_per_frame,
+					  unsigned options,
+					  pjmedia_wsola **p_wsola)
+{
+    pjmedia_wsola *wsola;
+
+    PJ_ASSERT_RETURN(pool && clock_rate && samples_per_frame && p_wsola,
+		     PJ_EINVAL);
+    PJ_ASSERT_RETURN(clock_rate <= 65535, PJ_EINVAL);
+    PJ_ASSERT_RETURN(samples_per_frame < clock_rate, PJ_EINVAL);
+
+    wsola = PJ_POOL_ZALLOC_T(pool, pjmedia_wsola);
+    
+    wsola->clock_rate= (pj_uint16_t) clock_rate;
+    wsola->samples_per_frame = (pj_uint16_t) samples_per_frame;
+    wsola->options   = (pj_uint16_t) options;
+    wsola->hist_cnt  = (pj_uint16_t)(samples_per_frame * HISTSZ);
+    wsola->buf_cnt   = (pj_uint16_t)(wsola->hist_cnt + 
+				    (samples_per_frame * FRAME_CNT));
+
+    wsola->cur_cnt   = (pj_uint16_t)(wsola->hist_cnt + 
+					wsola->samples_per_frame);
+    wsola->min_extra = 0;
+
+    if ((options & PJMEDIA_WSOLA_NO_PLC) == 0)
+	wsola->gen_extra = (pj_uint16_t)(GEN_EXTRA_PTIME * clock_rate / 1000);
+
+    wsola->template_size = (pj_uint16_t) (clock_rate * TEMPLATE_PTIME / 1000);
+    if (wsola->template_size > samples_per_frame)
+	wsola->template_size = wsola->samples_per_frame;
+
+    wsola->buf = (short*)pj_pool_calloc(pool, wsola->buf_cnt, 
+					sizeof(short));
+    wsola->frm = wsola->buf + wsola->hist_cnt;
+    
+    wsola->mergebuf = (short*)pj_pool_calloc(pool, samples_per_frame, 
+					     sizeof(short));
+
+    if ((options & PJMEDIA_WSOLA_NO_HANNING) == 0) {
+	create_win(pool, &wsola->hanning, wsola->samples_per_frame);
+    }
+
+    *p_wsola = wsola;
+    return PJ_SUCCESS;
+
+}
+
+PJ_DEF(pj_status_t) pjmedia_wsola_destroy(pjmedia_wsola *wsola)
+{
+    /* Nothing to do */
+    PJ_UNUSED_ARG(wsola);
+
+    return PJ_SUCCESS;
+}
+
+static void expand(pjmedia_wsola *wsola, unsigned needed)
+{
+    unsigned generated = 0;
+    unsigned frmsz = wsola->samples_per_frame;
+    unsigned rep;
+
+    for (rep=1;; ++rep) {
+	short *start;
+	unsigned dist;
+
+	start = find_pitch(wsola->frm, wsola->buf, 
+			   wsola->frm - (wsola->samples_per_frame >> 1),
+			   wsola->template_size, 1);
+
+	if (wsola->options & PJMEDIA_WSOLA_NO_HANNING) {
+	    overlapp_add_simple(wsola->mergebuf, wsola->samples_per_frame,
+				wsola->frm, start);
+	} else {
+	    overlapp_add(wsola->mergebuf, wsola->samples_per_frame,
+			 wsola->frm, start, wsola->hanning);
+	}
+
+	dist = wsola->frm - start;
+	memmove(wsola->frm + frmsz, start + frmsz, 
+		(wsola->buf+wsola->cur_cnt - (start+frmsz)) << 1);
+
+	memcpy(wsola->frm, wsola->mergebuf, frmsz << 1);
+
+	wsola->cur_cnt = (pj_uint16_t)(wsola->cur_cnt + dist);
+	generated += dist;
+
+	if (generated >= needed) {
+	    assert(wsola->cur_cnt <= wsola->buf_cnt);
+	    TRACE_((THIS_FILE, "WSOLA frame expanded after %d iterations", 
+		    rep));
+	    break;
+	}
+    }
+}
+
+
+static unsigned compress(pjmedia_wsola *wsola, short *buf, unsigned count,
+			 unsigned erase_cnt)
+{
+    unsigned samples_del = 0, rep;
+
+    for (rep=1; ; ++rep) {
+	short *start, *end;
+	unsigned frmsz = wsola->samples_per_frame;
+	unsigned dist;
+
+	if (count <= (erase_cnt << 1)) {
+	    TRACE_((THIS_FILE, "Not enough samples to compress!"));
+	    return samples_del;
+	}
+
+	start = buf + (frmsz >> 1);
+	end = start + frmsz;
+
+	if (end + frmsz > buf + count)
+	    end = buf+count-frmsz;
+
+	start = find_pitch(buf, start, end, wsola->template_size, 0);
+	dist = start - buf;
+
+	if (wsola->options & PJMEDIA_WSOLA_NO_HANNING) {
+	    overlapp_add_simple(buf, wsola->samples_per_frame, buf, start);
+	} else {
+	    overlapp_add(buf, wsola->samples_per_frame, buf, start, 
+			 wsola->hanning);
+	}
+
+	memmove(buf + frmsz, buf + frmsz + dist,
+		(count - frmsz - dist) * 2);
+
+	count -= dist;
+	samples_del += dist;
+
+	if (samples_del >= erase_cnt) {
+	    TRACE_((THIS_FILE, 
+		    "Erased %d of %d requested after %d iteration(s)",
+		    samples_del, erase_cnt, rep));
+	    break;
+	}
+    }
+
+    return samples_del;
+}
+
+
+
+PJ_DEF(pj_status_t) pjmedia_wsola_save( pjmedia_wsola *wsola, 
+					short frm[], 
+					pj_bool_t prev_lost)
+{
+    unsigned extra;
+
+    extra = wsola->cur_cnt - wsola->hist_cnt - wsola->samples_per_frame;
+    pj_assert(extra >= 0);
+
+    if (prev_lost && extra >= wsola->min_extra) {
+	short *dst = wsola->buf + wsola->hist_cnt + wsola->samples_per_frame;
+	unsigned i;
+
+	overlapp_add_simple(dst, extra, dst, frm);
+
+	for (i=extra; i<wsola->samples_per_frame; ++i)
+	    dst[i] = frm[i];
+
+	
+	memcpy(frm, wsola->frm, wsola->samples_per_frame << 1);
+	wsola->cur_cnt = (pj_uint16_t)(wsola->hist_cnt + 
+				       wsola->samples_per_frame);
+	memmove(wsola->buf, wsola->buf+wsola->samples_per_frame, 
+		wsola->cur_cnt << 1);
+
+    } else {
+	/* Just append to end of buffer */
+	if (prev_lost) {
+	    TRACE_((THIS_FILE, 
+		    "Appending new frame without interpolation"));
+	}
+
+	memcpy(wsola->buf + wsola->cur_cnt, frm, 
+	       wsola->samples_per_frame << 1);
+	memcpy(frm, wsola->frm, 
+	       wsola->samples_per_frame << 1);
+	memmove(wsola->buf, wsola->buf+wsola->samples_per_frame, 
+		wsola->cur_cnt << 1);
+    }
+    
+    wsola->expand_cnt = 0;
+    wsola->ts.u64 += wsola->samples_per_frame;
+
+    return PJ_SUCCESS;
+}
+
+
+PJ_DEF(pj_status_t) pjmedia_wsola_generate( pjmedia_wsola *wsola, 
+					    short frm[])
+{
+    unsigned extra;
+
+    extra = wsola->cur_cnt - wsola->hist_cnt - wsola->samples_per_frame;
+
+    if (extra >= wsola->samples_per_frame) {
+
+	/* We have one extra frame in the buffer, just return this frame
+	 * rather than generating a new one.
+	 */
+	memcpy(frm, wsola->frm, wsola->samples_per_frame << 1);
+	memmove(wsola->buf, wsola->buf+wsola->samples_per_frame, 
+	        (wsola->cur_cnt - wsola->samples_per_frame) << 1);
+
+	pj_assert(wsola->cur_cnt >= 
+		    wsola->hist_cnt + (wsola->samples_per_frame << 1));
+	wsola->cur_cnt = (pj_uint16_t)(wsola->cur_cnt - 
+				       wsola->samples_per_frame);
+
+    } else {
+	unsigned new_samples;
+
+	/* Calculate how many samples are need for a new frame */
+	new_samples = ((wsola->samples_per_frame << 1) + wsola->gen_extra - 
+		       (wsola->cur_cnt - wsola->hist_cnt));
+
+	/* Expand buffer */
+	expand(wsola, new_samples);
+
+	memcpy(frm, wsola->frm, wsola->samples_per_frame << 1);
+	memmove(wsola->buf, wsola->buf+wsola->samples_per_frame, 
+		(wsola->cur_cnt - wsola->samples_per_frame) << 1);
+
+	pj_assert(wsola->cur_cnt >= 
+		   wsola->hist_cnt + (wsola->samples_per_frame << 1));
+
+	wsola->cur_cnt = (pj_uint16_t)(wsola->cur_cnt - 
+					wsola->samples_per_frame);
+	wsola->expand_cnt++;
+    }
+
+    wsola->ts.u64 += wsola->samples_per_frame;
+
+    return PJ_SUCCESS;
+}
+
+
+PJ_DEF(pj_status_t) pjmedia_wsola_discard( pjmedia_wsola *wsola, 
+					   short buf[],
+					   unsigned buf_cnt, 
+					   unsigned *erase_cnt)
+{
+    PJ_ASSERT_RETURN(wsola && buf && buf_cnt && erase_cnt, PJ_EINVAL);
+    PJ_ASSERT_RETURN(*erase_cnt, PJ_EINVAL);
+
+    *erase_cnt = compress(wsola, buf, buf_cnt, *erase_cnt);
+    return (*erase_cnt) > 0 ? PJ_SUCCESS : PJ_ETOOSMALL;
+}
+
+