mxssb.c 41.1 KB
Newer Older
Marek Vasut's avatar
Marek Vasut committed
1
/*
2
 * Freescale i.MX23/i.MX28 SB image generator
Marek Vasut's avatar
Marek Vasut committed
3
 *
4
 * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
Marek Vasut's avatar
Marek Vasut committed
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 */

#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
22
#include <limits.h>
Marek Vasut's avatar
Marek Vasut committed
23
24
25
26
27

#include <openssl/evp.h>

#include "mxssb.h"

28
29
30
31
32
33
34
35
36
37
/*
 * IMAGE
 *   |-SECTION
 *   |    |-CMD
 *   |    |-CMD
 *   |    `-CMD
 *   |-SECTION
 *   |    |-CMD
 *   :    :
 */
38
39
40
struct sb_cmd_list {
	char				*cmd;
	size_t				len;
41
	unsigned int			lineno;
42
43
};

44
struct sb_cmd_ctx {
45
46
	uint32_t			size;

47
48
49
50
51
52
	struct sb_cmd_ctx		*cmd;

	uint8_t				*data;
	uint32_t			length;

	struct sb_command		payload;
53
	struct sb_command		c_payload;
54
55
56
};

struct sb_section_ctx {
57
	uint32_t			size;
58

59
60
61
	/* Section flags */
	unsigned int			boot:1;

62
	struct sb_section_ctx		*sect;
63
64
65

	struct sb_cmd_ctx		*cmd_head;
	struct sb_cmd_ctx		*cmd_tail;
66
67
68
69

	struct sb_sections_header	payload;
};

70
struct sb_image_ctx {
71
	/* Image configuration */
72
	unsigned int			verbose_boot:1;
73
74
	char				*input_filename;
	char				*output_filename;
75
76
77
78
79
80
81
82
	char				*cfg_filename;

	/* Number of section in the image */
	unsigned int			sect_count;
	/* Bootable section */
	unsigned int			sect_boot;
	unsigned int			sect_boot_found:1;

83
84
	struct sb_section_ctx		*sect_head;
	struct sb_section_ctx		*sect_tail;
85

86
87
	EVP_CIPHER_CTX			cipher_ctx;
	EVP_MD_CTX			md_ctx;
88
	uint8_t				digest[32];
89
90
	struct sb_key_dictionary_key	sb_dict_key;

91
	struct sb_boot_image_header	payload;
92
93
};

94
95
96
97
98
99
/*
 * Instruction semantics:
 * NOOP
 * TAG [LAST]
 * LOAD       address file
 * LOAD  IVT  address IVT_entry_point
100
 * FILL address pattern length
Marek Vasut's avatar
Marek Vasut committed
101
 * JUMP [HAB] address [r0_arg]
102
 * CALL [HAB] address [r0_arg]
103
104
105
106
107
 * MODE mode
 *      For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
 *                         JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
 *      For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
 *                         JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
108
 */
Marek Vasut's avatar
Marek Vasut committed
109
110
111
112
113
114
115

/* Blank image key. */
static uint8_t image_key[16] = {0};

/*
 * AES libcrypto
 */
116
static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
Marek Vasut's avatar
Marek Vasut committed
117
{
118
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
119
120
	int ret;

Marek Vasut's avatar
Marek Vasut committed
121
122
123
124
125
	/* If there is no init vector, init vector is all zeroes. */
	if (!iv)
		iv = image_key;

	EVP_CIPHER_CTX_init(ctx);
126
127
128
129
	ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), image_key, iv, enc);
	if (ret == 1)
		EVP_CIPHER_CTX_set_padding(ctx, 0);
	return ret;
Marek Vasut's avatar
Marek Vasut committed
130
131
}

132
static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
Marek Vasut's avatar
Marek Vasut committed
133
134
			uint8_t *out_data, int in_len)
{
135
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
Marek Vasut's avatar
Marek Vasut committed
136
137
138
139
140
141
142
143
	int ret, outlen;
	uint8_t *outbuf;

	outbuf = malloc(in_len);
	if (!outbuf)
		return -ENOMEM;
	memset(outbuf, 0, sizeof(in_len));

144
	ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
Marek Vasut's avatar
Marek Vasut committed
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
	if (!ret) {
		ret = -EINVAL;
		goto err;
	}

	if (out_data)
		memcpy(out_data, outbuf, outlen);

err:
	free(outbuf);
	return ret;
}

static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
{
	return EVP_CIPHER_CTX_cleanup(ctx);
}

163
static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
Marek Vasut's avatar
Marek Vasut committed
164
165
{
	int ret;
166
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
167
168
	struct sb_boot_image_header *sb_header = &ictx->payload;
	uint8_t *iv = sb_header->iv;
169

Marek Vasut's avatar
Marek Vasut committed
170
171
172
	ret = sb_aes_deinit(ctx);
	if (!ret)
		return ret;
173
	return sb_aes_init(ictx, iv, enc);
Marek Vasut's avatar
Marek Vasut committed
174
175
176
177
178
}

/*
 * CRC32
 */
179
static uint32_t crc32(uint8_t *data, uint32_t len)
Marek Vasut's avatar
Marek Vasut committed
180
181
182
{
	const uint32_t poly = 0x04c11db7;
	uint32_t crc32 = 0xffffffff;
183
	unsigned int byte, bit;
Marek Vasut's avatar
Marek Vasut committed
184
185
186
187
188

	for (byte = 0; byte < len; byte++) {
		crc32 ^= data[byte] << 24;

		for (bit = 8; bit > 0; bit--) {
189
			if (crc32 & (1UL << 31))
Marek Vasut's avatar
Marek Vasut committed
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
				crc32 = (crc32 << 1) ^ poly;
			else
				crc32 = (crc32 << 1);
		}
	}

	return crc32;
}

/*
 * Code
 */
static time_t sb_get_timestamp(void)
{
	struct tm time_2000 = {
		.tm_yday	= 1,	/* Jan. 1st */
		.tm_year	= 100,	/* 2000 */
	};
	time_t seconds_to_2000 = mktime(&time_2000);
	time_t seconds_to_now = time(NULL);

	return seconds_to_now - seconds_to_2000;
}

214
215
216
217
218
219
220
221
222
223
224
225
226
static int sb_get_time(time_t time, struct tm *tm)
{
	struct tm time_2000 = {
		.tm_yday	= 1,	/* Jan. 1st */
		.tm_year	= 0,	/* 1900 */
	};
	const time_t seconds_to_2000 = mktime(&time_2000);
	const time_t seconds_to_now = seconds_to_2000 + time;
	struct tm *ret;
	ret = gmtime_r(&seconds_to_now, tm);
	return ret ? 0 : -EINVAL;
}

227
static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
Marek Vasut's avatar
Marek Vasut committed
228
{
229
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
230
	struct sb_boot_image_header *sb_header = &ictx->payload;
Marek Vasut's avatar
Marek Vasut committed
231
232
233
	uint8_t *sb_header_ptr = (uint8_t *)sb_header;

	/* Encrypt the header, compute the digest. */
234
	sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
Marek Vasut's avatar
Marek Vasut committed
235
236
237
	EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
}

238
static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx,
239
		uint8_t cbc_mac[sizeof(struct sb_key_dictionary_key)])
Marek Vasut's avatar
Marek Vasut committed
240
{
241
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
242
	struct sb_section_ctx *sctx = ictx->sect_head;
243
244
245
246
247
248
249
	struct sb_sections_header *shdr;
	uint8_t *sb_sections_header_ptr;
	const int size = sizeof(*shdr);

	while (sctx) {
		shdr = &sctx->payload;
		sb_sections_header_ptr = (uint8_t *)shdr;
Marek Vasut's avatar
Marek Vasut committed
250

251
		sb_aes_crypt(ictx, sb_sections_header_ptr, cbc_mac, size);
252
		EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
Marek Vasut's avatar
Marek Vasut committed
253

254
255
		sctx = sctx->sect;
	};
Marek Vasut's avatar
Marek Vasut committed
256
257
}

258
static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx,
Marek Vasut's avatar
Marek Vasut committed
259
260
		uint8_t cbc_mac[sizeof(struct sb_key_dictionary_key)])
{
261
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
262
	struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
263

Marek Vasut's avatar
Marek Vasut committed
264
265
266
267
268
269
	/*
	 * The key in the key dictionary contains CBC-MAC from the SB image
	 * header and SB sections header.
	 */
	memcpy(sb_dict_key->cbc_mac, cbc_mac, sizeof(sb_dict_key->cbc_mac));

270
	sb_aes_crypt(ictx, image_key, sb_dict_key->key, 16);
Marek Vasut's avatar
Marek Vasut committed
271
272
273
	EVP_DigestUpdate(md_ctx, sb_dict_key, sizeof(*sb_dict_key));
}

274
static void sb_encrypt_tag(struct sb_image_ctx *ictx,
275
		struct sb_cmd_ctx *cctx)
276
{
277
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
278
279
	struct sb_command *cmd = &cctx->payload;

280
	sb_aes_crypt(ictx, (uint8_t *)cmd,
281
282
		(uint8_t *)&cctx->c_payload, sizeof(*cmd));
	EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
Marek Vasut's avatar
Marek Vasut committed
283
284
}

285
static int sb_encrypt_image(struct sb_image_ctx *ictx)
Marek Vasut's avatar
Marek Vasut committed
286
287
{
	/* Start image-wide crypto. */
288
289
	EVP_MD_CTX_init(&ictx->md_ctx);
	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
Marek Vasut's avatar
Marek Vasut committed
290
291
292
293

	/*
	 * SB image header.
	 */
294
	sb_aes_init(ictx, NULL, 1);
295
	sb_encrypt_sb_header(ictx);
Marek Vasut's avatar
Marek Vasut committed
296
297
298
299
300
301

	/*
	 * SB sections header.
	 */
	uint8_t sb_section_header_cbc_mac[sizeof(struct sb_key_dictionary_key)];

302
	sb_encrypt_sb_sections_header(ictx, sb_section_header_cbc_mac);
Marek Vasut's avatar
Marek Vasut committed
303
304
305
306

	/*
	 * Key dictionary.
	 */
307
	sb_aes_reinit(ictx, 1);
Marek Vasut's avatar
Marek Vasut committed
308

309
	sb_encrypt_key_dictionary_key(ictx, sb_section_header_cbc_mac);
Marek Vasut's avatar
Marek Vasut committed
310
311

	/*
312
	 * Section tags.
Marek Vasut's avatar
Marek Vasut committed
313
	 */
314
315
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
316
	struct sb_section_ctx *sctx = ictx->sect_head;
Marek Vasut's avatar
Marek Vasut committed
317

318
	while (sctx) {
319
		cctx = sctx->cmd_head;
320

321
		sb_aes_reinit(ictx, 1);
322

323
324
325
		while (cctx) {
			ccmd = &cctx->payload;

326
			sb_encrypt_tag(ictx, cctx);
327
328

			if (ccmd->header.tag == ROM_TAG_CMD) {
329
				sb_aes_reinit(ictx, 1);
330
			} else if (ccmd->header.tag == ROM_LOAD_CMD) {
331
				sb_aes_crypt(ictx, cctx->data, cctx->data, cctx->length);
332
333
334
335
				EVP_DigestUpdate(&ictx->md_ctx, cctx->data, cctx->length);
			}

			cctx = cctx->cmd;
Marek Vasut's avatar
Marek Vasut committed
336
		}
337
338
339
340

		sctx = sctx->sect;
	};

Marek Vasut's avatar
Marek Vasut committed
341
342
343
	/*
	 * Dump the SHA1 of the whole image.
	 */
344
	sb_aes_reinit(ictx, 1);
Marek Vasut's avatar
Marek Vasut committed
345

346
	EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
347
	sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
Marek Vasut's avatar
Marek Vasut committed
348
349

	/* Stop the encryption session. */
350
	sb_aes_deinit(&ictx->cipher_ctx);
Marek Vasut's avatar
Marek Vasut committed
351

352
353
354
	return 0;
}

355
static int sb_write_image(struct sb_image_ctx *ictx)
356
357
358
359
360
{
	struct sb_boot_image_header *sb_header = &ictx->payload;
	struct sb_section_ctx *sctx;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
361
	struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
362

363
364
	int fd;
	ssize_t ret;
365

366
	fd = open(ictx->output_filename, O_CREAT | O_WRONLY, 0644);
367
368
369
370
371
372
373
	if (fd < 0)
		return -EINVAL;


	ret = write(fd, sb_header, sizeof(*sb_header));
	if (ret != sizeof(*sb_header))
		goto err;
374

375
	sctx = ictx->sect_head;
376
	while (sctx) {
377
378
379
		ret = write(fd, &sctx->payload, sizeof(struct sb_sections_header));
		if (ret != sizeof(struct sb_sections_header))
			goto err;
380
381
		sctx = sctx->sect;
	};
382
383
384
385
386

	ret = write(fd, sb_dict_key, sizeof(*sb_dict_key));
	if (ret != sizeof(*sb_dict_key))
		goto err;

387
	sctx = ictx->sect_head;
388
	while (sctx) {
389
		cctx = sctx->cmd_head;
390
391
392
		while (cctx) {
			ccmd = &cctx->payload;

393
394
395
396
397
398
399
400
401
			ret = write(fd, &cctx->c_payload, sizeof(cctx->payload));
			if (ret != sizeof(cctx->payload))
				goto err;

			if (ccmd->header.tag == ROM_LOAD_CMD) {
				ret = write(fd, cctx->data, cctx->length);
				if (ret != cctx->length)
					goto err;
			}
402
403
404
405
406
407
408

			cctx = cctx->cmd;
		}

		sctx = sctx->sect;
	};

409
410
411
	ret = write(fd, ictx->digest, sizeof(ictx->digest));
	if (ret != sizeof(ictx->digest))
		goto err;
Marek Vasut's avatar
Marek Vasut committed
412
413

	return 0;
414
415
416
417
err:
	fprintf(stderr, "Failed to write output file!\n");
	close(fd);
	return -EINVAL;
Marek Vasut's avatar
Marek Vasut committed
418
419
}

420
static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
421
422
423
424
425
{
	off_t real_size, roundup_size;
	uint8_t *data;
	int fd, ret;

426
	if (!filename) {
427
428
429
430
		fprintf(stderr, "ERR: Missing filename!\n");
		return -EINVAL;
	}

431
	fd = open(filename, O_RDONLY);
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
	if (fd < 0)
		goto err_open;

	real_size = lseek(fd, 0, SEEK_END);
	if (real_size < 0)
		goto err_file;

	lseek(fd, 0, SEEK_SET);

	roundup_size = roundup(real_size, SB_BLOCK_SIZE);
	data = calloc(1, roundup_size);
	if (!data)
		goto err_file;

	ret = read(fd, data, real_size);
	if (ret != real_size)
		goto err_alloc;

	cctx->data = data;
	cctx->length = roundup_size;

	close(fd);
	return 0;

err_alloc:
	free(data);
err_file:
	close(fd);
err_open:
461
	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
462
463
464
	return -EINVAL;
}

Marek Vasut's avatar
Marek Vasut committed
465
466
467
468
469
470
471
472
473
474
475
476
static uint8_t sb_command_checksum(struct sb_command *inst)
{
	uint8_t *inst_ptr = (uint8_t *)inst;
	uint8_t csum = 0;
	unsigned int i;

	for(i = 0; i < sizeof(struct sb_command); i++)
		csum += inst_ptr[i];

	return csum;
}

477
static int sb_token_to_long(char *tok, uint32_t *rid)
478
479
{
	char *endptr;
480
	unsigned long id;
481
482
483
484
485
486
487
488

	if (tok[0] != '0' || tok[1] != 'x') {
		fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
		return -EINVAL;
	}

	tok += 2;

489
490
491
492
493
494
495
496
497
	id = strtoul(tok, &endptr, 16);
	if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
		fprintf(stderr, "ERR: Value can't be decoded!\n");
		return -EINVAL;
	}

	/* Check for 32-bit overflow. */
	if (id > 0xffffffff) {
		fprintf(stderr, "ERR: Value too big!\n");
498
499
500
501
		return -EINVAL;
	}

	if (endptr == tok) {
502
		fprintf(stderr, "ERR: Deformed value!\n");
503
504
505
		return -EINVAL;
	}

506
	*rid = (uint32_t)id;
Marek Vasut's avatar
Marek Vasut committed
507
	return 0;
508
509
}

510
static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
511
{
512
513
	struct sb_section_ctx *sctx;
	struct sb_sections_header *shdr;
514
	char *tok;
515
	uint32_t bootable = 0;
516
	uint32_t id;
517
	int ret;
518

519
520
	sctx = calloc(1, sizeof(*sctx));
	if (!sctx)
521
		return -ENOMEM;
522

523
524
525
526
527
528
529
	/* Read section number. */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Section without number!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err_sect;
530
	}
531

532
	/* Parse the section number. */
Marek Vasut's avatar
Marek Vasut committed
533
534
	ret = sb_token_to_long(tok, &id);
	if (ret) {
535
536
537
		fprintf(stderr, "#%i ERR: Malformed section number!\n",
			cmd->lineno);
		goto err_sect;
538
539
	}

540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
	/* Read section's BOOTABLE flag. */
	tok = strtok(NULL, " ");
	if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
		bootable = SB_SECTION_FLAG_BOOTABLE;

	sctx->boot = bootable;

	shdr = &sctx->payload;
	shdr->section_number = id;
	shdr->section_flags = bootable;

	/*
	 * The section is now constructed. Append it to the list.
	 * WARNING: The section size is still not computed and will
	 * be updated while parsing it's commands.
	 */
556
557
558
559
560
561
562
563
564
565
566
567
568
	ictx->sect_count++;

	/* Mark that this section is bootable one. */
	if (bootable) {
		if (ictx->sect_boot_found) {
			fprintf(stderr, "#%i WARN: Multiple bootable section!\n",
				cmd->lineno);
		} else {
			ictx->sect_boot = id;
			ictx->sect_boot_found = 1;
		}
	}

569
570
571
572
573
574
	if (!ictx->sect_head) {
		ictx->sect_head = sctx;
		ictx->sect_tail = sctx;
	} else {
		ictx->sect_tail->sect = sctx;
		ictx->sect_tail = sctx;
575
576
	}

577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
	return 0;

err_sect:
	free(sctx);
	return ret;
}

static int sb_build_command_nop(struct sb_image_ctx *ictx)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_NOP_CMD;

	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
613
614
	}

615
616
	return 0;
}
617

618
619
620
621
622
623
static int sb_build_command_tag(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
624

625
626
627
	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;
628

629
	ccmd = &cctx->payload;
630

631
632
633
634
635
636
637
	/*
	 * Prepare the command.
	 */
	/* Check for the LAST keyword. */
	tok = strtok(cmd->cmd, " ");
	if (tok && !strcmp(tok, "LAST"))
		ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
638

639
640
641
642
643
	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_TAG_CMD;
644

645
646
647
648
649
650
651
652
653
654
655
656
	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}
657

658
659
	return 0;
}
660

661
662
663
664
665
666
667
static int sb_build_command_load(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
	int ret, is_ivt = 0;
668
	uint32_t dest;
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
			cmd->lineno);
		ret = -EINVAL;
684
		goto err;
685
686
687
688
689
	}

	/* Check for "IVT" flag. */
	if (!strcmp(tok, "IVT")) {
		is_ivt = 1;
690
		tok = strtok(NULL, " ");
691
		if (!tok) {
692
693
			fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
				cmd->lineno);
694
695
696
			ret = -EINVAL;
			goto err;
		}
697
	}
698

699
	/* Read load destination address. */
Marek Vasut's avatar
Marek Vasut committed
700
701
	ret = sb_token_to_long(tok, &dest);
	if (ret) {
702
703
		fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
			cmd->lineno);
704
		goto err;
705
706
	}

707
708
709
710
711
712
713
714
	/* Read filename or IVT entrypoint. */
	tok = strtok(NULL, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing LOAD filename or IVT ep!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}
715

716
717
718
	if (is_ivt) {
		/* Handle IVT. */
		struct sb_ivt_header *ivt;
719
		uint32_t ivtep;
Marek Vasut's avatar
Marek Vasut committed
720
		ret = sb_token_to_long(tok, &ivtep);
721

Marek Vasut's avatar
Marek Vasut committed
722
		if (ret) {
723
724
725
			fprintf(stderr,
				"#%i ERR: Incorrect IVT entry point!\n",
				cmd->lineno);
726
			goto err;
727
		}
728

729
730
731
732
733
		ivt = calloc(1, sizeof(*ivt));
		if (!ivt) {
			ret = -ENOMEM;
			goto err;
		}
734

735
736
737
738
739
740
741
742
743
744
745
746
		ivt->header = SB_HAB_IVT_HEADER(sizeof(*ivt)),
		ivt->entry = ivtep,
		ivt->self = dest,

		cctx->data = (uint8_t *)ivt;
		cctx->length = sizeof(*ivt);
	} else {
		/* Regular LOAD of a file. */
		ret = sb_load_file(cctx, tok);
		if (ret) {
			fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
				cmd->lineno, tok);
747
			goto err;
748
749
		}
	}
750

751
752
753
754
	if (cctx->length & (SB_BLOCK_SIZE - 1)) {
		fprintf(stderr, "#%i ERR: Unaligned payload!\n",
			cmd->lineno);
	}
755

756
757
758
759
760
761
762
763
	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_LOAD_CMD;
	ccmd->load.address	= dest;
	ccmd->load.count	= cctx->length;
	ccmd->load.crc32	= crc32(cctx->data, cctx->length);
764

765
	cctx->size = sizeof(*ccmd) + cctx->length;
766

767
768
769
770
771
772
773
774
775
776
	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}
777
778
779

	return 0;

780
781
782
err:
	free(cctx);
	return ret;
783
784
}

785
786
787
788
789
790
static int sb_build_command_fill(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
791
	uint32_t address, pattern, length;
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
	int ret;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing FILL address!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Read fill destination address. */
Marek Vasut's avatar
Marek Vasut committed
812
813
	ret = sb_token_to_long(tok, &address);
	if (ret) {
814
815
816
817
818
819
820
821
822
823
824
825
826
827
		fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
			cmd->lineno);
		goto err;
	}

	tok = strtok(NULL, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Read fill pattern address. */
Marek Vasut's avatar
Marek Vasut committed
828
829
	ret = sb_token_to_long(tok, &pattern);
	if (ret) {
830
831
832
833
834
835
836
837
838
839
840
841
842
843
		fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
			cmd->lineno);
		goto err;
	}

	tok = strtok(NULL, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing FILL length!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/* Read fill pattern address. */
Marek Vasut's avatar
Marek Vasut committed
844
845
	ret = sb_token_to_long(tok, &length);
	if (ret) {
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
		fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
			cmd->lineno);
		goto err;
	}

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_FILL_CMD;

	ccmd->fill.address	= address;
	ccmd->fill.count	= length;
	ccmd->fill.pattern	= pattern;

	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}

	return 0;

err:
	free(cctx);
	return ret;
}

Marek Vasut's avatar
Marek Vasut committed
881
882
883
static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
				      struct sb_cmd_list *cmd,
				      unsigned int is_call)
884
{
885
886
887
888
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
889
	uint32_t dest, arg = 0x0;
890
	uint32_t hab = 0;
891
	int ret;
Marek Vasut's avatar
Marek Vasut committed
892
	const char *cmdname = is_call ? "CALL" : "JUMP";
893

894
895
896
	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;
897

898
	ccmd = &cctx->payload;
Marek Vasut's avatar
Marek Vasut committed
899

900
901
902
903
904
905
	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr,
Marek Vasut's avatar
Marek Vasut committed
906
907
			"#%i ERR: Missing %s address or 'HAB'!\n",
			cmd->lineno, cmdname);
908
909
910
		ret = -EINVAL;
		goto err;
	}
911

912
913
	/* Check for "HAB" flag. */
	if (!strcmp(tok, "HAB")) {
Marek Vasut's avatar
Marek Vasut committed
914
		hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
915
916
		tok = strtok(NULL, " ");
		if (!tok) {
Marek Vasut's avatar
Marek Vasut committed
917
918
			fprintf(stderr, "#%i ERR: Missing %s address!\n",
				cmd->lineno, cmdname);
919
920
921
922
923
			ret = -EINVAL;
			goto err;
		}
	}
	/* Read load destination address. */
Marek Vasut's avatar
Marek Vasut committed
924
925
	ret = sb_token_to_long(tok, &dest);
	if (ret) {
Marek Vasut's avatar
Marek Vasut committed
926
927
		fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
			cmd->lineno, cmdname);
928
		goto err;
Marek Vasut's avatar
Marek Vasut committed
929
930
	}

931
932
	tok = strtok(NULL, " ");
	if (tok) {
Marek Vasut's avatar
Marek Vasut committed
933
934
		ret = sb_token_to_long(tok, &arg);
		if (ret) {
935
			fprintf(stderr,
Marek Vasut's avatar
Marek Vasut committed
936
937
				"#%i ERR: Incorrect %s argument!\n",
				cmd->lineno, cmdname);
938
939
940
			goto err;
		}
	}
941

942
943
944
945
	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
Marek Vasut's avatar
Marek Vasut committed
946
	ccmd->header.tag	= is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
947
	ccmd->header.flags	= hab;
948

949
950
	ccmd->call.address	= dest;
	ccmd->call.argument	= arg;
Marek Vasut's avatar
Marek Vasut committed
951

952
	cctx->size = sizeof(*ccmd);
Marek Vasut's avatar
Marek Vasut committed
953

954
955
956
957
958
959
960
961
962
	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
Marek Vasut's avatar
Marek Vasut committed
963
964
	}

965
966
967
968
	return 0;

err:
	free(cctx);
969
	return ret;
970
971
}

Marek Vasut's avatar
Marek Vasut committed
972
973
974
975
976
977
978
979
980
981
static int sb_build_command_jump(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	return sb_build_command_jump_call(ictx, cmd, 0);
}

static int sb_build_command_call(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	return sb_build_command_jump_call(ictx, cmd, 1);
}

982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
static int sb_build_command_mode(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
	int ret;
	unsigned int i;
	uint32_t mode = 0xffffffff;

	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;

	ccmd = &cctx->payload;

	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

1009
	for (i = 0; i < ARRAY_SIZE(modetable); i++) {
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
		if (!strcmp(tok, modetable[i].name)) {
			mode = modetable[i].mode;
			break;
		}

		if (!modetable[i].altname)
			continue;

		if (!strcmp(tok, modetable[i].altname)) {
			mode = modetable[i].mode;
			break;
		}
	}

	if (mode == 0xffffffff) {
		fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}

	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_MODE_CMD;

	ccmd->mode.mode		= mode;

	cctx->size = sizeof(*ccmd);

	/*
	 * Append the command to the last section.
	 */
	if (!sctx->cmd_head) {
		sctx->cmd_head = cctx;
		sctx->cmd_tail = cctx;
	} else {
		sctx->cmd_tail->cmd = cctx;
		sctx->cmd_tail = cctx;
	}

	return 0;

err:
	free(cctx);
	return ret;
}

1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
static int sb_prefill_image_header(struct sb_image_ctx *ictx)
{
	struct sb_boot_image_header *hdr = &ictx->payload;

	/* Fill signatures */
	memcpy(hdr->signature1, "STMP", 4);
	memcpy(hdr->signature2, "sgtl", 4);

	/* SB Image version 1.1 */
	hdr->major_version = SB_VERSION_MAJOR;
	hdr->minor_version = SB_VERSION_MINOR;

	/* Boot image major version */
1072
1073
1074
	hdr->product_version.major = htons(0x999);
	hdr->product_version.minor = htons(0x999);
	hdr->product_version.revision = htons(0x999);
1075
	/* Boot image major version */
1076
1077
1078
	hdr->component_version.major = htons(0x999);
	hdr->component_version.minor = htons(0x999);
	hdr->component_version.revision = htons(0x999);
1079
1080
1081
1082
1083
1084
1085
1086
1087

	/* Drive tag must be 0x0 for i.MX23 */
	hdr->drive_tag = 0;

	hdr->header_blocks = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
	hdr->section_header_size = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
	hdr->timestamp_us = sb_get_timestamp() * 1000000;

	/* FIXME -- add proper config option */
1088
	hdr->flags = ictx->verbose_boot ? SB_IMAGE_FLAG_VERBOSE : 0,
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098

	/* FIXME -- We support only default key */
	hdr->key_count = 1;

	return 0;
}

static int sb_postfill_image_header(struct sb_image_ctx *ictx)
{
	struct sb_boot_image_header *hdr = &ictx->payload;
1099
	struct sb_section_ctx *sctx = ictx->sect_head;
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
	uint32_t kd_size, sections_blocks;
	EVP_MD_CTX md_ctx;

	/* The main SB header size in blocks. */
	hdr->image_blocks = hdr->header_blocks;

	/* Size of the key dictionary, which has single zero entry. */
	kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
	hdr->image_blocks += kd_size / SB_BLOCK_SIZE;

	/* Now count the payloads. */
1111
	hdr->section_count = ictx->sect_count;
1112
	while (sctx) {
1113
1114
		hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
		sctx = sctx->sect;
1115
1116
	}

1117
1118
1119
1120
1121
	if (!ictx->sect_boot_found) {
		fprintf(stderr, "ERR: No bootable section selected!\n");
		return -EINVAL;
	}
	hdr->first_boot_section_id = ictx->sect_boot;
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148

	/* The n * SB section size in blocks. */
	sections_blocks = hdr->section_count * hdr->section_header_size;
	hdr->image_blocks += sections_blocks;

	/* Key dictionary offset. */
	hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;

	/* Digest of the whole image. */
	hdr->image_blocks += 2;

	/* Pointer past the dictionary. */
	hdr->first_boot_tag_block =
		hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;

	/* Compute header digest. */
	EVP_MD_CTX_init(&md_ctx);

	EVP_DigestInit(&md_ctx, EVP_sha1());
	EVP_DigestUpdate(&md_ctx, hdr->signature1,
			sizeof(struct sb_boot_image_header) -
			sizeof(hdr->digest));
	EVP_DigestFinal(&md_ctx, hdr->digest, NULL);

	return 0;
}

1149
1150
1151
1152
static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
{
	/* Fixup the placement of sections. */
	struct sb_boot_image_header *ihdr = &ictx->payload;
1153
	struct sb_section_ctx *sctx = ictx->sect_head;
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
	struct sb_sections_header *shdr;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	uint32_t offset = ihdr->first_boot_tag_block;

	while (sctx) {
		shdr = &sctx->payload;

		/* Fill in the section TAG offset. */
		shdr->section_offset = offset + 1;
		offset += shdr->section_size;

		/* Section length is measured from the TAG block. */
		shdr->section_size--;

		/* Fixup the TAG command. */
1170
		cctx = sctx->cmd_head;
1171
1172
1173
1174
1175
1176
1177
		while (cctx) {
			ccmd = &cctx->payload;
			if (ccmd->header.tag == ROM_TAG_CMD) {
				ccmd->tag.section_number = shdr->section_number;
				ccmd->tag.section_length = shdr->section_size;
				ccmd->tag.section_flags = shdr->section_flags;
			}
1178
1179
1180
1181

			/* Update the command checksum. */
			ccmd->header.checksum = sb_command_checksum(ccmd);

1182
1183
1184
1185
1186
1187
1188
1189
1190
			cctx = cctx->cmd;
		}

		sctx = sctx->sect;
	}

	return 0;
}

1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
{
	char *tok;
	char *line = cmd->cmd;
	char *rptr;
	int ret;

	/* Analyze the identifier on this line first. */
	tok = strtok_r(line, " ", &rptr);
	if (!tok || (strlen(tok) == 0)) {
		fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
		return -EINVAL;
	}

	cmd->cmd = rptr;

	/* Section */
	if (!strcmp(tok, "SECTION")) {
		sb_build_section(ictx, cmd);
		return 0;
	}

	if (!ictx->sect_tail) {
		fprintf(stderr, "#%i ERR: Data outside of a section!\n",
			cmd->lineno);
		return -EINVAL;
	}

	/* Commands */
	if (!strcmp(tok, "NOP"))
		ret = sb_build_command_nop(ictx);
	else if (!strcmp(tok, "TAG"))
		ret = sb_build_command_tag(ictx, cmd);
	else if (!strcmp(tok, "LOAD"))
		ret = sb_build_command_load(ictx, cmd);
1226
1227
	else if (!strcmp(tok, "FILL"))
		ret = sb_build_command_fill(ictx, cmd);
Marek Vasut's avatar
Marek Vasut committed
1228
1229
	else if (!strcmp(tok, "JUMP"))
		ret = sb_build_command_jump(ictx, cmd);
1230
1231
	else if (!strcmp(tok, "CALL"))
		ret = sb_build_command_call(ictx, cmd);
1232
1233
	else if (!strcmp(tok, "MODE"))
		ret = sb_build_command_mode(ictx, cmd);
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
	else {
		fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
			cmd->lineno, tok);
		return -ENOTSUP;
	}

	/*
	 * Here we have at least one section with one command, otherwise we
	 * would have failed already higher above.
	 *
	 * FIXME -- should the updating happen here ?
	 */
	if (!ret) {
		ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
		ictx->sect_tail->payload.section_size = ictx->sect_tail->size / SB_BLOCK_SIZE;
	}

	return ret;
}

1254
1255
static int sb_load_cmdfile(struct sb_image_ctx *ictx)
{
1256
	struct sb_cmd_list cmd;
1257
	int fd, lineno = 1;
1258
1259
1260
	FILE *fp;
	char *line = NULL;
	ssize_t rlen;
1261
	size_t len;
1262

1263
1264
1265
1266
1267
1268
1269
1270
1271
	fd = open(ictx->cfg_filename, O_RDONLY);
	if (fd < 0)
		goto err_open;

	fp = fdopen(fd, "r");
	if (!fp)
		goto err_file;

	while ((rlen = getline(&line, &len, fp)) > 0) {
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
		memset(&cmd, 0, sizeof(cmd));

		/* Strip the trailing newline. */
		line[rlen - 1] = '\0';

		cmd.cmd = line;
		cmd.len = rlen;
		cmd.lineno = lineno++;

		sb_parse_line(ictx, &cmd);
1282
1283
	}

1284
1285
1286
1287
	free(line);

	fclose(fp);

1288
	return 0;
1289

1290
1291
1292
1293
1294
1295
err_file:
	fclose(fp);
err_open:
	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", ictx->cfg_filename);
	return -EINVAL;

1296
1297
}

1298
static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
1299
1300
1301
{
	int ret;

1302
1303
1304
1305
	ret = sb_load_cmdfile(ictx);
	if (ret)
		return ret;

1306
1307
1308
1309
1310
	ret = sb_prefill_image_header(ictx);
	if (ret)
		return ret;

	ret = sb_postfill_image_header(ictx);
Marek Vasut's avatar
Marek Vasut committed
1311
	if (ret)
1312
1313
		return ret;

1314
1315
1316
1317
	ret = sb_fixup_sections_and_tags(ictx);
	if (ret)
		return ret;

1318
1319
1320
	return 0;
}

1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
static int sb_verify_image_header(struct sb_image_ctx *ictx,
				  int fd, off_t fsize)
{
	/* Verify static fields in the image header. */
	struct sb_boot_image_header *hdr = &ictx->payload;
	const char *stat[2] = { "[PASS]", "[FAIL]" };
	struct tm tm;
	int sz, ret = 0;
	unsigned char digest[20];
	EVP_MD_CTX md_ctx;
	ssize_t size;

	/* Start image-wide crypto. */
	EVP_MD_CTX_init(&ictx->md_ctx);
	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());

	fprintf(stdout, "---------- Verifying SB Image Header ----------\n");

	size = read(fd, &ictx->payload, sizeof(ictx->payload));
	if (size != sizeof(ictx->payload)) {
		fprintf(stderr, "ERR: SB image header too short!\n");
		return -EINVAL;
	}

	/* Compute header digest. */
	EVP_MD_CTX_init(&md_ctx);
	EVP_DigestInit(&md_ctx, EVP_sha1());
	EVP_DigestUpdate(&md_ctx, hdr->signature1,
			sizeof(struct sb_boot_image_header) -
			sizeof(hdr->digest));
	EVP_DigestFinal(&md_ctx, digest, NULL);

	sb_aes_init(ictx, NULL, 1);
	sb_encrypt_sb_header(ictx);

	if (memcmp(digest, hdr->digest, 20))
		ret = -EINVAL;
	fprintf(stdout, "%s Image header checksum:        %s\n", stat[!!ret],
		ret ? "BAD" : "OK");
	if (ret)
		return ret;

	if (memcmp(hdr->signature1, "STMP", 4) ||
	    memcmp(hdr->signature2, "sgtl", 4))
		ret = -EINVAL;
	fprintf(stdout, "%s Signatures:                   '%.4s' '%.4s'\n",
		stat[!!ret], hdr->signature1, hdr->signature2);
	if (ret)
		return ret;

	if ((hdr->major_version != SB_VERSION_MAJOR) ||
	    ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
		ret = -EINVAL;
	fprintf(stdout, "%s Image version:                v%i.%i\n", stat[!!ret],
		hdr->major_version, hdr->minor_version);
	if (ret)
		return ret;

	ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
	fprintf(stdout, "%s Creation time:                "
			"%02i:%02i:%02i %02i/%02i/%04i\n",
		stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
		tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
	if (ret)
		return ret;

	fprintf(stdout, "%s Product version:              %x.%x.%x\n", stat[0],
1388
1389
1390
		ntohs(hdr->product_version.major),
		ntohs(hdr->product_version.minor),
		ntohs(hdr->product_version.revision));
1391
	fprintf(stdout, "%s Component version:            %x.%x.%x\n", stat[0],
1392
1393
1394
		ntohs(hdr->component_version.major),
		ntohs(hdr->component_version.minor),
		ntohs(hdr->component_version.revision));
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471
1472
1473
1474
1475
1476
1477
1478
1479
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498
1499
1500
1501
1502
1503
1504
1505
1506
1507
1508
1509
1510
1511
1512
1513
1514
1515
1516
1517
1518
1519
1520
1521
1522
1523

	if (hdr->flags & ~SB_IMAGE_FLAG_VERBOSE)
		ret = -EINVAL;
	fprintf(stdout, "%s Image flags:                  %s\n", stat[!!ret],
		hdr->flags & SB_IMAGE_FLAG_VERBOSE ? "Verbose_boot" : "");
	if (ret)
		return ret;

	if (hdr->drive_tag != 0)
		ret = -EINVAL;
	fprintf(stdout, "%s Drive tag:                    %i\n", stat[!!ret],
		hdr->drive_tag);
	if (ret)
		return ret;

	sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
	if (hdr->header_blocks != sz)
		ret = -EINVAL;
	fprintf(stdout, "%s Image header size (blocks):   %i\n", stat[!!ret],
		hdr->header_blocks);
	if (ret)
		return ret;

	sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
	if (hdr->section_header_size != sz)
		ret = -EINVAL;
	fprintf(stdout, "%s Section header size (blocks): %i\n", stat[!!ret],
		hdr->section_header_size);
	if (ret)
		return ret;

	fprintf(stdout, "%s Sections count:               %i\n", stat[!!ret],
		hdr->section_count);
	fprintf(stdout, "%s First bootable section        %i\n", stat[!!ret],
		hdr->first_boot_section_id);

	if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)