Detect E1 alignment errors and fix alignment

git-svn-id: http://svn.digium.com/svn/zaptel/trunk@279 5390a7c7-147a-4af0-8ec9-7488f05a26cb

1 files changed, 54 insertions, 5 deletions

diff --git a/wct4xxp.c b/wct4xxp.c
index 8cf8f9c..57f6fd9 100755
--- a/wct4xxp.c
+++ b/wct4xxp.c
@@ -100,7 +100,9 @@ struct t4 {
 	int syncpos[4];			/* span-relative sync sources */
 	int master;				/* Are we master */
 	int ledreg;				/* LED Register */
+	int e1check[4];			/* E1 check */
 	unsigned int dmactrl;
+	int e1recover;			/* E1 recovery timer */
 	dma_addr_t 	readdma;
 	dma_addr_t	writedma;
 	unsigned long memaddr;		/* Base address of card */
@@ -138,6 +140,7 @@ static int t4_startup(struct zt_span *span);
 static int t4_shutdown(struct zt_span *span);
 static int t4_rbsbits(struct zt_chan *chan, int bits);
 static int t4_maint(struct zt_span *span, int cmd);
+static int t4_reset_dma(struct t4 *wc);
 static int t4_ioctl(struct zt_chan *chan, unsigned int cmd, unsigned long data);
 
 #define WC_RDADDR	0
@@ -905,6 +908,27 @@ found:
 	return 0;
 }
 
+static inline void e1_check(struct t4 *wc, int span, int val)
+{
+	if ((wc->spans[span].channels > 24) &&
+	    (wc->spans[span].flags & ZT_FLAG_RUNNING) &&
+	    !(wc->spans[span].alarms) &&
+	    (!wc->e1recover))   {
+		if (val != 0x1b) {
+			wc->e1check[span]++;
+		} else
+			wc->e1check[span] = 0;
+		if (wc->e1check[span] > 100) {
+			/* Wait 1000 ms */
+			wc->e1recover = 1000 * 8;
+			memset(wc->e1check, 0, sizeof(wc->e1check));
+			if (debug)
+				printk("Detected loss of E1 alignment on span %d!\n", span);
+			t4_reset_dma(wc);
+		}
+	}
+}
+
 static void t4_receiveprep(struct t4 *wc, int irq)
 {
 	volatile unsigned int *readchunk;
@@ -913,12 +937,12 @@ static void t4_receiveprep(struct t4 *wc, int irq)
 	unsigned int tmp;
 	if (irq & 1) {
 		/* First part */
-		readchunk = wc->readchunk + 1;
+		readchunk = wc->readchunk;
 		if (!wc->last0) 
 			dbl = 1;
 		wc->last0 = 0;
 	} else {
-		readchunk = wc->readchunk + ZT_CHUNKSIZE * 32 + 1;
+		readchunk = wc->readchunk + ZT_CHUNKSIZE * 32;
 		if (wc->last0) 
 			dbl = 1;
 		wc->last0 = 1;
@@ -930,19 +954,25 @@ static void t4_receiveprep(struct t4 *wc, int irq)
 			printk("TE410P: Double/missed interrupt detected\n");
 	}
 	for (x=0;x<ZT_CHUNKSIZE;x++) {
-		/* Once per chunk */
 		for (z=0;z<24;z++) {
 			/* All T1/E1 channels */
-			tmp = readchunk[z];
+			tmp = readchunk[z+1];
 			wc->spans[3].chans[z].readchunk[x] = tmp & 0xff;
 			wc->spans[2].chans[z].readchunk[x] = (tmp & 0xff00) >> 8;
 			wc->spans[1].chans[z].readchunk[x] = (tmp & 0xff0000) >> 16;
 			wc->spans[0].chans[z].readchunk[x] = tmp >> 24;
 		}
 		if (wc->t1e1) {
+			if (wc->e1recover > 0)
+				wc->e1recover--;
+			tmp = readchunk[0];
+			e1_check(wc, 3, (tmp & 0x7f));
+			e1_check(wc, 2, (tmp & 0x7f00) >> 8);
+			e1_check(wc, 1, (tmp & 0x7f0000) >> 16);
+			e1_check(wc, 0, (tmp & 0x7f000000) >> 24);
 			for (z=24;z<31;z++) {
 				/* Only E1 channels now */
-				tmp = readchunk[z];
+				tmp = readchunk[z+1];
 				if (wc->spans[3].channels > 24)
 					wc->spans[3].chans[z].readchunk[x] = tmp & 0xff;
 				if (wc->spans[2].channels > 24)
@@ -1355,6 +1385,25 @@ static void t4_interrupt(int irq, void *dev_id, struct pt_regs *regs)
 
 }
 
+static int t4_reset_dma(struct t4 *wc)
+{
+	/* Turn off DMA and such */
+	wc->dmactrl = 0x0;
+	t4_pci_out(wc, WC_DMACTRL, wc->dmactrl);
+	t4_pci_out(wc, WC_COUNT, 0);
+	t4_pci_out(wc, WC_RDADDR, 0);
+	t4_pci_out(wc, WC_WRADDR, 0);
+	t4_pci_out(wc, WC_INTR, 0);
+	/* Turn it all back on */
+	t4_pci_out(wc, WC_RDADDR, wc->readdma);
+	t4_pci_out(wc, WC_WRADDR, wc->writedma);
+	t4_pci_out(wc, WC_COUNT, ((ZT_MAX_CHUNKSIZE * 2 * 32 - 1) << 18) | ((ZT_MAX_CHUNKSIZE * 2 * 32 - 1) << 2));
+	t4_pci_out(wc, WC_INTR, 0);
+	wc->dmactrl = 0xc0000003 | (1 << 29);
+	t4_pci_out(wc, WC_DMACTRL, wc->dmactrl);
+	return 0;
+}
+
 static int t4_hardware_init(struct t4 *wc)
 {
 	int x;