vpd_reader.c 5.7 KB

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