1 /*
2 * fec.c -- forward error correction based on Vandermonde matrices
3 *
4 * (C) 1997-98 Luigi Rizzo (luigi@iet.unipi.it)
5 * (C) 2001 Alain Knaff (alain@knaff.lu)
6 * (C) 2017 Iwan Timmer (irtimmer@gmail.com)
7 *
8 * Portions derived from code by Phil Karn (karn@ka9q.ampr.org),
9 * Robert Morelos-Zaragoza (robert@spectra.eng.hawaii.edu) and Hari
10 * Thirumoorthy (harit@spectra.eng.hawaii.edu), Aug 1995
11 *
12 * Redistribution and use in source and binary forms, with or without
13 * modification, are permitted provided that the following conditions
14 * are met:
15 *
16 * 1. Redistributions of source code must retain the above copyright
17 * notice, this list of conditions and the following disclaimer.
18 * 2. Redistributions in binary form must reproduce the above
19 * copyright notice, this list of conditions and the following
20 * disclaimer in the documentation and/or other materials
21 * provided with the distribution.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND
24 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
25 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
26 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS
27 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
28 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
29 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
30 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
31 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
32 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
33 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
34 * OF SUCH DAMAGE.
35 */
36
37 #include <stdio.h>
38 #include <stdlib.h>
39 #include <string.h>
40
41 #include <assert.h>
42 #include "rs.h"
43
44 #ifdef _MSC_VER
45 #define NEED_ALLOCA
46 #define alloca(x) _alloca(x)
47 #endif
48
49 typedef unsigned char gf;
50
51 #define GF_BITS 8
52 #define GF_PP "101110001"
53 #define GF_SIZE ((1 << GF_BITS) - 1)
54
55 #define SWAP(a,b,t) {t tmp; tmp=a; a=b; b=tmp;}
56
57 /*
58 * USE_GF_MULC, GF_MULC0(c) and GF_ADDMULC(x) can be used when multiplying
59 * many numbers by the same constant. In this case the first
60 * call sets the constant, and others perform the multiplications.
61 * A value related to the multiplication is held in a local variable
62 * declared with USE_GF_MULC . See usage in addmul1().
63 */
64 #define USE_GF_MULC register gf * __gf_mulc_
65 #define GF_MULC0(c) __gf_mulc_ = &gf_mul_table[(c)<<8]
66 #define GF_ADDMULC(dst, x) dst ^= __gf_mulc_[x]
67 #define GF_MULC(dst, x) dst = __gf_mulc_[x]
68
69 #define gf_mul(x,y) gf_mul_table[(x<<8)+y]
70
71 /*
72 * To speed up computations, we have tables for logarithm, exponent
73 * multiplication and inverse of a number.
74 */
75 static gf gf_exp[2*GF_SIZE];
76 static int gf_log[GF_SIZE + 1];
77 static gf inverse[GF_SIZE+1];
78 #ifdef _MSC_VER
79 static gf __declspec(align (256)) gf_mul_table[(GF_SIZE + 1)*(GF_SIZE + 1)];
80 #else
81 static gf gf_mul_table[(GF_SIZE + 1)*(GF_SIZE + 1)] __attribute__((aligned (256)));
82 #endif
83
84 /*
85 * modnn(x) computes x % GF_SIZE, where GF_SIZE is 2**GF_BITS - 1,
86 * without a slow divide.
87 */
modnn(int x)88 static inline gf modnn(int x) {
89 while (x >= GF_SIZE) {
90 x -= GF_SIZE;
91 x = (x >> GF_BITS) + (x & GF_SIZE);
92 }
93 return x;
94 }
95
addmul(gf * dst1,gf * src1,gf c,int sz)96 static void addmul(gf *dst1, gf *src1, gf c, int sz) {
97 USE_GF_MULC;
98 if (c != 0) {
99 register gf *dst = dst1, *src = src1;
100 gf *lim = &dst[sz];
101
102 GF_MULC0(c);
103 for (; dst < lim; dst++, src++)
104 GF_ADDMULC(*dst, *src);
105 }
106 }
107
mul(gf * dst1,gf * src1,gf c,int sz)108 static void mul(gf *dst1, gf *src1, gf c, int sz) {
109 USE_GF_MULC;
110 if (c != 0) {
111 register gf *dst = dst1, *src = src1;
112 gf *lim = &dst[sz];
113 GF_MULC0(c);
114 for (; dst < lim; dst++, src++)
115 GF_MULC(*dst , *src);
116 } else
117 memset(dst1, 0, c);
118 }
119
120 /* y = a.dot(b) */
multiply1(gf * a,int ar,int ac,gf * b,int br,int bc)121 static gf* multiply1(gf *a, int ar, int ac, gf *b, int br, int bc) {
122 gf *new_m, tg;
123 int r, c, i, ptr = 0;
124
125 assert(ac == br);
126 new_m = (gf*) calloc(1, ar*bc);
127 if (NULL != new_m) {
128
129 /* this multiply is slow */
130 for (r = 0; r < ar; r++) {
131 for (c = 0; c < bc; c++) {
132 tg = 0;
133 for (i = 0; i < ac; i++)
134 tg ^= gf_mul(a[r*ac+i], b[i*bc+c]);
135
136 new_m[ptr++] = tg;
137 }
138 }
139 }
140
141 return new_m;
142 }
143
init_mul_table(void)144 static void init_mul_table(void) {
145 int i, j;
146 for (i=0; i< GF_SIZE+1; i++)
147 for (j=0; j< GF_SIZE+1; j++)
148 gf_mul_table[(i<<8)+j] = gf_exp[modnn(gf_log[i] + gf_log[j]) ] ;
149
150 for (j=0; j< GF_SIZE+1; j++)
151 gf_mul_table[j] = gf_mul_table[j<<8] = 0;
152 }
153
154 /*
155 * initialize the data structures used for computations in GF.
156 */
generate_gf(void)157 static void generate_gf(void) {
158 int i;
159 gf mask;
160
161 mask = 1;
162 gf_exp[GF_BITS] = 0;
163 /*
164 * first, generate the (polynomial representation of) powers of \alpha,
165 * which are stored in gf_exp[i] = \alpha ** i .
166 * At the same time build gf_log[gf_exp[i]] = i .
167 * The first GF_BITS powers are simply bits shifted to the left.
168 */
169 for (i = 0; i < GF_BITS; i++, mask <<= 1) {
170 gf_exp[i] = mask;
171 gf_log[gf_exp[i]] = i;
172 /*
173 * If GF_PP[i] == 1 then \alpha ** i occurs in poly-repr
174 * gf_exp[GF_BITS] = \alpha ** GF_BITS
175 */
176 if (GF_PP[i] == '1')
177 gf_exp[GF_BITS] ^= mask;
178 }
179 /*
180 * now gf_exp[GF_BITS] = \alpha ** GF_BITS is complete, so can als
181 * compute its inverse.
182 */
183 gf_log[gf_exp[GF_BITS]] = GF_BITS;
184 /*
185 * Poly-repr of \alpha ** (i+1) is given by poly-repr of
186 * \alpha ** i shifted left one-bit and accounting for any
187 * \alpha ** GF_BITS term that may occur when poly-repr of
188 * \alpha ** i is shifted.
189 */
190 mask = 1 << (GF_BITS - 1) ;
191 for (i = GF_BITS + 1; i < GF_SIZE; i++) {
192 if (gf_exp[i - 1] >= mask)
193 gf_exp[i] = gf_exp[GF_BITS] ^ ((gf_exp[i - 1] ^ mask) << 1);
194 else
195 gf_exp[i] = gf_exp[i - 1] << 1;
196
197 gf_log[gf_exp[i]] = i;
198 }
199 /*
200 * log(0) is not defined, so use a special value
201 */
202 gf_log[0] = GF_SIZE;
203 /* set the extended gf_exp values for fast multiply */
204 for (i = 0; i < GF_SIZE; i++)
205 gf_exp[i + GF_SIZE] = gf_exp[i];
206
207 /*
208 * again special cases. 0 has no inverse. This used to
209 * be initialized to GF_SIZE, but it should make no difference
210 * since noone is supposed to read from here.
211 */
212 inverse[0] = 0;
213 inverse[1] = 1;
214 for (i=2; i<=GF_SIZE; i++)
215 inverse[i] = gf_exp[GF_SIZE-gf_log[i]];
216 }
217
218 /*
219 * invert_mat() takes a matrix and produces its inverse
220 * k is the size of the matrix.
221 * (Gauss-Jordan, adapted from Numerical Recipes in C)
222 * Return non-zero if singular.
223 */
invert_mat(gf * src,int k)224 static int invert_mat(gf *src, int k) {
225 gf c, *p;
226 int irow, icol, row, col, i, ix;
227
228 int error = 1;
229 #ifdef NEED_ALLOCA
230 int *indxc = alloca(k*sizeof(int));
231 int *indxr = alloca(k*sizeof(int));
232 int *ipiv = alloca(k*sizeof(int));
233 gf *id_row = alloca(k*sizeof(gf));
234 #else
235 int indxc[k];
236 int indxr[k];
237 int ipiv[k];
238 gf id_row[k];
239 #endif
240
241 memset(id_row, 0, k*sizeof(gf));
242 /*
243 * ipiv marks elements already used as pivots.
244 */
245 for (i = 0; i < k; i++)
246 ipiv[i] = 0;
247
248 for (col = 0; col < k; col++) {
249 gf *pivot_row;
250 /*
251 * Zeroing column 'col', look for a non-zero element.
252 * First try on the diagonal, if it fails, look elsewhere.
253 */
254 irow = icol = -1;
255 if (ipiv[col] != 1 && src[col*k + col] != 0) {
256 irow = col;
257 icol = col;
258 goto found_piv;
259 }
260 for (row = 0; row < k; row++) {
261 if (ipiv[row] != 1) {
262 for (ix = 0; ix < k; ix++) {
263 if (ipiv[ix] == 0) {
264 if (src[row*k + ix] != 0) {
265 irow = row;
266 icol = ix;
267 goto found_piv;
268 }
269 } else if (ipiv[ix] > 1) {
270 fprintf(stderr, "singular matrix\n");
271 goto fail;
272 }
273 }
274 }
275 }
276 if (icol == -1) {
277 fprintf(stderr, "XXX pivot not found!\n");
278 goto fail ;
279 }
280
281 found_piv:
282 ++(ipiv[icol]);
283 /*
284 * swap rows irow and icol, so afterwards the diagonal
285 * element will be correct. Rarely done, not worth
286 * optimizing.
287 */
288 if (irow != icol) {
289 for (ix = 0; ix < k; ix++) {
290 SWAP(src[irow*k + ix], src[icol*k + ix], gf);
291 }
292 }
293 indxr[col] = irow;
294 indxc[col] = icol;
295 pivot_row = &src[icol*k];
296 c = pivot_row[icol];
297 if (c == 0) {
298 fprintf(stderr, "singular matrix 2\n");
299 goto fail;
300 } else if (c != 1 ) {
301 /*
302 * this is done often , but optimizing is not so
303 * fruitful, at least in the obvious ways (unrolling)
304 */
305 c = inverse[ c ];
306 pivot_row[icol] = 1;
307 for (ix = 0; ix < k; ix++)
308 pivot_row[ix] = gf_mul(c, pivot_row[ix]);
309 }
310 /*
311 * from all rows, remove multiples of the selected row
312 * to zero the relevant entry (in fact, the entry is not zero
313 * because we know it must be zero).
314 * (Here, if we know that the pivot_row is the identity,
315 * we can optimize the addmul).
316 */
317 id_row[icol] = 1;
318 if (memcmp(pivot_row, id_row, k*sizeof(gf)) != 0) {
319 for (p = src, ix = 0 ; ix < k ; ix++, p += k) {
320 if (ix != icol) {
321 c = p[icol];
322 p[icol] = 0;
323 addmul(p, pivot_row, c, k);
324 }
325 }
326 }
327 id_row[icol] = 0;
328 }
329 for (col = k-1 ; col >= 0 ; col-- ) {
330 if (indxr[col] <0 || indxr[col] >= k)
331 fprintf(stderr, "AARGH, indxr[col] %d\n", indxr[col]);
332 else if (indxc[col] <0 || indxc[col] >= k)
333 fprintf(stderr, "AARGH, indxc[col] %d\n", indxc[col]);
334 else
335 if (indxr[col] != indxc[col] ) {
336 for (row = 0 ; row < k ; row++ )
337 SWAP( src[row*k + indxr[col]], src[row*k + indxc[col]], gf);
338 }
339 }
340 error = 0;
341
342 fail:
343 return error ;
344 }
345
346 /*
347 * Not check for input params
348 * */
sub_matrix(gf * matrix,int rmin,int cmin,int rmax,int cmax,int nrows,int ncols)349 static gf* sub_matrix(gf* matrix, int rmin, int cmin, int rmax, int cmax, int nrows, int ncols) {
350 int i, j, ptr = 0;
351 gf* new_m = (gf*) malloc((rmax-rmin) * (cmax-cmin));
352 if (NULL != new_m) {
353 for (i = rmin; i < rmax; i++) {
354 for (j = cmin; j < cmax; j++) {
355 new_m[ptr++] = matrix[i*ncols + j];
356 }
357 }
358 }
359
360 return new_m;
361 }
362
363 /* copy from golang rs version */
code_some_shards(gf * matrixRows,gf ** inputs,gf ** outputs,int dataShards,int outputCount,int byteCount)364 static inline int code_some_shards(gf* matrixRows, gf** inputs, gf** outputs, int dataShards, int outputCount, int byteCount) {
365 gf* in;
366 int iRow, c;
367 for (c = 0; c < dataShards; c++) {
368 in = inputs[c];
369 for (iRow = 0; iRow < outputCount; iRow++) {
370 if (0 == c)
371 mul(outputs[iRow], in, matrixRows[iRow*dataShards+c], byteCount);
372 else
373 addmul(outputs[iRow], in, matrixRows[iRow*dataShards+c], byteCount);
374 }
375 }
376
377 return 0;
378 }
379
reed_solomon_init(void)380 void reed_solomon_init(void) {
381 generate_gf();
382 init_mul_table();
383 }
384
reed_solomon_new(int data_shards,int parity_shards)385 reed_solomon* reed_solomon_new(int data_shards, int parity_shards) {
386 gf* vm = NULL;
387 gf* top = NULL;
388 int err = 0;
389 reed_solomon* rs = NULL;
390
391 do {
392 rs = malloc(sizeof(reed_solomon));
393 if (NULL == rs)
394 return NULL;
395
396 rs->data_shards = data_shards;
397 rs->parity_shards = parity_shards;
398 rs->shards = (data_shards + parity_shards);
399 rs->m = NULL;
400 rs->parity = NULL;
401
402 if (rs->shards > DATA_SHARDS_MAX || data_shards <= 0 || parity_shards <= 0) {
403 err = 1;
404 break;
405 }
406
407 vm = (gf*)malloc(data_shards * rs->shards);
408
409 if (NULL == vm) {
410 err = 2;
411 break;
412 }
413
414 int ptr = 0;
415 for (int row = 0; row < rs->shards; row++) {
416 for (int col = 0; col < data_shards; col++)
417 vm[ptr++] = row == col ? 1 : 0;
418 }
419
420 top = sub_matrix(vm, 0, 0, data_shards, data_shards, rs->shards, data_shards);
421 if (NULL == top) {
422 err = 3;
423 break;
424 }
425
426 err = invert_mat(top, data_shards);
427 assert(0 == err);
428
429 rs->m = multiply1(vm, rs->shards, data_shards, top, data_shards, data_shards);
430 if (NULL == rs->m) {
431 err = 4;
432 break;
433 }
434
435 for (int j = 0; j < parity_shards; j++) {
436 for (int i = 0; i < data_shards; i++)
437 rs->m[(data_shards + j)*data_shards + i] = inverse[(parity_shards + i) ^ j];
438 }
439
440 rs->parity = sub_matrix(rs->m, data_shards, 0, rs->shards, data_shards, rs->shards, data_shards);
441 if (NULL == rs->parity) {
442 err = 5;
443 break;
444 }
445
446 free(vm);
447 free(top);
448 vm = NULL;
449 top = NULL;
450 return rs;
451
452 } while(0);
453
454 fprintf(stderr, "err=%d\n", err);
455 if (NULL != vm)
456 free(vm);
457
458 if (NULL != top)
459 free(top);
460
461 if (NULL != rs) {
462 if (NULL != rs->m)
463 free(rs->m);
464
465 if (NULL != rs->parity)
466 free(rs->parity);
467
468 free(rs);
469 }
470
471 return NULL;
472 }
473
reed_solomon_release(reed_solomon * rs)474 void reed_solomon_release(reed_solomon* rs) {
475 if (NULL != rs) {
476 if (NULL != rs->m)
477 free(rs->m);
478
479 if (NULL != rs->parity)
480 free(rs->parity);
481
482 free(rs);
483 }
484 }
485
486 /**
487 * decode one shard
488 * input:
489 * rs
490 * original data_blocks[rs->data_shards][block_size]
491 * dec_fec_blocks[nr_fec_blocks][block_size]
492 * fec_block_nos: fec pos number in original fec_blocks
493 * erased_blocks: erased blocks in original data_blocks
494 * nr_fec_blocks: the number of erased blocks
495 * */
reed_solomon_decode(reed_solomon * rs,unsigned char ** data_blocks,int block_size,unsigned char ** dec_fec_blocks,unsigned int * fec_block_nos,unsigned int * erased_blocks,int nr_fec_blocks)496 static int reed_solomon_decode(reed_solomon* rs, unsigned char **data_blocks, int block_size, unsigned char **dec_fec_blocks, unsigned int *fec_block_nos, unsigned int *erased_blocks, int nr_fec_blocks) {
497 /* use stack instead of malloc, define a small number of DATA_SHARDS_MAX to save memory */
498 gf dataDecodeMatrix[DATA_SHARDS_MAX*DATA_SHARDS_MAX];
499 unsigned char* subShards[DATA_SHARDS_MAX];
500 unsigned char* outputs[DATA_SHARDS_MAX];
501 gf* m = rs->m;
502 int i, j, c, swap, subMatrixRow, dataShards, nos, nshards;
503
504 /* the erased_blocks should always sorted
505 * if sorted, nr_fec_blocks times to check it
506 * if not, sort it here
507 * */
508 for (i = 0; i < nr_fec_blocks; i++) {
509 swap = 0;
510 for (j = i+1; j < nr_fec_blocks; j++) {
511 if (erased_blocks[i] > erased_blocks[j]) {
512 /* the prefix is bigger than the following, swap */
513 c = erased_blocks[i];
514 erased_blocks[i] = erased_blocks[j];
515 erased_blocks[j] = c;
516
517 swap = 1;
518 }
519 }
520 if (!swap)
521 break;
522 }
523
524 j = 0;
525 subMatrixRow = 0;
526 nos = 0;
527 nshards = 0;
528 dataShards = rs->data_shards;
529 for (i = 0; i < dataShards; i++) {
530 if (j < nr_fec_blocks && i == erased_blocks[j])
531 j++;
532 else {
533 /* this row is ok */
534 for (c = 0; c < dataShards; c++)
535 dataDecodeMatrix[subMatrixRow*dataShards + c] = m[i*dataShards + c];
536
537 subShards[subMatrixRow] = data_blocks[i];
538 subMatrixRow++;
539 }
540 }
541
542 for (i = 0; i < nr_fec_blocks && subMatrixRow < dataShards; i++) {
543 subShards[subMatrixRow] = dec_fec_blocks[i];
544 j = dataShards + fec_block_nos[i];
545 for (c = 0; c < dataShards; c++)
546 dataDecodeMatrix[subMatrixRow*dataShards + c] = m[j*dataShards + c];
547
548 subMatrixRow++;
549 }
550
551 if (subMatrixRow < dataShards)
552 return -1;
553
554 invert_mat(dataDecodeMatrix, dataShards);
555
556 for (i = 0; i < nr_fec_blocks; i++) {
557 j = erased_blocks[i];
558 outputs[i] = data_blocks[j];
559 memmove(dataDecodeMatrix+i*dataShards, dataDecodeMatrix+j*dataShards, dataShards);
560 }
561
562 return code_some_shards(dataDecodeMatrix, subShards, outputs, dataShards, nr_fec_blocks, block_size);
563 }
564
565 /**
566 * encode a big size of buffer
567 * input:
568 * rs
569 * nr_shards: assert(0 == nr_shards % rs->shards)
570 * shards[nr_shards][block_size]
571 * */
reed_solomon_encode(reed_solomon * rs,unsigned char ** shards,int nr_shards,int block_size)572 int reed_solomon_encode(reed_solomon* rs, unsigned char** shards, int nr_shards, int block_size) {
573 unsigned char** data_blocks;
574 unsigned char** fec_blocks;
575 int i, ds = rs->data_shards, ps = rs->parity_shards, ss = rs->shards;
576 i = nr_shards / ss;
577 data_blocks = shards;
578 fec_blocks = &shards[(i*ds)];
579
580 for (i = 0; i < nr_shards; i += ss) {
581 code_some_shards(rs->parity, data_blocks, fec_blocks, rs->data_shards, rs->parity_shards, block_size);
582 data_blocks += ds;
583 fec_blocks += ps;
584 }
585 return 0;
586 }
587
588 /**
589 * reconstruct a big size of buffer
590 * input:
591 * rs
592 * nr_shards: assert(0 == nr_shards % rs->data_shards)
593 * shards[nr_shards][block_size]
594 * marks[nr_shards] marks as errors
595 * */
reed_solomon_reconstruct(reed_solomon * rs,unsigned char ** shards,unsigned char * marks,int nr_shards,int block_size)596 int reed_solomon_reconstruct(reed_solomon* rs, unsigned char** shards, unsigned char* marks, int nr_shards, int block_size) {
597 unsigned char *dec_fec_blocks[DATA_SHARDS_MAX];
598 unsigned int fec_block_nos[DATA_SHARDS_MAX];
599 unsigned int erased_blocks[DATA_SHARDS_MAX];
600 unsigned char* fec_marks;
601 unsigned char **data_blocks, **fec_blocks;
602 int i, j, dn, pn, n;
603 int ds = rs->data_shards;
604 int ps = rs->parity_shards;
605 int err = 0;
606
607 data_blocks = shards;
608 n = nr_shards / rs->shards;
609 fec_marks = marks + n*ds; //after all data, is't fec marks
610 fec_blocks = shards + n*ds;
611
612 for (j = 0; j < n; j++) {
613 dn = 0;
614 for (i = 0; i < ds; i++) {
615 if (marks[i])
616 erased_blocks[dn++] = i;
617 }
618 if (dn > 0) {
619 pn = 0;
620 for (i = 0; i < ps && pn < dn; i++) {
621 if (!fec_marks[i]) {
622 //got valid fec row
623 fec_block_nos[pn] = i;
624 dec_fec_blocks[pn] = fec_blocks[i];
625 pn++;
626 }
627 }
628
629 if (dn == pn) {
630 reed_solomon_decode(rs, data_blocks, block_size, dec_fec_blocks, fec_block_nos, erased_blocks, dn);
631 } else
632 err = -1;
633 }
634 data_blocks += ds;
635 marks += ds;
636 fec_blocks += ps;
637 fec_marks += ps;
638 }
639
640 return err;
641 }
642