xref: /openbmc/u-boot/tools/mxsimage.c (revision 49cad547)
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 
20 #include "imagetool.h"
21 #include "mxsimage.h"
22 #include "pbl_crc32.h"
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 */
128 	unsigned int			display_progress:1;
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 
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 
483 	errno = 0;
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;
979 	ccmd->load.crc32	= pbl_crc32(0,
980 					    (const char *)cctx->data,
981 					    cctx->length);
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 
1311 	hdr->flags = ictx->display_progress ?
1312 		SB_IMAGE_FLAG_DISPLAY_PROGRESS : 0;
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;
1419 	char *rptr = NULL;
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 
1431 	/* set DISPLAY_PROGRESS flag */
1432 	if (!strcmp(tok, "DISPLAYPROGRESS")) {
1433 		ictx->display_progress = 1;
1434 		return 0;
1435 	}
1436 
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 
1690 	if (hdr->flags & ~SB_IMAGE_FLAGS_MASK)
1691 		ret = -EINVAL;
1692 	soprintf(ictx, "%s Image flags:                  %s\n", stat[!!ret],
1693 		 hdr->flags & SB_IMAGE_FLAG_DISPLAY_PROGRESS ?
1694 		 "Display_progress" : "");
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 
1824 		if (ccmd->load.crc32 != pbl_crc32(0,
1825 						  (const char *)cctx->data,
1826 						  asize)) {
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,
2141 				struct image_tool_params *params)
2142 {
2143 }
2144 
2145 int mxsimage_check_params(struct image_tool_params *params)
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,
2186 			struct image_tool_params *params)
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 
2284 static int mxsimage_generate(struct image_tool_params *params,
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;
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 {
2329 	register_image_type(&mxsimage_params);
2330 }
2331 
2332 #else
2333 void init_mxs_image_type(void)
2334 {
2335 }
2336 #endif
2337