mxssb.c 24.2 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
28
29

#include <asm/byteorder.h>

#include <openssl/evp.h>

#include "mxssb.h"

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

46
struct sb_cmd_ctx {
47
48
	uint32_t			size;

49
50
51
52
53
54
	struct sb_cmd_ctx		*cmd;

	uint8_t				*data;
	uint32_t			length;

	struct sb_command		payload;
55
	struct sb_command		c_payload;
56
57
58
};

struct sb_section_ctx {
59
	uint32_t			size;
60

61
62
63
	/* Section flags */
	unsigned int			boot:1;

64
	struct sb_section_ctx		*sect;
65
66
67

	struct sb_cmd_ctx		*cmd_head;
	struct sb_cmd_ctx		*cmd_tail;
68
69
70
71

	struct sb_sections_header	payload;
};

72
struct sb_image_ctx {
73
	/* Image configuration */
74
	unsigned int			verbose_boot:1;
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
100
101
/*
 * Instruction semantics:
 * NOOP
 * TAG [LAST]
 * LOAD       address file
 * LOAD  IVT  address IVT_entry_point
 * CALL [HAB] address [r0_arg]
 */
Marek Vasut's avatar
Marek Vasut committed
102
103
104
105
106
107
108

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

/*
 * AES libcrypto
 */
109
static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv)
Marek Vasut's avatar
Marek Vasut committed
110
{
111
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
Marek Vasut's avatar
Marek Vasut committed
112
113
114
115
116
117
118
119
	/* If there is no init vector, init vector is all zeroes. */
	if (!iv)
		iv = image_key;

	EVP_CIPHER_CTX_init(ctx);
	return EVP_EncryptInit(ctx, EVP_aes_128_cbc(), image_key, iv);
}

120
static int sb_aes_encrypt(struct sb_image_ctx *ictx, uint8_t *in_data,
Marek Vasut's avatar
Marek Vasut committed
121
122
			uint8_t *out_data, int in_len)
{
123
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
Marek Vasut's avatar
Marek Vasut committed
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
	int ret, outlen;
	uint8_t *outbuf;

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

	ret = EVP_EncryptUpdate(ctx, outbuf, &outlen, in_data, in_len);
	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);
}

151
static int sb_aes_reinit(struct sb_image_ctx *ictx)
Marek Vasut's avatar
Marek Vasut committed
152
153
{
	int ret;
154
	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
155
156
	struct sb_boot_image_header *sb_header = &ictx->payload;
	uint8_t *iv = sb_header->iv;
157

Marek Vasut's avatar
Marek Vasut committed
158
159
160
	ret = sb_aes_deinit(ctx);
	if (!ret)
		return ret;
161
	return sb_aes_init(ictx, iv);
Marek Vasut's avatar
Marek Vasut committed
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
}

/*
 * CRC32
 */
static uint32_t crc32(uint8_t *data, int len)
{
	const uint32_t poly = 0x04c11db7;
	uint32_t crc32 = 0xffffffff;
	int byte, bit;

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

		for (bit = 8; bit > 0; bit--) {
			if (crc32 & (1 << 31))
				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;
}

202
static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
Marek Vasut's avatar
Marek Vasut committed
203
{
204
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
205
	struct sb_boot_image_header *sb_header = &ictx->payload;
Marek Vasut's avatar
Marek Vasut committed
206
207
208
	uint8_t *sb_header_ptr = (uint8_t *)sb_header;

	/* Encrypt the header, compute the digest. */
209
	sb_aes_encrypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
Marek Vasut's avatar
Marek Vasut committed
210
211
212
	EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
}

213
static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx,
214
		uint8_t cbc_mac[sizeof(struct sb_key_dictionary_key)])
Marek Vasut's avatar
Marek Vasut committed
215
{
216
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
217
	struct sb_section_ctx *sctx = ictx->sect_head;
218
219
220
221
222
223
224
	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
225

226
227
		sb_aes_encrypt(ictx, sb_sections_header_ptr, cbc_mac, size);
		EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
Marek Vasut's avatar
Marek Vasut committed
228

229
230
		sctx = sctx->sect;
	};
Marek Vasut's avatar
Marek Vasut committed
231
232
}

233
static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx,
Marek Vasut's avatar
Marek Vasut committed
234
235
		uint8_t cbc_mac[sizeof(struct sb_key_dictionary_key)])
{
236
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
237
	struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
238

Marek Vasut's avatar
Marek Vasut committed
239
240
241
242
243
244
	/*
	 * 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));

245
	sb_aes_encrypt(ictx, image_key, sb_dict_key->key, 16);
Marek Vasut's avatar
Marek Vasut committed
246
247
248
	EVP_DigestUpdate(md_ctx, sb_dict_key, sizeof(*sb_dict_key));
}

249
static void sb_encrypt_tag(struct sb_image_ctx *ictx,
250
		struct sb_cmd_ctx *cctx)
251
{
252
	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
253
254
255
256
257
	struct sb_command *cmd = &cctx->payload;

	sb_aes_encrypt(ictx, (uint8_t *)cmd,
		(uint8_t *)&cctx->c_payload, sizeof(*cmd));
	EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
Marek Vasut's avatar
Marek Vasut committed
258
259
}

260
static int sb_create_image(struct sb_image_ctx *ictx)
Marek Vasut's avatar
Marek Vasut committed
261
262
{
	/* Start image-wide crypto. */
263
264
	EVP_MD_CTX_init(&ictx->md_ctx);
	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
Marek Vasut's avatar
Marek Vasut committed
265
266
267
268

	/*
	 * SB image header.
	 */
269
	sb_aes_init(ictx, NULL);
270
	sb_encrypt_sb_header(ictx);
Marek Vasut's avatar
Marek Vasut committed
271
272
273
274
275
276

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

277
	sb_encrypt_sb_sections_header(ictx, sb_section_header_cbc_mac);
Marek Vasut's avatar
Marek Vasut committed
278
279
280
281

	/*
	 * Key dictionary.
	 */
282
	sb_aes_reinit(ictx);
Marek Vasut's avatar
Marek Vasut committed
283

284
	sb_encrypt_key_dictionary_key(ictx, sb_section_header_cbc_mac);
Marek Vasut's avatar
Marek Vasut committed
285
286

	/*
287
	 * Section tags.
Marek Vasut's avatar
Marek Vasut committed
288
	 */
289
290
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
291
	struct sb_section_ctx *sctx = ictx->sect_head;
Marek Vasut's avatar
Marek Vasut committed
292

293
	while (sctx) {
294
		cctx = sctx->cmd_head;
295

296
		sb_aes_reinit(ictx);
297

298
299
300
		while (cctx) {
			ccmd = &cctx->payload;

301
			sb_encrypt_tag(ictx, cctx);
302
303

			if (ccmd->header.tag == ROM_TAG_CMD) {
304
				sb_aes_reinit(ictx);
305
306
307
308
309
310
			} else if (ccmd->header.tag == ROM_LOAD_CMD) {
				sb_aes_encrypt(ictx, cctx->data, cctx->data, cctx->length);
				EVP_DigestUpdate(&ictx->md_ctx, cctx->data, cctx->length);
			}

			cctx = cctx->cmd;
Marek Vasut's avatar
Marek Vasut committed
311
		}
312
313
314
315

		sctx = sctx->sect;
	};

Marek Vasut's avatar
Marek Vasut committed
316
317
318
	/*
	 * Dump the SHA1 of the whole image.
	 */
319
	sb_aes_reinit(ictx);
Marek Vasut's avatar
Marek Vasut committed
320

321
322
	EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
	sb_aes_encrypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
Marek Vasut's avatar
Marek Vasut committed
323
324

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

327
328
329
	return 0;
}

330
char *output_filename = NULL;
Marek Vasut's avatar
Marek Vasut committed
331

332
static int sb_assemble_image(struct sb_image_ctx *ictx)
333
334
335
336
337
{
	struct sb_boot_image_header *sb_header = &ictx->payload;
	struct sb_section_ctx *sctx;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
338
	struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
339

340
341
	int fd;
	ssize_t ret;
342

343
344
345
346
347
348
349
350
	fd = open(output_filename, O_CREAT | O_WRONLY, 0644);
	if (fd < 0)
		return -EINVAL;


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

352
	sctx = ictx->sect_head;
353
	while (sctx) {
354
355
356
		ret = write(fd, &sctx->payload, sizeof(struct sb_sections_header));
		if (ret != sizeof(struct sb_sections_header))
			goto err;
357
358
		sctx = sctx->sect;
	};
359
360
361
362
363

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

364
	sctx = ictx->sect_head;
365
	while (sctx) {
366
		cctx = sctx->cmd_head;
367
368
369
		while (cctx) {
			ccmd = &cctx->payload;

370
371
372
373
374
375
376
377
378
			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;
			}
379
380
381
382
383
384
385

			cctx = cctx->cmd;
		}

		sctx = sctx->sect;
	};

386
387
388
	ret = write(fd, ictx->digest, sizeof(ictx->digest));
	if (ret != sizeof(ictx->digest))
		goto err;
Marek Vasut's avatar
Marek Vasut committed
389
390

	return 0;
391
392
393
394
err:
	fprintf(stderr, "Failed to write output file!\n");
	close(fd);
	return -EINVAL;
Marek Vasut's avatar
Marek Vasut committed
395
396
}

397
static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
398
399
400
401
402
{
	off_t real_size, roundup_size;
	uint8_t *data;
	int fd, ret;

403
	if (!filename) {
404
405
406
407
		fprintf(stderr, "ERR: Missing filename!\n");
		return -EINVAL;
	}

408
	fd = open(filename, O_RDONLY);
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
	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:
438
	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
439
440
441
	return -EINVAL;
}

Marek Vasut's avatar
Marek Vasut committed
442
443
444
445
446
447
448
449
450
451
452
453
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;
}

454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
static long sb_token_to_long(char *tok)
{
	char *endptr;
	long id;

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

	tok += 2;

	id = strtol(tok, &endptr, 16);
	if ((errno == ERANGE && (id == LONG_MAX || id == LONG_MIN)) ||
	    (errno != 0 && id == 0)) {
		fprintf(stderr, "ERR: Section number can't be decoded!\n");
		return -EINVAL;
	}

	if (endptr == tok) {
		fprintf(stderr, "ERR: Deformed section ID!\n");
		return -EINVAL;
	}

	return id;
}

481
static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
482
{
483
484
	struct sb_section_ctx *sctx;
	struct sb_sections_header *shdr;
485
	char *tok;
486
487
488
	uint32_t bootable = 0;
	long id;
	int ret;
489

490
491
	sctx = calloc(1, sizeof(*sctx));
	if (!sctx)
492
		return -ENOMEM;
493

494
495
496
497
498
499
500
	/* Read section number. */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr, "#%i ERR: Section without number!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err_sect;
501
	}
502

503
504
505
506
507
	/* Parse the section number. */
	id = sb_token_to_long(tok);
	if (id < 0) {
		fprintf(stderr, "#%i ERR: Malformed section number!\n",
			cmd->lineno);
508
		ret = -EINVAL;
509
		goto err_sect;
510
511
	}

512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
	/* 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.
	 */
528
529
530
531
532
533
534
535
536
537
538
539
540
	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;
		}
	}

541
542
543
544
545
546
	if (!ictx->sect_head) {
		ictx->sect_head = sctx;
		ictx->sect_tail = sctx;
	} else {
		ictx->sect_tail->sect = sctx;
		ictx->sect_tail = sctx;
547
548
	}

549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
	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;
585
586
	}

587
588
	return 0;
}
589

590
591
592
593
594
595
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;
596

597
598
599
	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;
600

601
	ccmd = &cctx->payload;
602

603
604
605
606
607
608
609
	/*
	 * 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;
610

611
612
613
614
615
	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_TAG_CMD;
616

617
618
619
620
621
622
623
624
625
626
627
628
	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;
	}
629

630
631
	return 0;
}
632

633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
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;
	long dest;

	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;
656
		goto err;
657
658
659
660
661
	}

	/* Check for "IVT" flag. */
	if (!strcmp(tok, "IVT")) {
		is_ivt = 1;
662
		tok = strtok(NULL, " ");
663
		if (!tok) {
664
665
			fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
				cmd->lineno);
666
667
668
			ret = -EINVAL;
			goto err;
		}
669
	}
670

671
672
673
674
675
676
	/* Read load destination address. */
	dest = sb_token_to_long(tok);
	if (dest < 0) {
		fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
			cmd->lineno);
		ret = -EINVAL;
677
		goto err;
678
679
	}

680
681
682
683
684
685
686
687
	/* 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;
	}
688

689
690
691
692
	if (is_ivt) {
		/* Handle IVT. */
		struct sb_ivt_header *ivt;
		long ivtep = sb_token_to_long(tok);
693

694
695
696
697
698
		if (ivtep < 0) {
			fprintf(stderr,
				"#%i ERR: Incorrect IVT entry point!\n",
				cmd->lineno);
			ret = -EINVAL;
699
			goto err;
700
		}
701

702
703
704
705
706
		ivt = calloc(1, sizeof(*ivt));
		if (!ivt) {
			ret = -ENOMEM;
			goto err;
		}
707

708
709
710
711
712
713
714
715
716
717
718
719
720
		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);
			ret = ret;
721
			goto err;
722
723
		}
	}
724

725
726
727
728
	if (cctx->length & (SB_BLOCK_SIZE - 1)) {
		fprintf(stderr, "#%i ERR: Unaligned payload!\n",
			cmd->lineno);
	}
729

730
731
732
733
734
735
736
737
	/*
	 * 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);
738

739
	cctx->size = sizeof(*ccmd) + cctx->length;
740

741
742
743
744
745
746
747
748
749
750
	/*
	 * 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;
	}
751
752
753

	return 0;

754
755
756
err:
	free(cctx);
	return ret;
757
758
}

759
static int sb_build_command_call(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
760
{
761
762
763
764
765
766
	struct sb_section_ctx *sctx = ictx->sect_tail;
	struct sb_cmd_ctx *cctx;
	struct sb_command *ccmd;
	char *tok;
	long dest, arg = 0x0;
	uint32_t hab = 0;
767
768
	int ret;

769
770
771
	cctx = calloc(1, sizeof(*cctx));
	if (!cctx)
		return -ENOMEM;
772

773
	ccmd = &cctx->payload;
Marek Vasut's avatar
Marek Vasut committed
774

775
776
777
778
779
780
781
782
783
784
785
	/*
	 * Prepare the command.
	 */
	tok = strtok(cmd->cmd, " ");
	if (!tok) {
		fprintf(stderr,
			"#%i ERR: Missing CALL address or 'HAB'!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
	}
786

787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
	/* Check for "HAB" flag. */
	if (!strcmp(tok, "HAB")) {
		hab = ROM_CALL_CMD_FLAG_HAB;
		tok = strtok(NULL, " ");
		if (!tok) {
			fprintf(stderr, "#%i ERR: Missing CALL address!\n",
				cmd->lineno);
			ret = -EINVAL;
			goto err;
		}
	}
	/* Read load destination address. */
	dest = sb_token_to_long(tok);
	if (dest < 0) {
		fprintf(stderr, "#%i ERR: Incorrect CALL address!\n",
			cmd->lineno);
		ret = -EINVAL;
		goto err;
Marek Vasut's avatar
Marek Vasut committed
805
806
	}

807
808
809
810
811
812
813
814
815
816
817
	tok = strtok(NULL, " ");
	if (tok) {
		arg = sb_token_to_long(tok);
		if (arg < 0) {
			fprintf(stderr,
				"#%i ERR: Incorrect CALL argument!\n",
				cmd->lineno);
			ret = -EINVAL;
			goto err;
		}
	}
818

819
820
821
822
823
824
	/*
	 * Construct the command.
	 */
	ccmd->header.checksum	= 0x5a;
	ccmd->header.tag	= ROM_CALL_CMD;
	ccmd->header.flags	= hab;
825

826
827
	ccmd->call.address	= dest;
	ccmd->call.argument	= arg;
Marek Vasut's avatar
Marek Vasut committed
828

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

831
832
833
834
835
836
837
838
839
	/*
	 * 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
840
841
	}

842
843
844
845
	return 0;

err:
	free(cctx);
846
	return ret;
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
}

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 */
	hdr->product_version.major = __cpu_to_be16(0x999);
	hdr->product_version.minor = __cpu_to_be16(0x999);
	hdr->product_version.revision = __cpu_to_be16(0x999);
	/* Boot image major version */
	hdr->component_version.major = __cpu_to_be16(0x999);
	hdr->component_version.minor = __cpu_to_be16(0x999);
	hdr->component_version.revision = __cpu_to_be16(0x999);

	/* 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 */
878
	hdr->flags = ictx->verbose_boot ? SB_IMAGE_FLAG_VERBOSE : 0,
879
880
881
882
883
884
885
886
887
888

	/* 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;
889
	struct sb_section_ctx *sctx = ictx->sect_head;
890
891
892
893
894
895
896
897
898
899
900
	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. */
901
	hdr->section_count = ictx->sect_count;
902
	while (sctx) {
903
904
		hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
		sctx = sctx->sect;
905
906
	}

907
908
909
910
911
	if (!ictx->sect_boot_found) {
		fprintf(stderr, "ERR: No bootable section selected!\n");
		return -EINVAL;
	}
	hdr->first_boot_section_id = ictx->sect_boot;
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938

	/* 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;
}

939
940
941
942
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;
943
	struct sb_section_ctx *sctx = ictx->sect_head;
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
	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. */
960
		cctx = sctx->cmd_head;
961
962
963
964
965
966
967
		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;
			}
968
969
970
971

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

972
973
974
975
976
977
978
979
980
			cctx = cctx->cmd;
		}

		sctx = sctx->sect;
	}

	return 0;
}

981
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
1009
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
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);
	else if (!strcmp(tok, "CALL"))
		ret = sb_build_command_call(ictx, cmd);
	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;
}

1038
1039
static int sb_load_cmdfile(struct sb_image_ctx *ictx)
{
1040
	struct sb_cmd_list cmd;
1041
	int fd, lineno = 1;
1042
1043
1044
	FILE *fp;
	char *line = NULL;
	ssize_t rlen;
1045
	size_t len;
1046

1047
1048
1049
1050
1051
1052
1053
1054
1055
	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) {
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
		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);
1066
1067
	}

1068
1069
1070
1071
	free(line);

	fclose(fp);

1072
	return 0;
1073

1074
1075
1076
1077
1078
1079
err_file:
	fclose(fp);
err_open:
	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", ictx->cfg_filename);
	return -EINVAL;

1080
1081
}

1082
1083
1084
1085
static int sb_parse_image(struct sb_image_ctx *ictx)
{
	int ret;

1086
1087
1088
1089
	ret = sb_load_cmdfile(ictx);
	if (ret)
		return ret;

1090
1091
1092
1093
1094
	ret = sb_prefill_image_header(ictx);
	if (ret)
		return ret;

	ret = sb_postfill_image_header(ictx);
Marek Vasut's avatar
Marek Vasut committed
1095
	if (ret)
1096
1097
		return ret;

1098
1099
1100
1101
	ret = sb_fixup_sections_and_tags(ictx);
	if (ret)
		return ret;

1102
1103
1104
1105
1106
	return 0;
}

static void sb_free_image(struct sb_image_ctx *ictx)
{
1107
	struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
1108
1109
1110
1111
	struct sb_cmd_ctx *cctx, *c_head;

	while (sctx) {
		s_head = sctx;
1112
		c_head = sctx->cmd_head;
1113
1114
1115
1116

		while (c_head) {
			cctx = c_head;
			c_head = c_head->cmd;
Marek Vasut's avatar
Marek Vasut committed
1117
			if (cctx->data)
1118
				free(cctx->data);
1119
1120
1121
1122
1123
1124
			free(cctx);
		}

		sctx = sctx->sect;
		free(s_head);
	}
Marek Vasut's avatar
Marek Vasut committed
1125
1126
1127
1128
1129
}

static void print_help(const char *pn)
{
	printf(
1130
1131
		"Usage: %s -c FILE -o FILE [-v] [-h]\n\n"
		"   -c FILE ..... path to command file (mxsimage.cfg)\n"
Marek Vasut's avatar
Marek Vasut committed
1132
1133
1134
1135
1136
		"   -o FILE ..... path to output SB file (u-boot.sb)\n"
		"   -v .......... verbose boot output from BootROM\n"
		"   -h .......... print help\n", pn);
}

1137
int main(int argc, char **argv)
Marek Vasut's avatar
Marek Vasut committed
1138
1139
{
	int ret;
1140
1141
	int opt;

1142
	struct sb_image_ctx ctx;
1143
	memset(&ctx, 0, sizeof(ctx));
1144

1145
	while ((opt = getopt(argc, argv, "c:o:vh")) != -1) {
1146
		switch (opt) {
Marek Vasut's avatar
Marek Vasut committed
1147
		case 'c':
1148
			ctx.cfg_filename = optarg;
1149
1150
1151
1152
1153
			break;
		case 'o':
			output_filename = optarg;
			break;
		case 'v':
1154
			ctx.verbose_boot = 1;
1155
1156
1157
			break;
		case 'h':
			print_help(argv[0]);
Marek Vasut's avatar
Marek Vasut committed
1158
			return -EINVAL;
1159
1160
1161
		default:
			fprintf(stderr, "ERROR: Invalid option '%c'.\n\n", opt);
			print_help(argv[0]);
Marek Vasut's avatar
Marek Vasut committed
1162
			return -EINVAL;
1163
1164
1165
		}
	}

Marek Vasut's avatar
Marek Vasut committed
1166
	if (optind != argc) {
1167
1168
		fprintf(stderr, "ERROR: Expected argument after options!\n\n");
		print_help(argv[0]);
Marek Vasut's avatar
Marek Vasut committed
1169
		return -EINVAL;
1170
1171
	}

1172
	if (!ctx.cfg_filename || !output_filename) {
1173
1174
		fprintf(stderr, "ERROR: Input files not specified!\n\n");
		print_help(argv[0]);
Marek Vasut's avatar
Marek Vasut committed
1175
		return -EINVAL;
1176
	}
Marek Vasut's avatar
Marek Vasut committed
1177

1178
1179
1180
1181
	ret = sb_parse_image(&ctx);
	if (ret)
		goto fail;

1182
	ret = sb_create_image(&ctx);
1183
1184
	if (!ret)
		ret = sb_assemble_image(&ctx);
Marek Vasut's avatar
Marek Vasut committed
1185

1186
1187
1188
fail:
	sb_free_image(&ctx);

Marek Vasut's avatar
Marek Vasut committed
1189
1190
	return ret;
}