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