grub-core/lib/reed_solomon.c

   1 /*
   2  *  GRUB  --  GRand Unified Bootloader
   3  *  Copyright (C) 2010  Free Software Foundation, Inc.
   4  *
   5  *  GRUB is free software: you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation, either version 3 of the License, or
   8  *  (at your option) any later version.
   9  *
  10  *  GRUB is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with GRUB.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #ifdef TEST
  20 #include <stdio.h>
  21 #include <string.h>
  22 #include <stdlib.h>
  23 #define xcalloc calloc
  24 #define xmalloc malloc
  25 #define grub_memset memset
  26 #define grub_memcpy memcpy
  27 #endif
  28
  29 #ifndef STANDALONE
  30 #include <assert.h>
  31 #endif
  32
  33 #ifndef STANDALONE
  34 #ifdef TEST
  35 typedef unsigned int grub_size_t;
  36 typedef unsigned char grub_uint8_t;
  37 #else
  38 #include <grub/types.h>
  39 #include <grub/reed_solomon.h>
  40 #include <grub/util/misc.h>
  41 #include <grub/misc.h>
  42 #endif
  43 #endif
  44
  45 #define SECTOR_SIZE 512
  46 #define MAX_BLOCK_SIZE (200 * SECTOR_SIZE)
  47
  48 #ifdef STANDALONE
  49 #ifdef TEST
  50 typedef unsigned int grub_size_t;
  51 typedef unsigned char grub_uint8_t;
  52 #else
  53 #include <grub/types.h>
  54 #include <grub/misc.h>
  55 #endif
  56 #ifdef __i386__
  57 #define REED_SOLOMON_ATTRIBUTE  __attribute__ ((regparm(3)))
  58 #else
  59 #define REED_SOLOMON_ATTRIBUTE
  60 #endif
  61 void
  62 grub_reed_solomon_recover (void *ptr_, grub_size_t s, grub_size_t rs)
  63   REED_SOLOMON_ATTRIBUTE;
  64 #else
  65 #define REED_SOLOMON_ATTRIBUTE
  66 #endif
  67
  68 #define GF_SIZE 8
  69 typedef grub_uint8_t gf_single_t;
  70 #define GF_POLYNOMIAL 0x1d
  71 #define GF_INVERT2 0x8e
  72 #if defined (STANDALONE) && !defined (TEST)
  73
  74 #ifdef __APPLE__
  75 #define ATTRIBUTE_TEXT __attribute__ ((section("_text,_text")))
  76 #else
  77 #define ATTRIBUTE_TEXT __attribute__ ((section(".text")))
  78 #endif
  79
  80 static gf_single_t * const gf_powx ATTRIBUTE_TEXT = (void *) 0x100000;
  81 static gf_single_t * const gf_powx_inv ATTRIBUTE_TEXT = (void *) 0x100200;
  82 static int *const chosenstat ATTRIBUTE_TEXT = (void *) 0x100300;
  83 static gf_single_t *const sigma ATTRIBUTE_TEXT = (void *) 0x100700;
  84 static gf_single_t *const errpot ATTRIBUTE_TEXT = (void *) 0x100800;
  85 static int *const errpos ATTRIBUTE_TEXT = (void *) 0x100900;
  86 static gf_single_t *const sy ATTRIBUTE_TEXT = (void *) 0x100d00;
  87 static gf_single_t *const mstat ATTRIBUTE_TEXT = (void *) 0x100e00;
  88 static gf_single_t *const errvals ATTRIBUTE_TEXT = (void *) 0x100f00;
  89 static gf_single_t *const eqstat ATTRIBUTE_TEXT = (void *) 0x101000;
  90 /* Next available address: (void *) 0x112000.  */
  91 #else
  92
  93 static gf_single_t gf_powx[255 * 2];
  94 static gf_single_t gf_powx_inv[256];
  95 static int chosenstat[256];
  96 static gf_single_t sigma[256];
  97 static gf_single_t errpot[256];
  98 static int errpos[256];
  99 static gf_single_t sy[256];
 100 static gf_single_t mstat[256];
 101 static gf_single_t errvals[256];
 102 static gf_single_t eqstat[65536 + 256];
 103 #endif
 104
 105 #if __GNUC__ == 12
 106 #pragma GCC diagnostic push
 107 #pragma GCC diagnostic ignored "-Warray-bounds"
 108 #endif
 109
 110 static gf_single_t
 111 gf_mul (gf_single_t a, gf_single_t b)
 112 {
 113   if (a == 0 || b == 0)
 114     return 0;
 115   return gf_powx[(int) gf_powx_inv[a] + (int) gf_powx_inv[b]];
 116 }
 117
 118 static inline gf_single_t
 119 gf_invert (gf_single_t a)
 120 {
 121   return gf_powx[255 - (int) gf_powx_inv[a]];
 122 }
 123
 124 static void
 125 init_powx (void)
 126 {
 127   int i;
 128   grub_uint8_t cur = 1;
 129
 130   gf_powx_inv[0] = 0;
 131   for (i = 0; i < 255; i++)
 132     {
 133       gf_powx[i] = cur;
 134       gf_powx[i + 255] = cur;
 135       gf_powx_inv[cur] = i;
 136       if (cur & (1ULL << (GF_SIZE - 1)))
 137         cur = (cur << 1) ^ GF_POLYNOMIAL;
 138       else
 139         cur <<= 1;
 140     }
 141 }
 142
 143 static gf_single_t
 144 pol_evaluate (gf_single_t *pol, grub_size_t degree, int log_x)
 145 {
 146   int i;
 147   gf_single_t s = 0;
 148   int log_xn = 0;
 149   for (i = degree; i >= 0; i--)
 150     {
 151       if (pol[i])
 152         s ^= gf_powx[(int) gf_powx_inv[pol[i]] + log_xn];
 153       log_xn += log_x;
 154       if (log_xn >= ((1 << GF_SIZE) - 1))
 155         log_xn -= ((1 << GF_SIZE) - 1);
 156     }
 157   return s;
 158 }
 159
 160 #if !defined (STANDALONE)
 161 static void
 162 rs_encode (gf_single_t *data, grub_size_t s, grub_size_t rs)
 163 {
 164   gf_single_t *rs_polynomial;
 165   int i, j;
 166   gf_single_t *m;
 167   m = xcalloc (s + rs, sizeof (gf_single_t));
 168   grub_memcpy (m, data, s * sizeof (gf_single_t));
 169   rs_polynomial = xcalloc (rs + 1, sizeof (gf_single_t));
 170   rs_polynomial[rs] = 1;
 171   /* Multiply with X - a^r */
 172   for (j = 0; j < rs; j++)
 173     {
 174       for (i = 0; i < rs; i++)
 175         if (rs_polynomial[i])
 176           rs_polynomial[i] = (rs_polynomial[i + 1]
 177                               ^ gf_powx[j + (int) gf_powx_inv[rs_polynomial[i]]]);
 178         else
 179           rs_polynomial[i] = rs_polynomial[i + 1];
 180       if (rs_polynomial[rs])
 181         rs_polynomial[rs] = gf_powx[j + (int) gf_powx_inv[rs_polynomial[rs]]];
 182     }
 183   for (j = 0; j < s; j++)
 184     if (m[j])
 185       {
 186         gf_single_t f = m[j];
 187         for (i = 0; i <= rs; i++)
 188           m[i+j] ^= gf_mul (rs_polynomial[i], f);
 189       }
 190   free (rs_polynomial);
 191   grub_memcpy (data + s, m + s, rs * sizeof (gf_single_t));
 192   free (m);
 193 }
 194 #endif
 195
 196 static void
 197 gauss_eliminate (gf_single_t *eq, int n, int m, int *chosen)
 198 {
 199   int i, j;
 200
 201   for (i = 0 ; i < n; i++)
 202     {
 203       int nzidx;
 204       int k;
 205       gf_single_t r;
 206       for (nzidx = 0; nzidx < m && (eq[i * (m + 1) + nzidx] == 0);
 207            nzidx++);
 208       if (nzidx == m)
 209         continue;
 210       chosen[i] = nzidx;
 211       r = gf_invert (eq[i * (m + 1) + nzidx]);
 212       for (j = 0; j < m + 1; j++)
 213         eq[i * (m + 1) + j] = gf_mul (eq[i * (m + 1) + j], r);
 214       for (j = i + 1; j < n; j++)
 215         {
 216           gf_single_t rr = eq[j * (m + 1) + nzidx];
 217           for (k = 0; k < m + 1; k++)
 218             eq[j * (m + 1) + k] ^= gf_mul (eq[i * (m + 1) + k], rr);
 219         }
 220     }
 221 }
 222
 223 static void
 224 gauss_solve (gf_single_t *eq, int n, int m, gf_single_t *sol)
 225 {
 226   int i, j;
 227
 228   for (i = 0; i < n; i++)
 229     chosenstat[i] = -1;
 230   for (i = 0; i < m; i++)
 231     sol[i] = 0;
 232   gauss_eliminate (eq, n, m, chosenstat);
 233   for (i = n - 1; i >= 0; i--)
 234     {
 235       gf_single_t s = 0;
 236       if (chosenstat[i] == -1)
 237         continue;
 238       for (j = 0; j < m; j++)
 239         s ^= gf_mul (eq[i * (m + 1) + j], sol[j]);
 240       s ^= eq[i * (m + 1) + m];
 241       sol[chosenstat[i]] = s;
 242     }
 243 }
 244
 245 static void
 246 rs_recover (gf_single_t *mm, grub_size_t s, grub_size_t rs)
 247 {
 248   grub_size_t rs2 = rs / 2;
 249   int errnum = 0;
 250   int i, j;
 251
 252   for (i = 0; i < (int) rs; i++)
 253     sy[i] = pol_evaluate (mm, s + rs - 1, i);
 254
 255   for (i = 0; i < (int) rs; i++)
 256     if (sy[i] != 0)
 257       break;
 258
 259   /* No error detected.  */
 260   if (i == (int) rs)
 261     return;
 262
 263   {
 264
 265     for (i = 0; i < (int) rs2; i++)
 266       for (j = 0; j < (int) rs2 + 1; j++)
 267         eqstat[i * (rs2 + 1) + j] = sy[i+j];
 268
 269     for (i = 0; i < (int) rs2; i++)
 270       sigma[i] = 0;
 271
 272     gauss_solve (eqstat, rs2, rs2, sigma);
 273   }
 274
 275   for (i = 0; i < (int) (rs + s); i++)
 276     if (pol_evaluate (sigma, rs2 - 1, 255 - i) == gf_powx[i])
 277       {
 278         errpot[errnum] = gf_powx[i];
 279         errpos[errnum++] = s + rs - i - 1;
 280       }
 281   {
 282     for (j = 0; j < errnum; j++)
 283       eqstat[j] = 1;
 284     eqstat[errnum] = sy[0];
 285     for (i = 1; i < (int) rs; i++)
 286       {
 287         for (j = 0; j < (int) errnum; j++)
 288           eqstat[(errnum + 1) * i + j] = gf_mul (errpot[j],
 289                                                  eqstat[(errnum + 1) * (i - 1)
 290                                                         + j]);
 291         eqstat[(errnum + 1) * i + errnum] = sy[i];
 292       }
 293
 294     gauss_solve (eqstat, rs, errnum, errvals);
 295
 296     for (i = 0; i < (int) errnum; i++)
 297       mm[errpos[i]] ^= errvals[i];
 298   }
 299 }
 300
 301 static void
 302 decode_block (gf_single_t *ptr, grub_size_t s,
 303               gf_single_t *rptr, grub_size_t rs)
 304 {
 305   int i, j;
 306   for (i = 0; i < SECTOR_SIZE; i++)
 307     {
 308       grub_size_t ds = (s + SECTOR_SIZE - 1 - i) / SECTOR_SIZE;
 309       grub_size_t rr = (rs + SECTOR_SIZE - 1 - i) / SECTOR_SIZE;
 310
 311       /* Nothing to do.  */
 312       if (!ds || !rr)
 313         continue;
 314
 315       for (j = 0; j < (int) ds; j++)
 316         mstat[j] = ptr[SECTOR_SIZE * j + i];
 317       for (j = 0; j < (int) rr; j++)
 318         mstat[j + ds] = rptr[SECTOR_SIZE * j + i];
 319
 320       rs_recover (mstat, ds, rr);
 321
 322       for (j = 0; j < (int) ds; j++)
 323         ptr[SECTOR_SIZE * j + i] = mstat[j];
 324     }
 325 }
 326
 327 #if __GNUC__ == 12
 328 #pragma GCC diagnostic pop
 329 #endif
 330
 331 #if !defined (STANDALONE)
 332 static void
 333 encode_block (gf_single_t *ptr, grub_size_t s,
 334               gf_single_t *rptr, grub_size_t rs)
 335 {
 336   int i, j;
 337   for (i = 0; i < SECTOR_SIZE; i++)
 338     {
 339       grub_size_t ds = (s + SECTOR_SIZE - 1 - i) / SECTOR_SIZE;
 340       grub_size_t rr = (rs + SECTOR_SIZE - 1 - i) / SECTOR_SIZE;
 341       gf_single_t *m;
 342
 343       if (!ds || !rr)
 344         continue;
 345
 346       m = xmalloc (ds + rr);
 347       for (j = 0; j < ds; j++)
 348         m[j] = ptr[SECTOR_SIZE * j + i];
 349       rs_encode (m, ds, rr);
 350       for (j = 0; j < rr; j++)
 351         rptr[SECTOR_SIZE * j + i] = m[j + ds];
 352       free (m);
 353     }
 354 }
 355 #endif
 356
 357 #if !defined (STANDALONE)
 358 void
 359 grub_reed_solomon_add_redundancy (void *buffer, grub_size_t data_size,
 360                                   grub_size_t redundancy)
 361 {
 362   grub_size_t s = data_size;
 363   grub_size_t rs = redundancy;
 364   gf_single_t *ptr = buffer;
 365   gf_single_t *rptr = ptr + s;
 366   void *tmp;
 367
 368   tmp = xmalloc (data_size);
 369   grub_memcpy (tmp, buffer, data_size);
 370
 371   /* Nothing to do.  */
 372   if (!rs)
 373     goto exit;
 374
 375   init_powx ();
 376
 377   while (s > 0)
 378     {
 379       grub_size_t tt;
 380       grub_size_t cs, crs;
 381       cs = s;
 382       crs = rs;
 383       tt = cs + crs;
 384       if (tt > MAX_BLOCK_SIZE)
 385         {
 386           cs = ((cs * (MAX_BLOCK_SIZE / 512)) / tt) * 512;
 387           crs = ((crs * (MAX_BLOCK_SIZE / 512)) / tt) * 512;
 388         }
 389       encode_block (ptr, cs, rptr, crs);
 390       ptr += cs;
 391       rptr += crs;
 392       s -= cs;
 393       rs -= crs;
 394     }
 395
 396 #ifndef TEST
 397   assert (grub_memcmp (tmp, buffer, data_size) == 0);
 398 #endif
 399 exit:
 400   free (tmp);
 401 }
 402 #endif
 403
 404 void REED_SOLOMON_ATTRIBUTE
 405 grub_reed_solomon_recover (void *ptr_, grub_size_t s, grub_size_t rs)
 406 {
 407   gf_single_t *ptr = ptr_;
 408   gf_single_t *rptr = ptr + s;
 409   grub_uint8_t *cptr;
 410
 411   /* Nothing to do.  */
 412   if (!rs)
 413     return;
 414
 415   for (cptr = rptr + rs - 1; cptr >= rptr; cptr--)
 416     if (*cptr)
 417       break;
 418   if (rptr + rs - 1 - cptr > (grub_ssize_t) rs / 2)
 419     return;
 420
 421   init_powx ();
 422
 423   while (s > 0)
 424     {
 425       grub_size_t tt;
 426       grub_size_t cs, crs;
 427       cs = s;
 428       crs = rs;
 429       tt = cs + crs;
 430       if (tt > MAX_BLOCK_SIZE)
 431         {
 432           cs = ((cs * (MAX_BLOCK_SIZE / 512)) / tt) * 512;
 433           crs = ((crs * (MAX_BLOCK_SIZE / 512)) / tt) * 512;
 434         }
 435       decode_block (ptr, cs, rptr, crs);
 436       ptr += cs;
 437       rptr += crs;
 438       s -= cs;
 439       rs -= crs;
 440     }
 441 }
 442
 443 #ifdef TEST
 444 int
 445 main (int argc, char **argv)
 446 {
 447   FILE *in, *out;
 448   grub_size_t s, rs;
 449   char *buf;
 450
 451   grub_memset (gf_powx, 0xee, sizeof (gf_powx));
 452   grub_memset (gf_powx_inv, 0xdd, sizeof (gf_powx_inv));
 453
 454 #ifndef STANDALONE
 455   init_powx ();
 456 #endif
 457
 458 #ifndef STANDALONE
 459   in = grub_util_fopen ("tst.bin", "rb");
 460   if (!in)
 461     return 1;
 462   fseek (in, 0, SEEK_END);
 463   s = ftell (in);
 464   fseek (in, 0, SEEK_SET);
 465   rs = 0x7007;
 466   buf = xmalloc (s + rs + SECTOR_SIZE);
 467   fread (buf, 1, s, in);
 468   fclose (in);
 469
 470   grub_reed_solomon_add_redundancy (buf, s, rs);
 471
 472   out = grub_util_fopen ("tst_rs.bin", "wb");
 473   fwrite (buf, 1, s + rs, out);
 474   fclose (out);
 475 #else
 476   out = grub_util_fopen ("tst_rs.bin", "rb");
 477   fseek (out, 0, SEEK_END);
 478   s = ftell (out);
 479   fseek (out, 0, SEEK_SET);
 480   rs = 0x7007;
 481   s -= rs;
 482
 483   buf = xmalloc (s + rs + SECTOR_SIZE);
 484   fread (buf, 1, s + rs, out);
 485   fclose (out);
 486 #endif
 487 #if 1
 488   grub_memset (buf + 512 * 15, 0, 512);
 489 #endif
 490
 491   out = grub_util_fopen ("tst_dam.bin", "wb");
 492   fwrite (buf, 1, s + rs, out);
 493   fclose (out);
 494   grub_reed_solomon_recover (buf, s, rs);
 495
 496   out = grub_util_fopen ("tst_rec.bin", "wb");
 497   fwrite (buf, 1, s, out);
 498   fclose (out);
 499
 500   return 0;
 501 }
 502 #endif