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