wcte12xp, wctdm24xxp: Add compile time option CONFIG_VOICEBUS_ECREFERENCE.

Add compile time option to improve the reference signal provided to
software echo cancelers.  The intent here is for this functionality to
become the default behavior but more testing and work on the edge cases
is needed.  It's being brought in now as a compile time option since there
have been reports that it helps in some environments.

Instead of using two buffers, which means that at best we're two
milliseconds behind, use a circular buffer where audio data is written
on the transmit side and read on the receive path.  In this way high
latency values will not interfere with the operation of software
echo cancelers. DAHDI-291. DAHDI-387.

This work was originally on:
http://svn.asterisk.org/svn/dahdi/linux/team/sruffell/improved_ecreference@9143

and includes a generic kfifo replacement by Matt Fredrickson.

git-svn-id: http://svn.asterisk.org/svn/dahdi/linux/trunk@9144 a0bf4364-ded3-4de4-8d8a-66a801d63aff

6 files changed, 216 insertions, 5 deletions

diff --git a/drivers/dahdi/voicebus/voicebus.c b/drivers/dahdi/voicebus/voicebus.c
index 9bc7386..351ccc9 100644
--- a/drivers/dahdi/voicebus/voicebus.c
+++ b/drivers/dahdi/voicebus/voicebus.c
@@ -94,6 +94,111 @@
 
 #define OWN_BIT (1 << 31)
 
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+
+/*
+ * These dahdi_fifo_xxx functions are currently only used by the voicebus
+ * drivers, but are named more generally to facilitate moving out in the
+ * future.  They probably also could stand to be changed in order to use a
+ * kfifo implementation from the kernel if one is available.
+ *
+ */
+
+struct dahdi_fifo {
+	size_t total_size;
+	u32 start;
+	u32 end;
+	u8 data[0];
+};
+
+static unsigned int dahdi_fifo_used_space(struct dahdi_fifo *fifo)
+{
+	return (fifo->end >= fifo->start) ? fifo->end - fifo->start :
+			    fifo->total_size - fifo->start + fifo->end;
+}
+
+unsigned int __dahdi_fifo_put(struct dahdi_fifo *fifo, u8 *data, size_t size)
+{
+	int newinsertposition;
+	int cpy_one_len, cpy_two_len;
+
+	if ((size + dahdi_fifo_used_space(fifo)) > (fifo->total_size - 1))
+		return -1;
+
+	if ((fifo->end + size) >= fifo->total_size) {
+		cpy_one_len = fifo->total_size - fifo->end;
+		cpy_two_len = fifo->end + size - fifo->total_size;
+		newinsertposition = cpy_two_len;
+	} else {
+		cpy_one_len = size;
+		cpy_two_len = 0;
+		newinsertposition = fifo->end + size;
+	}
+
+	memcpy(&fifo->data[fifo->end], data, cpy_one_len);
+
+	if (cpy_two_len)
+		memcpy(&fifo->data[0], &data[cpy_one_len], cpy_two_len);
+
+	fifo->end = newinsertposition;
+
+	return size;
+}
+EXPORT_SYMBOL(__dahdi_fifo_put);
+
+unsigned int __dahdi_fifo_get(struct dahdi_fifo *fifo, u8 *data, size_t size)
+{
+	int newbegin;
+	int cpy_one_len, cpy_two_len;
+
+	if (size > dahdi_fifo_used_space(fifo))
+		return 0;
+
+	if ((fifo->start + size) >= fifo->total_size) {
+		cpy_one_len = fifo->total_size - fifo->start;
+		cpy_two_len = fifo->start + size - fifo->total_size;
+		newbegin = cpy_two_len;
+	} else {
+		cpy_one_len = size;
+		cpy_two_len = 0;
+		newbegin = fifo->start + size;
+	}
+
+	memcpy(&data[0], &fifo->data[fifo->start], cpy_one_len);
+
+	if (cpy_two_len)
+		memcpy(&data[cpy_one_len], &fifo->data[0], cpy_two_len);
+
+	fifo->start = newbegin;
+
+	return size;
+}
+EXPORT_SYMBOL(__dahdi_fifo_get);
+
+void dahdi_fifo_free(struct dahdi_fifo *fifo)
+{
+	kfree(fifo);
+}
+EXPORT_SYMBOL(dahdi_fifo_free);
+
+struct dahdi_fifo *dahdi_fifo_alloc(u32 maxsize, gfp_t alloc_flags)
+{
+	struct dahdi_fifo *fifo;
+
+	fifo = kmalloc(maxsize + sizeof(*fifo) + 1, alloc_flags);
+
+	if (!fifo)
+		return NULL;
+
+	fifo->start = fifo->end = 0;
+	fifo->total_size = maxsize + 1;
+
+	return fifo;
+}
+EXPORT_SYMBOL(dahdi_fifo_alloc);
+#endif /* CONFIG_VOICEBUS_ECREFERENCE */
+
+
 #if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 20)
 kmem_cache_t *voicebus_vbb_cache;
 #else
diff --git a/drivers/dahdi/voicebus/voicebus.h b/drivers/dahdi/voicebus/voicebus.h
index 6c67f15..53714f6 100644
--- a/drivers/dahdi/voicebus/voicebus.h
+++ b/drivers/dahdi/voicebus/voicebus.h
@@ -62,6 +62,21 @@
  * (and not tasklet). */
 #define CONFIG_VOICEBUS_INTERRUPT
 
+/* Define this to use a FIFO for the software echocan reference.
+ * (experimental) */
+#undef CONFIG_VOICEBUS_ECREFERENCE
+
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+
+struct dahdi_fifo;
+unsigned int __dahdi_fifo_put(struct dahdi_fifo *fifo, u8 *data, size_t size);
+unsigned int __dahdi_fifo_get(struct dahdi_fifo *fifo, u8 *data, size_t size);
+void dahdi_fifo_free(struct dahdi_fifo *fifo);
+struct dahdi_fifo *dahdi_fifo_alloc(size_t maxsize, gfp_t alloc_flags);
+
+#endif
+
+
 struct voicebus;
 
 struct vbb {
diff --git a/drivers/dahdi/wctdm24xxp/base.c b/drivers/dahdi/wctdm24xxp/base.c
index 4a7f1fd..eefba37 100644
--- a/drivers/dahdi/wctdm24xxp/base.c
+++ b/drivers/dahdi/wctdm24xxp/base.c
@@ -943,6 +943,13 @@ static inline void wctdm_transmitprep(struct wctdm *wc, unsigned char *writechun
 			}
 		}
 		insert_tdm_data(wc, writechunk);
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+		for (x = 0; x < wc->avchannels; ++x) {
+			__dahdi_fifo_put(wc->ec_reference[x],
+					 wc->chans[x]->chan.writechunk,
+					 DAHDI_CHUNKSIZE);
+		}
+#endif
 	}
 
 	for (x = 0; x < DAHDI_CHUNKSIZE; x++) {
@@ -1192,7 +1199,14 @@ static inline void wctdm_receiveprep(struct wctdm *wc, const u8 *readchunk)
 	if (likely(wc->initialized)) {
 		for (x = 0; x < wc->avchannels; x++) {
 			struct dahdi_chan *c = &wc->chans[x]->chan;
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+			unsigned char buffer[DAHDI_CHUNKSIZE];
+			__dahdi_fifo_get(wc->ec_reference[x], buffer,
+				    ARRAY_SIZE(buffer));
+			dahdi_ec_chunk(c, c->readchunk, buffer);
+#else
 			dahdi_ec_chunk(c, c->readchunk, c->writechunk);
+#endif
 		}
 
 		for (x = 0; x < MAX_SPANS; x++) {
@@ -4317,6 +4331,12 @@ static void wctdm_back_out_gracefully(struct wctdm *wc)
 	LIST_HEAD(local_list);
 
 	voicebus_release(&wc->vb);
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	for (i = 0; i < ARRAY_SIZE(wc->ec_reference); ++i) {
+		if (wc->ec_reference[i])
+			dahdi_fifo_free(wc->ec_reference[i]);
+	}
+#endif
 
 	for (i = 0; i < ARRAY_SIZE(wc->spans); ++i) {
 		if (wc->spans[i] && wc->spans[i]->span.chans)
@@ -4872,6 +4892,22 @@ __wctdm_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 	up(&ifacelock);
 
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	for (i = 0; i < ARRAY_SIZE(wc->ec_reference); ++i) {
+		/* 256 is the smallest power of 2 that will contains the
+		 * maximum possible amount of latency. */
+		wc->ec_reference[i] = dahdi_fifo_alloc(256, GFP_KERNEL);
+
+		if (IS_ERR(wc->ec_reference[i])) {
+			ret = PTR_ERR(wc->ec_reference[i]);
+			wc->ec_reference[i] = NULL;
+			wctdm_back_out_gracefully(wc);
+			return ret;
+		}
+	}
+#endif
+
+
 	wc->desc = (struct wctdm_desc *)ent->driver_data;
 
 	/* This is to insure that the analog span is given lowest priority */
diff --git a/drivers/dahdi/wctdm24xxp/wctdm24xxp.h b/drivers/dahdi/wctdm24xxp/wctdm24xxp.h
index 1452850..ebacff1 100644
--- a/drivers/dahdi/wctdm24xxp/wctdm24xxp.h
+++ b/drivers/dahdi/wctdm24xxp/wctdm24xxp.h
@@ -269,6 +269,9 @@ struct wctdm {
 	struct wctdm_span *aspan;			/* pointer to the spans[] holding the analog span */
 	struct wctdm_span *spans[MAX_SPANS];
 	struct wctdm_chan *chans[NUM_MODULES];
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	struct dahdi_fifo *ec_reference[NUM_MODULES];
+#endif
 
 	/* Only care about digital spans here */
 	/* int span_timing_prio[MAX_SPANS - 1]; */
diff --git a/drivers/dahdi/wcte12xp/base.c b/drivers/dahdi/wcte12xp/base.c
index 03f40ae..46cd955 100644
--- a/drivers/dahdi/wcte12xp/base.c
+++ b/drivers/dahdi/wcte12xp/base.c
@@ -710,6 +710,13 @@ static void free_wc(struct t1 *wc)
 	if (wc->wq)
 		destroy_workqueue(wc->wq);
 
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	for (x = 0; x < ARRAY_SIZE(wc->ec_reference); ++x) {
+		if (wc->ec_reference[x])
+			dahdi_fifo_free(wc->ec_reference[x]);
+	}
+#endif
+
 	kfree(wc);
 }
 
@@ -952,16 +959,20 @@ static void set_span_devicetype(struct t1 *wc)
 static int t1xxp_startup(struct dahdi_span *span)
 {
 	struct t1 *wc = container_of(span, struct t1, span);
-	int i;
+#ifndef CONFIG_VOICEBUS_ECREFERENCE
+	unsigned int i;
+#endif
 
 	check_and_load_vpm(wc);
 	set_span_devicetype(wc);
 
+#ifndef CONFIG_VOICEBUS_ECREFERENCE
 	/* initialize the start value for the entire chunk of last ec buffer */
 	for (i = 0; i < span->channels; i++) {
 		memset(wc->ec_chunk1[i], DAHDI_LIN2X(0, span->chans[i]), DAHDI_CHUNKSIZE);
 		memset(wc->ec_chunk2[i], DAHDI_LIN2X(0, span->chans[i]), DAHDI_CHUNKSIZE);
 	}
+#endif
 
 	/* Reset framer with proper parameters and start */
 	t1xxp_framer_start(wc, span);
@@ -1901,6 +1912,13 @@ static inline void t1_transmitprep(struct t1 *wc, u8 *writechunk)
 		dahdi_transmit(&wc->span);
 	}
 
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	for (chan = 0; chan < wc->span.channels; chan++) {
+		__dahdi_fifo_put(wc->ec_reference[chan],
+			    wc->chans[chan]->writechunk, DAHDI_CHUNKSIZE);
+	}
+#endif
+
 	for (x = 0; x < DAHDI_CHUNKSIZE; x++) {
 		if (likely(test_bit(INITIALIZED, &wc->bit_flags))) {
 			for (chan = 0; chan < wc->span.channels; chan++)
@@ -1924,6 +1942,8 @@ static inline void t1_transmitprep(struct t1 *wc, u8 *writechunk)
 		}
 		writechunk += (EFRAME_SIZE + EFRAME_GAP);
 	}
+
+
 }
 
 /**
@@ -1978,11 +1998,23 @@ static inline void t1_receiveprep(struct t1 *wc, const u8* readchunk)
 	
 	/* echo cancel */
 	if (likely(test_bit(INITIALIZED, &wc->bit_flags))) {
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+		unsigned char buffer[DAHDI_CHUNKSIZE];
+		for (x = 0; x < wc->span.channels; x++) {
+			__dahdi_fifo_get(wc->ec_reference[x], buffer,
+				    ARRAY_SIZE(buffer));
+			dahdi_ec_chunk(wc->chans[x], wc->chans[x]->readchunk,
+				       buffer);
+		}
+#else
 		for (x = 0; x < wc->span.channels; x++) {
 			dahdi_ec_chunk(wc->chans[x], wc->chans[x]->readchunk, wc->ec_chunk2[x]);
-			memcpy(wc->ec_chunk2[x],wc->ec_chunk1[x],DAHDI_CHUNKSIZE);
-			memcpy(wc->ec_chunk1[x],wc->chans[x]->writechunk,DAHDI_CHUNKSIZE);
+			memcpy(wc->ec_chunk2[x], wc->ec_chunk1[x],
+				DAHDI_CHUNKSIZE);
+			memcpy(wc->ec_chunk1[x], wc->chans[x]->writechunk,
+				DAHDI_CHUNKSIZE);
 		}
+#endif
 		dahdi_receive(&wc->span);
 	}
 }
@@ -2263,6 +2295,22 @@ static int __devinit te12xp_init_one(struct pci_dev *pdev, const struct pci_devi
 	INIT_WORK(&wc->vpm_check_work, vpm_check_func);
 #	endif
 
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	for (x = 0; x < ARRAY_SIZE(wc->ec_reference); ++x) {
+		/* 256 is used here since it is the largest power of two that
+		 * will contain 8 * VOICBUS_DEFAULT_LATENCY */
+		wc->ec_reference[x] = dahdi_fifo_alloc(256, GFP_KERNEL);
+
+		if (IS_ERR(wc->ec_reference[x])) {
+			res = PTR_ERR(wc->ec_reference[x]);
+			wc->ec_reference[x] = NULL;
+			free_wc(wc);
+			return res;
+		}
+
+	}
+#endif /* CONFIG_VOICEBUS_ECREFERENCE */
+
 	snprintf(wc->name, sizeof(wc->name)-1, "wcte12xp%d", index);
 	pci_set_drvdata(pdev, wc);
 	wc->vb.ops = &voicebus_operations;
diff --git a/drivers/dahdi/wcte12xp/wcte12xp.h b/drivers/dahdi/wcte12xp/wcte12xp.h
index 5994fa2..8c04afe 100644
--- a/drivers/dahdi/wcte12xp/wcte12xp.h
+++ b/drivers/dahdi/wcte12xp/wcte12xp.h
@@ -116,11 +116,15 @@ struct t1 {
 	unsigned long bit_flags;
 	unsigned long alarmtimer;
 	unsigned char ledstate;
-	unsigned char ec_chunk1[32][DAHDI_CHUNKSIZE];
-	unsigned char ec_chunk2[32][DAHDI_CHUNKSIZE];
 	struct dahdi_span span;						/* Span */
 	struct dahdi_chan *chans[32];					/* Channels */
 	struct dahdi_echocan_state *ec[32];				/* Echocan state for channels */
+#ifdef CONFIG_VOICEBUS_ECREFERENCE
+	struct dahdi_fifo *ec_reference[32];
+#else
+	unsigned char ec_chunk1[32][DAHDI_CHUNKSIZE];
+	unsigned char ec_chunk2[32][DAHDI_CHUNKSIZE];
+#endif
 	unsigned long ctlreg;
 	struct voicebus vb;
 	atomic_t txints;