Import FEC code from Luigi Rizzo's RMDP

Paper: http://info.iet.unipi.it/~luigi/mccr6.ps.gz
Code:  http://info.iet.unipi.it/~luigi/rmdp980703.tgz

Signed-off-by: David Woodhouse <dwmw2@infradead.org>

6 files changed, 1267 insertions, 178 deletions

diff --git a/Makefile b/Makefile
index 2283c78..b7ffd58 100644
--- a/Makefile
+++ b/Makefile
@@ -73,12 +73,17 @@ $(BUILDDIR)/jffs2dump: $(BUILDDIR)/jffs2dump.o $(BUILDDIR)/crc32.o
 $(BUILDDIR)/sumtool: $(BUILDDIR)/sumtool.o $(BUILDDIR)/crc32.o
 	$(CC) $(LDFLAGS) -o $@ $^
 
-$(BUILDDIR)/serve_image: $(BUILDDIR)/serve_image.o $(BUILDDIR)/crc32.o
+$(BUILDDIR)/serve_image: $(BUILDDIR)/serve_image.o $(BUILDDIR)/crc32.o $(BUILDDIR)/fec.o
 	$(CC) $(LDFLAGS) -o $@ $^
 
-$(BUILDDIR)/recv_image: $(BUILDDIR)/recv_image.o $(BUILDDIR)/crc32.o
+$(BUILDDIR)/recv_image: $(BUILDDIR)/recv_image.o $(BUILDDIR)/crc32.o $(BUILDDIR)/fec.o
 	$(CC) $(LDFLAGS) -o $@ $^
 
+$(BUILDDIR)/fectest: $(BUILDDIR)/fectest.o $(BUILDDIR)/crc32.o $(BUILDDIR)/fec.o
+	$(CC) $(LDFLAGS) -o $@ $^
+
+
+
 install: ${TARGETS}
 	mkdir -p ${DESTDIR}/${SBINDIR}
 	install -m0755 ${TARGETS} ${DESTDIR}/${SBINDIR}/
diff --git a/fec.c b/fec.c
new file mode 100644
index 0000000..e374dfe
--- /dev/null
+++ b/fec.c
@@ -0,0 +1,898 @@
+/*
+ * fec.c -- forward error correction based on Vandermonde matrices
+ * 980624
+ * (C) 1997-98 Luigi Rizzo (luigi@iet.unipi.it)
+ *
+ * Portions derived from code by Phil Karn (karn@ka9q.ampr.org),
+ * Robert Morelos-Zaragoza (robert@spectra.eng.hawaii.edu) and Hari
+ * Thirumoorthy (harit@spectra.eng.hawaii.edu), Aug 1995
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above
+ *    copyright notice, this list of conditions and the following
+ *    disclaimer in the documentation and/or other materials
+ *    provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+ * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
+ * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
+ * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+ * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGE.
+ */
+
+/*
+ * The following parameter defines how many bits are used for
+ * field elements. The code supports any value from 2 to 16
+ * but fastest operation is achieved with 8 bit elements
+ * This is the only parameter you may want to change.
+ */
+#ifndef GF_BITS
+#define GF_BITS  8	/* code over GF(2**GF_BITS) - change to suit */
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+/*
+ * compatibility stuff
+ */
+#ifdef MSDOS	/* but also for others, e.g. sun... */
+#define NEED_BCOPY
+#define bcmp(a,b,n) memcmp(a,b,n)
+#endif
+
+#ifdef NEED_BCOPY
+#define bcopy(s, d, siz)        memcpy((d), (s), (siz))
+#define bzero(d, siz)   memset((d), '\0', (siz))
+#endif
+
+/*
+ * stuff used for testing purposes only
+ */
+
+#ifdef	TEST
+#define DEB(x)
+#define DDB(x) x
+#define	DEBUG	0	/* minimal debugging */
+#ifdef	MSDOS
+#include <time.h>
+struct timeval {
+    unsigned long ticks;
+};
+#define gettimeofday(x, dummy) { (x)->ticks = clock() ; }
+#define DIFF_T(a,b) (1+ 1000000*(a.ticks - b.ticks) / CLOCKS_PER_SEC )
+typedef unsigned long u_long ;
+typedef unsigned short u_short ;
+#else /* typically, unix systems */
+#include <sys/time.h>
+#define DIFF_T(a,b) \
+	(1+ 1000000*(a.tv_sec - b.tv_sec) + (a.tv_usec - b.tv_usec) )
+#endif
+
+#define TICK(t) \
+	{struct timeval x ; \
+	gettimeofday(&x, NULL) ; \
+	t = x.tv_usec + 1000000* (x.tv_sec & 0xff ) ; \
+	}
+#define TOCK(t) \
+	{ u_long t1 ; TICK(t1) ; \
+	  if (t1 < t) t = 256000000 + t1 - t ; \
+	  else t = t1 - t ; \
+	  if (t == 0) t = 1 ;}
+	
+u_long ticks[10];	/* vars for timekeeping */
+#else
+#define DEB(x)
+#define DDB(x)
+#define TICK(x)
+#define TOCK(x)
+#endif /* TEST */
+
+/*
+ * You should not need to change anything beyond this point.
+ * The first part of the file implements linear algebra in GF.
+ *
+ * gf is the type used to store an element of the Galois Field.
+ * Must constain at least GF_BITS bits.
+ *
+ * Note: unsigned char will work up to GF(256) but int seems to run
+ * faster on the Pentium. We use int whenever have to deal with an
+ * index, since they are generally faster.
+ */
+#if (GF_BITS < 2  && GF_BITS >16)
+#error "GF_BITS must be 2 .. 16"
+#endif
+#if (GF_BITS <= 8)
+typedef unsigned char gf;
+#else
+typedef unsigned short gf;
+#endif
+
+#define	GF_SIZE ((1 << GF_BITS) - 1)	/* powers of \alpha */
+
+/*
+ * Primitive polynomials - see Lin & Costello, Appendix A,
+ * and  Lee & Messerschmitt, p. 453.
+ */
+static char *allPp[] = {    /* GF_BITS	polynomial		*/
+    NULL,		    /*  0	no code			*/
+    NULL,		    /*  1	no code			*/
+    "111",		    /*  2	1+x+x^2			*/
+    "1101",		    /*  3	1+x+x^3			*/
+    "11001",		    /*  4	1+x+x^4			*/
+    "101001",		    /*  5	1+x^2+x^5		*/
+    "1100001",		    /*  6	1+x+x^6			*/
+    "10010001",		    /*  7	1 + x^3 + x^7		*/
+    "101110001",	    /*  8	1+x^2+x^3+x^4+x^8	*/
+    "1000100001",	    /*  9	1+x^4+x^9		*/
+    "10010000001",	    /* 10	1+x^3+x^10		*/
+    "101000000001",	    /* 11	1+x^2+x^11		*/
+    "1100101000001",	    /* 12	1+x+x^4+x^6+x^12	*/
+    "11011000000001",	    /* 13	1+x+x^3+x^4+x^13	*/
+    "110000100010001",	    /* 14	1+x+x^6+x^10+x^14	*/
+    "1100000000000001",	    /* 15	1+x+x^15		*/
+    "11010000000010001"	    /* 16	1+x+x^3+x^12+x^16	*/
+};
+
+
+/*
+ * To speed up computations, we have tables for logarithm, exponent
+ * and inverse of a number. If GF_BITS <= 8, we use a table for
+ * multiplication as well (it takes 64K, no big deal even on a PDA,
+ * especially because it can be pre-initialized an put into a ROM!),
+ * otherwhise we use a table of logarithms.
+ * In any case the macro gf_mul(x,y) takes care of multiplications.
+ */
+
+static gf gf_exp[2*GF_SIZE];	/* index->poly form conversion table	*/
+static int gf_log[GF_SIZE + 1];	/* Poly->index form conversion table	*/
+static gf inverse[GF_SIZE+1];	/* inverse of field elem.		*/
+				/* inv[\alpha**i]=\alpha**(GF_SIZE-i-1)	*/
+
+/*
+ * modnn(x) computes x % GF_SIZE, where GF_SIZE is 2**GF_BITS - 1,
+ * without a slow divide.
+ */
+static inline gf
+modnn(int x)
+{
+    while (x >= GF_SIZE) {
+	x -= GF_SIZE;
+	x = (x >> GF_BITS) + (x & GF_SIZE);
+    }
+    return x;
+}
+
+#define SWAP(a,b,t) {t tmp; tmp=a; a=b; b=tmp;}
+
+/*
+ * gf_mul(x,y) multiplies two numbers. If GF_BITS<=8, it is much
+ * faster to use a multiplication table.
+ *
+ * USE_GF_MULC, GF_MULC0(c) and GF_ADDMULC(x) can be used when multiplying
+ * many numbers by the same constant. In this case the first
+ * call sets the constant, and others perform the multiplications.
+ * A value related to the multiplication is held in a local variable
+ * declared with USE_GF_MULC . See usage in addmul1().
+ */
+#if (GF_BITS <= 8)
+static gf gf_mul_table[GF_SIZE + 1][GF_SIZE + 1];
+
+#define gf_mul(x,y) gf_mul_table[x][y]
+
+#define USE_GF_MULC register gf * __gf_mulc_
+#define GF_MULC0(c) __gf_mulc_ = gf_mul_table[c]
+#define GF_ADDMULC(dst, x) dst ^= __gf_mulc_[x]
+
+static void
+init_mul_table()
+{
+    int i, j;
+    for (i=0; i< GF_SIZE+1; i++)
+	for (j=0; j< GF_SIZE+1; j++)
+	    gf_mul_table[i][j] = gf_exp[modnn(gf_log[i] + gf_log[j]) ] ;
+
+    for (j=0; j< GF_SIZE+1; j++)
+	    gf_mul_table[0][j] = gf_mul_table[j][0] = 0;
+}
+#else	/* GF_BITS > 8 */
+static inline gf
+gf_mul(x,y)
+{
+    if ( (x) == 0 || (y)==0 ) return 0;
+     
+    return gf_exp[gf_log[x] + gf_log[y] ] ;
+}
+#define init_mul_table()
+
+#define USE_GF_MULC register gf * __gf_mulc_
+#define GF_MULC0(c) __gf_mulc_ = &gf_exp[ gf_log[c] ]
+#define GF_ADDMULC(dst, x) { if (x) dst ^= __gf_mulc_[ gf_log[x] ] ; }
+#endif
+
+/*
+ * Generate GF(2**m) from the irreducible polynomial p(X) in p[0]..p[m]
+ * Lookup tables:
+ *     index->polynomial form		gf_exp[] contains j= \alpha^i;
+ *     polynomial form -> index form	gf_log[ j = \alpha^i ] = i
+ * \alpha=x is the primitive element of GF(2^m)
+ *
+ * For efficiency, gf_exp[] has size 2*GF_SIZE, so that a simple
+ * multiplication of two numbers can be resolved without calling modnn
+ */
+
+/*
+ * i use malloc so many times, it is easier to put checks all in
+ * one place.
+ */
+static void *
+my_malloc(int sz, char *err_string)
+{
+    void *p = malloc( sz );
+    if (p == NULL) {
+	fprintf(stderr, "-- malloc failure allocating %s\n", err_string);
+	exit(1) ;
+    }
+    return p ;
+}
+
+#define NEW_GF_MATRIX(rows, cols) \
+    (gf *)my_malloc(rows * cols * sizeof(gf), " ## __LINE__ ## " )
+
+/*
+ * initialize the data structures used for computations in GF.
+ */
+static void
+generate_gf(void)
+{
+    int i;
+    gf mask;
+    char *Pp =  allPp[GF_BITS] ;
+
+    mask = 1;	/* x ** 0 = 1 */
+    gf_exp[GF_BITS] = 0; /* will be updated at the end of the 1st loop */
+    /*
+     * first, generate the (polynomial representation of) powers of \alpha,
+     * which are stored in gf_exp[i] = \alpha ** i .
+     * At the same time build gf_log[gf_exp[i]] = i .
+     * The first GF_BITS powers are simply bits shifted to the left.
+     */
+    for (i = 0; i < GF_BITS; i++, mask <<= 1 ) {
+	gf_exp[i] = mask;
+	gf_log[gf_exp[i]] = i;
+	/*
+	 * If Pp[i] == 1 then \alpha ** i occurs in poly-repr
+	 * gf_exp[GF_BITS] = \alpha ** GF_BITS
+	 */
+	if ( Pp[i] == '1' )
+	    gf_exp[GF_BITS] ^= mask;
+    }
+    /*
+     * now gf_exp[GF_BITS] = \alpha ** GF_BITS is complete, so can als
+     * compute its inverse.
+     */
+    gf_log[gf_exp[GF_BITS]] = GF_BITS;
+    /*
+     * Poly-repr of \alpha ** (i+1) is given by poly-repr of
+     * \alpha ** i shifted left one-bit and accounting for any
+     * \alpha ** GF_BITS term that may occur when poly-repr of
+     * \alpha ** i is shifted.
+     */
+    mask = 1 << (GF_BITS - 1 ) ;
+    for (i = GF_BITS + 1; i < GF_SIZE; i++) {
+	if (gf_exp[i - 1] >= mask)
+	    gf_exp[i] = gf_exp[GF_BITS] ^ ((gf_exp[i - 1] ^ mask) << 1);
+	else
+	    gf_exp[i] = gf_exp[i - 1] << 1;
+	gf_log[gf_exp[i]] = i;
+    }
+    /*
+     * log(0) is not defined, so use a special value
+     */
+    gf_log[0] =	GF_SIZE ;
+    /* set the extended gf_exp values for fast multiply */
+    for (i = 0 ; i < GF_SIZE ; i++)
+	gf_exp[i + GF_SIZE] = gf_exp[i] ;
+
+    /*
+     * again special cases. 0 has no inverse. This used to
+     * be initialized to GF_SIZE, but it should make no difference
+     * since noone is supposed to read from here.
+     */
+    inverse[0] = 0 ;
+    inverse[1] = 1;
+    for (i=2; i<=GF_SIZE; i++)
+	inverse[i] = gf_exp[GF_SIZE-gf_log[i]];
+}
+
+/*
+ * Various linear algebra operations that i use often.
+ */
+
+/*
+ * addmul() computes dst[] = dst[] + c * src[]
+ * This is used often, so better optimize it! Currently the loop is
+ * unrolled 16 times, a good value for 486 and pentium-class machines.
+ * The case c=0 is also optimized, whereas c=1 is not. These
+ * calls are unfrequent in my typical apps so I did not bother.
+ * 
+ * Note that gcc on
+ */
+#define addmul(dst, src, c, sz) \
+    if (c != 0) addmul1(dst, src, c, sz)
+
+#define UNROLL 16 /* 1, 4, 8, 16 */
+static void
+addmul1(gf *dst1, gf *src1, gf c, int sz)
+{
+    USE_GF_MULC ;
+    register gf *dst = dst1, *src = src1 ;
+    gf *lim = &dst[sz - UNROLL + 1] ;
+
+    GF_MULC0(c) ;
+
+#if (UNROLL > 1) /* unrolling by 8/16 is quite effective on the pentium */
+    for (; dst < lim ; dst += UNROLL, src += UNROLL ) {
+	GF_ADDMULC( dst[0] , src[0] );
+	GF_ADDMULC( dst[1] , src[1] );
+	GF_ADDMULC( dst[2] , src[2] );
+	GF_ADDMULC( dst[3] , src[3] );
+#if (UNROLL > 4)
+	GF_ADDMULC( dst[4] , src[4] );
+	GF_ADDMULC( dst[5] , src[5] );
+	GF_ADDMULC( dst[6] , src[6] );
+	GF_ADDMULC( dst[7] , src[7] );
+#endif
+#if (UNROLL > 8)
+	GF_ADDMULC( dst[8] , src[8] );
+	GF_ADDMULC( dst[9] , src[9] );
+	GF_ADDMULC( dst[10] , src[10] );
+	GF_ADDMULC( dst[11] , src[11] );
+	GF_ADDMULC( dst[12] , src[12] );
+	GF_ADDMULC( dst[13] , src[13] );
+	GF_ADDMULC( dst[14] , src[14] );
+	GF_ADDMULC( dst[15] , src[15] );
+#endif
+    }
+#endif
+    lim += UNROLL - 1 ;
+    for (; dst < lim; dst++, src++ )		/* final components */
+	GF_ADDMULC( *dst , *src );
+}
+
+/*
+ * computes C = AB where A is n*k, B is k*m, C is n*m
+ */
+static void
+matmul(gf *a, gf *b, gf *c, int n, int k, int m)
+{
+    int row, col, i ;
+
+    for (row = 0; row < n ; row++) {
+	for (col = 0; col < m ; col++) {
+	    gf *pa = &a[ row * k ];
+	    gf *pb = &b[ col ];
+	    gf acc = 0 ;
+	    for (i = 0; i < k ; i++, pa++, pb += m )
+		acc ^= gf_mul( *pa, *pb ) ;
+	    c[ row * m + col ] = acc ;
+	}
+    }
+}
+
+#ifdef DEBUG
+/*
+ * returns 1 if the square matrix is identiy
+ * (only for test)
+ */
+static int
+is_identity(gf *m, int k)
+{
+    int row, col ;
+    for (row=0; row<k; row++)
+	for (col=0; col<k; col++)
+	    if ( (row==col && *m != 1) ||
+		 (row!=col && *m != 0) )
+		 return 0 ;
+	    else
+		m++ ;
+    return 1 ;
+}
+#endif /* debug */
+
+/*
+ * invert_mat() takes a matrix and produces its inverse
+ * k is the size of the matrix.
+ * (Gauss-Jordan, adapted from Numerical Recipes in C)
+ * Return non-zero if singular.
+ */
+DEB( int pivloops=0; int pivswaps=0 ; /* diagnostic */)
+static int
+invert_mat(gf *src, int k)
+{
+    gf c, *p ;
+    int irow, icol, row, col, i, ix ;
+
+    int error = 1 ;
+    int *indxc = my_malloc(k*sizeof(int), "indxc");
+    int *indxr = my_malloc(k*sizeof(int), "indxr");
+    int *ipiv = my_malloc(k*sizeof(int), "ipiv");
+    gf *id_row = NEW_GF_MATRIX(1, k);
+    gf *temp_row = NEW_GF_MATRIX(1, k);
+
+    bzero(id_row, k*sizeof(gf));
+    DEB( pivloops=0; pivswaps=0 ; /* diagnostic */ )
+    /*
+     * ipiv marks elements already used as pivots.
+     */
+    for (i = 0; i < k ; i++)
+	ipiv[i] = 0 ;
+
+    for (col = 0; col < k ; col++) {
+	gf *pivot_row ;
+	/*
+	 * Zeroing column 'col', look for a non-zero element.
+	 * First try on the diagonal, if it fails, look elsewhere.
+	 */
+	irow = icol = -1 ;
+	if (ipiv[col] != 1 && src[col*k + col] != 0) {
+	    irow = col ;
+	    icol = col ;
+	    goto found_piv ;
+	}
+	for (row = 0 ; row < k ; row++) {
+	    if (ipiv[row] != 1) {
+		for (ix = 0 ; ix < k ; ix++) {
+		    DEB( pivloops++ ; )
+		    if (ipiv[ix] == 0) {
+			if (src[row*k + ix] != 0) {
+			    irow = row ;
+			    icol = ix ;
+			    goto found_piv ;
+			}
+		    } else if (ipiv[ix] > 1) {
+			fprintf(stderr, "singular matrix\n");
+			goto fail ; 
+		    }
+		}
+	    }
+	}
+	if (icol == -1) {
+	    fprintf(stderr, "XXX pivot not found!\n");
+	    goto fail ;
+	}
+found_piv:
+	++(ipiv[icol]) ;
+	/*
+	 * swap rows irow and icol, so afterwards the diagonal
+	 * element will be correct. Rarely done, not worth
+	 * optimizing.
+	 */
+	if (irow != icol) {
+	    for (ix = 0 ; ix < k ; ix++ ) {
+		SWAP( src[irow*k + ix], src[icol*k + ix], gf) ;
+	    }
+	}
+	indxr[col] = irow ;
+	indxc[col] = icol ;
+	pivot_row = &src[icol*k] ;
+	c = pivot_row[icol] ;
+	if (c == 0) {
+	    fprintf(stderr, "singular matrix 2\n");
+	    goto fail ;
+	}
+	if (c != 1 ) { /* otherwhise this is a NOP */
+	    /*
+	     * this is done often , but optimizing is not so
+	     * fruitful, at least in the obvious ways (unrolling)
+	     */
+	    DEB( pivswaps++ ; )
+	    c = inverse[ c ] ;
+	    pivot_row[icol] = 1 ;
+	    for (ix = 0 ; ix < k ; ix++ )
+		pivot_row[ix] = gf_mul(c, pivot_row[ix] );
+	}
+	/*
+	 * from all rows, remove multiples of the selected row
+	 * to zero the relevant entry (in fact, the entry is not zero
+	 * because we know it must be zero).
+	 * (Here, if we know that the pivot_row is the identity,
+	 * we can optimize the addmul).
+	 */
+	id_row[icol] = 1;
+	if (bcmp(pivot_row, id_row, k*sizeof(gf)) != 0) {
+	    for (p = src, ix = 0 ; ix < k ; ix++, p += k ) {
+		if (ix != icol) {
+		    c = p[icol] ;
+		    p[icol] = 0 ;
+		    addmul(p, pivot_row, c, k );
+		}
+	    }
+	}
+	id_row[icol] = 0;
+    } /* done all columns */
+    for (col = k-1 ; col >= 0 ; col-- ) {
+	if (indxr[col] <0 || indxr[col] >= k)
+	    fprintf(stderr, "AARGH, indxr[col] %d\n", indxr[col]);
+	else if (indxc[col] <0 || indxc[col] >= k)
+	    fprintf(stderr, "AARGH, indxc[col] %d\n", indxc[col]);
+	else
+	if (indxr[col] != indxc[col] ) {
+	    for (row = 0 ; row < k ; row++ ) {
+		SWAP( src[row*k + indxr[col]], src[row*k + indxc[col]], gf) ;
+	    }
+	}
+    }
+    error = 0 ;
+fail:
+    free(indxc);
+    free(indxr);
+    free(ipiv);
+    free(id_row);
+    free(temp_row);
+    return error ;
+}
+
+/*
+ * fast code for inverting a vandermonde matrix.
+ * XXX NOTE: It assumes that the matrix
+ * is not singular and _IS_ a vandermonde matrix. Only uses
+ * the second column of the matrix, containing the p_i's.
+ *
+ * Algorithm borrowed from "Numerical recipes in C" -- sec.2.8, but
+ * largely revised for my purposes.
+ * p = coefficients of the matrix (p_i)
+ * q = values of the polynomial (known)
+ */
+
+int
+invert_vdm(gf *src, int k)
+{
+    int i, j, row, col ;
+    gf *b, *c, *p;
+    gf t, xx ;
+
+    if (k == 1) 	/* degenerate case, matrix must be p^0 = 1 */
+	return 0 ;
+    /*
+     * c holds the coefficient of P(x) = Prod (x - p_i), i=0..k-1
+     * b holds the coefficient for the matrix inversion
+     */
+    c = NEW_GF_MATRIX(1, k);
+    b = NEW_GF_MATRIX(1, k);
+
+    p = NEW_GF_MATRIX(1, k);
+   
+    for ( j=1, i = 0 ; i < k ; i++, j+=k ) {
+	c[i] = 0 ;
+	p[i] = src[j] ;    /* p[i] */
+    }
+    /*
+     * construct coeffs. recursively. We know c[k] = 1 (implicit)
+     * and start P_0 = x - p_0, then at each stage multiply by
+     * x - p_i generating P_i = x P_{i-1} - p_i P_{i-1}
+     * After k steps we are done.
+     */
+    c[k-1] = p[0] ;	/* really -p(0), but x = -x in GF(2^m) */
+    for (i = 1 ; i < k ; i++ ) {
+	gf p_i = p[i] ; /* see above comment */
+	for (j = k-1  - ( i - 1 ) ; j < k-1 ; j++ )
+	    c[j] ^= gf_mul( p_i, c[j+1] ) ;
+	c[k-1] ^= p_i ;
+    }
+
+    for (row = 0 ; row < k ; row++ ) {
+	/*
+	 * synthetic division etc.
+	 */
+	xx = p[row] ;
+	t = 1 ;
+	b[k-1] = 1 ; /* this is in fact c[k] */
+	for (i = k-2 ; i >= 0 ; i-- ) {
+	    b[i] = c[i+1] ^ gf_mul(xx, b[i+1]) ;
+	    t = gf_mul(xx, t) ^ b[i] ;
+	}
+	for (col = 0 ; col < k ; col++ )
+	    src[col*k + row] = gf_mul(inverse[t], b[col] );
+    }
+    free(c) ;
+    free(b) ;
+    free(p) ;
+    return 0 ;
+}
+
+static int fec_initialized = 0 ;
+static void
+init_fec()
+{
+    TICK(ticks[0]);
+    generate_gf();
+    TOCK(ticks[0]);
+    DDB(fprintf(stderr, "generate_gf took %ldus\n", ticks[0]);)
+    TICK(ticks[0]);
+    init_mul_table();
+    TOCK(ticks[0]);
+    DDB(fprintf(stderr, "init_mul_table took %ldus\n", ticks[0]);)
+    fec_initialized = 1 ;
+}
+
+/*
+ * This section contains the proper FEC encoding/decoding routines.
+ * The encoding matrix is computed starting with a Vandermonde matrix,
+ * and then transforming it into a systematic matrix.
+ */
+
+#define FEC_MAGIC	0xFECC0DEC
+
+struct fec_parms {
+    u_long magic ;
+    int k, n ;		/* parameters of the code */
+    gf *enc_matrix ;
+} ;
+
+void
+fec_free(struct fec_parms *p)
+{
+    if (p==NULL ||
+       p->magic != ( ( (FEC_MAGIC ^ p->k) ^ p->n) ^ (int)(p->enc_matrix)) ) {
+	fprintf(stderr, "bad parameters to fec_free\n");
+	return ;
+    }
+    free(p->enc_matrix);
+    free(p);
+}
+
+/*
+ * create a new encoder, returning a descriptor. This contains k,n and
+ * the encoding matrix.
+ */
+struct fec_parms *
+fec_new(int k, int n)
+{
+    int row, col ;
+    gf *p, *tmp_m ;
+
+    struct fec_parms *retval ;
+
+    if (fec_initialized == 0)
+	init_fec();
+
+    if (k > GF_SIZE + 1 || n > GF_SIZE + 1 || k > n ) {
+	fprintf(stderr, "Invalid parameters k %d n %d GF_SIZE %d\n",
+		k, n, GF_SIZE );
+	return NULL ;
+    }
+    retval = my_malloc(sizeof(struct fec_parms), "new_code");
+    retval->k = k ;
+    retval->n = n ;
+    retval->enc_matrix = NEW_GF_MATRIX(n, k);
+    retval->magic = ( ( FEC_MAGIC ^ k) ^ n) ^ (int)(retval->enc_matrix) ;
+    tmp_m = NEW_GF_MATRIX(n, k);
+    /*
+     * fill the matrix with powers of field elements, starting from 0.
+     * The first row is special, cannot be computed with exp. table.
+     */
+    tmp_m[0] = 1 ;
+    for (col = 1; col < k ; col++)
+	tmp_m[col] = 0 ;
+    for (p = tmp_m + k, row = 0; row < n-1 ; row++, p += k) {
+	for ( col = 0 ; col < k ; col ++ )
+	    p[col] = gf_exp[modnn(row*col)];
+    }
+
+    /*
+     * quick code to build systematic matrix: invert the top
+     * k*k vandermonde matrix, multiply right the bottom n-k rows
+     * by the inverse, and construct the identity matrix at the top.
+     */
+    TICK(ticks[3]);
+    invert_vdm(tmp_m, k); /* much faster than invert_mat */
+    matmul(tmp_m + k*k, tmp_m, retval->enc_matrix + k*k, n - k, k, k);
+    /*
+     * the upper matrix is I so do not bother with a slow multiply
+     */
+    bzero(retval->enc_matrix, k*k*sizeof(gf) );
+    for (p = retval->enc_matrix, col = 0 ; col < k ; col++, p += k+1 )
+	*p = 1 ;
+    free(tmp_m);
+    TOCK(ticks[3]);
+
+    DDB(fprintf(stderr, "--- %ld us to build encoding matrix\n",
+	    ticks[3]);)
+    DEB(pr_matrix(retval->enc_matrix, n, k, "encoding_matrix");)
+    return retval ;
+}
+
+/*
+ * fec_encode accepts as input pointers to n data packets of size sz,
+ * and produces as output a packet pointed to by fec, computed
+ * with index "index".
+ */
+void
+fec_encode(struct fec_parms *code, gf *src[], gf *fec, int index, int sz)
+{
+    int i, k = code->k ;
+    gf *p ;
+
+    if (GF_BITS > 8)
+	sz /= 2 ;
+
+    if (index < k)
+         bcopy(src[index], fec, sz*sizeof(gf) ) ;
+    else if (index < code->n) {
+	p = &(code->enc_matrix[index*k] );
+	bzero(fec, sz*sizeof(gf));
+	for (i = 0; i < k ; i++)
+	    addmul(fec, src[i], p[i], sz ) ;
+    } else
+	fprintf(stderr, "Invalid index %d (max %d)\n",
+	    index, code->n - 1 );
+}
+
+/*
+ * shuffle move src packets in their position
+ */
+static int
+shuffle(gf *pkt[], int index[], int k)
+{
+    int i;
+
+    for ( i = 0 ; i < k ; ) {
+	if (index[i] >= k || index[i] == i)
+	    i++ ;
+	else {
+	    /*
+	     * put pkt in the right position (first check for conflicts).
+	     */
+	    int c = index[i] ;
+
+	    if (index[c] == c) {
+		DEB(fprintf(stderr, "\nshuffle, error at %d\n", i);)
+		return 1 ;
+	    }
+	    SWAP(index[i], index[c], int) ;
+	    SWAP(pkt[i], pkt[c], gf *) ;
+	}
+    }
+    DEB( /* just test that it works... */
+    for ( i = 0 ; i < k ; i++ ) {
+	if (index[i] < k && index[i] != i) {
+	    fprintf(stderr, "shuffle: after\n");
+	    for (i=0; i<k ; i++) fprintf(stderr, "%3d ", index[i]);
+	    fprintf(stderr, "\n");
+	    return 1 ;
+	}
+    }
+    )
+    return 0 ;
+}
+
+/*
+ * build_decode_matrix constructs the encoding matrix given the
+ * indexes. The matrix must be already allocated as
+ * a vector of k*k elements, in row-major order
+ */
+static gf *
+build_decode_matrix(struct fec_parms *code, gf *pkt[], int index[])
+{
+    int i , k = code->k ;
+    gf *p, *matrix = NEW_GF_MATRIX(k, k);
+
+    TICK(ticks[9]);
+    for (i = 0, p = matrix ; i < k ; i++, p += k ) {
+#if 1 /* this is simply an optimization, not very useful indeed */
+	if (index[i] < k) {
+	    bzero(p, k*sizeof(gf) );
+	    p[i] = 1 ;
+	} else
+#endif
+	if (index[i] < code->n )
+	    bcopy( &(code->enc_matrix[index[i]*k]), p, k*sizeof(gf) ); 
+	else {
+	    fprintf(stderr, "decode: invalid index %d (max %d)\n",
+		index[i], code->n - 1 );
+	    free(matrix) ;
+	    return NULL ;
+	}
+    }
+    TICK(ticks[9]);
+    if (invert_mat(matrix, k)) {
+	free(matrix);
+	matrix = NULL ;
+    }
+    TOCK(ticks[9]);
+    return matrix ;
+}
+
+/*
+ * fec_decode receives as input a vector of packets, the indexes of
+ * packets, and produces the correct vector as output.
+ *
+ * Input:
+ *	code: pointer to code descriptor
+ *	pkt:  pointers to received packets. They are modified
+ *	      to store the output packets (in place)
+ *	index: pointer to packet indexes (modified)
+ *	sz:    size of each packet
+ */
+int
+fec_decode(struct fec_parms *code, gf *pkt[], int index[], int sz)
+{
+    gf *m_dec ; 
+    gf **new_pkt ;
+    int row, col , k = code->k ;
+
+    if (GF_BITS > 8)
+	sz /= 2 ;
+
+    if (shuffle(pkt, index, k))	/* error if true */
+	return 1 ;
+    m_dec = build_decode_matrix(code, pkt, index);
+
+    if (m_dec == NULL)
+	return 1 ; /* error */
+    /*
+     * do the actual decoding
+     */
+    new_pkt = my_malloc (k * sizeof (gf * ), "new pkt pointers" );
+    for (row = 0 ; row < k ; row++ ) {
+	if (index[row] >= k) {
+	    new_pkt[row] = my_malloc (sz * sizeof (gf), "new pkt buffer" );
+	    bzero(new_pkt[row], sz * sizeof(gf) ) ;
+	    for (col = 0 ; col < k ; col++ )
+		addmul(new_pkt[row], pkt[col], m_dec[row*k + col], sz) ;
+	}
+    }
+    /*
+     * move pkts to their final destination
+     */
+    for (row = 0 ; row < k ; row++ ) {
+	if (index[row] >= k) {
+	    bcopy(new_pkt[row], pkt[row], sz*sizeof(gf));
+	    free(new_pkt[row]);
+	}
+    }
+    free(new_pkt);
+    free(m_dec);
+
+    return 0;
+}
+
+/*********** end of FEC code -- beginning of test code ************/
+
+#if (TEST || DEBUG)
+void
+test_gf()
+{
+    int i ;
+    /*
+     * test gf tables. Sufficiently tested...
+     */
+    for (i=0; i<= GF_SIZE; i++) {
+        if (gf_exp[gf_log[i]] != i)
+	    fprintf(stderr, "bad exp/log i %d log %d exp(log) %d\n",
+		i, gf_log[i], gf_exp[gf_log[i]]);
+
+        if (i != 0 && gf_mul(i, inverse[i]) != 1)
+	    fprintf(stderr, "bad mul/inv i %d inv %d i*inv(i) %d\n",
+		i, inverse[i], gf_mul(i, inverse[i]) );
+	if (gf_mul(0,i) != 0)
+	    fprintf(stderr, "bad mul table 0,%d\n",i);
+	if (gf_mul(i,0) != 0)
+	    fprintf(stderr, "bad mul table %d,0\n",i);
+    }
+}
+#endif /* TEST */
diff --git a/fectest.c b/fectest.c
new file mode 100644
index 0000000..d5893b9
--- /dev/null
+++ b/fectest.c
@@ -0,0 +1,92 @@
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include "mcast_image.h"
+#include "crc32.h"
+
+#define ERASE_SIZE 131072
+//#define PKT_SIZE 1400
+#define NR_PKTS ((ERASE_SIZE + PKT_SIZE - 1) / PKT_SIZE)
+#define DROPS 8
+
+int main(void)
+{
+	int i, j;
+	unsigned char buf[NR_PKTS * PKT_SIZE];
+	unsigned char pktbuf[(NR_PKTS + DROPS) * PKT_SIZE];
+	struct fec_parms *fec;
+	unsigned char *srcs[NR_PKTS];
+	unsigned char *pkt[NR_PKTS + DROPS];
+	int pktnr[NR_PKTS + DROPS];
+	struct timeval then, now;
+
+	srand(3453);
+	for (i=0; i < sizeof(buf); i++)
+		if (i < ERASE_SIZE)
+			buf[i] = rand();
+		else
+			buf[i] = 0;
+
+	for (i=0; i < NR_PKTS + DROPS; i++)
+		srcs[i] = buf + (i * PKT_SIZE);
+
+	for (i=0; i < NR_PKTS + DROPS; i++) {
+		pkt[i] = malloc(PKT_SIZE);
+		pktnr[i] = -1;
+	}
+	fec = fec_new(NR_PKTS, NR_PKTS + DROPS);
+	if (!fec) {
+		printf("fec_init() failed\n");
+		exit(1);
+	}
+	j = 0;
+	for (i=0; i < NR_PKTS + DROPS; i++) {
+#if 1
+		if (i == 27  || i == 40  || i == 44 || i == 45 || i == 56 )
+			continue;
+#endif
+		if (i == 69 || i == 93 || i == 103)
+			continue;
+		fec_encode(fec, srcs, pkt[j], i, PKT_SIZE);
+		pktnr[j] = i;
+		j++;
+	}
+	gettimeofday(&then, NULL);
+	if (fec_decode(fec, pkt, pktnr, PKT_SIZE)) {
+		printf("Decode failed\n");
+		exit(1);
+	}
+
+	for (i=0; i < NR_PKTS; i++)
+		memcpy(pktbuf + (i*PKT_SIZE), pkt[i], PKT_SIZE);
+	gettimeofday(&now, NULL);
+	now.tv_sec -= then.tv_sec;
+	now.tv_usec -= then.tv_usec;
+	if (now.tv_usec < 0) {
+		now.tv_usec += 1000000;
+		now.tv_sec--;
+	}
+
+	if (memcmp(pktbuf, buf, ERASE_SIZE)) {
+		int fd;
+		printf("Compare failed\n");
+		fd = open("before", O_WRONLY|O_TRUNC|O_CREAT, 0644);
+		if (fd >= 0)
+			write(fd, buf, ERASE_SIZE);
+		close(fd);
+		fd = open("after", O_WRONLY|O_TRUNC|O_CREAT, 0644);
+		if (fd >= 0)
+			write(fd, pktbuf, ERASE_SIZE);
+		
+		exit(1);
+	}
+
+	printf("Decoded in %ld.%06lds\n", now.tv_sec, now.tv_usec);
+	return 0;
+}
diff --git a/mcast_image.h b/mcast_image.h
index 96aa752..26c675e 100644
--- a/mcast_image.h
+++ b/mcast_image.h
@@ -1,14 +1,16 @@
 #include <stdint.h>
 
-#define PKT_SIZE 1400
+#define PKT_SIZE 1410
 
 struct image_pkt_hdr {
 	uint32_t resend;
 	uint32_t totcrc;
 	uint32_t nr_blocks;
 	uint32_t blocksize;
+	uint32_t block_crc;
 	uint32_t block_nr;
-	uint32_t block_ofs;
+	uint16_t pkt_nr;
+	uint16_t nr_pkts;
 	uint32_t thislen;
 	uint32_t thiscrc;
 };
@@ -17,3 +19,30 @@ struct image_pkt {
 	struct image_pkt_hdr hdr;
 	unsigned char data[PKT_SIZE];
 };
+
+struct fec_parms;
+
+/* k - number of actual data packets
+ * n - total number of packets including data and redundant packets 
+ *   (actual packet size isn't relevant here) */
+struct fec_parms *fec_new(int k, int n);
+void fec_free(struct fec_parms *p);
+
+/* src   - array of (n) pointers to data packets
+ * fec   - buffer for packet to be generated
+ * index - index of packet to be generated (0 <= index < n)
+ * sz    - data packet size
+ */
+void fec_encode(struct fec_parms *code, unsigned char *src[],
+		unsigned char *fec, int index, int sz);
+
+/* data  - array of (k) pointers to data packets, in arbitrary order (see i)
+ * i     - indices of (data) packets
+ * sz    - data packet size
+ * 
+ * Will never fail as long as you give it (k) individual data packets.
+ * Will re-order the (data) pointers but not the indices -- data packets
+ * are ordered on return.
+ */
+int fec_decode(struct fec_parms *code, unsigned char *data[],
+	       int i[], int sz);
diff --git a/recv_image.c b/recv_image.c
index d2b8813..f9705f6 100644
--- a/recv_image.c
+++ b/recv_image.c
@@ -22,33 +22,31 @@
 
 int main(int argc, char **argv)
 {
-	struct sockaddr_storage server_addr;
-	socklen_t server_addrlen = sizeof(server_addr);
 	struct addrinfo *ai;
 	struct addrinfo hints;
 	struct addrinfo *runp;
 	int ret;
 	int sock;
-	struct image_pkt pktbuf;
 	size_t len;
 	int flfd;
 	struct mtd_info_user meminfo;
 	unsigned char *eb_buf;
 	unsigned char *blockmap = NULL;
-	unsigned char *subblockmap;
 	int nr_blocks = 0;
-	int nr_subblocks = 0;
+	int *pkt_indices;
+	unsigned char **pkts;
+	int nr_pkts = 0;
 	int pkts_per_block;
 	int block_nr = -1;
 	uint32_t image_crc;
 	uint32_t blocks_received = 0;
-	uint32_t block_ofs;
 	loff_t mtdoffset = 0;
 	int *stats;
 	int badcrcs = 0;
 	int duplicates = 0;
-	int missing = -1;
 	int file_mode = 0;
+	struct fec_parms *fec;
+	int i;
 
 	if (argc != 4) {
 		fprintf(stderr, "usage: %s <host> <port> <mtddev>\n",
@@ -84,26 +82,32 @@ int main(int argc, char **argv)
 
 	pkts_per_block = (meminfo.erasesize + PKT_SIZE - 1) / PKT_SIZE;
 
-	stats = malloc(pkts_per_block + 1);
-	if (!stats) {
-		fprintf(stderr, "No memory for statistics\n");
-		exit(1);
-	}
-	memset(stats, 0, sizeof(int) * (pkts_per_block + 1));
-
 	eb_buf = malloc(pkts_per_block * PKT_SIZE);
 	if (!eb_buf) {
 		fprintf(stderr, "No memory for eraseblock buffer\n");
 		exit(1);
 	}
-	memset(eb_buf, 0, pkts_per_block * PKT_SIZE);
 
-	subblockmap = malloc(pkts_per_block + 1);
-	if (!subblockmap) {
-		fprintf(stderr, "No memory for subblock map\n");
+	pkt_indices = malloc(sizeof(int) * pkts_per_block);
+	if (!pkt_indices) {
+		fprintf(stderr, "No memory for packet indices\n");
+		exit(1);
+	}
+	memset(pkt_indices, 0, sizeof(int) * pkts_per_block);
+
+	pkts = malloc(sizeof(unsigned char *) * pkts_per_block);
+	if (!pkts) {
+		fprintf(stderr, "No memory for packet pointers\n");
 		exit(1);
 	}
-	memset(subblockmap, 0, pkts_per_block + 1);
+	for (i=0; i<pkts_per_block; i++) {
+		pkts[i] = malloc(sizeof(struct image_pkt_hdr) + PKT_SIZE);
+		if (!pkts[i]) {
+			printf("No memory for packets\n");
+			exit(1);
+		}
+		pkts[i] += sizeof(struct image_pkt_hdr);
+	}
 
 	memset(&hints, 0, sizeof(hints));
 	hints.ai_flags = AI_ADDRCONFIG;
@@ -143,7 +147,7 @@ int main(int argc, char **argv)
 				close(sock);
 				continue;
 			}
-		} else printf("not multicast?\n");
+		}
 		if (bind(sock, runp->ai_addr, runp->ai_addrlen)) {
 			perror("bind");
 			close(sock);
@@ -154,56 +158,85 @@ int main(int argc, char **argv)
 	if (!runp)
 		exit(1);
 
-	while ((len = read(sock, &pktbuf, sizeof(pktbuf))) >= 0) {
-		if (len < sizeof(pktbuf.hdr)) {
-			fprintf(stderr, "Short read %d bytes\n", len);
-			continue;
+	while (1) {
+		struct image_pkt *thispkt;
+
+		if (nr_pkts < pkts_per_block)
+			thispkt = (void *)(pkts[nr_pkts] - sizeof(struct image_pkt_hdr));
+		else
+			thispkt = (void *)(pkts[0] - sizeof(struct image_pkt_hdr));
+
+		len = read(sock, thispkt, sizeof(*thispkt));
+
+		if (len < 0) {
+			perror("read socket");
+			break;
 		}
-		if (len != sizeof(pktbuf.hdr) + ntohl(pktbuf.hdr.thislen)) {
-			fprintf(stderr, "Wrong length %d bytes (expected %d+%d)\n",
-				len, sizeof(pktbuf.hdr), ntohl(pktbuf.hdr.thislen));
+		if (len < sizeof(*thispkt)) {
+			fprintf(stderr, "Wrong length %d bytes (expected %d)\n",
+				len, sizeof(*thispkt));
 			continue;
 		}
-		/* Holds _data_ length */
-		len -= sizeof(pktbuf.hdr);
-			
 		if (!blockmap) {
-			image_crc = pktbuf.hdr.totcrc;
-			if (meminfo.erasesize != ntohl(pktbuf.hdr.blocksize)) {
+			image_crc = thispkt->hdr.totcrc;
+			if (meminfo.erasesize != ntohl(thispkt->hdr.blocksize)) {
 				fprintf(stderr, "Erasesize mismatch (0x%x not 0x%x)\n",
-					ntohl(pktbuf.hdr.blocksize), meminfo.erasesize);
+					ntohl(thispkt->hdr.blocksize), meminfo.erasesize);
 				exit(1);
 			}
-			nr_blocks = ntohl(pktbuf.hdr.nr_blocks);
-			nr_subblocks = pkts_per_block + 2;
+			nr_blocks = ntohl(thispkt->hdr.nr_blocks);
+			nr_pkts = 0;
+
+			fec = fec_new(pkts_per_block, ntohs(thispkt->hdr.nr_pkts));
+
 			blockmap = malloc(nr_blocks);
 			if (!blockmap) {
 				fprintf(stderr, "No memory for block map\n");
 				exit(1);
 			}
 			memset(blockmap, 0, nr_blocks);
+			stats = malloc(sizeof(int) * (ntohs(thispkt->hdr.nr_pkts) + 1));
+			if (!stats) {
+				fprintf(stderr, "No memory for statistics\n");
+				exit(1);
+			}
+			memset(stats, 0, sizeof(int) * (ntohs(thispkt->hdr.nr_pkts) + 1));
 		}
-		if (image_crc != pktbuf.hdr.totcrc) {
+		if (image_crc != thispkt->hdr.totcrc) {
 			fprintf(stderr, "Image CRC changed from 0x%x to 0x%x. Aborting\n",
-				ntohl(image_crc), ntohl(pktbuf.hdr.totcrc));
+				ntohl(image_crc), ntohl(thispkt->hdr.totcrc));
 			exit(1);
 		}
-		if (ntohl(pktbuf.hdr.block_nr) != block_nr) {
+		if (ntohl(thispkt->hdr.block_nr) != block_nr) {
 			/* Hm, new block */
-			if (nr_subblocks < pkts_per_block &&
-			    block_nr != -1) 
-				printf("Lost image block at %08x with only %d/%d packets\n",
-				       block_nr * meminfo.erasesize, nr_subblocks,
-				       pkts_per_block + 1);
+			if (block_nr != -1) {
+				if (!blockmap[block_nr]) {
+					printf("Lost image block %08x with only %d/%d (%d) packets\n",
+					       block_nr * meminfo.erasesize, nr_pkts, pkts_per_block,
+					       ntohs(thispkt->hdr.nr_pkts));
+				}
+				if (blockmap[block_nr] < 2) {
+					stats[nr_pkts]++;
+					if (blockmap[block_nr]) {
+						if (file_mode)
+							printf(" with %d/%d (%d) packets\n",
+							       nr_pkts, pkts_per_block,
+							       ntohs(thispkt->hdr.nr_pkts));
+						blockmap[block_nr] = 2;
+					}
+				}
+			}
+			/* Put this packet first */
+			if (nr_pkts != 0 && nr_pkts < pkts_per_block) {
+				unsigned char *tmp = pkts[0];
+				pkts[0] = pkts[nr_pkts];
+				pkts[nr_pkts] = tmp;
 
+			}
+			nr_pkts = 0;
 
-			if (nr_subblocks < pkts_per_block + 2)
-				stats[nr_subblocks]++;
+			block_nr = ntohl(thispkt->hdr.block_nr);
 
-			nr_subblocks = 0;
-			missing = -1;
-			memset(subblockmap, 0, pkts_per_block + 1);
-			block_nr = ntohl(pktbuf.hdr.block_nr);
 			if (block_nr > nr_blocks) {
 				fprintf(stderr, "Erroneous block_nr %d (> %d)\n",
 					block_nr, nr_blocks);
@@ -212,84 +245,64 @@ int main(int argc, char **argv)
 			if (blockmap[block_nr]) {
 				printf("Discard chunk at 0x%08x for already-flashed eraseblock (%d to go)\n",
 				       block_nr * meminfo.erasesize, nr_blocks - blocks_received);
-				nr_subblocks = pkts_per_block + 2;
 				continue;
 			}
 		}
-		if (nr_subblocks == pkts_per_block) {
+		if (nr_pkts >= pkts_per_block) {
 			/* We have a parity block but we didn't need it */
-			nr_subblocks++;
+			nr_pkts++;
 			continue;
 		}
 		if (blockmap[block_nr])
 			continue;
 
-		block_ofs = ntohl(pktbuf.hdr.block_ofs);
-		if (block_ofs == meminfo.erasesize)
-			block_ofs = PKT_SIZE * pkts_per_block;
-
-		if (len != PKT_SIZE && len + block_ofs != meminfo.erasesize) {
-			fprintf(stderr, "Bogus packet size 0x%x (expected 0x%x)\n",
-				ntohl(pktbuf.hdr.thislen),
-				min(PKT_SIZE, meminfo.erasesize - block_ofs));
-			exit(1);
-		}
+		for (i=0; i < nr_pkts; i++) {
+			if (pkt_indices[i] == ntohs(thispkt->hdr.pkt_nr)) {
+				printf("Discarding duplicate packet at %08x pkt %d\n",
+				       block_nr * meminfo.erasesize, pkt_indices[i]);
+				duplicates++;
+				break;
+			}
+		} /* And if we broke out, skip the packet... */
+		if (i < nr_pkts)
+			continue;
 
-		if (crc32(-1, pktbuf.data, len) != ntohl(pktbuf.hdr.thiscrc)) {
-			printf("Discard chunk %08x with bad CRC (%08x not %08x)\n",
-			       block_nr * meminfo.erasesize + block_ofs,
-			       crc32(-1, pktbuf.data, pktbuf.hdr.thislen),
-			       ntohl(pktbuf.hdr.thiscrc));
+		if (crc32(-1, thispkt->data, PKT_SIZE) != ntohl(thispkt->hdr.thiscrc)) {
+			printf("Discard %08x pkt %d with bad CRC (%08x not %08x)\n",
+			       block_nr * meminfo.erasesize, ntohs(thispkt->hdr.pkt_nr),
+			       crc32(-1, thispkt->data, PKT_SIZE),
+			       ntohl(thispkt->hdr.thiscrc));
 			badcrcs++;
 			continue;
 		}
-		if (subblockmap[block_ofs / PKT_SIZE]) {
-			printf("Discarding duplicate packet at %08x\n",
-			       block_nr * meminfo.erasesize + block_ofs);
-			duplicates++;
-			continue;
-		}
-		subblockmap[block_ofs / PKT_SIZE] = 1;
-		nr_subblocks++;
-		if (block_ofs < meminfo.erasesize) {
-			/* Normal data packet */
-			memcpy(eb_buf + block_ofs, pktbuf.data, len);
-//			printf("Received data block at %08x\n", block_nr * meminfo.erasesize + block_ofs);
-		} else {
-			/* Parity block */
-			int i;
 
-			/* If we don't have enough to recover, skip */
-			if (nr_subblocks < pkts_per_block)
-				continue;
+		pkt_indices[nr_pkts] = ntohs(thispkt->hdr.pkt_nr);
+		nr_pkts++;
 
-			for (i = 0; i<pkts_per_block; i++) {
-				if (subblockmap[i]) {
-					int j;
-					for (j=0; j<PKT_SIZE; j++)
-						pktbuf.data[j] ^= eb_buf[i*PKT_SIZE + j];
-				} else
-					missing = i;
-			}
+		if (nr_pkts == pkts_per_block) {
 
-			if (missing == -1) {
-				fprintf(stderr, "dwmw2 is stupid\n");
+			if (fec_decode(fec, pkts, pkt_indices, PKT_SIZE)) {
+				/* Eep. This cannot happen */
+				printf("The world is broken. fec_decode() returned error\n");
 				exit(1);
 			}
-//			printf("Recover missing packet at %08x from parity\n",
-//			       block_nr * meminfo.erasesize + missing * PKT_SIZE);
-			memcpy(eb_buf + (missing * PKT_SIZE), pktbuf.data, PKT_SIZE);
-		}
-
-		if (nr_subblocks == pkts_per_block) {
-
 			blockmap[block_nr] = 1;
 			blocks_received++;
 
+			/* Put data into order in eb_buf */
+			for (i=0; i < pkts_per_block; i++)
+				memcpy(eb_buf + (i * PKT_SIZE), pkts[i], PKT_SIZE);
+
+			if (crc32(-1, eb_buf, meminfo.erasesize) != ntohl(thispkt->hdr.block_crc)) {
+				printf("FEC error. CRC %08x != %08x\n", 
+				       crc32(-1, eb_buf, meminfo.erasesize),
+				       ntohl(thispkt->hdr.block_crc));
+				*(int *)0 = 0;
+				exit(1);
+			}
 			if (file_mode) {
-				printf("Received image block %08x%s (%d/%d)\n",
+				printf("Received image block %08x (%d/%d)",
 				       block_nr * meminfo.erasesize,
-				       (missing==-1)?"":" (parity)",
 				       blocks_received, nr_blocks);
 				pwrite(flfd, eb_buf, meminfo.erasesize, block_nr * meminfo.erasesize);
 			} else {
@@ -325,16 +338,16 @@ int main(int argc, char **argv)
 					mtdoffset += meminfo.erasesize;
 					goto again;
 				}
-				printf("Wrote image block %08x (%d/%d) to flash offset %08x%s\n",
+				printf("Wrote image block %08x (%d/%d) to flash offset %08x\n",
 				       block_nr * meminfo.erasesize, 
 				       blocks_received, nr_blocks,
-				       (uint32_t)mtdoffset,
-				       (missing==-1)?"":" (parity)");
+				       (uint32_t)mtdoffset);
 				mtdoffset += meminfo.erasesize;
 			}
 			if (!(blocks_received%100) || blocks_received == nr_blocks) {
 				int i, printed = 0;
-				for (i=0; i <= pkts_per_block + 1; i++) {
+				printf("\n");
+				for (i=0; i <= ntohs(thispkt->hdr.nr_pkts); i++) {
 					if (printed || stats[i]) {
 						printf("Number of blocks with %d packets received: %d\n",
 						       i, stats[i]);
diff --git a/serve_image.c b/serve_image.c
index dc434f0..0c7eab8 100644
--- a/serve_image.c
+++ b/serve_image.c
@@ -1,3 +1,7 @@
+#define _POSIX_C_SOURCE 199309
+
+#include <time.h>
+
 #include <errno.h>  	
 #include <error.h> 	
 #include <netdb.h> 	
@@ -16,7 +20,9 @@
 #include "mcast_image.h"
 
 int tx_rate = 80000;
-int pkt_delay = 12500 * PKT_SIZE / 1024;
+int pkt_delay;
+
+#undef RANDOMDROP
 
 int main(int argc, char **argv)
 {
@@ -30,26 +36,36 @@ int main(int argc, char **argv)
 	struct stat st;
 	int writeerrors = 0;
 	uint32_t erasesize;
-	unsigned char parbuf[PKT_SIZE];
 	unsigned char *image, *blockptr;
 	uint32_t block_nr;
-	uint32_t block_ofs;
 	int nr_blocks;
-	uint32_t droppoint = -1;
 	struct timeval then, now, nextpkt;
 	long time_msecs;
+	unsigned char **src_pkts;
+	unsigned char last_src_pkt[PKT_SIZE];
+	int pkts_extra = 6;
+	int pkts_per_block;
+	struct fec_parms *fec;
 
-	if (argc == 6) {
-		tx_rate = atol(argv[5]) * 1024;
+	if (argc == 7) {
+		tx_rate = atol(argv[6]) * 1024;
 		if (tx_rate < PKT_SIZE || tx_rate > 20000000) {
 			fprintf(stderr, "Bogus TX rate %d KiB/s\n", tx_rate);
 			exit(1);
 		}
+		argc = 6;
+	}
+	if (argc == 6) {
+		pkts_extra = atol(argv[5]);
+		if (pkts_extra < 0 || pkts_extra > 200) {
+			fprintf(stderr, "Bogus redundancy %d packets\n", pkts_extra);
+			exit(1);
+		}
 		argc = 5;
 	}
 	       
 	if (argc != 5) {
-		fprintf(stderr, "usage: %s <host> <port> <image> <erasesize> [<tx_rate>]\n",
+		fprintf(stderr, "usage: %s <host> <port> <image> <erasesize> [<redundancy>] [<tx_rate>]\n",
 			(strrchr(argv[0], '/')?:argv[0]-1)+1);
 		exit(1);
 	}
@@ -62,6 +78,22 @@ int main(int argc, char **argv)
 		fprintf(stderr, "erasesize cannot be zero\n");
 		exit(1);
 	}
+
+	pkts_per_block = (erasesize + PKT_SIZE - 1) / PKT_SIZE;
+	src_pkts = malloc(pkts_per_block * sizeof(unsigned char *));
+	if (!src_pkts) {
+		fprintf(stderr, "Failed to allocate memory for packet pointers\n");
+		exit(1);
+	}
+	/* We have to pad it with zeroes, so can't use it in-place */
+	src_pkts[pkts_per_block-1] = last_src_pkt;
+
+	fec = fec_new(pkts_per_block, pkts_per_block + pkts_extra);
+	if (!fec) {
+		fprintf(stderr, "Error initialising FEC\n");
+		exit(1);
+	}
+
 	memset(&hints, 0, sizeof(hints));
 	hints.ai_flags = AI_ADDRCONFIG;
 	hints.ai_socktype = SOCK_DGRAM;
@@ -118,85 +150,91 @@ int main(int argc, char **argv)
 	pktbuf.hdr.totcrc = htonl(crc32(-1, image, st.st_size));
 	pktbuf.hdr.nr_blocks = htonl(nr_blocks);
 	pktbuf.hdr.blocksize = htonl(erasesize);
+	pktbuf.hdr.thislen = htonl(PKT_SIZE);
+	pktbuf.hdr.nr_pkts = htons(pkts_per_block + pkts_extra);
 
 	printf("%08x\n", ntohl(pktbuf.hdr.totcrc));
 
+ again:
+	printf("Image size %ld KiB (%08lx). %d redundant packets per block (%d total)\n"
+	       "Data to send %d KiB. Estimated transmit time: %ds\n", 
+	       (long)st.st_size / 1024, (long) st.st_size, pkts_extra, pkts_extra+pkts_per_block,
+	       nr_blocks * PKT_SIZE * (pkts_per_block+pkts_extra) / 1024,
+	       nr_blocks * (pkts_per_block+pkts_extra) * pkt_delay / 1000000);
 	gettimeofday(&then, NULL);
 	nextpkt = then;
 
+#ifdef RANDOMDROP
+	srand((unsigned)then.tv_usec);
+	printf("Random seed %u\n", (unsigned)then.tv_usec);
+#endif
 	blockptr = image;
 
 	for (block_nr = 0; block_nr < nr_blocks; block_nr++) {
-		int len;
-		int dropped = 0;
+		int i;
+		long tosleep;
+
+		blockptr = image + (erasesize * block_nr);
+
+		pktbuf.hdr.block_crc = htonl(crc32(-1, blockptr, erasesize));
+
+		for (i=0; i < pkts_per_block-1; i++)
+			src_pkts[i] = blockptr + (i*PKT_SIZE);
+
+		memcpy(last_src_pkt, blockptr + (i*PKT_SIZE),
+		       erasesize - (i * PKT_SIZE));
 
 		pktbuf.hdr.block_nr = htonl(block_nr);
 
-		for (block_ofs = 0; block_ofs <= erasesize; block_ofs += len) {
-			int i;
+		for (i=0; i < pkts_per_block + pkts_extra; i++) {
+
+			fec_encode(fec, src_pkts, pktbuf.data, i, PKT_SIZE);
 			
-			if (block_ofs + PKT_SIZE > erasesize)
-				len = erasesize - block_ofs;
-			else
-				len = PKT_SIZE;
+			printf("\rSending data block %08x packet %3d/%d",
+			       block_nr * erasesize, i, pkts_per_block + pkts_extra);
 
-			if (block_ofs == erasesize) {
+			if (block_nr && !i) {
 				gettimeofday(&now, NULL);
 
 				time_msecs = (now.tv_sec - then.tv_sec) * 1000;
 				time_msecs += ((int)(now.tv_usec - then.tv_usec)) / 1000;
-
-				printf("\rSending parity block: %08x     (%ld KiB/s)    ",
-				       block_nr * erasesize,
-				       (block_ofs + (block_nr * (erasesize+sizeof(pktbuf)))) / 1024 * 1000 / time_msecs);
-
-				len = PKT_SIZE;
-				memcpy(pktbuf.data, parbuf, PKT_SIZE);
-			} else {
-				if (!block_ofs)
-					memcpy(parbuf, blockptr, PKT_SIZE);
-				else for (i=0; i < len; i++)
-					     parbuf[i] ^= blockptr[i];
-
-				memcpy(pktbuf.data, blockptr, len);
-				printf("\rSending data block at %08x",
-				       block_nr * erasesize + block_ofs);
-				blockptr += len;
+				printf("    (%ld KiB/s)    ",
+				       (block_nr * sizeof(pktbuf) * (pkts_per_block+pkts_extra))
+				       / 1024 * 1000 / time_msecs);
 			}
 
 			fflush(stdout);
-			pktbuf.hdr.thislen = htonl(len);
-			pktbuf.hdr.block_ofs = htonl(block_ofs);
-			pktbuf.hdr.thiscrc = htonl(crc32(-1, pktbuf.data, len));
-
-			if (droppoint == block_ofs && !dropped) {
-				dropped = 1;
-				if (droppoint == 0)
-					droppoint = erasesize;
-				else if (droppoint == erasesize)
-					droppoint = ((erasesize - 1) / PKT_SIZE) * PKT_SIZE;
-				else droppoint -= PKT_SIZE;
-				printf("\nDropping data block at %08x\n", block_ofs);
+			pktbuf.hdr.pkt_nr = htons(i);
+			pktbuf.hdr.thiscrc = htonl(crc32(-1, pktbuf.data, PKT_SIZE));
+
+#ifdef RANDOMDROP
+			if ((rand() % 1000) < 20) {
+				printf("\nDropping packet %d\n", i+1);
 				continue;
 			}
+#endif
+			gettimeofday(&now, NULL);
+#if 1
+			tosleep = nextpkt.tv_usec - now.tv_usec + 
+				(1000000 * (nextpkt.tv_sec - now.tv_sec));
 
-			if (write(sock, &pktbuf, sizeof(pktbuf.hdr)+len) < 0) {
-				perror("write");
-				writeerrors++;
-				if (writeerrors > 10) {
-					fprintf(stderr, "Too many consecutive write errors\n");
-					exit(1);
-				}
-			} else
-				writeerrors = 0;
+			/* We need hrtimers for this to actually work */
+			if (tosleep > 0) {
+				struct timespec req;
 
-			do {
-				gettimeofday(&now, NULL);
-			} while (now.tv_sec < nextpkt.tv_sec ||
-				 (now.tv_sec == nextpkt.tv_sec &&
-				  now.tv_usec < nextpkt.tv_usec));
+				req.tv_nsec = (tosleep % 1000000) * 1000;
+				req.tv_sec = tosleep / 1000000;
 
-			nextpkt.tv_usec = now.tv_usec + pkt_delay;
+				nanosleep(&req, NULL);
+			}
+#else
+			while (now.tv_sec < nextpkt.tv_sec ||
+				 (now.tv_sec == nextpkt.tv_sec &&
+				  now.tv_usec < nextpkt.tv_usec)) {
+				gettimeofday(&now, NULL);
+			}
+#endif
+			nextpkt.tv_usec += pkt_delay;
 			if (nextpkt.tv_usec >= 1000000) {
 				nextpkt.tv_sec += nextpkt.tv_usec / 1000000;
 				nextpkt.tv_usec %= 1000000;
@@ -206,20 +244,34 @@ int main(int argc, char **argv)
 			   passed, then we've lost time. Adjust our expected
 			   timings accordingly. */
 			if (now.tv_usec > (now.tv_usec + 
-					   1000000 * (nextpkt.tv_sec - now.tv_sec)))
+					   1000000 * (nextpkt.tv_sec - now.tv_sec))) { 
 				nextpkt = now;
+			}
+
+			if (write(sock, &pktbuf, sizeof(pktbuf)) < 0) {
+				perror("write");
+				writeerrors++;
+				if (writeerrors > 10) {
+					fprintf(stderr, "Too many consecutive write errors\n");
+					exit(1);
+				}
+			} else
+				writeerrors = 0;
+
+
 			
 		}
 	}
-	munmap(image, st.st_size);
-	close(rfd);
-	close(sock);
 	gettimeofday(&now, NULL);
 
 	time_msecs = (now.tv_sec - then.tv_sec) * 1000;
 	time_msecs += ((int)(now.tv_usec - then.tv_usec)) / 1000;
 	printf("\n%d KiB sent in %ldms (%ld KiB/s)\n",
-	       nr_blocks * (erasesize+sizeof(pktbuf)) / 1024, time_msecs,
-	       nr_blocks * (erasesize+sizeof(pktbuf)) / 1024 * 1000 / time_msecs);
+	       nr_blocks * sizeof(pktbuf) * (pkts_per_block+pkts_extra) / 1024, time_msecs,
+	       nr_blocks * sizeof(pktbuf) * (pkts_per_block+pkts_extra) / 1024 * 1000 / time_msecs);
+
+	munmap(image, st.st_size);
+	close(rfd);
+	close(sock);
 	return 0;
 }