board/ge/bx50v3/vpd_reader.c

   1 /*
   2  * Copyright 2016 General Electric Company
   3  *
   4  * SPDX-License-Identifier:     GPL-2.0+
   5  */
   6
   7 #include "vpd_reader.h"
   8
   9 #include <linux/bch.h>
  10 #include <stdlib.h>
  11
  12
  13 /* BCH configuration */
  14
  15 const struct {
  16         int header_ecc_capability_bits;
  17         int data_ecc_capability_bits;
  18         unsigned int prim_poly;
  19         struct {
  20                 int min;
  21                 int max;
  22         } galois_field_order;
  23 } bch_configuration = {
  24         .header_ecc_capability_bits = 4,
  25         .data_ecc_capability_bits = 16,
  26         .prim_poly = 0,
  27         .galois_field_order = {
  28                 .min = 5,
  29                 .max = 15,
  30         },
  31 };
  32
  33 static int calculate_galois_field_order(size_t source_length)
  34 {
  35         int gfo = bch_configuration.galois_field_order.min;
  36
  37         for (; gfo < bch_configuration.galois_field_order.max &&
  38              ((((1 << gfo) - 1) - ((int)source_length * 8)) < 0);
  39              gfo++) {
  40         }
  41
  42         if (gfo == bch_configuration.galois_field_order.max) {
  43                 return -1;
  44         }
  45
  46         return gfo + 1;
  47 }
  48
  49 static int verify_bch(int ecc_bits, unsigned int prim_poly,
  50         uint8_t * data, size_t data_length,
  51         const uint8_t * ecc, size_t ecc_length)
  52 {
  53         int gfo = calculate_galois_field_order(data_length);
  54         if (gfo < 0) {
  55                 return -1;
  56         }
  57
  58         struct bch_control * bch = init_bch(gfo, ecc_bits, prim_poly);
  59         if (!bch) {
  60                 return -1;
  61         }
  62
  63         if (bch->ecc_bytes != ecc_length) {
  64                 free_bch(bch);
  65                 return -1;
  66         }
  67
  68         unsigned * errloc = (unsigned *)calloc(data_length, sizeof(unsigned));
  69         int errors = decode_bch(
  70                         bch, data, data_length, ecc, NULL, NULL, errloc);
  71         free_bch(bch);
  72         if (errors < 0) {
  73                 free(errloc);
  74                 return -1;
  75         }
  76
  77         if (errors > 0) {
  78                 for (int n = 0; n < errors; n++) {
  79                         if (errloc[n] >= 8 * data_length) {
  80                                 /* n-th error located in ecc (no need for data correction) */
  81                         } else {
  82                                 /* n-th error located in data */
  83                                 data[errloc[n] / 8] ^= 1 << (errloc[n] % 8);
  84                         }
  85                 }
  86         }
  87
  88         free(errloc);
  89         return 0;
  90 }
  91
  92
  93 static const int ID = 0;
  94 static const int LEN = 1;
  95 static const int VER = 2;
  96 static const int TYP = 3;
  97 static const int BLOCK_SIZE = 4;
  98
  99 static const uint8_t HEADER_BLOCK_ID = 0x00;
 100 static const uint8_t HEADER_BLOCK_LEN = 18;
 101 static const uint32_t HEADER_BLOCK_MAGIC = 0xca53ca53;
 102 static const size_t HEADER_BLOCK_VERIFY_LEN = 14;
 103 static const size_t HEADER_BLOCK_ECC_OFF = 14;
 104 static const size_t HEADER_BLOCK_ECC_LEN = 4;
 105
 106 static const uint8_t ECC_BLOCK_ID = 0xFF;
 107
 108 int vpd_reader(
 109         size_t size,
 110         uint8_t * data,
 111         void * userdata,
 112         int (*fn)(
 113             void * userdata,
 114             uint8_t id,
 115             uint8_t version,
 116             uint8_t type,
 117             size_t size,
 118             uint8_t const * data))
 119 {
 120         if (   size < HEADER_BLOCK_LEN
 121             || data == NULL
 122             || fn == NULL) {
 123                 return -EINVAL;
 124         }
 125
 126         /*
 127          * +--------------------+--------------------+--//--+--------------------+
 128          * | header block       | data block         | ...  | ecc block          |
 129          * +--------------------+--------------------+--//--+--------------------+
 130          * :                    :                           :
 131          * +------+-------+-----+                           +------+-------------+
 132          * | id   | magic | ecc |                           | ...  | ecc         |
 133          * | len  | off   |     |                           +------+-------------+
 134          * | ver  | size  |     |                           :
 135          * | type |       |     |                           :
 136          * +------+-------+-----+                           :
 137          * :              :     :                           :
 138          * <----- [1] ---->     <----------- [2] ----------->
 139          *
 140          * Repair (if necessary) the contents of header block [1] by using a
 141          * 4 byte ECC located at the end of the header block.  A successful
 142          * return value means that we can trust the header.
 143          */
 144         int ret = verify_bch(
 145                 bch_configuration.header_ecc_capability_bits,
 146                 bch_configuration.prim_poly,
 147                 data,
 148                 HEADER_BLOCK_VERIFY_LEN,
 149                 &data[HEADER_BLOCK_ECC_OFF],
 150                 HEADER_BLOCK_ECC_LEN);
 151         if (ret < 0) {
 152                 return ret;
 153         }
 154
 155         /* Validate header block { id, length, version, type }. */
 156         if (   data[ID] != HEADER_BLOCK_ID
 157             || data[LEN] != HEADER_BLOCK_LEN
 158             || data[VER] != 0
 159             || data[TYP] != 0
 160             || ntohl(*(uint32_t *)(&data[4])) != HEADER_BLOCK_MAGIC) {
 161                 return -EINVAL;
 162         }
 163
 164         uint32_t offset = ntohl(*(uint32_t *)(&data[8]));
 165         uint16_t size_bits = ntohs(*(uint16_t *)(&data[12]));
 166
 167         /* Check that ECC header fits. */
 168         if (offset + 3 >= size) {
 169                 return -EINVAL;
 170         }
 171
 172         /* Validate ECC block. */
 173         uint8_t * ecc = &data[offset];
 174         if (   ecc[ID] != ECC_BLOCK_ID
 175             || ecc[LEN] < BLOCK_SIZE
 176             || ecc[LEN] + offset > size
 177             || ecc[LEN] - BLOCK_SIZE != size_bits / 8
 178             || ecc[VER] != 1
 179             || ecc[TYP] != 1) {
 180                 return -EINVAL;
 181         }
 182
 183         /*
 184          * Use the header block to locate the ECC block and verify the data
 185          * blocks [2] against the ecc block ECC.
 186          */
 187         ret = verify_bch(
 188                 bch_configuration.data_ecc_capability_bits,
 189                 bch_configuration.prim_poly,
 190                 &data[data[LEN]],
 191                 offset - data[LEN],
 192                 &data[offset + BLOCK_SIZE],
 193                 ecc[LEN] - BLOCK_SIZE);
 194         if (ret < 0) {
 195                 return ret;
 196         }
 197
 198         /* Stop after ECC.  Ignore possible zero padding. */
 199         size = offset;
 200
 201         for (;;) {
 202                 /* Move to next block. */
 203                 size -= data[LEN];
 204                 data += data[LEN];
 205
 206                 if (size == 0) {
 207                         /* Finished iterating through blocks. */
 208                         return 0;
 209                 }
 210
 211                 if (   size < BLOCK_SIZE
 212                     || data[LEN] < BLOCK_SIZE) {
 213                         /* Not enough data for a header, or short header. */
 214                         return -EINVAL;
 215                 }
 216
 217                 ret = fn(
 218                         userdata,
 219                         data[ID],
 220                         data[VER],
 221                         data[TYP],
 222                         data[LEN] - BLOCK_SIZE,
 223                         &data[BLOCK_SIZE]);
 224                 if (ret) {
 225                         return ret;
 226                 }
 227         }
 228 }