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