fec.c

当前很多文章多提到的lugi的reed-solomon编码的源代码

	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 */