[PATCH 7/7] Add raid6 recovery.

[Goffredo Baroncelli]

The code origins from "raid6_recovery.c". The code was changed because the
original one assumed that both the disk address and size are multiple of
GRUB_DISK_SECTOR_SIZE. This is not true for grub btrfs driver.

The code was made more generalized using a function pointer for reading
the data from the memory or disk.

Signed-off-by: Goffredo Baroncelli <address@hidden>
---
 grub-core/fs/btrfs.c | 211 +++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 204 insertions(+), 7 deletions(-)

diff --git a/grub-core/fs/btrfs.c b/grub-core/fs/btrfs.c
index 5c76a68f3..195313a31 100644
--- a/grub-core/fs/btrfs.c
+++ b/grub-core/fs/btrfs.c
@@ -30,6 +30,7 @@
 #include <grub/i18n.h>
 #include <grub/btrfs.h>
 #include <grub/crypto.h>
+#include <grub/diskfilter.h>
 
 GRUB_MOD_LICENSE ("GPLv3+");
 
@@ -695,19 +696,215 @@ rebuild_raid5 (struct raid56_buffer *buffers, 
grub_uint64_t nstripes,
                        csize);
 }
 
+
+/* copied from raid6_recover */
+/* x**y.  */
+static grub_uint8_t powx[255 * 2];
+/* Such an s that x**s = y */
+static unsigned powx_inv[256];
+static const grub_uint8_t poly = 0x1d;
+
+static void
+grub_raid_block_mulx (unsigned mul, char *buf, grub_size_t size)
+{
+  grub_size_t i;
+  grub_uint8_t *p;
+
+  p = (grub_uint8_t *) buf;
+  for (i = 0; i < size; i++, p++)
+    if (*p)
+      *p = powx[mul + powx_inv[*p]];
+}
+
+static void
+grub_raid6_init_table (void)
+{
+  unsigned i;
+
+  grub_uint8_t cur = 1;
+  for (i = 0; i < 255; i++)
+    {
+      powx[i] = cur;
+      powx[i + 255] = cur;
+      powx_inv[cur] = i;
+      if (cur & 0x80)
+       cur = (cur << 1) ^ poly;
+      else
+       cur <<= 1;
+    }
+}
+
+static unsigned
+mod_255 (unsigned x)
+{
+  while (x > 0xff)
+    x = (x >> 8) + (x & 0xff);
+  if (x == 0xff)
+    return 0;
+  return x;
+}
+
+typedef grub_err_t (* raid_recover_read_t)(void *data, int disk_nr,
+                                          grub_uint64_t addr, void *dest,
+                                          grub_size_t size);
+#if 0
+/*
+ * code example to be used in raid6_recover.
+ */
+static grub_err_t
+raid_recover_read_diskfilter_array (void *data, int disk_nr,
+                                   grub_uint64_t sector,
+                                   void *buf, grub_size_t size)
+{
+    struct grub_diskfilter_segment *array = data;
+    return grub_diskfilter_read_node (&array->nodes[disk_nr],
+                                     (grub_disk_addr_t)sector,
+                                     size >> GRUB_DISK_SECTOR_BITS, buf);
+}
+#endif
+
+static grub_err_t
+raid_recover_read_raid56_buffer (void *data, int disk_nr, grub_uint64_t addr,
+                                 void *dest, grub_size_t size)
+{
+    struct raid56_buffer *buffers = data;
+
+    (void)addr;
+    grub_memcpy(dest, buffers[disk_nr].buf, size);
+
+    grub_errno = buffers[disk_nr].ok ? GRUB_ERR_NONE : GRUB_ERR_READ_ERROR;
+    return grub_errno;
+}
+
+static grub_err_t
+grub_raid6_recover (void *data, grub_uint64_t nstripes, int disknr, int p,
+                    char *buf, grub_uint64_t sector, grub_size_t size,
+                   raid_recover_read_t read_func, int layout)
+{
+  int i, q, pos;
+  int bad1 = -1, bad2 = -1;
+  char *pbuf = 0, *qbuf = 0;
+  static int table_initializated = 0;
+
+  if (!table_initializated)
+    {
+      grub_raid6_init_table();
+      table_initializated = 1;
+    }
+
+  pbuf = grub_zalloc (size);
+  if (!pbuf)
+    goto quit;
+
+  qbuf = grub_zalloc (size);
+  if (!qbuf)
+    goto quit;
+
+  q = p + 1;
+  if (q == (int) nstripes)
+    q = 0;
+
+  pos = q + 1;
+  if (pos == (int) nstripes)
+    pos = 0;
+
+  for (i = 0; i < (int) nstripes - 2; i++)
+    {
+      int c;
+      if (layout & GRUB_RAID_LAYOUT_MUL_FROM_POS)
+       c = pos;
+      else
+       c = i;
+      if (pos == disknr)
+        bad1 = c;
+      else
+        {
+         if (!read_func(data, pos, sector, buf, size))
+            {
+              grub_crypto_xor (pbuf, pbuf, buf, size);
+              grub_raid_block_mulx (c, buf, size);
+              grub_crypto_xor (qbuf, qbuf, buf, size);
+            }
+          else
+            {
+              /* Too many bad devices */
+              if (bad2 >= 0)
+                goto quit;
+
+              bad2 = c;
+              grub_errno = GRUB_ERR_NONE;
+            }
+        }
+
+      pos++;
+      if (pos == (int) nstripes)
+        pos = 0;
+    }
+
+  /* Invalid disknr or p */
+  if (bad1 < 0)
+    goto quit;
+
+  if (bad2 < 0)
+    {
+      /* One bad device */
+      if (!read_func(data, p, sector, buf, size))
+        {
+          grub_crypto_xor (buf, buf, pbuf, size);
+          goto quit;
+        }
+
+      grub_errno = GRUB_ERR_NONE;
+      if (read_func(data, q, sector, buf, size))
+        goto quit;
+
+      grub_crypto_xor (buf, buf, qbuf, size);
+      grub_raid_block_mulx (255 - bad1, buf,
+                           size);
+    }
+  else
+    {
+      /* Two bad devices */
+      unsigned c;
+
+      if (read_func(data, p, sector, buf, size))
+        goto quit;
+
+      grub_crypto_xor (pbuf, pbuf, buf, size);
+
+      if (read_func(data, q, sector, buf, size))
+        goto quit;
+
+      grub_crypto_xor (qbuf, qbuf, buf, size);
+
+      c = mod_255((255 ^ bad1)
+                 + (255 ^ powx_inv[(powx[bad2 + (bad1 ^ 255)] ^ 1)]));
+      grub_raid_block_mulx (c, qbuf, size);
+
+      c = mod_255((unsigned) bad2 + c);
+      grub_raid_block_mulx (c, pbuf, size);
+
+      grub_crypto_xor (pbuf, pbuf, qbuf, size);
+      grub_memcpy (buf, pbuf, size);
+    }
+
+quit:
+  grub_free (pbuf);
+  grub_free (qbuf);
+
+  return grub_errno;
+}
+
+/* end copy */
 static void
 rebuild_raid6 (struct raid56_buffer *buffers, grub_uint64_t nstripes,
                grub_uint64_t csize, grub_uint64_t parities_pos, void *dest,
                grub_uint64_t stripen)
 
 {
-  (void)buffers;
-  (void)nstripes;
-  (void)csize;
-  (void)parities_pos;
-  (void)dest;
-  (void)stripen;
-  grub_dprintf ("btrfs", "called rebuild_raid6(), NOT IMPLEMENTED\n");
+  grub_raid6_recover (buffers, nstripes, stripen, parities_pos,
+                     dest, 0, csize,
+                     raid_recover_read_raid56_buffer, 0);
 }
 
 static grub_err_t
-- 
2.17.0