diff options
Diffstat (limited to 'grub-core/lib/reed_solomon.c')
| -rw-r--r-- | grub-core/lib/reed_solomon.c | 493 |
1 files changed, 493 insertions, 0 deletions
diff --git a/grub-core/lib/reed_solomon.c b/grub-core/lib/reed_solomon.c new file mode 100644 index 0000000..467305b --- /dev/null +++ b/grub-core/lib/reed_solomon.c @@ -0,0 +1,493 @@ +/* + * GRUB -- GRand Unified Bootloader + * Copyright (C) 2010 Free Software Foundation, Inc. + * + * GRUB is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GRUB is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GRUB. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifdef TEST +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#define xcalloc calloc +#define xmalloc malloc +#define grub_memset memset +#define grub_memcpy memcpy +#endif + +#ifndef STANDALONE +#include <assert.h> +#endif + +#ifndef STANDALONE +#ifdef TEST +typedef unsigned int grub_size_t; +typedef unsigned char grub_uint8_t; +#else +#include <grub/types.h> +#include <grub/reed_solomon.h> +#include <grub/util/misc.h> +#include <grub/misc.h> +#endif +#endif + +#define SECTOR_SIZE 512 +#define MAX_BLOCK_SIZE (200 * SECTOR_SIZE) + +#ifdef STANDALONE +#ifdef TEST +typedef unsigned int grub_size_t; +typedef unsigned char grub_uint8_t; +#else +#include <grub/types.h> +#include <grub/misc.h> +#endif +#ifdef __i386__ +#define REED_SOLOMON_ATTRIBUTE __attribute__ ((regparm(3))) +#else +#define REED_SOLOMON_ATTRIBUTE +#endif +void +grub_reed_solomon_recover (void *ptr_, grub_size_t s, grub_size_t rs) + REED_SOLOMON_ATTRIBUTE; +#else +#define REED_SOLOMON_ATTRIBUTE +#endif + +#define GF_SIZE 8 +typedef grub_uint8_t gf_single_t; +#define GF_POLYNOMIAL 0x1d +#define GF_INVERT2 0x8e +#if defined (STANDALONE) && !defined (TEST) + +#ifdef __APPLE__ +#define ATTRIBUTE_TEXT __attribute__ ((section("_text,_text"))) +#else +#define ATTRIBUTE_TEXT __attribute__ ((section(".text"))) +#endif + +static gf_single_t * const gf_powx ATTRIBUTE_TEXT = (void *) 0x100000; +static gf_single_t * const gf_powx_inv ATTRIBUTE_TEXT = (void *) 0x100200; +static int *const chosenstat ATTRIBUTE_TEXT = (void *) 0x100300; +static gf_single_t *const sigma ATTRIBUTE_TEXT = (void *) 0x100700; +static gf_single_t *const errpot ATTRIBUTE_TEXT = (void *) 0x100800; +static int *const errpos ATTRIBUTE_TEXT = (void *) 0x100900; +static gf_single_t *const sy ATTRIBUTE_TEXT = (void *) 0x100d00; +static gf_single_t *const mstat ATTRIBUTE_TEXT = (void *) 0x100e00; +static gf_single_t *const errvals ATTRIBUTE_TEXT = (void *) 0x100f00; +static gf_single_t *const eqstat ATTRIBUTE_TEXT = (void *) 0x101000; +/* Next available address: (void *) 0x112000. */ +#else + +static gf_single_t gf_powx[255 * 2]; +static gf_single_t gf_powx_inv[256]; +static int chosenstat[256]; +static gf_single_t sigma[256]; +static gf_single_t errpot[256]; +static int errpos[256]; +static gf_single_t sy[256]; +static gf_single_t mstat[256]; +static gf_single_t errvals[256]; +static gf_single_t eqstat[65536 + 256]; +#endif + +static gf_single_t +gf_mul (gf_single_t a, gf_single_t b) +{ + if (a == 0 || b == 0) + return 0; + return gf_powx[(int) gf_powx_inv[a] + (int) gf_powx_inv[b]]; +} + +static inline gf_single_t +gf_invert (gf_single_t a) +{ + return gf_powx[255 - (int) gf_powx_inv[a]]; +} + +static void +init_powx (void) +{ + int i; + grub_uint8_t cur = 1; + + gf_powx_inv[0] = 0; + for (i = 0; i < 255; i++) + { + gf_powx[i] = cur; + gf_powx[i + 255] = cur; + gf_powx_inv[cur] = i; + if (cur & (1ULL << (GF_SIZE - 1))) + cur = (cur << 1) ^ GF_POLYNOMIAL; + else + cur <<= 1; + } +} + +static gf_single_t +pol_evaluate (gf_single_t *pol, grub_size_t degree, int log_x) +{ + int i; + gf_single_t s = 0; + int log_xn = 0; + for (i = degree; i >= 0; i--) + { + if (pol[i]) + s ^= gf_powx[(int) gf_powx_inv[pol[i]] + log_xn]; + log_xn += log_x; + if (log_xn >= ((1 << GF_SIZE) - 1)) + log_xn -= ((1 << GF_SIZE) - 1); + } + return s; +} + +#if !defined (STANDALONE) +static void +rs_encode (gf_single_t *data, grub_size_t s, grub_size_t rs) +{ + gf_single_t *rs_polynomial; + int i, j; + gf_single_t *m; + m = xcalloc (s + rs, sizeof (gf_single_t)); + grub_memcpy (m, data, s * sizeof (gf_single_t)); + rs_polynomial = xcalloc (rs + 1, sizeof (gf_single_t)); + rs_polynomial[rs] = 1; + /* Multiply with X - a^r */ + for (j = 0; j < rs; j++) + { + for (i = 0; i < rs; i++) + if (rs_polynomial[i]) + rs_polynomial[i] = (rs_polynomial[i + 1] + ^ gf_powx[j + (int) gf_powx_inv[rs_polynomial[i]]]); + else + rs_polynomial[i] = rs_polynomial[i + 1]; + if (rs_polynomial[rs]) + rs_polynomial[rs] = gf_powx[j + (int) gf_powx_inv[rs_polynomial[rs]]]; + } + for (j = 0; j < s; j++) + if (m[j]) + { + gf_single_t f = m[j]; + for (i = 0; i <= rs; i++) + m[i+j] ^= gf_mul (rs_polynomial[i], f); + } + free (rs_polynomial); + grub_memcpy (data + s, m + s, rs * sizeof (gf_single_t)); + free (m); +} +#endif + +static void +gauss_eliminate (gf_single_t *eq, int n, int m, int *chosen) +{ + int i, j; + + for (i = 0 ; i < n; i++) + { + int nzidx; + int k; + gf_single_t r; + for (nzidx = 0; nzidx < m && (eq[i * (m + 1) + nzidx] == 0); + nzidx++); + if (nzidx == m) + continue; + chosen[i] = nzidx; + r = gf_invert (eq[i * (m + 1) + nzidx]); + for (j = 0; j < m + 1; j++) + eq[i * (m + 1) + j] = gf_mul (eq[i * (m + 1) + j], r); + for (j = i + 1; j < n; j++) + { + gf_single_t rr = eq[j * (m + 1) + nzidx]; + for (k = 0; k < m + 1; k++) + eq[j * (m + 1) + k] ^= gf_mul (eq[i * (m + 1) + k], rr); + } + } +} + +static void +gauss_solve (gf_single_t *eq, int n, int m, gf_single_t *sol) +{ + int i, j; + + for (i = 0; i < n; i++) + chosenstat[i] = -1; + for (i = 0; i < m; i++) + sol[i] = 0; + gauss_eliminate (eq, n, m, chosenstat); + for (i = n - 1; i >= 0; i--) + { + gf_single_t s = 0; + if (chosenstat[i] == -1) + continue; + for (j = 0; j < m; j++) + s ^= gf_mul (eq[i * (m + 1) + j], sol[j]); + s ^= eq[i * (m + 1) + m]; + sol[chosenstat[i]] = s; + } +} + +static void +rs_recover (gf_single_t *mm, grub_size_t s, grub_size_t rs) +{ + grub_size_t rs2 = rs / 2; + int errnum = 0; + int i, j; + + for (i = 0; i < (int) rs; i++) + sy[i] = pol_evaluate (mm, s + rs - 1, i); + + for (i = 0; i < (int) rs; i++) + if (sy[i] != 0) + break; + + /* No error detected. */ + if (i == (int) rs) + return; + + { + + for (i = 0; i < (int) rs2; i++) + for (j = 0; j < (int) rs2 + 1; j++) + eqstat[i * (rs2 + 1) + j] = sy[i+j]; + + for (i = 0; i < (int) rs2; i++) + sigma[i] = 0; + + gauss_solve (eqstat, rs2, rs2, sigma); + } + + for (i = 0; i < (int) (rs + s); i++) + if (pol_evaluate (sigma, rs2 - 1, 255 - i) == gf_powx[i]) + { + errpot[errnum] = gf_powx[i]; + errpos[errnum++] = s + rs - i - 1; + } + { + for (j = 0; j < errnum; j++) + eqstat[j] = 1; + eqstat[errnum] = sy[0]; + for (i = 1; i < (int) rs; i++) + { + for (j = 0; j < (int) errnum; j++) + eqstat[(errnum + 1) * i + j] = gf_mul (errpot[j], + eqstat[(errnum + 1) * (i - 1) + + j]); + eqstat[(errnum + 1) * i + errnum] = sy[i]; + } + + gauss_solve (eqstat, rs, errnum, errvals); + + for (i = 0; i < (int) errnum; i++) + mm[errpos[i]] ^= errvals[i]; + } +} + +static void +decode_block (gf_single_t *ptr, grub_size_t s, + gf_single_t *rptr, grub_size_t rs) +{ + int i, j; + for (i = 0; i < SECTOR_SIZE; i++) + { + grub_size_t ds = (s + SECTOR_SIZE - 1 - i) / SECTOR_SIZE; + grub_size_t rr = (rs + SECTOR_SIZE - 1 - i) / SECTOR_SIZE; + + /* Nothing to do. */ + if (!ds || !rr) + continue; + + for (j = 0; j < (int) ds; j++) + mstat[j] = ptr[SECTOR_SIZE * j + i]; + for (j = 0; j < (int) rr; j++) + mstat[j + ds] = rptr[SECTOR_SIZE * j + i]; + + rs_recover (mstat, ds, rr); + + for (j = 0; j < (int) ds; j++) + ptr[SECTOR_SIZE * j + i] = mstat[j]; + } +} + +#if !defined (STANDALONE) +static void +encode_block (gf_single_t *ptr, grub_size_t s, + gf_single_t *rptr, grub_size_t rs) +{ + int i, j; + for (i = 0; i < SECTOR_SIZE; i++) + { + grub_size_t ds = (s + SECTOR_SIZE - 1 - i) / SECTOR_SIZE; + grub_size_t rr = (rs + SECTOR_SIZE - 1 - i) / SECTOR_SIZE; + gf_single_t *m; + + if (!ds || !rr) + continue; + + m = xmalloc (ds + rr); + for (j = 0; j < ds; j++) + m[j] = ptr[SECTOR_SIZE * j + i]; + rs_encode (m, ds, rr); + for (j = 0; j < rr; j++) + rptr[SECTOR_SIZE * j + i] = m[j + ds]; + free (m); + } +} +#endif + +#if !defined (STANDALONE) +void +grub_reed_solomon_add_redundancy (void *buffer, grub_size_t data_size, + grub_size_t redundancy) +{ + grub_size_t s = data_size; + grub_size_t rs = redundancy; + gf_single_t *ptr = buffer; + gf_single_t *rptr = ptr + s; + void *tmp; + + tmp = xmalloc (data_size); + grub_memcpy (tmp, buffer, data_size); + + /* Nothing to do. */ + if (!rs) + goto exit; + + init_powx (); + + while (s > 0) + { + grub_size_t tt; + grub_size_t cs, crs; + cs = s; + crs = rs; + tt = cs + crs; + if (tt > MAX_BLOCK_SIZE) + { + cs = ((cs * (MAX_BLOCK_SIZE / 512)) / tt) * 512; + crs = ((crs * (MAX_BLOCK_SIZE / 512)) / tt) * 512; + } + encode_block (ptr, cs, rptr, crs); + ptr += cs; + rptr += crs; + s -= cs; + rs -= crs; + } + +#ifndef TEST + assert (grub_memcmp (tmp, buffer, data_size) == 0); +#endif +exit: + free (tmp); +} +#endif + +void REED_SOLOMON_ATTRIBUTE +grub_reed_solomon_recover (void *ptr_, grub_size_t s, grub_size_t rs) +{ + gf_single_t *ptr = ptr_; + gf_single_t *rptr = ptr + s; + grub_uint8_t *cptr; + + /* Nothing to do. */ + if (!rs) + return; + + for (cptr = rptr + rs - 1; cptr >= rptr; cptr--) + if (*cptr) + break; + if (rptr + rs - 1 - cptr > (grub_ssize_t) rs / 2) + return; + + init_powx (); + + while (s > 0) + { + grub_size_t tt; + grub_size_t cs, crs; + cs = s; + crs = rs; + tt = cs + crs; + if (tt > MAX_BLOCK_SIZE) + { + cs = ((cs * (MAX_BLOCK_SIZE / 512)) / tt) * 512; + crs = ((crs * (MAX_BLOCK_SIZE / 512)) / tt) * 512; + } + decode_block (ptr, cs, rptr, crs); + ptr += cs; + rptr += crs; + s -= cs; + rs -= crs; + } +} + +#ifdef TEST +int +main (int argc, char **argv) +{ + FILE *in, *out; + grub_size_t s, rs; + char *buf; + + grub_memset (gf_powx, 0xee, sizeof (gf_powx)); + grub_memset (gf_powx_inv, 0xdd, sizeof (gf_powx_inv)); + +#ifndef STANDALONE + init_powx (); +#endif + +#ifndef STANDALONE + in = grub_util_fopen ("tst.bin", "rb"); + if (!in) + return 1; + fseek (in, 0, SEEK_END); + s = ftell (in); + fseek (in, 0, SEEK_SET); + rs = 0x7007; + buf = xmalloc (s + rs + SECTOR_SIZE); + fread (buf, 1, s, in); + fclose (in); + + grub_reed_solomon_add_redundancy (buf, s, rs); + + out = grub_util_fopen ("tst_rs.bin", "wb"); + fwrite (buf, 1, s + rs, out); + fclose (out); +#else + out = grub_util_fopen ("tst_rs.bin", "rb"); + fseek (out, 0, SEEK_END); + s = ftell (out); + fseek (out, 0, SEEK_SET); + rs = 0x7007; + s -= rs; + + buf = xmalloc (s + rs + SECTOR_SIZE); + fread (buf, 1, s + rs, out); + fclose (out); +#endif +#if 1 + grub_memset (buf + 512 * 15, 0, 512); +#endif + + out = grub_util_fopen ("tst_dam.bin", "wb"); + fwrite (buf, 1, s + rs, out); + fclose (out); + grub_reed_solomon_recover (buf, s, rs); + + out = grub_util_fopen ("tst_rec.bin", "wb"); + fwrite (buf, 1, s, out); + fclose (out); + + return 0; +} +#endif |