]>
Commit | Line | Data |
---|---|---|
be2808c3 IR |
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 | } |