Merge branch 'maint' into next
[paraslash.git] / fec.c
1 /** \file fec.c Forward error correction based on Vandermonde matrices. */
2
3 /*
4  * 980624
5  * (C) 1997-98 Luigi Rizzo (luigi@iet.unipi.it)
6  *
7  * Portions derived from code by Phil Karn (karn@ka9q.ampr.org),
8  * Robert Morelos-Zaragoza (robert@spectra.eng.hawaii.edu) and Hari
9  * Thirumoorthy (harit@spectra.eng.hawaii.edu), Aug 1995
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * 1. Redistributions of source code must retain the above copyright
16  *    notice, this list of conditions and the following disclaimer.
17  * 2. Redistributions in binary form must reproduce the above
18  *    copyright notice, this list of conditions and the following
19  *    disclaimer in the documentation and/or other materials
20  *    provided with the distribution.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND
23  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
24  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
25  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS
26  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
27  * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
28  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
29  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
30  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
31  * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
32  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
33  * OF SUCH DAMAGE.
34  */
35
36 #include <regex.h>
37
38 #include "para.h"
39 #include "error.h"
40 #include "portable_io.h"
41 #include "string.h"
42 #include "fec.h"
43
44 #define GF_BITS  8 /* code over GF(256) */
45 #define GF_SIZE ((1 << GF_BITS) - 1)
46
47 /*
48  * To speed up computations, we have tables for logarithm, exponent and inverse
49  * of a number. We use a table for multiplication as well (it takes 64K, no big
50  * deal even on a PDA, especially because it can be pre-initialized an put into
51  * a ROM!). The macro gf_mul(x,y) takes care of multiplications.
52  */
53 static unsigned char gf_exp[2 * GF_SIZE]; /* index->poly form conversion table */
54 static int gf_log[GF_SIZE + 1]; /* Poly->index form conversion table    */
55 static unsigned char inverse[GF_SIZE + 1]; /* inverse of field elem. */
56 static unsigned char gf_mul_table[GF_SIZE + 1][GF_SIZE + 1];
57 /* Multiply two numbers. */
58 #define gf_mul(x,y) gf_mul_table[x][y]
59
60 /* Compute x % GF_SIZE without a slow divide. */
61 static inline unsigned char modnn(int x)
62 {
63         while (x >= GF_SIZE) {
64                 x -= GF_SIZE;
65                 x = (x >> GF_BITS) + (x & GF_SIZE);
66         }
67         return x;
68 }
69
70 static void init_mul_table(void)
71 {
72         int i, j;
73         for (i = 0; i < GF_SIZE + 1; i++)
74                 for (j = 0; j < GF_SIZE + 1; j++)
75                         gf_mul_table[i][j] =
76                                 gf_exp[modnn(gf_log[i] + gf_log[j])];
77
78         for (j = 0; j < GF_SIZE + 1; j++)
79                 gf_mul_table[0][j] = gf_mul_table[j][0] = 0;
80 }
81
82 static unsigned char *alloc_matrix(int rows, int cols)
83 {
84         return para_malloc(rows * cols);
85 }
86
87 /*
88  * Initialize the data structures used for computations in GF.
89  *
90  * This generates GF(2**GF_BITS) from the irreducible polynomial p(X) in
91  * p[0]..p[m].
92  *
93  * Lookup tables:
94  *     index->polynomial form           gf_exp[] contains j= \alpha^i;
95  *     polynomial form -> index form    gf_log[ j = \alpha^i ] = i
96  * \alpha=x is the primitive element of GF(2^m)
97  *
98  * For efficiency, gf_exp[] has size 2*GF_SIZE, so that a simple
99  * multiplication of two numbers can be resolved without calling modnn
100  */
101 static void generate_gf(void)
102 {
103         int i;
104         unsigned char mask = 1;
105         char *pp = "101110001"; /* The primitive polynomial 1+x^2+x^3+x^4+x^8 */
106         gf_exp[GF_BITS] = 0; /* will be updated at the end of the 1st loop */
107
108         /*
109          * first, generate the (polynomial representation of) powers of \alpha,
110          * which are stored in gf_exp[i] = \alpha ** i .
111          * At the same time build gf_log[gf_exp[i]] = i .
112          * The first GF_BITS powers are simply bits shifted to the left.
113          */
114         for (i = 0; i < GF_BITS; i++, mask <<= 1) {
115                 gf_exp[i] = mask;
116                 gf_log[gf_exp[i]] = i;
117                 /*
118                  * If pp[i] == 1 then \alpha ** i occurs in poly-repr
119                  * gf_exp[GF_BITS] = \alpha ** GF_BITS
120                  */
121                 if (pp[i] == '1')
122                         gf_exp[GF_BITS] ^= mask;
123         }
124         /*
125          * now gf_exp[GF_BITS] = \alpha ** GF_BITS is complete, so can also
126          * compute its inverse.
127          */
128         gf_log[gf_exp[GF_BITS]] = GF_BITS;
129         /*
130          * Poly-repr of \alpha ** (i+1) is given by poly-repr of \alpha ** i
131          * shifted left one-bit and accounting for any \alpha ** GF_BITS term
132          * that may occur when poly-repr of \alpha ** i is shifted.
133          */
134         mask = 1 << (GF_BITS - 1);
135         for (i = GF_BITS + 1; i < GF_SIZE; i++) {
136                 if (gf_exp[i - 1] >= mask)
137                         gf_exp[i] =
138                                 gf_exp[GF_BITS] ^ ((gf_exp[i - 1] ^ mask) << 1);
139                 else
140                         gf_exp[i] = gf_exp[i - 1] << 1;
141                 gf_log[gf_exp[i]] = i;
142         }
143         /*
144          * log(0) is not defined, so use a special value
145          */
146         gf_log[0] = GF_SIZE;
147         /* set the extended gf_exp values for fast multiply */
148         for (i = 0; i < GF_SIZE; i++)
149                 gf_exp[i + GF_SIZE] = gf_exp[i];
150
151         inverse[0] = 0; /* 0 has no inverse. */
152         inverse[1] = 1;
153         for (i = 2; i <= GF_SIZE; i++)
154                 inverse[i] = gf_exp[GF_SIZE - gf_log[i]];
155 }
156
157 /*
158  * Compute dst[] = dst[] + c * src[]
159  *
160  * This is used often, so better optimize it! Currently the loop is unrolled 16
161  * times. The case c=0 is also optimized, whereas c=1 is not.
162  */
163 #define UNROLL 16
164 static void addmul(unsigned char *dst1, const unsigned char const *src1,
165         unsigned char c, int sz)
166 {
167         if (c == 0)
168                 return;
169         unsigned char *dst = dst1, *lim = &dst[sz - UNROLL + 1],
170                 *col = gf_mul_table[c];
171         const unsigned char const *src = src1;
172
173         for (; dst < lim; dst += UNROLL, src += UNROLL) {
174                 dst[0] ^= col[src[0]];
175                 dst[1] ^= col[src[1]];
176                 dst[2] ^= col[src[2]];
177                 dst[3] ^= col[src[3]];
178                 dst[4] ^= col[src[4]];
179                 dst[5] ^= col[src[5]];
180                 dst[6] ^= col[src[6]];
181                 dst[7] ^= col[src[7]];
182                 dst[8] ^= col[src[8]];
183                 dst[9] ^= col[src[9]];
184                 dst[10] ^= col[src[10]];
185                 dst[11] ^= col[src[11]];
186                 dst[12] ^= col[src[12]];
187                 dst[13] ^= col[src[13]];
188                 dst[14] ^= col[src[14]];
189                 dst[15] ^= col[src[15]];
190         }
191         lim += UNROLL - 1;
192         for (; dst < lim; dst++, src++) /* final components */
193                 *dst ^= col[*src];
194 }
195
196 /*
197  * Compute C = AB where A is n*k, B is k*m, C is n*m
198  */
199 static void matmul(unsigned char *a, unsigned char *b, unsigned char *c,
200                 int n, int k, int m)
201 {
202         int row, col, i;
203
204         for (row = 0; row < n; row++) {
205                 for (col = 0; col < m; col++) {
206                         unsigned char *pa = &a[row * k], *pb = &b[col], acc = 0;
207                         for (i = 0; i < k; i++, pa++, pb += m)
208                                 acc ^= gf_mul(*pa, *pb);
209                         c[row * m + col] = acc;
210                 }
211         }
212 }
213
214 #define FEC_SWAP(a,b) {typeof(a) tmp = a; a = b; b = tmp;}
215
216 /*
217  * Compute the inverse of a matrix.
218  *
219  * k is the size of the matrix 'src' (Gauss-Jordan, adapted from Numerical
220  * Recipes in C). Returns negative on errors.
221  */
222 static int invert_mat(unsigned char *src, int k)
223 {
224         int irow, icol, row, col, ix, error;
225         int *indxc = para_malloc(k * sizeof(int));
226         int *indxr = para_malloc(k * sizeof(int));
227         int *ipiv = para_malloc(k * sizeof(int)); /* elements used as pivots */
228         unsigned char c, *p, *id_row = alloc_matrix(1, k),
229                 *temp_row = alloc_matrix(1, k);
230
231         memset(id_row, 0, k);
232         memset(ipiv, 0, k * sizeof(int));
233
234         for (col = 0; col < k; col++) {
235                 unsigned char *pivot_row;
236                 /*
237                  * Zeroing column 'col', look for a non-zero element.
238                  * First try on the diagonal, if it fails, look elsewhere.
239                  */
240                 irow = icol = -1;
241                 if (ipiv[col] != 1 && src[col * k + col] != 0) {
242                         irow = col;
243                         icol = col;
244                         goto found_piv;
245                 }
246                 for (row = 0; row < k; row++) {
247                         if (ipiv[row] != 1) {
248                                 for (ix = 0; ix < k; ix++) {
249                                         if (ipiv[ix] == 0) {
250                                                 if (src[row * k + ix] != 0) {
251                                                         irow = row;
252                                                         icol = ix;
253                                                         goto found_piv;
254                                                 }
255                                         } else if (ipiv[ix] > 1) {
256                                                 error = -E_FEC_PIVOT;
257                                                 goto fail;
258                                         }
259                                 }
260                         }
261                 }
262                 error = -E_FEC_PIVOT;
263                 if (icol == -1)
264                         goto fail;
265 found_piv:
266                 ++(ipiv[icol]);
267                 /*
268                  * swap rows irow and icol, so afterwards the diagonal element
269                  * will be correct. Rarely done, not worth optimizing.
270                  */
271                 if (irow != icol)
272                         for (ix = 0; ix < k; ix++)
273                                 FEC_SWAP(src[irow * k + ix], src[icol * k + ix]);
274                 indxr[col] = irow;
275                 indxc[col] = icol;
276                 pivot_row = &src[icol * k];
277                 error = -E_FEC_SINGULAR;
278                 c = pivot_row[icol];
279                 if (c == 0)
280                         goto fail;
281                 if (c != 1) { /* otherwise this is a NOP */
282                         /*
283                          * this is done often , but optimizing is not so
284                          * fruitful, at least in the obvious ways (unrolling)
285                          */
286                         c = inverse[c];
287                         pivot_row[icol] = 1;
288                         for (ix = 0; ix < k; ix++)
289                                 pivot_row[ix] = gf_mul(c, pivot_row[ix]);
290                 }
291                 /*
292                  * from all rows, remove multiples of the selected row to zero
293                  * the relevant entry (in fact, the entry is not zero because
294                  * we know it must be zero).  (Here, if we know that the
295                  * pivot_row is the identity, we can optimize the addmul).
296                  */
297                 id_row[icol] = 1;
298                 if (memcmp(pivot_row, id_row, k) != 0) {
299                         for (p = src, ix = 0; ix < k; ix++, p += k) {
300                                 if (ix != icol) {
301                                         c = p[icol];
302                                         p[icol] = 0;
303                                         addmul(p, pivot_row, c, k);
304                                 }
305                         }
306                 }
307                 id_row[icol] = 0;
308         }
309         for (col = k - 1; col >= 0; col--) {
310                 if (indxr[col] < 0 || indxr[col] >= k)
311                         PARA_CRIT_LOG("AARGH, indxr[col] %d\n", indxr[col]);
312                 else if (indxc[col] < 0 || indxc[col] >= k)
313                         PARA_CRIT_LOG("AARGH, indxc[col] %d\n", indxc[col]);
314                 else if (indxr[col] != indxc[col]) {
315                         for (row = 0; row < k; row++) {
316                                 FEC_SWAP(src[row * k + indxr[col]],
317                                         src[row * k + indxc[col]]);
318                         }
319                 }
320         }
321         error = 0;
322 fail:
323         free(indxc);
324         free(indxr);
325         free(ipiv);
326         free(id_row);
327         free(temp_row);
328         return error;
329 }
330
331 /*
332  * Invert a Vandermonde matrix.
333  *
334  * It assumes that the matrix is not singular and _IS_ a Vandermonde matrix.
335  * Only uses the second column of the matrix, containing the p_i's.
336  *
337  * Algorithm borrowed from "Numerical recipes in C" -- sec.2.8, but largely
338  * revised for GF purposes.
339  */
340 static void invert_vdm(unsigned char *src, int k)
341 {
342         int i, j, row, col;
343         unsigned char *b, *c, *p, t, xx;
344
345         if (k == 1) /* degenerate */
346                 return;
347         /*
348          * c holds the coefficient of P(x) = Prod (x - p_i), i=0..k-1
349          * b holds the coefficient for the matrix inversion
350          */
351         c = para_malloc(k);
352         b = para_malloc(k);
353         p = para_malloc(k);
354
355         for (j = 1, i = 0; i < k; i++, j += k) {
356                 c[i] = 0;
357                 p[i] = src[j];
358         }
359         /*
360          * construct coeffs recursively. We know c[k] = 1 (implicit) and start
361          * P_0 = x - p_0, then at each stage multiply by x - p_i generating P_i
362          * = x P_{i-1} - p_i P_{i-1} After k steps we are done.
363          */
364         c[k - 1] = p[0]; /* really -p(0), but x = -x in GF(2^m) */
365         for (i = 1; i < k; i++) {
366                 unsigned char p_i = p[i];
367                 for (j = k - 1 - (i - 1); j < k - 1; j++)
368                         c[j] ^= gf_mul(p_i, c[j + 1]);
369                 c[k - 1] ^= p_i;
370         }
371
372         for (row = 0; row < k; row++) {
373                 /*
374                  * synthetic division etc.
375                  */
376                 xx = p[row];
377                 t = 1;
378                 b[k - 1] = 1; /* this is in fact c[k] */
379                 for (i = k - 2; i >= 0; i--) {
380                         b[i] = c[i + 1] ^ gf_mul(xx, b[i + 1]);
381                         t = gf_mul(xx, t) ^ b[i];
382                 }
383                 for (col = 0; col < k; col++)
384                         src[col * k + row] = gf_mul(inverse[t], b[col]);
385         }
386         free(c);
387         free(b);
388         free(p);
389 }
390
391 static int fec_initialized;
392
393 static void init_fec(void)
394 {
395         generate_gf();
396         init_mul_table();
397         fec_initialized = 1;
398 }
399
400 /** Internal FEC parameters. */
401 struct fec_parms {
402         /** Number of data slices. */
403         int k;
404         /** Number of slices (including redundant slices). */
405         int n;
406         /** The encoding matrix, computed by init_fec(). */
407         unsigned char *enc_matrix;
408 };
409
410 /**
411  * Deallocate a fec params structure.
412  *
413  * \param p The structure to free.
414  */
415 void fec_free(struct fec_parms *p)
416 {
417         if (!p)
418                 return;
419         free(p->enc_matrix);
420         free(p);
421 }
422
423 /**
424  * Create a new encoder and return an opaque descriptor to it.
425  *
426  * \param k Number of input slices.
427  * \param n Number of output slices.
428  * \param result On success the Fec descriptor is returned here.
429  *
430  * \return Standard.
431  *
432  * This creates the k*n encoding matrix.  It is computed starting with a
433  * Vandermonde matrix, and then transformed into a systematic matrix.
434  */
435 int fec_new(int k, int n, struct fec_parms **result)
436 {
437         int row, col;
438         unsigned char *p, *tmp_m;
439         struct fec_parms *parms;
440
441         if (!fec_initialized)
442                 init_fec();
443
444         if (k < 1 || k > GF_SIZE + 1 || n > GF_SIZE + 1 || k > n)
445                 return -E_FEC_PARMS;
446         parms = para_malloc(sizeof(struct fec_parms));
447         parms->k = k;
448         parms->n = n;
449         parms->enc_matrix = alloc_matrix(n, k);
450         tmp_m = alloc_matrix(n, k);
451         /*
452          * fill the matrix with powers of field elements, starting from 0.
453          * The first row is special, cannot be computed with exp. table.
454          */
455         tmp_m[0] = 1;
456         for (col = 1; col < k; col++)
457                 tmp_m[col] = 0;
458         for (p = tmp_m + k, row = 0; row < n - 1; row++, p += k) {
459                 for (col = 0; col < k; col++)
460                         p[col] = gf_exp[modnn(row * col)];
461         }
462
463         /*
464          * quick code to build systematic matrix: invert the top
465          * k*k vandermonde matrix, multiply right the bottom n-k rows
466          * by the inverse, and construct the identity matrix at the top.
467          */
468         invert_vdm(tmp_m, k); /* much faster than invert_mat */
469         matmul(tmp_m + k * k, tmp_m, parms->enc_matrix + k * k, n - k, k, k);
470         /*
471          * the upper matrix is I so do not bother with a slow multiply
472          */
473         memset(parms->enc_matrix, 0, k * k);
474         for (p = parms->enc_matrix, col = 0; col < k; col++, p += k + 1)
475                 *p = 1;
476         free(tmp_m);
477         *result = parms;
478         return 0;
479 }
480
481 /**
482  * Compute one encoded slice of the given input.
483  *
484  * \param parms The fec parameters returned earlier by fec_new().
485  * \param src The \a k data slices to encode.
486  * \param dst Result pointer.
487  * \param idx The index of the slice to compute.
488  * \param sz The size of the input data packets.
489  *
490  * Encode the \a k slices of size \a sz given by \a src and store the output
491  * slice number \a idx in \a dst.
492  */
493 void fec_encode(struct fec_parms *parms, const unsigned char * const *src,
494                 unsigned char *dst, int idx, int sz)
495 {
496         int i, k = parms->k;
497         unsigned char *p;
498
499         assert(idx <= parms->n);
500
501         if (idx < k) {
502                 memcpy(dst, src[idx], sz);
503                 return;
504         }
505         p = &(parms->enc_matrix[idx * k]);
506         memset(dst, 0, sz);
507         for (i = 0; i < k; i++)
508                 addmul(dst, src[i], p[i], sz);
509 }
510
511 /* Move src packets in their position. */
512 static int shuffle(unsigned char **data, int *idx, int k)
513 {
514         int i;
515
516         for (i = 0; i < k;) {
517                 if (idx[i] >= k || idx[i] == i)
518                         i++;
519                 else { /* put index and data at the right position */
520                         int c = idx[i];
521
522                         if (idx[c] == c) /* conflict */
523                                 return -E_FEC_BAD_IDX;
524                         FEC_SWAP(idx[i], idx[c]);
525                         FEC_SWAP(data[i], data[c]);
526                 }
527         }
528         return 0;
529 }
530
531 /*
532  * Construct the decoding matrix given the indices. The encoding matrix must
533  * already be allocated.
534  */
535 static int build_decode_matrix(struct fec_parms *parms, int *idx,
536                 unsigned char **result)
537 {
538         int ret = -E_FEC_BAD_IDX, i, k = parms->k;
539         unsigned char *p, *matrix = alloc_matrix(k, k);
540
541         for (i = 0, p = matrix; i < k; i++, p += k) {
542                 if (idx[i] >= parms->n) /* invalid index */
543                         goto err;
544                 if (idx[i] < k) {
545                         memset(p, 0, k);
546                         p[i] = 1;
547                 } else
548                         memcpy(p, &(parms->enc_matrix[idx[i] * k]), k);
549         }
550         ret = invert_mat(matrix, k);
551         if (ret < 0)
552                 goto err;
553         *result = matrix;
554         return 0;
555 err:
556         free(matrix);
557         *result = NULL;
558         return ret;
559 }
560
561 /**
562  * Decode one slice from the group of received slices.
563  *
564  * \param parms Pointer to fec params structure.
565  * \param data Pointers to received packets.
566  * \param idx Pointer to packet indices (gets modified).
567  * \param sz Size of each packet.
568  *
569  * \return Zero on success, -1 on errors.
570  *
571  * The \a data vector of received slices and the indices of slices are used to
572  * produce the correct output slice. The data slices are modified in-place.
573  */
574 int fec_decode(struct fec_parms *parms, unsigned char **data, int *idx,
575                 int sz)
576 {
577         unsigned char *m_dec, **slice;
578         int ret, row, col, k = parms->k;
579
580         ret = shuffle(data, idx, k);
581         if (ret < 0)
582                 return ret;
583         ret = build_decode_matrix(parms, idx, &m_dec);
584         if (ret < 0)
585                 return ret;
586         /* do the actual decoding */
587         slice = para_malloc(k * sizeof(unsigned char *));
588         for (row = 0; row < k; row++) {
589                 if (idx[row] >= k) {
590                         slice[row] = para_calloc(sz);
591                         for (col = 0; col < k; col++)
592                                 addmul(slice[row], data[col],
593                                         m_dec[row * k + col], sz);
594                 }
595         }
596         /* move slices to their final destination */
597         for (row = 0; row < k; row++) {
598                 if (idx[row] >= k) {
599                         memcpy(data[row], slice[row], sz);
600                         free(slice[row]);
601                 }
602         }
603         free(slice);
604         free(m_dec);
605         return 0;
606 }