RS编译码C代码

#include<stdio.h> #include<string.h> #include<stdlib.h> #include <sys/time.h> #define IN #define OUT #define IN_OUT #define MOD_VALUE 0x11D #define DEBUG 0 #if DEBUG #define printf_d(x, ...) printf(x, ##__VA_ARGS__) #else #define printf_d(x, ...) #endif unsigned char RsExpTable[256] = {0}; unsigned char RsLogTable[256] = {0}; unsigned char rs_exp(unsigned char exp) { unsigned char mod[2] = {0}; unsigned char buf[32] = {0}; unsigned char ci = 0; unsigned char rs = 0; unsigned char cpos = exp / 8; unsigned char bpos = exp % 8; buf[cpos] = 0x1<<bpos; while (cpos > 0) { mod[1] = (((MOD_VALUE<<bpos) & 0xFF00)>>8); mod[0] = ((MOD_VALUE<<bpos) & 0xFF); buf[cpos] = buf[cpos]^mod[1]; buf[cpos-1] = buf[cpos-1]^mod[0]; while ((buf[cpos] == 0) && (cpos > 0)) { cpos--; } for (ci = 7; ci >= 0; ci--) { if (buf[cpos]&(0x1<<ci)) break; } bpos = ci; } printf_d("2^%d = %d\r\n", exp, buf[cpos]); return buf[cpos]; } unsigned char rs_add(unsigned char x, unsigned char y) { return x^y; } unsigned char rs_sub(unsigned char x, unsigned char y) { return x^y; } unsigned char rs_mul(unsigned char x, unsigned char y) { if ((x == 0) || (y == 0)) return 0; return RsExpTable[(RsLogTable[x]+RsLogTable[y])%255]; } unsigned char rs_div(unsigned char x, unsigned char y) { if (x == 0) return 0; if (y == 0) { printf("div don't 0\n"); return 0; } return RsExpTable[(RsLogTable[x]+255-RsLogTable[y])%255]; } //x^y unsigned char rs_pow(unsigned char x, unsigned int y) { if (x == 0) return 0; return RsExpTable[(RsLogTable[x]*y)%255]; } unsigned char rs_inverse(unsigned char x) { if (x == 0) return 0; return RsExpTable[255-RsLogTable[x]]; } int rs_init_exp_log_table(void) { int i = 0; for (i = 0; i < 256; i++) { RsExpTable[i] = rs_exp(i); RsLogTable[RsExpTable[i]] = i; // printf("exp %d, vlaue: 0x%x\r\n", i, RsExpTable[i]); } return 0; } int rs_poly_div(unsigned char *sbuf, unsigned int ssize, unsigned char *pbuf, unsigned int psize, unsigned char *cbuf, unsigned int csize, unsigned char *rbuf, unsigned int rsize) { unsigned long bsize = ssize; unsigned long bpos = 0; unsigned int bpi = 0; unsigned char tdiv = 0; unsigned char *buf = malloc(bsize); if (buf == NULL) { printf("malloc buf fail\r\n"); return -1; } memset(buf, 0, bsize); memcpy(buf, sbuf, ssize); if (psize > (rsize+1)) { free(buf); printf("receive rbuf is too small\r\n"); return -1; } while (buf[bpos] == 0) { bpos++; } while (bsize - bpos >= psize) { tdiv = rs_div(buf[bpos], pbuf[0]); cbuf[bpos] = tdiv; for(bpi = 0; bpi < psize; bpi++) { buf[bpos+bpi] = buf[bpos+bpi]^rs_mul(tdiv, pbuf[bpi]); } while ((buf[bpos] == 0) && (bsize - bpos >= psize)) { bpos++; } } memcpy(rbuf, buf+bpos, bsize-bpos); free(buf); return 0; } int rs_exp_to_gploy(unsigned char *poly, unsigned int psize) { unsigned int pi = 0; for (pi = 0; pi < psize; pi++) { poly[pi] = RsExpTable[poly[pi]]; } return 0; } unsigned char rs_poly_eval(unsigned char *poly, unsigned int psize, unsigned char x) { unsigned char eval = 0; unsigned int pi = 0; eval = poly[0]; for (pi = 1; pi < psize; pi++) { eval = rs_add(rs_mul(eval, x), poly[pi]); } return eval; } int rs_poly_scale(unsigned char *poly, unsigned int psize, unsigned char scale) { unsigned int pi = 0; for (pi = 0; pi < psize; pi++) { poly[pi] = rs_mul(poly[pi], scale); } return 0; } int rs_poly_scale_to_poly(unsigned char *poly, unsigned int psize, unsigned char *rpoly, unsigned int rsize, unsigned char scale) { unsigned int pi = 0; if (rsize < psize) { printf("recv buf is too small....\r\n"); return -1; } for (pi = 0; pi < psize; pi++) { rpoly[pi] = rs_mul(poly[pi], scale); } return 0; } //if p1szie==p2size then add to s1poly int rs_poly_add(unsigned char *s1poly, unsigned int p1size, unsigned char *s2poly, unsigned int p2size) { unsigned char *despoly = NULL; unsigned int pi = 0; unsigned int i = 0; if (p1size >= p2size) { pi = p2size; i = p1size - p2size; s1poly += i; despoly = s1poly; } else { pi = p1size; i = p2size - p1size; s2poly += i; despoly = s2poly; } for (i = 0; i < pi; i++) { despoly[i] = rs_add(s2poly[i], s1poly[i]); } return 0; } int rs_poly_mul(unsigned char *poly1, const unsigned int psize1, unsigned char *poly2, const unsigned int psize2, unsigned char *pmul, const unsigned int mulsize) { unsigned int i = 0; unsigned int j = 0; unsigned char * buf = NULL; if (mulsize < (psize1+psize2-1)) return -1; buf = malloc(mulsize); if (buf == NULL) { return -1; } memset(buf, 0, mulsize); for (i = 0; i < psize1; i++) { for (j = 0; j < psize2; j++) { buf[i+j] = rs_add(buf[i+j], rs_mul(poly1[i], poly2[j])); } } memcpy(pmul, buf, mulsize); free(buf); return 0; } int rs_generator_poly(unsigned char *gpoly, const unsigned int n) { unsigned char cpoly[2] = {1, 1}; unsigned int gpi = 0; gpoly[0] = 1; for (gpi = 0; gpi < n; gpi++) { cpoly[1] = rs_pow(2, gpi); rs_poly_mul(gpoly, gpi+1, cpoly, 2, gpoly, n); } return 0; } int rs_calc_syndromes(unsigned char *msg, const unsigned int msglen, unsigned char *syndpoly, unsigned int n) { unsigned int syndi = 0; for (syndi = 0; syndi < n; syndi++) { syndpoly[syndi] = rs_poly_eval(msg, msglen, rs_pow(2, syndi)); } } //Berlekamp-Massey int rs_find_error_locator(unsigned char *syndpoly, const unsigned int syndlen, unsigned char *err_loc, unsigned int *err_count) { unsigned int err_len = 0; unsigned int temp_len = 0; unsigned int err_mlen = *err_count; unsigned int old_len = 0; unsigned int i = 0, j = 0, n = 0; unsigned int shift_synd = 0; unsigned int K = 0; unsigned char delta = 0; unsigned char bmvalue = 0; unsigned char *old_loc = malloc(err_mlen); if (old_loc == NULL) { printf("malloc faild!\r\n"); return -1; } old_loc[0] = 1; err_loc[0] = 1; old_len = 1; err_len = 1; for(i = 0; i < syndlen; i++) { K = i + shift_synd; delta = syndpoly[K]; bmvalue = 0; for (j = 1; j < err_len; j++) { printf_d("before add bmvalue %d, rs_mul(%d, %d) %d\r\n", bmvalue, err_loc[err_len-j-1], syndpoly[K-j], rs_mul(err_loc[err_len-j-1], syndpoly[K-j])); bmvalue = rs_add(bmvalue, rs_mul(err_loc[err_len-j-1], syndpoly[K-j])); //delta ^= rs_mul(err_loc[err_len-j-1], syndpoly[K-j]); printf_d("add result bmvalue %d, err_len %d, j %d, i %d\r\n", bmvalue, err_len, j, i); } printf_d("bmvalue %d, delta %d\r\n", bmvalue, delta); delta = rs_sub(bmvalue, delta); old_loc[old_len++] = 0; printf_d("delta %d, old_loc %d, err_len %d\r\n", delta, old_len, err_len); if (delta != 0) { if (old_len > err_len) { unsigned char *t_loc = malloc(old_len); unsigned int t_len = 0; if (t_loc == NULL) { printf("t_loc malloc faild!\r\n"); goto faild; } rs_poly_scale(old_loc, old_len, delta); memcpy(t_loc, old_loc, old_len); t_len = old_len; rs_poly_scale(old_loc, old_len, rs_inverse(delta)); rs_poly_scale(err_loc, err_len, rs_inverse(delta)); memcpy(old_loc, err_loc, err_len); old_len = err_len; rs_poly_scale(err_loc, err_len, delta); memcpy(err_loc, t_loc, t_len); err_len = t_len; free(t_loc); } #if DEBUG printf_d("old_loc: "); for (n = 0; n < old_len; n++)