arduino-audio-tools
Loading...
Searching...
No Matches
rs.hpp
1/* Author: Mike Lubinets (aka mersinvald)
2 * Date: 29.12.15
3 *
4 * See LICENSE */
5
6#ifndef RS_HPP
7#define RS_HPP
8#include <string.h>
9#include <stdint.h>
10#include "poly.hpp"
11#include "gf.hpp"
12
13#if !defined DEBUG && !defined __CC_ARM
14#include <assert.h>
15#else
16#define assert(dummy)
17#endif
18
20namespace RS {
21
22#define MSG_CNT 3 // message-length polynomials count
23#define POLY_CNT 14 // (ecc_length*2)-length polynomials count
24
25template <const uint8_t msg_length, // Message length without correction code
26 const uint8_t ecc_length> // Length of correction code
27
29public:
30 ReedSolomon() {
31 const uint8_t enc_len = msg_length + ecc_length;
32 const uint8_t poly_len = ecc_length * 2;
33 uint8_t** memptr = &memory;
34 uint16_t offset = 0;
35
36 /* Initialize first six polys manually cause their amount depends on template parameters */
37
38 polynoms[0].Init(ID_MSG_IN, offset, enc_len, memptr);
39 offset += enc_len;
40
41 polynoms[1].Init(ID_MSG_OUT, offset, enc_len, memptr);
42 offset += enc_len;
43
44 for(uint8_t i = ID_GENERATOR; i < ID_MSG_E; i++) {
45 polynoms[i].Init(i, offset, poly_len, memptr);
46 offset += poly_len;
47 }
48
49 polynoms[5].Init(ID_MSG_E, offset, enc_len, memptr);
50 offset += enc_len;
51
52 for(uint8_t i = ID_TPOLY3; i < ID_ERR_EVAL+2; i++) {
53 polynoms[i].Init(i, offset, poly_len, memptr);
54 offset += poly_len;
55 }
56 }
57
58 ~ReedSolomon() {
59 // Dummy destructor, gcc-generated one crashes program
60 memory = NULL;
61 }
62
63 /* @brief Message block encoding
64 * @param *src - input message buffer (msg_length size)
65 * @param *dst - output buffer for ecc (ecc_length size at least) */
66 void EncodeBlock(const void* src, void* dst) {
67 assert(msg_length + ecc_length < 256);
68
69 /* Generator cache, it dosn't change for one template parameters */
70 static uint8_t generator_cache[ecc_length+1] = {0};
71 static bool generator_cached = false;
72
73 /* Allocating memory on stack for polynomials storage */
74 uint8_t stack_memory[MSG_CNT * msg_length + POLY_CNT * ecc_length * 2];
75 this->memory = stack_memory;
76
77 const uint8_t* src_ptr = (const uint8_t*) src;
78 uint8_t* dst_ptr = (uint8_t*) dst;
79
80 Poly *msg_in = &polynoms[ID_MSG_IN];
81 Poly *msg_out = &polynoms[ID_MSG_OUT];
82 Poly *gen = &polynoms[ID_GENERATOR];
83
84 // Weird shit, but without resetting msg_in it simply doesn't work
85 msg_in->Reset();
86 msg_out->Reset();
87
88 // Using cached generator or generating new one
89 if(generator_cached) {
90 gen->Set(generator_cache, sizeof(generator_cache));
91 } else {
92 GeneratorPoly();
93 memcpy(generator_cache, gen->ptr(), gen->length);
94 generator_cached = true;
95 }
96
97 // Copying input message to internal polynomial
98 msg_in->Set(src_ptr, msg_length);
99 msg_out->Set(src_ptr, msg_length);
100 msg_out->length = msg_in->length + ecc_length;
101
102 // Here all the magic happens
103 uint8_t coef = 0; // cache
104 for(uint8_t i = 0; i < msg_length; i++){
105 coef = msg_out->at(i);
106 if(coef != 0){
107 for(uint32_t j = 1; j < gen->length; j++){
108 msg_out->at(i+j) ^= gf::mul(gen->at(j), coef);
109 }
110 }
111 }
112
113 // Copying ECC to the output buffer
114 memcpy(dst_ptr, msg_out->ptr()+msg_length, ecc_length * sizeof(uint8_t));
115 }
116
117 /* @brief Message encoding
118 * @param *src - input message buffer (msg_length size)
119 * @param *dst - output buffer (msg_length + ecc_length size at least) */
120 void Encode(const void* src, void* dst) {
121 uint8_t* dst_ptr = (uint8_t*) dst;
122
123 // Copying message to the output buffer
124 memcpy(dst_ptr, src, msg_length * sizeof(uint8_t));
125
126 // Calling EncodeBlock to write ecc to out[ut buffer
127 EncodeBlock(src, dst_ptr+msg_length);
128 }
129
130 /* @brief Message block decoding
131 * @param *src - encoded message buffer (msg_length size)
132 * @param *ecc - ecc buffer (ecc_length size)
133 * @param *msg_out - output buffer (msg_length size at least)
134 * @param *erase_pos - known errors positions
135 * @param erase_count - count of known errors
136 * @return RESULT_SUCCESS if successful, error code otherwise */
137 int DecodeBlock(const void* src, const void* ecc, void* dst, uint8_t* erase_pos = NULL, size_t erase_count = 0) {
138 assert(msg_length + ecc_length < 256);
139
140 const uint8_t *src_ptr = (const uint8_t*) src;
141 const uint8_t *ecc_ptr = (const uint8_t*) ecc;
142 uint8_t *dst_ptr = (uint8_t*) dst;
143
144 const uint8_t src_len = msg_length + ecc_length;
145 const uint8_t dst_len = msg_length;
146
147 bool ok;
148
149 /* Allocation memory on stack */
150 uint8_t stack_memory[MSG_CNT * msg_length + POLY_CNT * ecc_length * 2];
151 this->memory = stack_memory;
152
153 Poly *msg_in = &polynoms[ID_MSG_IN];
154 Poly *msg_out = &polynoms[ID_MSG_OUT];
155 Poly *epos = &polynoms[ID_ERASURES];
156
157 // Copying message to polynomials memory
158 msg_in->Set(src_ptr, msg_length);
159 msg_in->Set(ecc_ptr, ecc_length, msg_length);
160 msg_out->Copy(msg_in);
161
162 // Copying known errors to polynomial
163 if(erase_pos == NULL) {
164 epos->length = 0;
165 } else {
166 epos->Set(erase_pos, erase_count);
167 for(uint8_t i = 0; i < epos->length; i++){
168 msg_in->at(epos->at(i)) = 0;
169 }
170 }
171
172 // Too many errors
173 if(epos->length > ecc_length) return 1;
174
175 Poly *synd = &polynoms[ID_SYNDROMES];
176 Poly *eloc = &polynoms[ID_ERRORS_LOC];
177 Poly *reloc = &polynoms[ID_TPOLY1];
178 Poly *err = &polynoms[ID_ERRORS];
179 Poly *forney = &polynoms[ID_FORNEY];
180
181 // Calculating syndrome
182 CalcSyndromes(msg_in);
183
184 // Checking for errors
185 bool has_errors = false;
186 for(uint8_t i = 0; i < synd->length; i++) {
187 if(synd->at(i) != 0) {
188 has_errors = true;
189 break;
190 }
191 }
192
193 // Going to exit if no errors
194 if(!has_errors) goto return_corrected_msg;
195
196 CalcForneySyndromes(synd, epos, src_len);
197 FindErrorLocator(forney, NULL, epos->length);
198
199 // Reversing syndrome
200 // TODO optimize through special Poly flag
201 reloc->length = eloc->length;
202 for(int8_t i = eloc->length-1, j = 0; i >= 0; i--, j++){
203 reloc->at(j) = eloc->at(i);
204 }
205
206 // Find errors
207 ok = FindErrors(reloc, src_len);
208 if(!ok) return 1;
209
210 // Error happened while finding errors (so helpful :D)
211 if(err->length == 0) return 1;
212
213 /* Adding found errors with known */
214 for(uint8_t i = 0; i < err->length; i++) {
215 epos->Append(err->at(i));
216 }
217
218 // Correcting errors
219 CorrectErrata(synd, epos, msg_in);
220
221 return_corrected_msg:
222 // Writing corrected message to output buffer
223 msg_out->length = dst_len;
224 memcpy(dst_ptr, msg_out->ptr(), msg_out->length * sizeof(uint8_t));
225 return 0;
226 }
227
228 /* @brief Message block decoding
229 * @param *src - encoded message buffer (msg_length + ecc_length size)
230 * @param *msg_out - output buffer (msg_length size at least)
231 * @param *erase_pos - known errors positions
232 * @param erase_count - count of known errors
233 * @return RESULT_SUCCESS if successful, error code otherwise */
234 int Decode(const void* src, void* dst, uint8_t* erase_pos = NULL, size_t erase_count = 0) {
235 const uint8_t *src_ptr = (const uint8_t*) src;
236 const uint8_t *ecc_ptr = src_ptr + msg_length;
237
238 return DecodeBlock(src, ecc_ptr, dst, erase_pos, erase_count);
239 }
240
241#ifndef DEBUG
242private:
243#endif
244
245 enum POLY_ID {
246 ID_MSG_IN = 0,
247 ID_MSG_OUT,
248 ID_GENERATOR, // 3
249 ID_TPOLY1, // T for Temporary
250 ID_TPOLY2,
251
252 ID_MSG_E, // 5
253
254 ID_TPOLY3, // 6
255 ID_TPOLY4,
256
257 ID_SYNDROMES,
258 ID_FORNEY,
259
260 ID_ERASURES_LOC,
261 ID_ERRORS_LOC,
262
263 ID_ERASURES,
264 ID_ERRORS,
265
266 ID_COEF_POS,
267 ID_ERR_EVAL
268 };
269
270 // Pointer for polynomials memory on stack
271 uint8_t* memory;
272 Poly polynoms[MSG_CNT + POLY_CNT];
273
274 void GeneratorPoly() {
275 Poly *gen = polynoms + ID_GENERATOR;
276 gen->at(0) = 1;
277 gen->length = 1;
278
279 Poly *mulp = polynoms + ID_TPOLY1;
280 Poly *temp = polynoms + ID_TPOLY2;
281 mulp->length = 2;
282
283 for(int8_t i = 0; i < ecc_length; i++){
284 mulp->at(0) = 1;
285 mulp->at(1) = gf::pow(2, i);
286
287 gf::poly_mul(gen, mulp, temp);
288
289 gen->Copy(temp);
290 }
291 }
292
293 void CalcSyndromes(const Poly *msg) {
294 Poly *synd = &polynoms[ID_SYNDROMES];
295 synd->length = ecc_length+1;
296 synd->at(0) = 0;
297 for(uint8_t i = 1; i < ecc_length+1; i++){
298 synd->at(i) = gf::poly_eval(msg, gf::pow(2, i-1));
299 }
300 }
301
302 void FindErrataLocator(const Poly *epos) {
303 Poly *errata_loc = &polynoms[ID_ERASURES_LOC];
304 Poly *mulp = &polynoms[ID_TPOLY1];
305 Poly *addp = &polynoms[ID_TPOLY2];
306 Poly *apol = &polynoms[ID_TPOLY3];
307 Poly *temp = &polynoms[ID_TPOLY4];
308
309 errata_loc->length = 1;
310 errata_loc->at(0) = 1;
311
312 mulp->length = 1;
313 addp->length = 2;
314
315 for(uint8_t i = 0; i < epos->length; i++){
316 mulp->at(0) = 1;
317 addp->at(0) = gf::pow(2, epos->at(i));
318 addp->at(1) = 0;
319
320 gf::poly_add(mulp, addp, apol);
321 gf::poly_mul(errata_loc, apol, temp);
322
323 errata_loc->Copy(temp);
324 }
325 }
326
327 void FindErrorEvaluator(const Poly *synd, const Poly *errata_loc, Poly *dst, uint8_t ecclen) {
328 Poly *mulp = &polynoms[ID_TPOLY1];
329 gf::poly_mul(synd, errata_loc, mulp);
330
331 Poly *divisor = &polynoms[ID_TPOLY2];
332 divisor->length = ecclen+2;
333
334 divisor->Reset();
335 divisor->at(0) = 1;
336
337 gf::poly_div(mulp, divisor, dst);
338 }
339
340 void CorrectErrata(const Poly *synd, const Poly *err_pos, const Poly *msg_in) {
341 Poly *c_pos = &polynoms[ID_COEF_POS];
342 Poly *corrected = &polynoms[ID_MSG_OUT];
343 c_pos->length = err_pos->length;
344
345 for(uint8_t i = 0; i < err_pos->length; i++)
346 c_pos->at(i) = msg_in->length - 1 - err_pos->at(i);
347
348 /* uses t_poly 1, 2, 3, 4 */
349 FindErrataLocator(c_pos);
350 Poly *errata_loc = &polynoms[ID_ERASURES_LOC];
351
352 /* reversing syndromes */
353 Poly *rsynd = &polynoms[ID_TPOLY3];
354 rsynd->length = synd->length;
355
356 for(int8_t i = synd->length-1, j = 0; i >= 0; i--, j++) {
357 rsynd->at(j) = synd->at(i);
358 }
359
360 /* getting reversed error evaluator polynomial */
361 Poly *re_eval = &polynoms[ID_TPOLY4];
362
363 /* uses T_POLY 1, 2 */
364 FindErrorEvaluator(rsynd, errata_loc, re_eval, errata_loc->length-1);
365
366 /* reversing it back */
367 Poly *e_eval = &polynoms[ID_ERR_EVAL];
368 e_eval->length = re_eval->length;
369 for(int8_t i = re_eval->length-1, j = 0; i >= 0; i--, j++) {
370 e_eval->at(j) = re_eval->at(i);
371 }
372
373 Poly *X = &polynoms[ID_TPOLY1]; /* this will store errors positions */
374 X->length = 0;
375
376 int16_t l;
377 for(uint8_t i = 0; i < c_pos->length; i++){
378 l = 255 - c_pos->at(i);
379 X->Append(gf::pow(2, -l));
380 }
381
382 /* Magnitude polynomial
383 Shit just got real */
384 Poly *E = &polynoms[ID_MSG_E];
385 E->Reset();
386 E->length = msg_in->length;
387
388 uint8_t Xi_inv;
389
390 Poly *err_loc_prime_temp = &polynoms[ID_TPOLY2];
391
392 uint8_t err_loc_prime;
393 uint8_t y;
394
395 for(uint8_t i = 0; i < X->length; i++){
396 Xi_inv = gf::inverse(X->at(i));
397
398 err_loc_prime_temp->length = 0;
399 for(uint8_t j = 0; j < X->length; j++){
400 if(j != i){
401 err_loc_prime_temp->Append(gf::sub(1, gf::mul(Xi_inv, X->at(j))));
402 }
403 }
404
405 err_loc_prime = 1;
406 for(uint8_t j = 0; j < err_loc_prime_temp->length; j++){
407 err_loc_prime = gf::mul(err_loc_prime, err_loc_prime_temp->at(j));
408 }
409
410 y = gf::poly_eval(re_eval, Xi_inv);
411 y = gf::mul(gf::pow(X->at(i), 1), y);
412
413 E->at(err_pos->at(i)) = gf::div(y, err_loc_prime);
414 }
415
416 gf::poly_add(msg_in, E, corrected);
417 }
418
419 bool FindErrorLocator(const Poly *synd, Poly *erase_loc = NULL, size_t erase_count = 0) {
420 Poly *error_loc = &polynoms[ID_ERRORS_LOC];
421 Poly *err_loc = &polynoms[ID_TPOLY1];
422 Poly *old_loc = &polynoms[ID_TPOLY2];
423 Poly *temp = &polynoms[ID_TPOLY3];
424 Poly *temp2 = &polynoms[ID_TPOLY4];
425
426 if(erase_loc != NULL) {
427 err_loc->Copy(erase_loc);
428 old_loc->Copy(erase_loc);
429 } else {
430 err_loc->length = 1;
431 old_loc->length = 1;
432 err_loc->at(0) = 1;
433 old_loc->at(0) = 1;
434 }
435
436 uint8_t synd_shift = 0;
437 if(synd->length > ecc_length) {
438 synd_shift = synd->length - ecc_length;
439 }
440
441 uint8_t K = 0;
442 uint8_t delta = 0;
443 uint8_t index;
444
445 for(uint8_t i = 0; i < ecc_length - erase_count; i++){
446 if(erase_loc != NULL)
447 K = erase_count + i + synd_shift;
448 else
449 K = i + synd_shift;
450
451 delta = synd->at(K);
452 for(uint8_t j = 1; j < err_loc->length; j++) {
453 index = err_loc->length - j - 1;
454 delta ^= gf::mul(err_loc->at(index), synd->at(K-j));
455 }
456
457 old_loc->Append(0);
458
459 if(delta != 0) {
460 if(old_loc->length > err_loc->length) {
461 gf::poly_scale(old_loc, temp, delta);
462 gf::poly_scale(err_loc, old_loc, gf::inverse(delta));
463 err_loc->Copy(temp);
464 }
465 gf::poly_scale(old_loc, temp, delta);
466 gf::poly_add(err_loc, temp, temp2);
467 err_loc->Copy(temp2);
468 }
469 }
470
471 uint32_t shift = 0;
472 while(err_loc->length && err_loc->at(shift) == 0) shift++;
473
474 uint32_t errs = err_loc->length - shift - 1;
475 if(((errs - erase_count) * 2 + erase_count) > ecc_length){
476 return false; /* Error count is greater than we can fix! */
477 }
478
479 memcpy(error_loc->ptr(), err_loc->ptr() + shift, (err_loc->length - shift) * sizeof(uint8_t));
480 error_loc->length = (err_loc->length - shift);
481 return true;
482 }
483
484 bool FindErrors(const Poly *error_loc, size_t msg_in_size) {
485 Poly *err = &polynoms[ID_ERRORS];
486
487 uint8_t errs = error_loc->length - 1;
488 err->length = 0;
489
490 for(uint8_t i = 0; i < msg_in_size; i++) {
491 if(gf::poly_eval(error_loc, gf::pow(2, i)) == 0) {
492 err->Append(msg_in_size - 1 - i);
493 }
494 }
495
496 /* Sanity check:
497 * the number of err/errata positions found
498 * should be exactly the same as the length of the errata locator polynomial */
499 if(err->length != errs)
500 /* couldn't find error locations */
501 return false;
502 return true;
503 }
504
505 void CalcForneySyndromes(const Poly *synd, const Poly *erasures_pos, size_t msg_in_size) {
506 Poly *erase_pos_reversed = &polynoms[ID_TPOLY1];
507 Poly *forney_synd = &polynoms[ID_FORNEY];
508 erase_pos_reversed->length = 0;
509
510 for(uint8_t i = 0; i < erasures_pos->length; i++){
511 erase_pos_reversed->Append(msg_in_size - 1 - erasures_pos->at(i));
512 }
513
514 forney_synd->Reset();
515 forney_synd->Set(synd->ptr()+1, synd->length-1);
516
517 uint8_t x;
518 for(uint8_t i = 0; i < erasures_pos->length; i++) {
519 x = gf::pow(2, erase_pos_reversed->at(i));
520 for(int8_t j = 0; j < forney_synd->length - 1; j++){
521 forney_synd->at(j) = gf::mul(forney_synd->at(j), x) ^ forney_synd->at(j+1);
522 }
523 }
524 }
525};
526
527}
528
529#endif // RS_HPP
530
Definition rs.hpp:28
AudioTools internal: Reed-Solomon.
Definition gf.hpp:19
Definition poly.hpp:20