]>
Commit | Line | Data |
---|---|---|
bce88370 MV |
1 | /* |
2 | * Freescale i.MX23/i.MX28 SB image generator | |
3 | * | |
4 | * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de> | |
5 | * | |
6 | * SPDX-License-Identifier: GPL-2.0+ | |
7 | */ | |
8 | ||
9 | #ifdef CONFIG_MXS | |
10 | ||
11 | #include <errno.h> | |
12 | #include <fcntl.h> | |
13 | #include <stdio.h> | |
14 | #include <string.h> | |
15 | #include <unistd.h> | |
16 | #include <limits.h> | |
17 | ||
18 | #include <openssl/evp.h> | |
19 | ||
f86ed6a8 | 20 | #include "imagetool.h" |
bce88370 | 21 | #include "mxsimage.h" |
25308f45 | 22 | #include "pbl_crc32.h" |
bce88370 MV |
23 | #include <image.h> |
24 | ||
25 | ||
26 | /* | |
27 | * DCD block | |
28 | * |-Write to address command block | |
29 | * | 0xf00 == 0xf33d | |
30 | * | 0xba2 == 0xb33f | |
31 | * |-ORR address with mask command block | |
32 | * | 0xf00 |= 0x1337 | |
33 | * |-Write to address command block | |
34 | * | 0xba2 == 0xd00d | |
35 | * : | |
36 | */ | |
37 | #define SB_HAB_DCD_WRITE 0xccUL | |
38 | #define SB_HAB_DCD_CHECK 0xcfUL | |
39 | #define SB_HAB_DCD_NOOP 0xc0UL | |
40 | #define SB_HAB_DCD_MASK_BIT (1 << 3) | |
41 | #define SB_HAB_DCD_SET_BIT (1 << 4) | |
42 | ||
43 | /* Addr.n = Value.n */ | |
44 | #define SB_DCD_WRITE \ | |
45 | (SB_HAB_DCD_WRITE << 24) | |
46 | /* Addr.n &= ~Value.n */ | |
47 | #define SB_DCD_ANDC \ | |
48 | ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT) | |
49 | /* Addr.n |= Value.n */ | |
50 | #define SB_DCD_ORR \ | |
51 | ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT) | |
52 | /* (Addr.n & Value.n) == 0 */ | |
53 | #define SB_DCD_CHK_EQZ \ | |
54 | (SB_HAB_DCD_CHECK << 24) | |
55 | /* (Addr.n & Value.n) == Value.n */ | |
56 | #define SB_DCD_CHK_EQ \ | |
57 | ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT) | |
58 | /* (Addr.n & Value.n) != Value.n */ | |
59 | #define SB_DCD_CHK_NEQ \ | |
60 | ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT) | |
61 | /* (Addr.n & Value.n) != 0 */ | |
62 | #define SB_DCD_CHK_NEZ \ | |
63 | ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT) | |
64 | /* NOP */ | |
65 | #define SB_DCD_NOOP \ | |
66 | (SB_HAB_DCD_NOOP << 24) | |
67 | ||
68 | struct sb_dcd_ctx { | |
69 | struct sb_dcd_ctx *dcd; | |
70 | ||
71 | uint32_t id; | |
72 | ||
73 | /* The DCD block. */ | |
74 | uint32_t *payload; | |
75 | /* Size of the whole DCD block. */ | |
76 | uint32_t size; | |
77 | ||
78 | /* Pointer to previous DCD command block. */ | |
79 | uint32_t *prev_dcd_head; | |
80 | }; | |
81 | ||
82 | /* | |
83 | * IMAGE | |
84 | * |-SECTION | |
85 | * | |-CMD | |
86 | * | |-CMD | |
87 | * | `-CMD | |
88 | * |-SECTION | |
89 | * | |-CMD | |
90 | * : : | |
91 | */ | |
92 | struct sb_cmd_list { | |
93 | char *cmd; | |
94 | size_t len; | |
95 | unsigned int lineno; | |
96 | }; | |
97 | ||
98 | struct sb_cmd_ctx { | |
99 | uint32_t size; | |
100 | ||
101 | struct sb_cmd_ctx *cmd; | |
102 | ||
103 | uint8_t *data; | |
104 | uint32_t length; | |
105 | ||
106 | struct sb_command payload; | |
107 | struct sb_command c_payload; | |
108 | }; | |
109 | ||
110 | struct sb_section_ctx { | |
111 | uint32_t size; | |
112 | ||
113 | /* Section flags */ | |
114 | unsigned int boot:1; | |
115 | ||
116 | struct sb_section_ctx *sect; | |
117 | ||
118 | struct sb_cmd_ctx *cmd_head; | |
119 | struct sb_cmd_ctx *cmd_tail; | |
120 | ||
121 | struct sb_sections_header payload; | |
122 | }; | |
123 | ||
124 | struct sb_image_ctx { | |
125 | unsigned int in_section:1; | |
126 | unsigned int in_dcd:1; | |
127 | /* Image configuration */ | |
7a139959 | 128 | unsigned int display_progress:1; |
bce88370 MV |
129 | unsigned int silent_dump:1; |
130 | char *input_filename; | |
131 | char *output_filename; | |
132 | char *cfg_filename; | |
133 | uint8_t image_key[16]; | |
134 | ||
135 | /* Number of section in the image */ | |
136 | unsigned int sect_count; | |
137 | /* Bootable section */ | |
138 | unsigned int sect_boot; | |
139 | unsigned int sect_boot_found:1; | |
140 | ||
141 | struct sb_section_ctx *sect_head; | |
142 | struct sb_section_ctx *sect_tail; | |
143 | ||
144 | struct sb_dcd_ctx *dcd_head; | |
145 | struct sb_dcd_ctx *dcd_tail; | |
146 | ||
147 | EVP_CIPHER_CTX cipher_ctx; | |
148 | EVP_MD_CTX md_ctx; | |
149 | uint8_t digest[32]; | |
150 | struct sb_key_dictionary_key sb_dict_key; | |
151 | ||
152 | struct sb_boot_image_header payload; | |
153 | }; | |
154 | ||
155 | /* | |
156 | * Instruction semantics: | |
157 | * NOOP | |
158 | * TAG [LAST] | |
159 | * LOAD address file | |
160 | * LOAD IVT address IVT_entry_point | |
161 | * FILL address pattern length | |
162 | * JUMP [HAB] address [r0_arg] | |
163 | * CALL [HAB] address [r0_arg] | |
164 | * MODE mode | |
165 | * For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH | |
166 | * JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1 | |
167 | * For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH | |
168 | * JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1 | |
169 | */ | |
170 | ||
171 | /* | |
172 | * AES libcrypto | |
173 | */ | |
174 | static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc) | |
175 | { | |
176 | EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx; | |
177 | int ret; | |
178 | ||
179 | /* If there is no init vector, init vector is all zeroes. */ | |
180 | if (!iv) | |
181 | iv = ictx->image_key; | |
182 | ||
183 | EVP_CIPHER_CTX_init(ctx); | |
184 | ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc); | |
185 | if (ret == 1) | |
186 | EVP_CIPHER_CTX_set_padding(ctx, 0); | |
187 | return ret; | |
188 | } | |
189 | ||
190 | static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data, | |
191 | uint8_t *out_data, int in_len) | |
192 | { | |
193 | EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx; | |
194 | int ret, outlen; | |
195 | uint8_t *outbuf; | |
196 | ||
197 | outbuf = malloc(in_len); | |
198 | if (!outbuf) | |
199 | return -ENOMEM; | |
200 | memset(outbuf, 0, sizeof(in_len)); | |
201 | ||
202 | ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len); | |
203 | if (!ret) { | |
204 | ret = -EINVAL; | |
205 | goto err; | |
206 | } | |
207 | ||
208 | if (out_data) | |
209 | memcpy(out_data, outbuf, outlen); | |
210 | ||
211 | err: | |
212 | free(outbuf); | |
213 | return ret; | |
214 | } | |
215 | ||
216 | static int sb_aes_deinit(EVP_CIPHER_CTX *ctx) | |
217 | { | |
218 | return EVP_CIPHER_CTX_cleanup(ctx); | |
219 | } | |
220 | ||
221 | static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc) | |
222 | { | |
223 | int ret; | |
224 | EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx; | |
225 | struct sb_boot_image_header *sb_header = &ictx->payload; | |
226 | uint8_t *iv = sb_header->iv; | |
227 | ||
228 | ret = sb_aes_deinit(ctx); | |
229 | if (!ret) | |
230 | return ret; | |
231 | return sb_aes_init(ictx, iv, enc); | |
232 | } | |
233 | ||
bce88370 MV |
234 | /* |
235 | * Debug | |
236 | */ | |
237 | static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...) | |
238 | { | |
239 | va_list ap; | |
240 | ||
241 | if (ictx->silent_dump) | |
242 | return; | |
243 | ||
244 | va_start(ap, fmt); | |
245 | vfprintf(stdout, fmt, ap); | |
246 | va_end(ap); | |
247 | } | |
248 | ||
249 | /* | |
250 | * Code | |
251 | */ | |
252 | static time_t sb_get_timestamp(void) | |
253 | { | |
254 | struct tm time_2000 = { | |
255 | .tm_yday = 1, /* Jan. 1st */ | |
256 | .tm_year = 100, /* 2000 */ | |
257 | }; | |
258 | time_t seconds_to_2000 = mktime(&time_2000); | |
259 | time_t seconds_to_now = time(NULL); | |
260 | ||
261 | return seconds_to_now - seconds_to_2000; | |
262 | } | |
263 | ||
264 | static int sb_get_time(time_t time, struct tm *tm) | |
265 | { | |
266 | struct tm time_2000 = { | |
267 | .tm_yday = 1, /* Jan. 1st */ | |
268 | .tm_year = 0, /* 1900 */ | |
269 | }; | |
270 | const time_t seconds_to_2000 = mktime(&time_2000); | |
271 | const time_t seconds_to_now = seconds_to_2000 + time; | |
272 | struct tm *ret; | |
273 | ret = gmtime_r(&seconds_to_now, tm); | |
274 | return ret ? 0 : -EINVAL; | |
275 | } | |
276 | ||
277 | static void sb_encrypt_sb_header(struct sb_image_ctx *ictx) | |
278 | { | |
279 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
280 | struct sb_boot_image_header *sb_header = &ictx->payload; | |
281 | uint8_t *sb_header_ptr = (uint8_t *)sb_header; | |
282 | ||
283 | /* Encrypt the header, compute the digest. */ | |
284 | sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header)); | |
285 | EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header)); | |
286 | } | |
287 | ||
288 | static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx) | |
289 | { | |
290 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
291 | struct sb_section_ctx *sctx = ictx->sect_head; | |
292 | struct sb_sections_header *shdr; | |
293 | uint8_t *sb_sections_header_ptr; | |
294 | const int size = sizeof(*shdr); | |
295 | ||
296 | while (sctx) { | |
297 | shdr = &sctx->payload; | |
298 | sb_sections_header_ptr = (uint8_t *)shdr; | |
299 | ||
300 | sb_aes_crypt(ictx, sb_sections_header_ptr, | |
301 | ictx->sb_dict_key.cbc_mac, size); | |
302 | EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size); | |
303 | ||
304 | sctx = sctx->sect; | |
305 | }; | |
306 | } | |
307 | ||
308 | static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx) | |
309 | { | |
310 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
311 | ||
312 | sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key, | |
313 | sizeof(ictx->sb_dict_key.key)); | |
314 | EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key)); | |
315 | } | |
316 | ||
317 | static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx) | |
318 | { | |
319 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
320 | ||
321 | EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key)); | |
322 | sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key, | |
323 | sizeof(ictx->sb_dict_key.key)); | |
324 | } | |
325 | ||
326 | static void sb_encrypt_tag(struct sb_image_ctx *ictx, | |
327 | struct sb_cmd_ctx *cctx) | |
328 | { | |
329 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
330 | struct sb_command *cmd = &cctx->payload; | |
331 | ||
332 | sb_aes_crypt(ictx, (uint8_t *)cmd, | |
333 | (uint8_t *)&cctx->c_payload, sizeof(*cmd)); | |
334 | EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd)); | |
335 | } | |
336 | ||
337 | static int sb_encrypt_image(struct sb_image_ctx *ictx) | |
338 | { | |
339 | /* Start image-wide crypto. */ | |
340 | EVP_MD_CTX_init(&ictx->md_ctx); | |
341 | EVP_DigestInit(&ictx->md_ctx, EVP_sha1()); | |
342 | ||
343 | /* | |
344 | * SB image header. | |
345 | */ | |
346 | sb_aes_init(ictx, NULL, 1); | |
347 | sb_encrypt_sb_header(ictx); | |
348 | ||
349 | /* | |
350 | * SB sections header. | |
351 | */ | |
352 | sb_encrypt_sb_sections_header(ictx); | |
353 | ||
354 | /* | |
355 | * Key dictionary. | |
356 | */ | |
357 | sb_aes_reinit(ictx, 1); | |
358 | sb_encrypt_key_dictionary_key(ictx); | |
359 | ||
360 | /* | |
361 | * Section tags. | |
362 | */ | |
363 | struct sb_cmd_ctx *cctx; | |
364 | struct sb_command *ccmd; | |
365 | struct sb_section_ctx *sctx = ictx->sect_head; | |
366 | ||
367 | while (sctx) { | |
368 | cctx = sctx->cmd_head; | |
369 | ||
370 | sb_aes_reinit(ictx, 1); | |
371 | ||
372 | while (cctx) { | |
373 | ccmd = &cctx->payload; | |
374 | ||
375 | sb_encrypt_tag(ictx, cctx); | |
376 | ||
377 | if (ccmd->header.tag == ROM_TAG_CMD) { | |
378 | sb_aes_reinit(ictx, 1); | |
379 | } else if (ccmd->header.tag == ROM_LOAD_CMD) { | |
380 | sb_aes_crypt(ictx, cctx->data, cctx->data, | |
381 | cctx->length); | |
382 | EVP_DigestUpdate(&ictx->md_ctx, cctx->data, | |
383 | cctx->length); | |
384 | } | |
385 | ||
386 | cctx = cctx->cmd; | |
387 | } | |
388 | ||
389 | sctx = sctx->sect; | |
390 | }; | |
391 | ||
392 | /* | |
393 | * Dump the SHA1 of the whole image. | |
394 | */ | |
395 | sb_aes_reinit(ictx, 1); | |
396 | ||
397 | EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL); | |
398 | sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest)); | |
399 | ||
400 | /* Stop the encryption session. */ | |
401 | sb_aes_deinit(&ictx->cipher_ctx); | |
402 | ||
403 | return 0; | |
404 | } | |
405 | ||
406 | static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename) | |
407 | { | |
408 | long real_size, roundup_size; | |
409 | uint8_t *data; | |
410 | long ret; | |
411 | unsigned long size; | |
412 | FILE *fp; | |
413 | ||
414 | if (!filename) { | |
415 | fprintf(stderr, "ERR: Missing filename!\n"); | |
416 | return -EINVAL; | |
417 | } | |
418 | ||
419 | fp = fopen(filename, "r"); | |
420 | if (!fp) | |
421 | goto err_open; | |
422 | ||
423 | ret = fseek(fp, 0, SEEK_END); | |
424 | if (ret < 0) | |
425 | goto err_file; | |
426 | ||
427 | real_size = ftell(fp); | |
428 | if (real_size < 0) | |
429 | goto err_file; | |
430 | ||
431 | ret = fseek(fp, 0, SEEK_SET); | |
432 | if (ret < 0) | |
433 | goto err_file; | |
434 | ||
435 | roundup_size = roundup(real_size, SB_BLOCK_SIZE); | |
436 | data = calloc(1, roundup_size); | |
437 | if (!data) | |
438 | goto err_file; | |
439 | ||
440 | size = fread(data, 1, real_size, fp); | |
441 | if (size != (unsigned long)real_size) | |
442 | goto err_alloc; | |
443 | ||
444 | cctx->data = data; | |
445 | cctx->length = roundup_size; | |
446 | ||
447 | fclose(fp); | |
448 | return 0; | |
449 | ||
450 | err_alloc: | |
451 | free(data); | |
452 | err_file: | |
453 | fclose(fp); | |
454 | err_open: | |
455 | fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename); | |
456 | return -EINVAL; | |
457 | } | |
458 | ||
459 | static uint8_t sb_command_checksum(struct sb_command *inst) | |
460 | { | |
461 | uint8_t *inst_ptr = (uint8_t *)inst; | |
462 | uint8_t csum = 0; | |
463 | unsigned int i; | |
464 | ||
465 | for (i = 0; i < sizeof(struct sb_command); i++) | |
466 | csum += inst_ptr[i]; | |
467 | ||
468 | return csum; | |
469 | } | |
470 | ||
471 | static int sb_token_to_long(char *tok, uint32_t *rid) | |
472 | { | |
473 | char *endptr; | |
474 | unsigned long id; | |
475 | ||
476 | if (tok[0] != '0' || tok[1] != 'x') { | |
477 | fprintf(stderr, "ERR: Invalid hexadecimal number!\n"); | |
478 | return -EINVAL; | |
479 | } | |
480 | ||
481 | tok += 2; | |
482 | ||
5b5a82eb | 483 | errno = 0; |
bce88370 MV |
484 | id = strtoul(tok, &endptr, 16); |
485 | if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) { | |
486 | fprintf(stderr, "ERR: Value can't be decoded!\n"); | |
487 | return -EINVAL; | |
488 | } | |
489 | ||
490 | /* Check for 32-bit overflow. */ | |
491 | if (id > 0xffffffff) { | |
492 | fprintf(stderr, "ERR: Value too big!\n"); | |
493 | return -EINVAL; | |
494 | } | |
495 | ||
496 | if (endptr == tok) { | |
497 | fprintf(stderr, "ERR: Deformed value!\n"); | |
498 | return -EINVAL; | |
499 | } | |
500 | ||
501 | *rid = (uint32_t)id; | |
502 | return 0; | |
503 | } | |
504 | ||
505 | static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size) | |
506 | { | |
507 | uint32_t *tmp; | |
508 | ||
509 | if (!inc_size) | |
510 | return 0; | |
511 | ||
512 | dctx->size += inc_size; | |
513 | tmp = realloc(dctx->payload, dctx->size); | |
514 | if (!tmp) | |
515 | return -ENOMEM; | |
516 | ||
517 | dctx->payload = tmp; | |
518 | ||
519 | /* Assemble and update the HAB DCD header. */ | |
520 | dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) | | |
521 | (dctx->size << 8) | | |
522 | SB_HAB_VERSION); | |
523 | ||
524 | return 0; | |
525 | } | |
526 | ||
527 | static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) | |
528 | { | |
529 | struct sb_dcd_ctx *dctx; | |
530 | ||
531 | char *tok; | |
532 | uint32_t id; | |
533 | int ret; | |
534 | ||
535 | dctx = calloc(1, sizeof(*dctx)); | |
536 | if (!dctx) | |
537 | return -ENOMEM; | |
538 | ||
539 | ret = sb_grow_dcd(dctx, 4); | |
540 | if (ret) | |
541 | goto err_dcd; | |
542 | ||
543 | /* Read DCD block number. */ | |
544 | tok = strtok(cmd->cmd, " "); | |
545 | if (!tok) { | |
546 | fprintf(stderr, "#%i ERR: DCD block without number!\n", | |
547 | cmd->lineno); | |
548 | ret = -EINVAL; | |
549 | goto err_dcd; | |
550 | } | |
551 | ||
552 | /* Parse the DCD block number. */ | |
553 | ret = sb_token_to_long(tok, &id); | |
554 | if (ret) { | |
555 | fprintf(stderr, "#%i ERR: Malformed DCD block number!\n", | |
556 | cmd->lineno); | |
557 | goto err_dcd; | |
558 | } | |
559 | ||
560 | dctx->id = id; | |
561 | ||
562 | /* | |
563 | * The DCD block is now constructed. Append it to the list. | |
564 | * WARNING: The DCD size is still not computed and will be | |
565 | * updated while parsing it's commands. | |
566 | */ | |
567 | if (!ictx->dcd_head) { | |
568 | ictx->dcd_head = dctx; | |
569 | ictx->dcd_tail = dctx; | |
570 | } else { | |
571 | ictx->dcd_tail->dcd = dctx; | |
572 | ictx->dcd_tail = dctx; | |
573 | } | |
574 | ||
575 | return 0; | |
576 | ||
577 | err_dcd: | |
578 | free(dctx->payload); | |
579 | free(dctx); | |
580 | return ret; | |
581 | } | |
582 | ||
583 | static int sb_build_dcd_block(struct sb_image_ctx *ictx, | |
584 | struct sb_cmd_list *cmd, | |
585 | uint32_t type) | |
586 | { | |
587 | char *tok; | |
588 | uint32_t address, value, length; | |
589 | int ret; | |
590 | ||
591 | struct sb_dcd_ctx *dctx = ictx->dcd_tail; | |
592 | uint32_t *dcd; | |
593 | ||
594 | if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) && | |
595 | ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) { | |
596 | /* Same instruction as before, just append it. */ | |
597 | ret = sb_grow_dcd(dctx, 8); | |
598 | if (ret) | |
599 | return ret; | |
600 | } else if (type == SB_DCD_NOOP) { | |
601 | ret = sb_grow_dcd(dctx, 4); | |
602 | if (ret) | |
603 | return ret; | |
604 | ||
605 | /* Update DCD command block pointer. */ | |
606 | dctx->prev_dcd_head = dctx->payload + | |
607 | dctx->size / sizeof(*dctx->payload) - 1; | |
608 | ||
609 | /* NOOP has only 4 bytes and no payload. */ | |
610 | goto noop; | |
611 | } else { | |
612 | /* | |
613 | * Either a different instruction block started now | |
614 | * or this is the first instruction block. | |
615 | */ | |
616 | ret = sb_grow_dcd(dctx, 12); | |
617 | if (ret) | |
618 | return ret; | |
619 | ||
620 | /* Update DCD command block pointer. */ | |
621 | dctx->prev_dcd_head = dctx->payload + | |
622 | dctx->size / sizeof(*dctx->payload) - 3; | |
623 | } | |
624 | ||
625 | dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2; | |
626 | ||
627 | /* | |
628 | * Prepare the command. | |
629 | */ | |
630 | tok = strtok(cmd->cmd, " "); | |
631 | if (!tok) { | |
632 | fprintf(stderr, "#%i ERR: Missing DCD address!\n", | |
633 | cmd->lineno); | |
634 | ret = -EINVAL; | |
635 | goto err; | |
636 | } | |
637 | ||
638 | /* Read DCD destination address. */ | |
639 | ret = sb_token_to_long(tok, &address); | |
640 | if (ret) { | |
641 | fprintf(stderr, "#%i ERR: Incorrect DCD address!\n", | |
642 | cmd->lineno); | |
643 | goto err; | |
644 | } | |
645 | ||
646 | tok = strtok(NULL, " "); | |
647 | if (!tok) { | |
648 | fprintf(stderr, "#%i ERR: Missing DCD value!\n", | |
649 | cmd->lineno); | |
650 | ret = -EINVAL; | |
651 | goto err; | |
652 | } | |
653 | ||
654 | /* Read DCD operation value. */ | |
655 | ret = sb_token_to_long(tok, &value); | |
656 | if (ret) { | |
657 | fprintf(stderr, "#%i ERR: Incorrect DCD value!\n", | |
658 | cmd->lineno); | |
659 | goto err; | |
660 | } | |
661 | ||
662 | /* Fill in the new DCD entry. */ | |
663 | dcd[0] = htonl(address); | |
664 | dcd[1] = htonl(value); | |
665 | ||
666 | noop: | |
667 | /* Update the DCD command block. */ | |
668 | length = dctx->size - | |
669 | ((dctx->prev_dcd_head - dctx->payload) * | |
670 | sizeof(*dctx->payload)); | |
671 | dctx->prev_dcd_head[0] = htonl(type | (length << 8)); | |
672 | ||
673 | err: | |
674 | return ret; | |
675 | } | |
676 | ||
677 | static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) | |
678 | { | |
679 | struct sb_section_ctx *sctx; | |
680 | struct sb_sections_header *shdr; | |
681 | char *tok; | |
682 | uint32_t bootable = 0; | |
683 | uint32_t id; | |
684 | int ret; | |
685 | ||
686 | sctx = calloc(1, sizeof(*sctx)); | |
687 | if (!sctx) | |
688 | return -ENOMEM; | |
689 | ||
690 | /* Read section number. */ | |
691 | tok = strtok(cmd->cmd, " "); | |
692 | if (!tok) { | |
693 | fprintf(stderr, "#%i ERR: Section without number!\n", | |
694 | cmd->lineno); | |
695 | ret = -EINVAL; | |
696 | goto err_sect; | |
697 | } | |
698 | ||
699 | /* Parse the section number. */ | |
700 | ret = sb_token_to_long(tok, &id); | |
701 | if (ret) { | |
702 | fprintf(stderr, "#%i ERR: Malformed section number!\n", | |
703 | cmd->lineno); | |
704 | goto err_sect; | |
705 | } | |
706 | ||
707 | /* Read section's BOOTABLE flag. */ | |
708 | tok = strtok(NULL, " "); | |
709 | if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8)) | |
710 | bootable = SB_SECTION_FLAG_BOOTABLE; | |
711 | ||
712 | sctx->boot = bootable; | |
713 | ||
714 | shdr = &sctx->payload; | |
715 | shdr->section_number = id; | |
716 | shdr->section_flags = bootable; | |
717 | ||
718 | /* | |
719 | * The section is now constructed. Append it to the list. | |
720 | * WARNING: The section size is still not computed and will | |
721 | * be updated while parsing it's commands. | |
722 | */ | |
723 | ictx->sect_count++; | |
724 | ||
725 | /* Mark that this section is bootable one. */ | |
726 | if (bootable) { | |
727 | if (ictx->sect_boot_found) { | |
728 | fprintf(stderr, | |
729 | "#%i WARN: Multiple bootable section!\n", | |
730 | cmd->lineno); | |
731 | } else { | |
732 | ictx->sect_boot = id; | |
733 | ictx->sect_boot_found = 1; | |
734 | } | |
735 | } | |
736 | ||
737 | if (!ictx->sect_head) { | |
738 | ictx->sect_head = sctx; | |
739 | ictx->sect_tail = sctx; | |
740 | } else { | |
741 | ictx->sect_tail->sect = sctx; | |
742 | ictx->sect_tail = sctx; | |
743 | } | |
744 | ||
745 | return 0; | |
746 | ||
747 | err_sect: | |
748 | free(sctx); | |
749 | return ret; | |
750 | } | |
751 | ||
752 | static int sb_build_command_nop(struct sb_image_ctx *ictx) | |
753 | { | |
754 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
755 | struct sb_cmd_ctx *cctx; | |
756 | struct sb_command *ccmd; | |
757 | ||
758 | cctx = calloc(1, sizeof(*cctx)); | |
759 | if (!cctx) | |
760 | return -ENOMEM; | |
761 | ||
762 | ccmd = &cctx->payload; | |
763 | ||
764 | /* | |
765 | * Construct the command. | |
766 | */ | |
767 | ccmd->header.checksum = 0x5a; | |
768 | ccmd->header.tag = ROM_NOP_CMD; | |
769 | ||
770 | cctx->size = sizeof(*ccmd); | |
771 | ||
772 | /* | |
773 | * Append the command to the last section. | |
774 | */ | |
775 | if (!sctx->cmd_head) { | |
776 | sctx->cmd_head = cctx; | |
777 | sctx->cmd_tail = cctx; | |
778 | } else { | |
779 | sctx->cmd_tail->cmd = cctx; | |
780 | sctx->cmd_tail = cctx; | |
781 | } | |
782 | ||
783 | return 0; | |
784 | } | |
785 | ||
786 | static int sb_build_command_tag(struct sb_image_ctx *ictx, | |
787 | struct sb_cmd_list *cmd) | |
788 | { | |
789 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
790 | struct sb_cmd_ctx *cctx; | |
791 | struct sb_command *ccmd; | |
792 | char *tok; | |
793 | ||
794 | cctx = calloc(1, sizeof(*cctx)); | |
795 | if (!cctx) | |
796 | return -ENOMEM; | |
797 | ||
798 | ccmd = &cctx->payload; | |
799 | ||
800 | /* | |
801 | * Prepare the command. | |
802 | */ | |
803 | /* Check for the LAST keyword. */ | |
804 | tok = strtok(cmd->cmd, " "); | |
805 | if (tok && !strcmp(tok, "LAST")) | |
806 | ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG; | |
807 | ||
808 | /* | |
809 | * Construct the command. | |
810 | */ | |
811 | ccmd->header.checksum = 0x5a; | |
812 | ccmd->header.tag = ROM_TAG_CMD; | |
813 | ||
814 | cctx->size = sizeof(*ccmd); | |
815 | ||
816 | /* | |
817 | * Append the command to the last section. | |
818 | */ | |
819 | if (!sctx->cmd_head) { | |
820 | sctx->cmd_head = cctx; | |
821 | sctx->cmd_tail = cctx; | |
822 | } else { | |
823 | sctx->cmd_tail->cmd = cctx; | |
824 | sctx->cmd_tail = cctx; | |
825 | } | |
826 | ||
827 | return 0; | |
828 | } | |
829 | ||
830 | static int sb_build_command_load(struct sb_image_ctx *ictx, | |
831 | struct sb_cmd_list *cmd) | |
832 | { | |
833 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
834 | struct sb_cmd_ctx *cctx; | |
835 | struct sb_command *ccmd; | |
836 | char *tok; | |
837 | int ret, is_ivt = 0, is_dcd = 0; | |
838 | uint32_t dest, dcd = 0; | |
839 | ||
840 | cctx = calloc(1, sizeof(*cctx)); | |
841 | if (!cctx) | |
842 | return -ENOMEM; | |
843 | ||
844 | ccmd = &cctx->payload; | |
845 | ||
846 | /* | |
847 | * Prepare the command. | |
848 | */ | |
849 | tok = strtok(cmd->cmd, " "); | |
850 | if (!tok) { | |
851 | fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n", | |
852 | cmd->lineno); | |
853 | ret = -EINVAL; | |
854 | goto err; | |
855 | } | |
856 | ||
857 | /* Check for "IVT" flag. */ | |
858 | if (!strcmp(tok, "IVT")) | |
859 | is_ivt = 1; | |
860 | if (!strcmp(tok, "DCD")) | |
861 | is_dcd = 1; | |
862 | if (is_ivt || is_dcd) { | |
863 | tok = strtok(NULL, " "); | |
864 | if (!tok) { | |
865 | fprintf(stderr, "#%i ERR: Missing LOAD address!\n", | |
866 | cmd->lineno); | |
867 | ret = -EINVAL; | |
868 | goto err; | |
869 | } | |
870 | } | |
871 | ||
872 | /* Read load destination address. */ | |
873 | ret = sb_token_to_long(tok, &dest); | |
874 | if (ret) { | |
875 | fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n", | |
876 | cmd->lineno); | |
877 | goto err; | |
878 | } | |
879 | ||
880 | /* Read filename or IVT entrypoint or DCD block ID. */ | |
881 | tok = strtok(NULL, " "); | |
882 | if (!tok) { | |
883 | fprintf(stderr, | |
884 | "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n", | |
885 | cmd->lineno); | |
886 | ret = -EINVAL; | |
887 | goto err; | |
888 | } | |
889 | ||
890 | if (is_ivt) { | |
891 | /* Handle IVT. */ | |
892 | struct sb_ivt_header *ivt; | |
893 | uint32_t ivtep; | |
894 | ret = sb_token_to_long(tok, &ivtep); | |
895 | ||
896 | if (ret) { | |
897 | fprintf(stderr, | |
898 | "#%i ERR: Incorrect IVT entry point!\n", | |
899 | cmd->lineno); | |
900 | goto err; | |
901 | } | |
902 | ||
903 | ivt = calloc(1, sizeof(*ivt)); | |
904 | if (!ivt) { | |
905 | ret = -ENOMEM; | |
906 | goto err; | |
907 | } | |
908 | ||
909 | ivt->header = sb_hab_ivt_header(); | |
910 | ivt->entry = ivtep; | |
911 | ivt->self = dest; | |
912 | ||
913 | cctx->data = (uint8_t *)ivt; | |
914 | cctx->length = sizeof(*ivt); | |
915 | } else if (is_dcd) { | |
916 | struct sb_dcd_ctx *dctx = ictx->dcd_head; | |
917 | uint32_t dcdid; | |
918 | uint8_t *payload; | |
919 | uint32_t asize; | |
920 | ret = sb_token_to_long(tok, &dcdid); | |
921 | ||
922 | if (ret) { | |
923 | fprintf(stderr, | |
924 | "#%i ERR: Incorrect DCD block ID!\n", | |
925 | cmd->lineno); | |
926 | goto err; | |
927 | } | |
928 | ||
929 | while (dctx) { | |
930 | if (dctx->id == dcdid) | |
931 | break; | |
932 | dctx = dctx->dcd; | |
933 | } | |
934 | ||
935 | if (!dctx) { | |
936 | fprintf(stderr, "#%i ERR: DCD block %08x not found!\n", | |
937 | cmd->lineno, dcdid); | |
938 | goto err; | |
939 | } | |
940 | ||
941 | asize = roundup(dctx->size, SB_BLOCK_SIZE); | |
942 | payload = calloc(1, asize); | |
943 | if (!payload) { | |
944 | ret = -ENOMEM; | |
945 | goto err; | |
946 | } | |
947 | ||
948 | memcpy(payload, dctx->payload, dctx->size); | |
949 | ||
950 | cctx->data = payload; | |
951 | cctx->length = asize; | |
952 | ||
953 | /* Set the Load DCD flag. */ | |
954 | dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD; | |
955 | } else { | |
956 | /* Regular LOAD of a file. */ | |
957 | ret = sb_load_file(cctx, tok); | |
958 | if (ret) { | |
959 | fprintf(stderr, "#%i ERR: Cannot load '%s'!\n", | |
960 | cmd->lineno, tok); | |
961 | goto err; | |
962 | } | |
963 | } | |
964 | ||
965 | if (cctx->length & (SB_BLOCK_SIZE - 1)) { | |
966 | fprintf(stderr, "#%i ERR: Unaligned payload!\n", | |
967 | cmd->lineno); | |
968 | } | |
969 | ||
970 | /* | |
971 | * Construct the command. | |
972 | */ | |
973 | ccmd->header.checksum = 0x5a; | |
974 | ccmd->header.tag = ROM_LOAD_CMD; | |
975 | ccmd->header.flags = dcd; | |
976 | ||
977 | ccmd->load.address = dest; | |
978 | ccmd->load.count = cctx->length; | |
25308f45 CM |
979 | ccmd->load.crc32 = pbl_crc32(0, |
980 | (const char *)cctx->data, | |
981 | cctx->length); | |
bce88370 MV |
982 | |
983 | cctx->size = sizeof(*ccmd) + cctx->length; | |
984 | ||
985 | /* | |
986 | * Append the command to the last section. | |
987 | */ | |
988 | if (!sctx->cmd_head) { | |
989 | sctx->cmd_head = cctx; | |
990 | sctx->cmd_tail = cctx; | |
991 | } else { | |
992 | sctx->cmd_tail->cmd = cctx; | |
993 | sctx->cmd_tail = cctx; | |
994 | } | |
995 | ||
996 | return 0; | |
997 | ||
998 | err: | |
999 | free(cctx); | |
1000 | return ret; | |
1001 | } | |
1002 | ||
1003 | static int sb_build_command_fill(struct sb_image_ctx *ictx, | |
1004 | struct sb_cmd_list *cmd) | |
1005 | { | |
1006 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
1007 | struct sb_cmd_ctx *cctx; | |
1008 | struct sb_command *ccmd; | |
1009 | char *tok; | |
1010 | uint32_t address, pattern, length; | |
1011 | int ret; | |
1012 | ||
1013 | cctx = calloc(1, sizeof(*cctx)); | |
1014 | if (!cctx) | |
1015 | return -ENOMEM; | |
1016 | ||
1017 | ccmd = &cctx->payload; | |
1018 | ||
1019 | /* | |
1020 | * Prepare the command. | |
1021 | */ | |
1022 | tok = strtok(cmd->cmd, " "); | |
1023 | if (!tok) { | |
1024 | fprintf(stderr, "#%i ERR: Missing FILL address!\n", | |
1025 | cmd->lineno); | |
1026 | ret = -EINVAL; | |
1027 | goto err; | |
1028 | } | |
1029 | ||
1030 | /* Read fill destination address. */ | |
1031 | ret = sb_token_to_long(tok, &address); | |
1032 | if (ret) { | |
1033 | fprintf(stderr, "#%i ERR: Incorrect FILL address!\n", | |
1034 | cmd->lineno); | |
1035 | goto err; | |
1036 | } | |
1037 | ||
1038 | tok = strtok(NULL, " "); | |
1039 | if (!tok) { | |
1040 | fprintf(stderr, "#%i ERR: Missing FILL pattern!\n", | |
1041 | cmd->lineno); | |
1042 | ret = -EINVAL; | |
1043 | goto err; | |
1044 | } | |
1045 | ||
1046 | /* Read fill pattern address. */ | |
1047 | ret = sb_token_to_long(tok, &pattern); | |
1048 | if (ret) { | |
1049 | fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n", | |
1050 | cmd->lineno); | |
1051 | goto err; | |
1052 | } | |
1053 | ||
1054 | tok = strtok(NULL, " "); | |
1055 | if (!tok) { | |
1056 | fprintf(stderr, "#%i ERR: Missing FILL length!\n", | |
1057 | cmd->lineno); | |
1058 | ret = -EINVAL; | |
1059 | goto err; | |
1060 | } | |
1061 | ||
1062 | /* Read fill pattern address. */ | |
1063 | ret = sb_token_to_long(tok, &length); | |
1064 | if (ret) { | |
1065 | fprintf(stderr, "#%i ERR: Incorrect FILL length!\n", | |
1066 | cmd->lineno); | |
1067 | goto err; | |
1068 | } | |
1069 | ||
1070 | /* | |
1071 | * Construct the command. | |
1072 | */ | |
1073 | ccmd->header.checksum = 0x5a; | |
1074 | ccmd->header.tag = ROM_FILL_CMD; | |
1075 | ||
1076 | ccmd->fill.address = address; | |
1077 | ccmd->fill.count = length; | |
1078 | ccmd->fill.pattern = pattern; | |
1079 | ||
1080 | cctx->size = sizeof(*ccmd); | |
1081 | ||
1082 | /* | |
1083 | * Append the command to the last section. | |
1084 | */ | |
1085 | if (!sctx->cmd_head) { | |
1086 | sctx->cmd_head = cctx; | |
1087 | sctx->cmd_tail = cctx; | |
1088 | } else { | |
1089 | sctx->cmd_tail->cmd = cctx; | |
1090 | sctx->cmd_tail = cctx; | |
1091 | } | |
1092 | ||
1093 | return 0; | |
1094 | ||
1095 | err: | |
1096 | free(cctx); | |
1097 | return ret; | |
1098 | } | |
1099 | ||
1100 | static int sb_build_command_jump_call(struct sb_image_ctx *ictx, | |
1101 | struct sb_cmd_list *cmd, | |
1102 | unsigned int is_call) | |
1103 | { | |
1104 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
1105 | struct sb_cmd_ctx *cctx; | |
1106 | struct sb_command *ccmd; | |
1107 | char *tok; | |
1108 | uint32_t dest, arg = 0x0; | |
1109 | uint32_t hab = 0; | |
1110 | int ret; | |
1111 | const char *cmdname = is_call ? "CALL" : "JUMP"; | |
1112 | ||
1113 | cctx = calloc(1, sizeof(*cctx)); | |
1114 | if (!cctx) | |
1115 | return -ENOMEM; | |
1116 | ||
1117 | ccmd = &cctx->payload; | |
1118 | ||
1119 | /* | |
1120 | * Prepare the command. | |
1121 | */ | |
1122 | tok = strtok(cmd->cmd, " "); | |
1123 | if (!tok) { | |
1124 | fprintf(stderr, | |
1125 | "#%i ERR: Missing %s address or 'HAB'!\n", | |
1126 | cmd->lineno, cmdname); | |
1127 | ret = -EINVAL; | |
1128 | goto err; | |
1129 | } | |
1130 | ||
1131 | /* Check for "HAB" flag. */ | |
1132 | if (!strcmp(tok, "HAB")) { | |
1133 | hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB; | |
1134 | tok = strtok(NULL, " "); | |
1135 | if (!tok) { | |
1136 | fprintf(stderr, "#%i ERR: Missing %s address!\n", | |
1137 | cmd->lineno, cmdname); | |
1138 | ret = -EINVAL; | |
1139 | goto err; | |
1140 | } | |
1141 | } | |
1142 | /* Read load destination address. */ | |
1143 | ret = sb_token_to_long(tok, &dest); | |
1144 | if (ret) { | |
1145 | fprintf(stderr, "#%i ERR: Incorrect %s address!\n", | |
1146 | cmd->lineno, cmdname); | |
1147 | goto err; | |
1148 | } | |
1149 | ||
1150 | tok = strtok(NULL, " "); | |
1151 | if (tok) { | |
1152 | ret = sb_token_to_long(tok, &arg); | |
1153 | if (ret) { | |
1154 | fprintf(stderr, | |
1155 | "#%i ERR: Incorrect %s argument!\n", | |
1156 | cmd->lineno, cmdname); | |
1157 | goto err; | |
1158 | } | |
1159 | } | |
1160 | ||
1161 | /* | |
1162 | * Construct the command. | |
1163 | */ | |
1164 | ccmd->header.checksum = 0x5a; | |
1165 | ccmd->header.tag = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD; | |
1166 | ccmd->header.flags = hab; | |
1167 | ||
1168 | ccmd->call.address = dest; | |
1169 | ccmd->call.argument = arg; | |
1170 | ||
1171 | cctx->size = sizeof(*ccmd); | |
1172 | ||
1173 | /* | |
1174 | * Append the command to the last section. | |
1175 | */ | |
1176 | if (!sctx->cmd_head) { | |
1177 | sctx->cmd_head = cctx; | |
1178 | sctx->cmd_tail = cctx; | |
1179 | } else { | |
1180 | sctx->cmd_tail->cmd = cctx; | |
1181 | sctx->cmd_tail = cctx; | |
1182 | } | |
1183 | ||
1184 | return 0; | |
1185 | ||
1186 | err: | |
1187 | free(cctx); | |
1188 | return ret; | |
1189 | } | |
1190 | ||
1191 | static int sb_build_command_jump(struct sb_image_ctx *ictx, | |
1192 | struct sb_cmd_list *cmd) | |
1193 | { | |
1194 | return sb_build_command_jump_call(ictx, cmd, 0); | |
1195 | } | |
1196 | ||
1197 | static int sb_build_command_call(struct sb_image_ctx *ictx, | |
1198 | struct sb_cmd_list *cmd) | |
1199 | { | |
1200 | return sb_build_command_jump_call(ictx, cmd, 1); | |
1201 | } | |
1202 | ||
1203 | static int sb_build_command_mode(struct sb_image_ctx *ictx, | |
1204 | struct sb_cmd_list *cmd) | |
1205 | { | |
1206 | struct sb_section_ctx *sctx = ictx->sect_tail; | |
1207 | struct sb_cmd_ctx *cctx; | |
1208 | struct sb_command *ccmd; | |
1209 | char *tok; | |
1210 | int ret; | |
1211 | unsigned int i; | |
1212 | uint32_t mode = 0xffffffff; | |
1213 | ||
1214 | cctx = calloc(1, sizeof(*cctx)); | |
1215 | if (!cctx) | |
1216 | return -ENOMEM; | |
1217 | ||
1218 | ccmd = &cctx->payload; | |
1219 | ||
1220 | /* | |
1221 | * Prepare the command. | |
1222 | */ | |
1223 | tok = strtok(cmd->cmd, " "); | |
1224 | if (!tok) { | |
1225 | fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n", | |
1226 | cmd->lineno); | |
1227 | ret = -EINVAL; | |
1228 | goto err; | |
1229 | } | |
1230 | ||
1231 | for (i = 0; i < ARRAY_SIZE(modetable); i++) { | |
1232 | if (!strcmp(tok, modetable[i].name)) { | |
1233 | mode = modetable[i].mode; | |
1234 | break; | |
1235 | } | |
1236 | ||
1237 | if (!modetable[i].altname) | |
1238 | continue; | |
1239 | ||
1240 | if (!strcmp(tok, modetable[i].altname)) { | |
1241 | mode = modetable[i].mode; | |
1242 | break; | |
1243 | } | |
1244 | } | |
1245 | ||
1246 | if (mode == 0xffffffff) { | |
1247 | fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n", | |
1248 | cmd->lineno); | |
1249 | ret = -EINVAL; | |
1250 | goto err; | |
1251 | } | |
1252 | ||
1253 | /* | |
1254 | * Construct the command. | |
1255 | */ | |
1256 | ccmd->header.checksum = 0x5a; | |
1257 | ccmd->header.tag = ROM_MODE_CMD; | |
1258 | ||
1259 | ccmd->mode.mode = mode; | |
1260 | ||
1261 | cctx->size = sizeof(*ccmd); | |
1262 | ||
1263 | /* | |
1264 | * Append the command to the last section. | |
1265 | */ | |
1266 | if (!sctx->cmd_head) { | |
1267 | sctx->cmd_head = cctx; | |
1268 | sctx->cmd_tail = cctx; | |
1269 | } else { | |
1270 | sctx->cmd_tail->cmd = cctx; | |
1271 | sctx->cmd_tail = cctx; | |
1272 | } | |
1273 | ||
1274 | return 0; | |
1275 | ||
1276 | err: | |
1277 | free(cctx); | |
1278 | return ret; | |
1279 | } | |
1280 | ||
1281 | static int sb_prefill_image_header(struct sb_image_ctx *ictx) | |
1282 | { | |
1283 | struct sb_boot_image_header *hdr = &ictx->payload; | |
1284 | ||
1285 | /* Fill signatures */ | |
1286 | memcpy(hdr->signature1, "STMP", 4); | |
1287 | memcpy(hdr->signature2, "sgtl", 4); | |
1288 | ||
1289 | /* SB Image version 1.1 */ | |
1290 | hdr->major_version = SB_VERSION_MAJOR; | |
1291 | hdr->minor_version = SB_VERSION_MINOR; | |
1292 | ||
1293 | /* Boot image major version */ | |
1294 | hdr->product_version.major = htons(0x999); | |
1295 | hdr->product_version.minor = htons(0x999); | |
1296 | hdr->product_version.revision = htons(0x999); | |
1297 | /* Boot image major version */ | |
1298 | hdr->component_version.major = htons(0x999); | |
1299 | hdr->component_version.minor = htons(0x999); | |
1300 | hdr->component_version.revision = htons(0x999); | |
1301 | ||
1302 | /* Drive tag must be 0x0 for i.MX23 */ | |
1303 | hdr->drive_tag = 0; | |
1304 | ||
1305 | hdr->header_blocks = | |
1306 | sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE; | |
1307 | hdr->section_header_size = | |
1308 | sizeof(struct sb_sections_header) / SB_BLOCK_SIZE; | |
1309 | hdr->timestamp_us = sb_get_timestamp() * 1000000; | |
1310 | ||
7a139959 AI |
1311 | hdr->flags = ictx->display_progress ? |
1312 | SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0; | |
bce88370 MV |
1313 | |
1314 | /* FIXME -- We support only default key */ | |
1315 | hdr->key_count = 1; | |
1316 | ||
1317 | return 0; | |
1318 | } | |
1319 | ||
1320 | static int sb_postfill_image_header(struct sb_image_ctx *ictx) | |
1321 | { | |
1322 | struct sb_boot_image_header *hdr = &ictx->payload; | |
1323 | struct sb_section_ctx *sctx = ictx->sect_head; | |
1324 | uint32_t kd_size, sections_blocks; | |
1325 | EVP_MD_CTX md_ctx; | |
1326 | ||
1327 | /* The main SB header size in blocks. */ | |
1328 | hdr->image_blocks = hdr->header_blocks; | |
1329 | ||
1330 | /* Size of the key dictionary, which has single zero entry. */ | |
1331 | kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key); | |
1332 | hdr->image_blocks += kd_size / SB_BLOCK_SIZE; | |
1333 | ||
1334 | /* Now count the payloads. */ | |
1335 | hdr->section_count = ictx->sect_count; | |
1336 | while (sctx) { | |
1337 | hdr->image_blocks += sctx->size / SB_BLOCK_SIZE; | |
1338 | sctx = sctx->sect; | |
1339 | } | |
1340 | ||
1341 | if (!ictx->sect_boot_found) { | |
1342 | fprintf(stderr, "ERR: No bootable section selected!\n"); | |
1343 | return -EINVAL; | |
1344 | } | |
1345 | hdr->first_boot_section_id = ictx->sect_boot; | |
1346 | ||
1347 | /* The n * SB section size in blocks. */ | |
1348 | sections_blocks = hdr->section_count * hdr->section_header_size; | |
1349 | hdr->image_blocks += sections_blocks; | |
1350 | ||
1351 | /* Key dictionary offset. */ | |
1352 | hdr->key_dictionary_block = hdr->header_blocks + sections_blocks; | |
1353 | ||
1354 | /* Digest of the whole image. */ | |
1355 | hdr->image_blocks += 2; | |
1356 | ||
1357 | /* Pointer past the dictionary. */ | |
1358 | hdr->first_boot_tag_block = | |
1359 | hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE; | |
1360 | ||
1361 | /* Compute header digest. */ | |
1362 | EVP_MD_CTX_init(&md_ctx); | |
1363 | ||
1364 | EVP_DigestInit(&md_ctx, EVP_sha1()); | |
1365 | EVP_DigestUpdate(&md_ctx, hdr->signature1, | |
1366 | sizeof(struct sb_boot_image_header) - | |
1367 | sizeof(hdr->digest)); | |
1368 | EVP_DigestFinal(&md_ctx, hdr->digest, NULL); | |
1369 | ||
1370 | return 0; | |
1371 | } | |
1372 | ||
1373 | static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx) | |
1374 | { | |
1375 | /* Fixup the placement of sections. */ | |
1376 | struct sb_boot_image_header *ihdr = &ictx->payload; | |
1377 | struct sb_section_ctx *sctx = ictx->sect_head; | |
1378 | struct sb_sections_header *shdr; | |
1379 | struct sb_cmd_ctx *cctx; | |
1380 | struct sb_command *ccmd; | |
1381 | uint32_t offset = ihdr->first_boot_tag_block; | |
1382 | ||
1383 | while (sctx) { | |
1384 | shdr = &sctx->payload; | |
1385 | ||
1386 | /* Fill in the section TAG offset. */ | |
1387 | shdr->section_offset = offset + 1; | |
1388 | offset += shdr->section_size; | |
1389 | ||
1390 | /* Section length is measured from the TAG block. */ | |
1391 | shdr->section_size--; | |
1392 | ||
1393 | /* Fixup the TAG command. */ | |
1394 | cctx = sctx->cmd_head; | |
1395 | while (cctx) { | |
1396 | ccmd = &cctx->payload; | |
1397 | if (ccmd->header.tag == ROM_TAG_CMD) { | |
1398 | ccmd->tag.section_number = shdr->section_number; | |
1399 | ccmd->tag.section_length = shdr->section_size; | |
1400 | ccmd->tag.section_flags = shdr->section_flags; | |
1401 | } | |
1402 | ||
1403 | /* Update the command checksum. */ | |
1404 | ccmd->header.checksum = sb_command_checksum(ccmd); | |
1405 | ||
1406 | cctx = cctx->cmd; | |
1407 | } | |
1408 | ||
1409 | sctx = sctx->sect; | |
1410 | } | |
1411 | ||
1412 | return 0; | |
1413 | } | |
1414 | ||
1415 | static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd) | |
1416 | { | |
1417 | char *tok; | |
1418 | char *line = cmd->cmd; | |
3cb4b713 | 1419 | char *rptr = NULL; |
bce88370 MV |
1420 | int ret; |
1421 | ||
1422 | /* Analyze the identifier on this line first. */ | |
1423 | tok = strtok_r(line, " ", &rptr); | |
1424 | if (!tok || (strlen(tok) == 0)) { | |
1425 | fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno); | |
1426 | return -EINVAL; | |
1427 | } | |
1428 | ||
1429 | cmd->cmd = rptr; | |
1430 | ||
7a139959 AI |
1431 | /* set DISPLAY_PROGRESS flag */ |
1432 | if (!strcmp(tok, "DISPLAYPROGRESS")) { | |
1433 | ictx->display_progress = 1; | |
1434 | return 0; | |
1435 | } | |
1436 | ||
bce88370 MV |
1437 | /* DCD */ |
1438 | if (!strcmp(tok, "DCD")) { | |
1439 | ictx->in_section = 0; | |
1440 | ictx->in_dcd = 1; | |
1441 | sb_build_dcd(ictx, cmd); | |
1442 | return 0; | |
1443 | } | |
1444 | ||
1445 | /* Section */ | |
1446 | if (!strcmp(tok, "SECTION")) { | |
1447 | ictx->in_section = 1; | |
1448 | ictx->in_dcd = 0; | |
1449 | sb_build_section(ictx, cmd); | |
1450 | return 0; | |
1451 | } | |
1452 | ||
1453 | if (!ictx->in_section && !ictx->in_dcd) { | |
1454 | fprintf(stderr, "#%i ERR: Data outside of a section!\n", | |
1455 | cmd->lineno); | |
1456 | return -EINVAL; | |
1457 | } | |
1458 | ||
1459 | if (ictx->in_section) { | |
1460 | /* Section commands */ | |
1461 | if (!strcmp(tok, "NOP")) { | |
1462 | ret = sb_build_command_nop(ictx); | |
1463 | } else if (!strcmp(tok, "TAG")) { | |
1464 | ret = sb_build_command_tag(ictx, cmd); | |
1465 | } else if (!strcmp(tok, "LOAD")) { | |
1466 | ret = sb_build_command_load(ictx, cmd); | |
1467 | } else if (!strcmp(tok, "FILL")) { | |
1468 | ret = sb_build_command_fill(ictx, cmd); | |
1469 | } else if (!strcmp(tok, "JUMP")) { | |
1470 | ret = sb_build_command_jump(ictx, cmd); | |
1471 | } else if (!strcmp(tok, "CALL")) { | |
1472 | ret = sb_build_command_call(ictx, cmd); | |
1473 | } else if (!strcmp(tok, "MODE")) { | |
1474 | ret = sb_build_command_mode(ictx, cmd); | |
1475 | } else { | |
1476 | fprintf(stderr, | |
1477 | "#%i ERR: Unsupported instruction '%s'!\n", | |
1478 | cmd->lineno, tok); | |
1479 | return -ENOTSUP; | |
1480 | } | |
1481 | } else if (ictx->in_dcd) { | |
1482 | char *lptr; | |
1483 | uint32_t ilen = '1'; | |
1484 | ||
1485 | tok = strtok_r(tok, ".", &lptr); | |
1486 | if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) { | |
1487 | fprintf(stderr, "#%i ERR: Invalid line!\n", | |
1488 | cmd->lineno); | |
1489 | return -EINVAL; | |
1490 | } | |
1491 | ||
1492 | if (lptr && | |
1493 | (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) { | |
1494 | fprintf(stderr, "#%i ERR: Invalid instruction width!\n", | |
1495 | cmd->lineno); | |
1496 | return -EINVAL; | |
1497 | } | |
1498 | ||
1499 | if (lptr) | |
1500 | ilen = lptr[0] - '1'; | |
1501 | ||
1502 | /* DCD commands */ | |
1503 | if (!strcmp(tok, "WRITE")) { | |
1504 | ret = sb_build_dcd_block(ictx, cmd, | |
1505 | SB_DCD_WRITE | ilen); | |
1506 | } else if (!strcmp(tok, "ANDC")) { | |
1507 | ret = sb_build_dcd_block(ictx, cmd, | |
1508 | SB_DCD_ANDC | ilen); | |
1509 | } else if (!strcmp(tok, "ORR")) { | |
1510 | ret = sb_build_dcd_block(ictx, cmd, | |
1511 | SB_DCD_ORR | ilen); | |
1512 | } else if (!strcmp(tok, "EQZ")) { | |
1513 | ret = sb_build_dcd_block(ictx, cmd, | |
1514 | SB_DCD_CHK_EQZ | ilen); | |
1515 | } else if (!strcmp(tok, "EQ")) { | |
1516 | ret = sb_build_dcd_block(ictx, cmd, | |
1517 | SB_DCD_CHK_EQ | ilen); | |
1518 | } else if (!strcmp(tok, "NEQ")) { | |
1519 | ret = sb_build_dcd_block(ictx, cmd, | |
1520 | SB_DCD_CHK_NEQ | ilen); | |
1521 | } else if (!strcmp(tok, "NEZ")) { | |
1522 | ret = sb_build_dcd_block(ictx, cmd, | |
1523 | SB_DCD_CHK_NEZ | ilen); | |
1524 | } else if (!strcmp(tok, "NOOP")) { | |
1525 | ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP); | |
1526 | } else { | |
1527 | fprintf(stderr, | |
1528 | "#%i ERR: Unsupported instruction '%s'!\n", | |
1529 | cmd->lineno, tok); | |
1530 | return -ENOTSUP; | |
1531 | } | |
1532 | } else { | |
1533 | fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n", | |
1534 | cmd->lineno, tok); | |
1535 | return -ENOTSUP; | |
1536 | } | |
1537 | ||
1538 | /* | |
1539 | * Here we have at least one section with one command, otherwise we | |
1540 | * would have failed already higher above. | |
1541 | * | |
1542 | * FIXME -- should the updating happen here ? | |
1543 | */ | |
1544 | if (ictx->in_section && !ret) { | |
1545 | ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size; | |
1546 | ictx->sect_tail->payload.section_size = | |
1547 | ictx->sect_tail->size / SB_BLOCK_SIZE; | |
1548 | } | |
1549 | ||
1550 | return ret; | |
1551 | } | |
1552 | ||
1553 | static int sb_load_cmdfile(struct sb_image_ctx *ictx) | |
1554 | { | |
1555 | struct sb_cmd_list cmd; | |
1556 | int lineno = 1; | |
1557 | FILE *fp; | |
1558 | char *line = NULL; | |
1559 | ssize_t rlen; | |
1560 | size_t len; | |
1561 | ||
1562 | fp = fopen(ictx->cfg_filename, "r"); | |
1563 | if (!fp) | |
1564 | goto err_file; | |
1565 | ||
1566 | while ((rlen = getline(&line, &len, fp)) > 0) { | |
1567 | memset(&cmd, 0, sizeof(cmd)); | |
1568 | ||
1569 | /* Strip the trailing newline. */ | |
1570 | line[rlen - 1] = '\0'; | |
1571 | ||
1572 | cmd.cmd = line; | |
1573 | cmd.len = rlen; | |
1574 | cmd.lineno = lineno++; | |
1575 | ||
1576 | sb_parse_line(ictx, &cmd); | |
1577 | } | |
1578 | ||
1579 | free(line); | |
1580 | ||
1581 | fclose(fp); | |
1582 | ||
1583 | return 0; | |
1584 | ||
1585 | err_file: | |
1586 | fclose(fp); | |
1587 | fprintf(stderr, "ERR: Failed to load file \"%s\"\n", | |
1588 | ictx->cfg_filename); | |
1589 | return -EINVAL; | |
1590 | } | |
1591 | ||
1592 | static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx) | |
1593 | { | |
1594 | int ret; | |
1595 | ||
1596 | ret = sb_load_cmdfile(ictx); | |
1597 | if (ret) | |
1598 | return ret; | |
1599 | ||
1600 | ret = sb_prefill_image_header(ictx); | |
1601 | if (ret) | |
1602 | return ret; | |
1603 | ||
1604 | ret = sb_postfill_image_header(ictx); | |
1605 | if (ret) | |
1606 | return ret; | |
1607 | ||
1608 | ret = sb_fixup_sections_and_tags(ictx); | |
1609 | if (ret) | |
1610 | return ret; | |
1611 | ||
1612 | return 0; | |
1613 | } | |
1614 | ||
1615 | static int sb_verify_image_header(struct sb_image_ctx *ictx, | |
1616 | FILE *fp, long fsize) | |
1617 | { | |
1618 | /* Verify static fields in the image header. */ | |
1619 | struct sb_boot_image_header *hdr = &ictx->payload; | |
1620 | const char *stat[2] = { "[PASS]", "[FAIL]" }; | |
1621 | struct tm tm; | |
1622 | int sz, ret = 0; | |
1623 | unsigned char digest[20]; | |
1624 | EVP_MD_CTX md_ctx; | |
1625 | unsigned long size; | |
1626 | ||
1627 | /* Start image-wide crypto. */ | |
1628 | EVP_MD_CTX_init(&ictx->md_ctx); | |
1629 | EVP_DigestInit(&ictx->md_ctx, EVP_sha1()); | |
1630 | ||
1631 | soprintf(ictx, "---------- Verifying SB Image Header ----------\n"); | |
1632 | ||
1633 | size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp); | |
1634 | if (size != sizeof(ictx->payload)) { | |
1635 | fprintf(stderr, "ERR: SB image header too short!\n"); | |
1636 | return -EINVAL; | |
1637 | } | |
1638 | ||
1639 | /* Compute header digest. */ | |
1640 | EVP_MD_CTX_init(&md_ctx); | |
1641 | EVP_DigestInit(&md_ctx, EVP_sha1()); | |
1642 | EVP_DigestUpdate(&md_ctx, hdr->signature1, | |
1643 | sizeof(struct sb_boot_image_header) - | |
1644 | sizeof(hdr->digest)); | |
1645 | EVP_DigestFinal(&md_ctx, digest, NULL); | |
1646 | ||
1647 | sb_aes_init(ictx, NULL, 1); | |
1648 | sb_encrypt_sb_header(ictx); | |
1649 | ||
1650 | if (memcmp(digest, hdr->digest, 20)) | |
1651 | ret = -EINVAL; | |
1652 | soprintf(ictx, "%s Image header checksum: %s\n", stat[!!ret], | |
1653 | ret ? "BAD" : "OK"); | |
1654 | if (ret) | |
1655 | return ret; | |
1656 | ||
1657 | if (memcmp(hdr->signature1, "STMP", 4) || | |
1658 | memcmp(hdr->signature2, "sgtl", 4)) | |
1659 | ret = -EINVAL; | |
1660 | soprintf(ictx, "%s Signatures: '%.4s' '%.4s'\n", | |
1661 | stat[!!ret], hdr->signature1, hdr->signature2); | |
1662 | if (ret) | |
1663 | return ret; | |
1664 | ||
1665 | if ((hdr->major_version != SB_VERSION_MAJOR) || | |
1666 | ((hdr->minor_version != 1) && (hdr->minor_version != 2))) | |
1667 | ret = -EINVAL; | |
1668 | soprintf(ictx, "%s Image version: v%i.%i\n", stat[!!ret], | |
1669 | hdr->major_version, hdr->minor_version); | |
1670 | if (ret) | |
1671 | return ret; | |
1672 | ||
1673 | ret = sb_get_time(hdr->timestamp_us / 1000000, &tm); | |
1674 | soprintf(ictx, | |
1675 | "%s Creation time: %02i:%02i:%02i %02i/%02i/%04i\n", | |
1676 | stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec, | |
1677 | tm.tm_mday, tm.tm_mon, tm.tm_year + 2000); | |
1678 | if (ret) | |
1679 | return ret; | |
1680 | ||
1681 | soprintf(ictx, "%s Product version: %x.%x.%x\n", stat[0], | |
1682 | ntohs(hdr->product_version.major), | |
1683 | ntohs(hdr->product_version.minor), | |
1684 | ntohs(hdr->product_version.revision)); | |
1685 | soprintf(ictx, "%s Component version: %x.%x.%x\n", stat[0], | |
1686 | ntohs(hdr->component_version.major), | |
1687 | ntohs(hdr->component_version.minor), | |
1688 | ntohs(hdr->component_version.revision)); | |
1689 | ||
7a139959 | 1690 | if (hdr->flags & ~SB_IMAGE_FLAGS_MASK) |
bce88370 MV |
1691 | ret = -EINVAL; |
1692 | soprintf(ictx, "%s Image flags: %s\n", stat[!!ret], | |
7a139959 AI |
1693 | hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ? |
1694 | "Display_progress" : ""); | |
bce88370 MV |
1695 | if (ret) |
1696 | return ret; | |
1697 | ||
1698 | if (hdr->drive_tag != 0) | |
1699 | ret = -EINVAL; | |
1700 | soprintf(ictx, "%s Drive tag: %i\n", stat[!!ret], | |
1701 | hdr->drive_tag); | |
1702 | if (ret) | |
1703 | return ret; | |
1704 | ||
1705 | sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE; | |
1706 | if (hdr->header_blocks != sz) | |
1707 | ret = -EINVAL; | |
1708 | soprintf(ictx, "%s Image header size (blocks): %i\n", stat[!!ret], | |
1709 | hdr->header_blocks); | |
1710 | if (ret) | |
1711 | return ret; | |
1712 | ||
1713 | sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE; | |
1714 | if (hdr->section_header_size != sz) | |
1715 | ret = -EINVAL; | |
1716 | soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret], | |
1717 | hdr->section_header_size); | |
1718 | if (ret) | |
1719 | return ret; | |
1720 | ||
1721 | soprintf(ictx, "%s Sections count: %i\n", stat[!!ret], | |
1722 | hdr->section_count); | |
1723 | soprintf(ictx, "%s First bootable section %i\n", stat[!!ret], | |
1724 | hdr->first_boot_section_id); | |
1725 | ||
1726 | if (hdr->image_blocks != fsize / SB_BLOCK_SIZE) | |
1727 | ret = -EINVAL; | |
1728 | soprintf(ictx, "%s Image size (blocks): %i\n", stat[!!ret], | |
1729 | hdr->image_blocks); | |
1730 | if (ret) | |
1731 | return ret; | |
1732 | ||
1733 | sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count; | |
1734 | if (hdr->key_dictionary_block != sz) | |
1735 | ret = -EINVAL; | |
1736 | soprintf(ictx, "%s Key dict offset (blocks): %i\n", stat[!!ret], | |
1737 | hdr->key_dictionary_block); | |
1738 | if (ret) | |
1739 | return ret; | |
1740 | ||
1741 | if (hdr->key_count != 1) | |
1742 | ret = -EINVAL; | |
1743 | soprintf(ictx, "%s Number of encryption keys: %i\n", stat[!!ret], | |
1744 | hdr->key_count); | |
1745 | if (ret) | |
1746 | return ret; | |
1747 | ||
1748 | sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count; | |
1749 | sz += hdr->key_count * | |
1750 | sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE; | |
1751 | if (hdr->first_boot_tag_block != (unsigned)sz) | |
1752 | ret = -EINVAL; | |
1753 | soprintf(ictx, "%s First TAG block (blocks): %i\n", stat[!!ret], | |
1754 | hdr->first_boot_tag_block); | |
1755 | if (ret) | |
1756 | return ret; | |
1757 | ||
1758 | return 0; | |
1759 | } | |
1760 | ||
1761 | static void sb_decrypt_tag(struct sb_image_ctx *ictx, | |
1762 | struct sb_cmd_ctx *cctx) | |
1763 | { | |
1764 | EVP_MD_CTX *md_ctx = &ictx->md_ctx; | |
1765 | struct sb_command *cmd = &cctx->payload; | |
1766 | ||
1767 | sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload, | |
1768 | (uint8_t *)&cctx->payload, sizeof(*cmd)); | |
1769 | EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd)); | |
1770 | } | |
1771 | ||
1772 | static int sb_verify_command(struct sb_image_ctx *ictx, | |
1773 | struct sb_cmd_ctx *cctx, FILE *fp, | |
1774 | unsigned long *tsize) | |
1775 | { | |
1776 | struct sb_command *ccmd = &cctx->payload; | |
1777 | unsigned long size, asize; | |
1778 | char *csum, *flag = ""; | |
1779 | int ret; | |
1780 | unsigned int i; | |
1781 | uint8_t csn, csc = ccmd->header.checksum; | |
1782 | ccmd->header.checksum = 0x5a; | |
1783 | csn = sb_command_checksum(ccmd); | |
1784 | ccmd->header.checksum = csc; | |
1785 | ||
1786 | if (csc == csn) | |
1787 | ret = 0; | |
1788 | else | |
1789 | ret = -EINVAL; | |
1790 | csum = ret ? "checksum BAD" : "checksum OK"; | |
1791 | ||
1792 | switch (ccmd->header.tag) { | |
1793 | case ROM_NOP_CMD: | |
1794 | soprintf(ictx, " NOOP # %s\n", csum); | |
1795 | return ret; | |
1796 | case ROM_TAG_CMD: | |
1797 | if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG) | |
1798 | flag = "LAST"; | |
1799 | soprintf(ictx, " TAG %s # %s\n", flag, csum); | |
1800 | sb_aes_reinit(ictx, 0); | |
1801 | return ret; | |
1802 | case ROM_LOAD_CMD: | |
1803 | soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n", | |
1804 | ccmd->load.address, ccmd->load.count, csum); | |
1805 | ||
1806 | cctx->length = ccmd->load.count; | |
1807 | asize = roundup(cctx->length, SB_BLOCK_SIZE); | |
1808 | cctx->data = malloc(asize); | |
1809 | if (!cctx->data) | |
1810 | return -ENOMEM; | |
1811 | ||
1812 | size = fread(cctx->data, 1, asize, fp); | |
1813 | if (size != asize) { | |
1814 | fprintf(stderr, | |
1815 | "ERR: SB LOAD command payload too short!\n"); | |
1816 | return -EINVAL; | |
1817 | } | |
1818 | ||
1819 | *tsize += size; | |
1820 | ||
1821 | EVP_DigestUpdate(&ictx->md_ctx, cctx->data, asize); | |
1822 | sb_aes_crypt(ictx, cctx->data, cctx->data, asize); | |
1823 | ||
25308f45 CM |
1824 | if (ccmd->load.crc32 != pbl_crc32(0, |
1825 | (const char *)cctx->data, | |
1826 | asize)) { | |
bce88370 MV |
1827 | fprintf(stderr, |
1828 | "ERR: SB LOAD command payload CRC32 invalid!\n"); | |
1829 | return -EINVAL; | |
1830 | } | |
1831 | return 0; | |
1832 | case ROM_FILL_CMD: | |
1833 | soprintf(ictx, | |
1834 | " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n", | |
1835 | ccmd->fill.address, ccmd->fill.count, | |
1836 | ccmd->fill.pattern, csum); | |
1837 | return 0; | |
1838 | case ROM_JUMP_CMD: | |
1839 | if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB) | |
1840 | flag = " HAB"; | |
1841 | soprintf(ictx, | |
1842 | " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n", | |
1843 | flag, ccmd->fill.address, ccmd->jump.argument, csum); | |
1844 | return 0; | |
1845 | case ROM_CALL_CMD: | |
1846 | if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB) | |
1847 | flag = " HAB"; | |
1848 | soprintf(ictx, | |
1849 | " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n", | |
1850 | flag, ccmd->fill.address, ccmd->jump.argument, csum); | |
1851 | return 0; | |
1852 | case ROM_MODE_CMD: | |
1853 | for (i = 0; i < ARRAY_SIZE(modetable); i++) { | |
1854 | if (ccmd->mode.mode == modetable[i].mode) { | |
1855 | soprintf(ictx, " MODE %s # %s\n", | |
1856 | modetable[i].name, csum); | |
1857 | break; | |
1858 | } | |
1859 | } | |
1860 | fprintf(stderr, " MODE !INVALID! # %s\n", csum); | |
1861 | return 0; | |
1862 | } | |
1863 | ||
1864 | return ret; | |
1865 | } | |
1866 | ||
1867 | static int sb_verify_commands(struct sb_image_ctx *ictx, | |
1868 | struct sb_section_ctx *sctx, FILE *fp) | |
1869 | { | |
1870 | unsigned long size, tsize = 0; | |
1871 | struct sb_cmd_ctx *cctx; | |
1872 | int ret; | |
1873 | ||
1874 | sb_aes_reinit(ictx, 0); | |
1875 | ||
1876 | while (tsize < sctx->size) { | |
1877 | cctx = calloc(1, sizeof(*cctx)); | |
1878 | if (!cctx) | |
1879 | return -ENOMEM; | |
1880 | if (!sctx->cmd_head) { | |
1881 | sctx->cmd_head = cctx; | |
1882 | sctx->cmd_tail = cctx; | |
1883 | } else { | |
1884 | sctx->cmd_tail->cmd = cctx; | |
1885 | sctx->cmd_tail = cctx; | |
1886 | } | |
1887 | ||
1888 | size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp); | |
1889 | if (size != sizeof(cctx->c_payload)) { | |
1890 | fprintf(stderr, "ERR: SB command header too short!\n"); | |
1891 | return -EINVAL; | |
1892 | } | |
1893 | ||
1894 | tsize += size; | |
1895 | ||
1896 | sb_decrypt_tag(ictx, cctx); | |
1897 | ||
1898 | ret = sb_verify_command(ictx, cctx, fp, &tsize); | |
1899 | if (ret) | |
1900 | return -EINVAL; | |
1901 | } | |
1902 | ||
1903 | return 0; | |
1904 | } | |
1905 | ||
1906 | static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp) | |
1907 | { | |
1908 | struct sb_boot_image_header *hdr = &ictx->payload; | |
1909 | struct sb_sections_header *shdr; | |
1910 | unsigned int i; | |
1911 | int ret; | |
1912 | struct sb_section_ctx *sctx; | |
1913 | unsigned long size; | |
1914 | char *bootable = ""; | |
1915 | ||
1916 | soprintf(ictx, "----- Verifying SB Sections and Commands -----\n"); | |
1917 | ||
1918 | for (i = 0; i < hdr->section_count; i++) { | |
1919 | sctx = calloc(1, sizeof(*sctx)); | |
1920 | if (!sctx) | |
1921 | return -ENOMEM; | |
1922 | if (!ictx->sect_head) { | |
1923 | ictx->sect_head = sctx; | |
1924 | ictx->sect_tail = sctx; | |
1925 | } else { | |
1926 | ictx->sect_tail->sect = sctx; | |
1927 | ictx->sect_tail = sctx; | |
1928 | } | |
1929 | ||
1930 | size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp); | |
1931 | if (size != sizeof(sctx->payload)) { | |
1932 | fprintf(stderr, "ERR: SB section header too short!\n"); | |
1933 | return -EINVAL; | |
1934 | } | |
1935 | } | |
1936 | ||
1937 | size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp); | |
1938 | if (size != sizeof(ictx->sb_dict_key)) { | |
1939 | fprintf(stderr, "ERR: SB key dictionary too short!\n"); | |
1940 | return -EINVAL; | |
1941 | } | |
1942 | ||
1943 | sb_encrypt_sb_sections_header(ictx); | |
1944 | sb_aes_reinit(ictx, 0); | |
1945 | sb_decrypt_key_dictionary_key(ictx); | |
1946 | ||
1947 | sb_aes_reinit(ictx, 0); | |
1948 | ||
1949 | sctx = ictx->sect_head; | |
1950 | while (sctx) { | |
1951 | shdr = &sctx->payload; | |
1952 | ||
1953 | if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) { | |
1954 | sctx->boot = 1; | |
1955 | bootable = " BOOTABLE"; | |
1956 | } | |
1957 | ||
1958 | sctx->size = (shdr->section_size * SB_BLOCK_SIZE) + | |
1959 | sizeof(struct sb_command); | |
1960 | soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n", | |
1961 | shdr->section_number, bootable, sctx->size); | |
1962 | ||
1963 | if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE) | |
1964 | fprintf(stderr, " WARN: Unknown section flag(s) %08x\n", | |
1965 | shdr->section_flags); | |
1966 | ||
1967 | if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) && | |
1968 | (hdr->first_boot_section_id != shdr->section_number)) { | |
1969 | fprintf(stderr, | |
1970 | " WARN: Bootable section does ID not match image header ID!\n"); | |
1971 | } | |
1972 | ||
1973 | ret = sb_verify_commands(ictx, sctx, fp); | |
1974 | if (ret) | |
1975 | return ret; | |
1976 | ||
1977 | sctx = sctx->sect; | |
1978 | } | |
1979 | ||
1980 | /* | |
1981 | * FIXME IDEA: | |
1982 | * check if the first TAG command is at sctx->section_offset | |
1983 | */ | |
1984 | return 0; | |
1985 | } | |
1986 | ||
1987 | static int sb_verify_image_end(struct sb_image_ctx *ictx, | |
1988 | FILE *fp, off_t filesz) | |
1989 | { | |
1990 | uint8_t digest[32]; | |
1991 | unsigned long size; | |
1992 | off_t pos; | |
1993 | int ret; | |
1994 | ||
1995 | soprintf(ictx, "------------- Verifying image end -------------\n"); | |
1996 | ||
1997 | size = fread(digest, 1, sizeof(digest), fp); | |
1998 | if (size != sizeof(digest)) { | |
1999 | fprintf(stderr, "ERR: SB key dictionary too short!\n"); | |
2000 | return -EINVAL; | |
2001 | } | |
2002 | ||
2003 | pos = ftell(fp); | |
2004 | if (pos != filesz) { | |
2005 | fprintf(stderr, "ERR: Trailing data past the image!\n"); | |
2006 | return -EINVAL; | |
2007 | } | |
2008 | ||
2009 | /* Check the image digest. */ | |
2010 | EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL); | |
2011 | ||
2012 | /* Decrypt the image digest from the input image. */ | |
2013 | sb_aes_reinit(ictx, 0); | |
2014 | sb_aes_crypt(ictx, digest, digest, sizeof(digest)); | |
2015 | ||
2016 | /* Check all of 20 bytes of the SHA1 hash. */ | |
2017 | ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0; | |
2018 | ||
2019 | if (ret) | |
2020 | soprintf(ictx, "[FAIL] Full-image checksum: BAD\n"); | |
2021 | else | |
2022 | soprintf(ictx, "[PASS] Full-image checksum: OK\n"); | |
2023 | ||
2024 | return ret; | |
2025 | } | |
2026 | ||
2027 | ||
2028 | static int sb_build_tree_from_img(struct sb_image_ctx *ictx) | |
2029 | { | |
2030 | long filesize; | |
2031 | int ret; | |
2032 | FILE *fp; | |
2033 | ||
2034 | if (!ictx->input_filename) { | |
2035 | fprintf(stderr, "ERR: Missing filename!\n"); | |
2036 | return -EINVAL; | |
2037 | } | |
2038 | ||
2039 | fp = fopen(ictx->input_filename, "r"); | |
2040 | if (!fp) | |
2041 | goto err_open; | |
2042 | ||
2043 | ret = fseek(fp, 0, SEEK_END); | |
2044 | if (ret < 0) | |
2045 | goto err_file; | |
2046 | ||
2047 | filesize = ftell(fp); | |
2048 | if (filesize < 0) | |
2049 | goto err_file; | |
2050 | ||
2051 | ret = fseek(fp, 0, SEEK_SET); | |
2052 | if (ret < 0) | |
2053 | goto err_file; | |
2054 | ||
2055 | if (filesize < (signed)sizeof(ictx->payload)) { | |
2056 | fprintf(stderr, "ERR: File too short!\n"); | |
2057 | goto err_file; | |
2058 | } | |
2059 | ||
2060 | if (filesize & (SB_BLOCK_SIZE - 1)) { | |
2061 | fprintf(stderr, "ERR: The file is not aligned!\n"); | |
2062 | goto err_file; | |
2063 | } | |
2064 | ||
2065 | /* Load and verify image header */ | |
2066 | ret = sb_verify_image_header(ictx, fp, filesize); | |
2067 | if (ret) | |
2068 | goto err_verify; | |
2069 | ||
2070 | /* Load and verify sections and commands */ | |
2071 | ret = sb_verify_sections_cmds(ictx, fp); | |
2072 | if (ret) | |
2073 | goto err_verify; | |
2074 | ||
2075 | ret = sb_verify_image_end(ictx, fp, filesize); | |
2076 | if (ret) | |
2077 | goto err_verify; | |
2078 | ||
2079 | ret = 0; | |
2080 | ||
2081 | err_verify: | |
2082 | soprintf(ictx, "-------------------- Result -------------------\n"); | |
2083 | soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED"); | |
2084 | ||
2085 | /* Stop the encryption session. */ | |
2086 | sb_aes_deinit(&ictx->cipher_ctx); | |
2087 | ||
2088 | fclose(fp); | |
2089 | return ret; | |
2090 | ||
2091 | err_file: | |
2092 | fclose(fp); | |
2093 | err_open: | |
2094 | fprintf(stderr, "ERR: Failed to load file \"%s\"\n", | |
2095 | ictx->input_filename); | |
2096 | return -EINVAL; | |
2097 | } | |
2098 | ||
2099 | static void sb_free_image(struct sb_image_ctx *ictx) | |
2100 | { | |
2101 | struct sb_section_ctx *sctx = ictx->sect_head, *s_head; | |
2102 | struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head; | |
2103 | struct sb_cmd_ctx *cctx, *c_head; | |
2104 | ||
2105 | while (sctx) { | |
2106 | s_head = sctx; | |
2107 | c_head = sctx->cmd_head; | |
2108 | ||
2109 | while (c_head) { | |
2110 | cctx = c_head; | |
2111 | c_head = c_head->cmd; | |
2112 | if (cctx->data) | |
2113 | free(cctx->data); | |
2114 | free(cctx); | |
2115 | } | |
2116 | ||
2117 | sctx = sctx->sect; | |
2118 | free(s_head); | |
2119 | } | |
2120 | ||
2121 | while (dctx) { | |
2122 | d_head = dctx; | |
2123 | dctx = dctx->dcd; | |
2124 | free(d_head->payload); | |
2125 | free(d_head); | |
2126 | } | |
2127 | } | |
2128 | ||
2129 | /* | |
2130 | * MXSSB-MKIMAGE glue code. | |
2131 | */ | |
2132 | static int mxsimage_check_image_types(uint8_t type) | |
2133 | { | |
2134 | if (type == IH_TYPE_MXSIMAGE) | |
2135 | return EXIT_SUCCESS; | |
2136 | else | |
2137 | return EXIT_FAILURE; | |
2138 | } | |
2139 | ||
2140 | static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd, | |
f86ed6a8 | 2141 | struct image_tool_params *params) |
bce88370 MV |
2142 | { |
2143 | } | |
2144 | ||
f86ed6a8 | 2145 | int mxsimage_check_params(struct image_tool_params *params) |
bce88370 MV |
2146 | { |
2147 | if (!params) | |
2148 | return -1; | |
2149 | if (!strlen(params->imagename)) { | |
2150 | fprintf(stderr, | |
2151 | "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n", | |
2152 | params->cmdname); | |
2153 | return -1; | |
2154 | } | |
2155 | ||
2156 | /* | |
2157 | * Check parameters: | |
2158 | * XIP is not allowed and verify that incompatible | |
2159 | * parameters are not sent at the same time | |
2160 | * For example, if list is required a data image must not be provided | |
2161 | */ | |
2162 | return (params->dflag && (params->fflag || params->lflag)) || | |
2163 | (params->fflag && (params->dflag || params->lflag)) || | |
2164 | (params->lflag && (params->dflag || params->fflag)) || | |
2165 | (params->xflag) || !(strlen(params->imagename)); | |
2166 | } | |
2167 | ||
2168 | static int mxsimage_verify_print_header(char *file, int silent) | |
2169 | { | |
2170 | int ret; | |
2171 | struct sb_image_ctx ctx; | |
2172 | ||
2173 | memset(&ctx, 0, sizeof(ctx)); | |
2174 | ||
2175 | ctx.input_filename = file; | |
2176 | ctx.silent_dump = silent; | |
2177 | ||
2178 | ret = sb_build_tree_from_img(&ctx); | |
2179 | sb_free_image(&ctx); | |
2180 | ||
2181 | return ret; | |
2182 | } | |
2183 | ||
2184 | char *imagefile; | |
2185 | static int mxsimage_verify_header(unsigned char *ptr, int image_size, | |
f86ed6a8 | 2186 | struct image_tool_params *params) |
bce88370 MV |
2187 | { |
2188 | struct sb_boot_image_header *hdr; | |
2189 | ||
2190 | if (!ptr) | |
2191 | return -EINVAL; | |
2192 | ||
2193 | hdr = (struct sb_boot_image_header *)ptr; | |
2194 | ||
2195 | /* | |
2196 | * Check if the header contains the MXS image signatures, | |
2197 | * if so, do a full-image verification. | |
2198 | */ | |
2199 | if (memcmp(hdr->signature1, "STMP", 4) || | |
2200 | memcmp(hdr->signature2, "sgtl", 4)) | |
2201 | return -EINVAL; | |
2202 | ||
2203 | imagefile = params->imagefile; | |
2204 | ||
2205 | return mxsimage_verify_print_header(params->imagefile, 1); | |
2206 | } | |
2207 | ||
2208 | static void mxsimage_print_header(const void *hdr) | |
2209 | { | |
2210 | if (imagefile) | |
2211 | mxsimage_verify_print_header(imagefile, 0); | |
2212 | } | |
2213 | ||
2214 | static int sb_build_image(struct sb_image_ctx *ictx, | |
2215 | struct image_type_params *tparams) | |
2216 | { | |
2217 | struct sb_boot_image_header *sb_header = &ictx->payload; | |
2218 | struct sb_section_ctx *sctx; | |
2219 | struct sb_cmd_ctx *cctx; | |
2220 | struct sb_command *ccmd; | |
2221 | struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key; | |
2222 | ||
2223 | uint8_t *image, *iptr; | |
2224 | ||
2225 | /* Calculate image size. */ | |
2226 | uint32_t size = sizeof(*sb_header) + | |
2227 | ictx->sect_count * sizeof(struct sb_sections_header) + | |
2228 | sizeof(*sb_dict_key) + sizeof(ictx->digest); | |
2229 | ||
2230 | sctx = ictx->sect_head; | |
2231 | while (sctx) { | |
2232 | size += sctx->size; | |
2233 | sctx = sctx->sect; | |
2234 | }; | |
2235 | ||
2236 | image = malloc(size); | |
2237 | if (!image) | |
2238 | return -ENOMEM; | |
2239 | iptr = image; | |
2240 | ||
2241 | memcpy(iptr, sb_header, sizeof(*sb_header)); | |
2242 | iptr += sizeof(*sb_header); | |
2243 | ||
2244 | sctx = ictx->sect_head; | |
2245 | while (sctx) { | |
2246 | memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header)); | |
2247 | iptr += sizeof(struct sb_sections_header); | |
2248 | sctx = sctx->sect; | |
2249 | }; | |
2250 | ||
2251 | memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key)); | |
2252 | iptr += sizeof(*sb_dict_key); | |
2253 | ||
2254 | sctx = ictx->sect_head; | |
2255 | while (sctx) { | |
2256 | cctx = sctx->cmd_head; | |
2257 | while (cctx) { | |
2258 | ccmd = &cctx->payload; | |
2259 | ||
2260 | memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload)); | |
2261 | iptr += sizeof(cctx->payload); | |
2262 | ||
2263 | if (ccmd->header.tag == ROM_LOAD_CMD) { | |
2264 | memcpy(iptr, cctx->data, cctx->length); | |
2265 | iptr += cctx->length; | |
2266 | } | |
2267 | ||
2268 | cctx = cctx->cmd; | |
2269 | } | |
2270 | ||
2271 | sctx = sctx->sect; | |
2272 | }; | |
2273 | ||
2274 | memcpy(iptr, ictx->digest, sizeof(ictx->digest)); | |
2275 | iptr += sizeof(ictx->digest); | |
2276 | ||
2277 | /* Configure the mkimage */ | |
2278 | tparams->hdr = image; | |
2279 | tparams->header_size = size; | |
2280 | ||
2281 | return 0; | |
2282 | } | |
2283 | ||
f86ed6a8 | 2284 | static int mxsimage_generate(struct image_tool_params *params, |
bce88370 MV |
2285 | struct image_type_params *tparams) |
2286 | { | |
2287 | int ret; | |
2288 | struct sb_image_ctx ctx; | |
2289 | ||
2290 | /* Do not copy the U-Boot image! */ | |
2291 | params->skipcpy = 1; | |
2292 | ||
2293 | memset(&ctx, 0, sizeof(ctx)); | |
2294 | ||
2295 | ctx.cfg_filename = params->imagename; | |
2296 | ctx.output_filename = params->imagefile; | |
bce88370 MV |
2297 | |
2298 | ret = sb_build_tree_from_cfg(&ctx); | |
2299 | if (ret) | |
2300 | goto fail; | |
2301 | ||
2302 | ret = sb_encrypt_image(&ctx); | |
2303 | if (!ret) | |
2304 | ret = sb_build_image(&ctx, tparams); | |
2305 | ||
2306 | fail: | |
2307 | sb_free_image(&ctx); | |
2308 | ||
2309 | return ret; | |
2310 | } | |
2311 | ||
2312 | /* | |
2313 | * mxsimage parameters | |
2314 | */ | |
2315 | static struct image_type_params mxsimage_params = { | |
2316 | .name = "Freescale MXS Boot Image support", | |
2317 | .header_size = 0, | |
2318 | .hdr = NULL, | |
2319 | .check_image_type = mxsimage_check_image_types, | |
2320 | .verify_header = mxsimage_verify_header, | |
2321 | .print_header = mxsimage_print_header, | |
2322 | .set_header = mxsimage_set_header, | |
2323 | .check_params = mxsimage_check_params, | |
2324 | .vrec_header = mxsimage_generate, | |
2325 | }; | |
2326 | ||
2327 | void init_mxs_image_type(void) | |
2328 | { | |
f86ed6a8 | 2329 | register_image_type(&mxsimage_params); |
bce88370 MV |
2330 | } |
2331 | ||
2332 | #else | |
2333 | void init_mxs_image_type(void) | |
2334 | { | |
2335 | } | |
2336 | #endif |