utils/sbtools/elftosb.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2011 Amaury Pouly
  11  *
  12  * This program is free software; you can redistribute it and/or
  13  * modify it under the terms of the GNU General Public License
  14  * as published by the Free Software Foundation; either version 2
  15  * of the License, or (at your option) any later version.
  16  *
  17  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  18  * KIND, either express or implied.
  19  *
  20  ****************************************************************************/
  21
  22 #define _ISOC99_SOURCE
  23 #include <stdio.h>
  24 #include <sys/types.h>
  25 #include <sys/stat.h>
  26 #include <fcntl.h>
  27 #include <errno.h>
  28 #include <unistd.h>
  29 #include <stdlib.h>
  30 #include <inttypes.h>
  31 #include <string.h>
  32 #include <ctype.h>
  33 #include <time.h>
  34 #include <stdarg.h>
  35 #include <strings.h>
  36
  37 #include "crypto.h"
  38 #include "elf.h"
  39 #include "sb.h"
  40
  41 #define bug(...) do { fprintf(stderr,"ERROR: "__VA_ARGS__); exit(1); } while(0)
  42 #define bugp(a) do { perror("ERROR: "a); exit(1); } while(0)
  43
  44 bool g_debug = false;
  45
  46 #define ROUND_UP(val, round) ((((val) + (round) - 1) / (round)) * (round))
  47
  48 /**
  49  * Misc
  50  */
  51
  52 char *s_getenv(const char *name)
  53 {
  54     char *s = getenv(name);
  55     return s ? s : "";
  56 }
  57
  58 void generate_random_data(void *buf, size_t sz)
  59 {
  60     static int rand_fd = -1;
  61     if(rand_fd == -1)
  62         rand_fd = open("/dev/urandom", O_RDONLY);
  63     if(rand_fd == -1)
  64         bugp("failed to open /dev/urandom");
  65     if(read(rand_fd, buf, sz) != (ssize_t)sz)
  66         bugp("failed to read /dev/urandom");
  67 }
  68
  69 void *xmalloc(size_t s) /* malloc helper, used in elf.c */
  70 {
  71     void * r = malloc(s);
  72     if(!r) bugp("malloc");
  73     return r;
  74 }
  75
  76 static int convxdigit(char digit, byte *val)
  77 {
  78     if(digit >= '0' && digit <= '9')
  79     {
  80         *val = digit - '0';
  81         return 0;
  82     }
  83     else if(digit >= 'A' && digit <= 'F')
  84     {
  85         *val = digit - 'A' + 10;
  86         return 0;
  87     }
  88     else if(digit >= 'a' && digit <= 'f')
  89     {
  90         *val = digit - 'a' + 10;
  91         return 0;
  92     }
  93     else
  94         return 1;
  95 }
  96
  97 /**
  98  * Key file parsing
  99  */
 100
 101 typedef byte (*key_array_t)[16];
 102
 103 int g_nr_keys;
 104 key_array_t g_key_array;
 105
 106 static key_array_t read_keys(const char *key_file, int *num_keys)
 107 {
 108     int size;
 109     struct stat st;
 110     int fd = open(key_file,O_RDONLY);
 111     if(fd == -1)
 112         bugp("opening key file failed");
 113     if(fstat(fd,&st) == -1)
 114         bugp("key file stat() failed");
 115     size = st.st_size;
 116     char *buf = xmalloc(size);
 117     if(read(fd, buf, size) != (ssize_t)size)
 118         bugp("reading key file");
 119     close(fd);
 120
 121     if(g_debug)
 122         printf("Parsing key file '%s'...\n", key_file);
 123     *num_keys = size ? 1 : 0;
 124     char *ptr = buf;
 125     /* allow trailing newline at the end (but no space after it) */
 126     while(ptr != buf + size && (ptr + 1) != buf + size)
 127     {
 128         if(*ptr++ == '\n')
 129             (*num_keys)++;
 130     }
 131
 132     key_array_t keys = xmalloc(sizeof(byte[16]) * *num_keys);
 133     int pos = 0;
 134     for(int i = 0; i < *num_keys; i++)
 135     {
 136         /* skip ws */
 137         while(pos < size && isspace(buf[pos]))
 138             pos++;
 139         /* enough space ? */
 140         if((pos + 32) > size)
 141             bugp("invalid key file");
 142         for(int j = 0; j < 16; j++)
 143         {
 144             byte a, b;
 145             if(convxdigit(buf[pos + 2 * j], &a) || convxdigit(buf[pos + 2 * j + 1], &b))
 146                 bugp(" invalid key, it should be a 128-bit key written in hexadecimal\n");
 147             keys[i][j] = (a << 4) | b;
 148         }
 149         if(g_debug)
 150         {
 151             printf("Add key: ");
 152             for(int j = 0; j < 16; j++)
 153                 printf("%02x", keys[i][j]);
 154                printf("\n");
 155         }
 156         pos += 32;
 157     }
 158     free(buf);
 159
 160     return keys;
 161 }
 162
 163 /**
 164  * Command file parsing
 165  */
 166
 167 struct cmd_source_t
 168 {
 169     char *identifier;
 170     char *filename;
 171     struct cmd_source_t *next;
 172     /* for later use */
 173     bool elf_loaded;
 174     struct elf_params_t elf;
 175 };
 176
 177 enum cmd_inst_type_t
 178 {
 179     CMD_LOAD,
 180     CMD_JUMP,
 181     CMD_CALL
 182 };
 183
 184 struct cmd_inst_t
 185 {
 186     enum cmd_inst_type_t type;
 187     char *identifier;
 188     struct cmd_inst_t *next;
 189 };
 190
 191 struct cmd_section_t
 192 {
 193     uint32_t identifier;
 194     struct cmd_inst_t *inst_list;
 195     struct cmd_section_t *next;
 196 };
 197
 198 struct cmd_file_t
 199 {
 200     struct cmd_source_t *source_list;
 201     struct cmd_section_t *section_list;
 202 };
 203
 204 enum lexem_type_t
 205 {
 206     LEX_IDENTIFIER,
 207     LEX_LPAREN,
 208     LEX_RPAREN,
 209     LEX_NUMBER,
 210     LEX_STRING, /* double-quoted string */
 211     LEX_EQUAL,
 212     LEX_SEMICOLON,
 213     LEX_LBRACE,
 214     LEX_RBRACE,
 215     LEX_EOF
 216 };
 217
 218 struct lexem_t
 219 {
 220     enum lexem_type_t type;
 221     char *str;
 222     uint32_t num;
 223 };
 224
 225 static void __parse_string(char **ptr, char *end, void *user, void (*emit_fn)(void *user, char c))
 226 {
 227     while(*ptr != end)
 228     {
 229         if(**ptr == '"')
 230             break;
 231         else if(**ptr == '\\')
 232         {
 233             (*ptr)++;
 234             if(*ptr == end)
 235                 bug("Unfinished string");
 236             if(**ptr == '\\') emit_fn(user, '\\');
 237             else if(**ptr == '\'') emit_fn(user, '\'');
 238             else if(**ptr == '\"') emit_fn(user, '\"');
 239             else bug("Unknown escape sequence \\%c", **ptr);
 240             (*ptr)++;
 241         }
 242         else
 243             emit_fn(user, *(*ptr)++);
 244     }
 245     if(*ptr == end || **ptr != '"')
 246         bug("unfinished string");
 247     (*ptr)++;
 248 }
 249
 250 static void __parse_string_emit(void *user, char c)
 251 {
 252     char **pstr = (char **)user;
 253     *(*pstr)++ = c;
 254 }
 255
 256 static void __parse_string_count(void *user, char c)
 257 {
 258     (void) c;
 259     (*(int *)user)++;
 260 }
 261
 262 static void parse_string(char **ptr, char *end, struct lexem_t *lexem)
 263 {
 264     /* skip " */
 265     (*ptr)++;
 266     char *p = *ptr;
 267     /* compute length */
 268     int length = 0;
 269     __parse_string(&p, end, (void *)&length, __parse_string_count);
 270     /* parse again */
 271     lexem->type = LEX_STRING;
 272     lexem->str = xmalloc(length + 1);
 273     lexem->str[length] = 0;
 274     char *pstr = lexem->str;
 275     __parse_string(ptr, end, (void *)&pstr, __parse_string_emit);
 276 }
 277
 278 static void parse_number(char **ptr, char *end, struct lexem_t *lexem)
 279 {
 280     int base = 10;
 281     if(**ptr == '0' && (*ptr) + 1 != end && (*ptr)[1] == 'x')
 282     {
 283         (*ptr) += 2;
 284         base = 16;
 285     }
 286
 287     lexem->type = LEX_NUMBER;
 288     lexem->num = 0;
 289     while(*ptr != end && isxdigit(**ptr))
 290     {
 291         if(base == 10 && !isdigit(**ptr))
 292             break;
 293         byte v;
 294         if(convxdigit(**ptr, &v))
 295             break;
 296         lexem->num = base * lexem->num + v;
 297         (*ptr)++;
 298     }
 299 }
 300
 301 static void parse_identifier(char **ptr, char *end, struct lexem_t *lexem)
 302 {
 303     /* remember position */
 304     char *old = *ptr;
 305     while(*ptr != end && (isalnum(**ptr) || **ptr == '_'))
 306         (*ptr)++;
 307     lexem->type = LEX_IDENTIFIER;
 308     int len = *ptr - old;
 309     lexem->str = xmalloc(len + 1);
 310     lexem->str[len] = 0;
 311     memcpy(lexem->str, old, len);
 312 }
 313
 314 static void next_lexem(char **ptr, char *end, struct lexem_t *lexem)
 315 {
 316     #define ret_simple(t, advance) ({(*ptr) += advance; lexem->type = t; return;})
 317     while(*ptr != end)
 318     {
 319         /* skip whitespace */
 320         if(**ptr == ' ' || **ptr == '\t' || **ptr == '\n' || **ptr == '\r')
 321         {
 322             (*ptr)++;
 323             continue;
 324         }
 325         /* skip comments */
 326         if(**ptr == '/' && (*ptr) + 1 != end && (*ptr)[1] == '/')
 327         {
 328             while(*ptr != end && **ptr != '\n')
 329                 (*ptr)++;
 330             continue;
 331         }
 332         break;
 333     }
 334     if(*ptr == end) ret_simple(LEX_EOF, 0);
 335     if(**ptr == '(') ret_simple(LEX_LPAREN, 1);
 336     if(**ptr == ')') ret_simple(LEX_RPAREN, 1);
 337     if(**ptr == '{') ret_simple(LEX_LBRACE, 1);
 338     if(**ptr == '}') ret_simple(LEX_RBRACE, 1);
 339     if(**ptr == '=') ret_simple(LEX_EQUAL, 1);
 340     if(**ptr == ';') ret_simple(LEX_SEMICOLON, 1);
 341     if(**ptr == '"') return parse_string(ptr, end, lexem);
 342     if(isdigit(**ptr)) return parse_number(ptr, end, lexem);
 343     if(isalpha(**ptr) || **ptr == '_') return parse_identifier(ptr, end, lexem);
 344     bug("Unexpected character '%c' in command file\n", **ptr);
 345     #undef ret_simple
 346 }
 347
 348 #if 0
 349 static void log_lexem(struct lexem_t *lexem)
 350 {
 351     switch(lexem->type)
 352     {
 353         case LEX_EOF: printf("<eof>"); break;
 354         case LEX_EQUAL: printf("="); break;
 355         case LEX_IDENTIFIER: printf("id(%s)", lexem->str); break;
 356         case LEX_LPAREN: printf("("); break;
 357         case LEX_RPAREN: printf(")"); break;
 358         case LEX_LBRACE: printf("{"); break;
 359         case LEX_RBRACE: printf("}"); break;
 360         case LEX_SEMICOLON: printf(";"); break;
 361         case LEX_NUMBER: printf("num(%d)", lexem->num); break;
 362         case LEX_STRING: printf("str(%s)", lexem->str); break;
 363         default: printf("<unk>");
 364     }
 365 }
 366 #endif
 367
 368 static struct cmd_source_t *find_source_by_id(struct cmd_file_t *cmd_file, const char *id)
 369 {
 370     struct cmd_source_t *src = cmd_file->source_list;
 371     while(src)
 372     {
 373         if(strcmp(src->identifier, id) == 0)
 374             return src;
 375         src = src->next;
 376     }
 377     return NULL;
 378 }
 379
 380 static struct cmd_file_t *read_command_file(const char *file)
 381 {
 382     int size;
 383     struct stat st;
 384     int fd = open(file,O_RDONLY);
 385     if(fd == -1)
 386         bugp("opening command file failed");
 387     if(fstat(fd,&st) == -1)
 388         bugp("command file stat() failed");
 389     size = st.st_size;
 390     char *buf = xmalloc(size);
 391     if(read(fd, buf, size) != (ssize_t)size)
 392         bugp("reading command file");
 393     close(fd);
 394
 395     if(g_debug)
 396         printf("Parsing command file '%s'...\n", file);
 397     struct cmd_file_t *cmd_file = xmalloc(sizeof(struct cmd_file_t));
 398     memset(cmd_file, 0, sizeof(struct cmd_file_t));
 399
 400     struct lexem_t lexem;
 401     char *p = buf;
 402     char *end = buf + size;
 403     #define next() next_lexem(&p, end, &lexem)
 404     /* sources */
 405     next();
 406     if(lexem.type != LEX_IDENTIFIER || strcmp(lexem.str, "sources") != 0)
 407         bug("invalid command file: 'sources' expected");
 408     next();
 409     if(lexem.type != LEX_LBRACE)
 410         bug("invalid command file: '{' expected after 'sources'");
 411
 412     while(true)
 413     {
 414         next();
 415         if(lexem.type == LEX_RBRACE)
 416             break;
 417         struct cmd_source_t *src = xmalloc(sizeof(struct cmd_source_t));
 418         memset(src, 0, sizeof(struct cmd_source_t));
 419         src->next = cmd_file->source_list;
 420         if(lexem.type != LEX_IDENTIFIER)
 421             bug("invalid command file: identifier expected in sources");
 422         src->identifier = lexem.str;
 423         next();
 424         if(lexem.type != LEX_EQUAL)
 425             bug("invalid command file: '=' expected after identifier");
 426         next();
 427         if(lexem.type != LEX_STRING)
 428             bug("invalid command file: string expected after '='");
 429         src->filename = lexem.str;
 430         next();
 431         if(lexem.type != LEX_SEMICOLON)
 432             bug("invalid command file: ';' expected after string");
 433         if(find_source_by_id(cmd_file, src->identifier) != NULL)
 434             bug("invalid command file: duplicated source identifier");
 435         cmd_file->source_list = src;
 436     }
 437
 438     /* sections */
 439     struct cmd_section_t *end_sec = NULL;
 440     while(true)
 441     {
 442         struct cmd_section_t *sec = xmalloc(sizeof(struct cmd_section_t));
 443         struct cmd_inst_t *end_list = NULL;
 444         memset(sec, 0, sizeof(struct cmd_section_t));
 445         next();
 446         if(lexem.type == LEX_EOF)
 447             break;
 448         if(lexem.type != LEX_IDENTIFIER || strcmp(lexem.str, "section") != 0)
 449             bug("invalid command file: 'section' expected");
 450         next();
 451         if(lexem.type != LEX_LPAREN)
 452             bug("invalid command file: '(' expected after 'section'");
 453         next();
 454         if(lexem.type != LEX_NUMBER)
 455             bug("invalid command file: number expected as section identifier");
 456         sec->identifier = lexem.num;
 457         next();
 458         if(lexem.type != LEX_RPAREN)
 459             bug("invalid command file: ')' expected after section identifier");
 460         next();
 461         if(lexem.type != LEX_LBRACE)
 462             bug("invalid command file: '{' expected after section directive");
 463         /* commands */
 464         while(true)
 465         {
 466             struct cmd_inst_t *inst = xmalloc(sizeof(struct cmd_inst_t));
 467             memset(inst, 0, sizeof(struct cmd_inst_t));
 468             next();
 469             if(lexem.type == LEX_RBRACE)
 470                 break;
 471             if(lexem.type != LEX_IDENTIFIER)
 472                 bug("invalid command file: instruction expected in section");
 473             if(strcmp(lexem.str, "load") == 0)
 474                 inst->type = CMD_LOAD;
 475             else if(strcmp(lexem.str, "call") == 0)
 476                 inst->type = CMD_CALL;
 477             else if(strcmp(lexem.str, "jump") == 0)
 478                 inst->type = CMD_JUMP;
 479             else
 480                 bug("invalid command file: instruction expected in section");
 481             next();
 482             if(lexem.type != LEX_IDENTIFIER)
 483                 bug("invalid command file: identifier expected after instruction");
 484             inst->identifier = lexem.str;
 485             if(find_source_by_id(cmd_file, inst->identifier) == NULL)
 486                 bug("invalid command file: undefined reference to source '%s'", inst->identifier);
 487             next();
 488             if(lexem.type != LEX_SEMICOLON)
 489                 bug("invalid command file: expected ';' after command");
 490
 491             if(end_list == NULL)
 492             {
 493                 sec->inst_list = inst;
 494                 end_list = inst;
 495             }
 496             else
 497             {
 498                 end_list->next = inst;
 499                 end_list = inst;
 500             }
 501         }
 502
 503         if(end_sec == NULL)
 504         {
 505             cmd_file->section_list = sec;
 506             end_sec = sec;
 507         }
 508         else
 509         {
 510             end_sec->next = sec;
 511             end_sec = sec;
 512         }
 513     }
 514     #undef next
 515
 516     return cmd_file;
 517 }
 518
 519 /**
 520  * command file to sb conversion
 521  */
 522
 523 struct sb_inst_t
 524 {
 525     uint8_t inst; /* SB_INST_* */
 526     uint32_t size;
 527     // <union>
 528     void *data;
 529     uint32_t pattern;
 530     uint32_t addr;
 531     // </union>
 532     /* for production use */
 533     uint32_t padding_size;
 534     uint8_t *padding;
 535 };
 536
 537 struct sb_section_t
 538 {
 539     uint32_t identifier;
 540     int nr_insts;
 541     struct sb_inst_t *insts;
 542     /* for production use */
 543     uint32_t file_offset; /* in blocks */
 544     uint32_t sec_size; /* in blocks */
 545 };
 546
 547 struct sb_file_t
 548 {
 549     int nr_sections;
 550     struct sb_section_t *sections;
 551     /* for production use */
 552     uint32_t image_size; /* in blocks */
 553 };
 554
 555 static bool elf_read(void *user, uint32_t addr, void *buf, size_t count)
 556 {
 557     if(lseek(*(int *)user, addr, SEEK_SET) == (off_t)-1)
 558         return false;
 559     return read(*(int *)user, buf, count) == (ssize_t)count;
 560 }
 561
 562 static void elf_printf(void *user, bool error, const char *fmt, ...)
 563 {
 564     if(!g_debug && !error)
 565         return;
 566     (void) user;
 567     va_list args;
 568     va_start(args, fmt);
 569     vprintf(fmt, args);
 570     va_end(args);
 571 }
 572
 573 static void load_elf_by_id(struct cmd_file_t *cmd_file, const char *id)
 574 {
 575     struct cmd_source_t *src = find_source_by_id(cmd_file, id);
 576     if(src == NULL)
 577         bug("undefined reference to source '%s'\n", id);
 578     /* avoid reloading */
 579     if(src->elf_loaded)
 580         return;
 581     int fd = open(src->filename, O_RDONLY);
 582     if(fd < 0)
 583         bug("cannot open '%s' (id '%s')\n", src->filename, id);
 584     if(g_debug)
 585         printf("Loading ELF file '%s'...\n", src->filename);
 586     elf_init(&src->elf);
 587     src->elf_loaded = elf_read_file(&src->elf, elf_read, elf_printf, &fd);
 588     close(fd);
 589     if(!src->elf_loaded)
 590         bug("error loading elf file '%s' (id '%s')\n", src->filename, id);
 591 }
 592
 593 static struct sb_file_t *apply_cmd_file(struct cmd_file_t *cmd_file)
 594 {
 595     struct sb_file_t *sb = xmalloc(sizeof(struct sb_file_t));
 596     memset(sb, 0, sizeof(struct sb_file_t));
 597
 598     if(g_debug)
 599         printf("Applying command file...\n");
 600     /* count sections */
 601     struct cmd_section_t *csec = cmd_file->section_list;
 602     while(csec)
 603     {
 604         sb->nr_sections++;
 605         csec = csec->next;
 606     }
 607
 608     sb->sections = xmalloc(sb->nr_sections * sizeof(struct sb_section_t));
 609     memset(sb->sections, 0, sb->nr_sections * sizeof(struct sb_section_t));
 610     /* flatten sections */
 611     csec = cmd_file->section_list;
 612     for(int i = 0; i < sb->nr_sections; i++, csec = csec->next)
 613     {
 614         struct sb_section_t *sec = &sb->sections[i];
 615         sec->identifier = csec->identifier;
 616         /* count instructions */
 617         struct cmd_inst_t *cinst = csec->inst_list;
 618         while(cinst)
 619         {
 620             load_elf_by_id(cmd_file, cinst->identifier);
 621             struct elf_params_t *elf = &find_source_by_id(cmd_file, cinst->identifier)->elf;
 622
 623             if(cinst->type == CMD_LOAD)
 624                 sec->nr_insts += elf_get_nr_sections(elf);
 625             else if(cinst->type == CMD_JUMP || cinst->type == CMD_CALL)
 626             {
 627                 if(!elf_get_start_addr(elf, NULL))
 628                     bug("cannot jump/call '%s' because it has no starting point !\n", cinst->identifier);
 629                 sec->nr_insts++;
 630             }
 631
 632             cinst = cinst->next;
 633         }
 634
 635         sec->insts = xmalloc(sec->nr_insts * sizeof(struct sb_inst_t));
 636         memset(sec->insts, 0, sec->nr_insts * sizeof(struct sb_inst_t));
 637         /* flatten */
 638         int idx = 0;
 639         cinst = csec->inst_list;
 640         while(cinst)
 641         {
 642             struct elf_params_t *elf = &find_source_by_id(cmd_file, cinst->identifier)->elf;
 643
 644             if(cinst->type == CMD_LOAD)
 645             {
 646                 struct elf_section_t *esec = elf->first_section;
 647                 while(esec)
 648                 {
 649                     if(esec->type == EST_LOAD)
 650                     {
 651                         sec->insts[idx].inst = SB_INST_LOAD;
 652                         sec->insts[idx].addr = esec->addr;
 653                         sec->insts[idx].size = esec->size;
 654                         sec->insts[idx++].data = esec->section;
 655                     }
 656                     else if(esec->type == EST_FILL)
 657                     {
 658                         sec->insts[idx].inst = SB_INST_FILL;
 659                         sec->insts[idx].addr = esec->addr;
 660                         sec->insts[idx].size = esec->size;
 661                         sec->insts[idx++].pattern = esec->pattern;
 662                     }
 663                     esec = esec->next;
 664                 }
 665             }
 666             else if(cinst->type == CMD_JUMP || cinst->type == CMD_CALL)
 667             {
 668                 sec->insts[idx].inst = (cinst->type == CMD_JUMP) ? SB_INST_JUMP : SB_INST_CALL;
 669                 sec->insts[idx++].addr = elf->start_addr;
 670             }
 671
 672             cinst = cinst->next;
 673         }
 674     }
 675
 676     return sb;
 677 }
 678
 679 /**
 680  * Sb file production
 681  */
 682
 683 static void fill_gaps(struct sb_file_t *sb)
 684 {
 685     for(int i = 0; i < sb->nr_sections; i++)
 686     {
 687         struct sb_section_t *sec = &sb->sections[i];
 688         for(int j = 0; j < sec->nr_insts; j++)
 689         {
 690             struct sb_inst_t *inst = &sec->insts[j];
 691             if(inst->inst != SB_INST_LOAD)
 692                 continue;
 693             inst->padding_size = ROUND_UP(inst->size, BLOCK_SIZE) - inst->size;
 694             /* emulate elftosb2 behaviour: generate 15 bytes (that's a safe maximum) */
 695             inst->padding = xmalloc(15);
 696             generate_random_data(inst->padding, 15);
 697         }
 698     }
 699 }
 700
 701 static void compute_sb_offsets(struct sb_file_t *sb)
 702 {
 703     sb->image_size = 0;
 704     /* sb header */
 705     sb->image_size += sizeof(struct sb_header_t) / BLOCK_SIZE;
 706     /* sections headers */
 707     sb->image_size += sb->nr_sections * sizeof(struct sb_section_header_t) / BLOCK_SIZE;
 708     /* key dictionary */
 709     sb->image_size += g_nr_keys * sizeof(struct sb_key_dictionary_entry_t) / BLOCK_SIZE;
 710     /* sections */
 711     for(int i = 0; i < sb->nr_sections; i++)
 712     {
 713         /* each section has a preliminary TAG command */
 714         sb->image_size += sizeof(struct sb_instruction_tag_t) / BLOCK_SIZE;
 715
 716         struct sb_section_t *sec = &sb->sections[i];
 717         sec->file_offset = sb->image_size;
 718         for(int j = 0; j < sec->nr_insts; j++)
 719         {
 720             struct sb_inst_t *inst = &sec->insts[j];
 721             if(inst->inst == SB_INST_CALL || inst->inst == SB_INST_JUMP)
 722             {
 723                 if(g_debug)
 724                     printf("%s | addr=0x%08x | arg=0x%08x\n",
 725                         inst->inst == SB_INST_CALL ? "CALL" : "JUMP", inst->addr, 0);
 726                 sb->image_size += sizeof(struct sb_instruction_call_t) / BLOCK_SIZE;
 727                 sec->sec_size += sizeof(struct sb_instruction_call_t) / BLOCK_SIZE;
 728             }
 729             else if(inst->inst == SB_INST_FILL)
 730             {
 731                 if(g_debug)
 732                     printf("FILL | addr=0x%08x | len=0x%08x | pattern=0x%08x\n",
 733                         inst->addr, inst->size, inst->pattern);
 734                 sb->image_size += sizeof(struct sb_instruction_fill_t) / BLOCK_SIZE;
 735                 sec->sec_size += sizeof(struct sb_instruction_fill_t) / BLOCK_SIZE;
 736             }
 737             else if(inst->inst == SB_INST_LOAD)
 738             {
 739                 if(g_debug)
 740                     printf("LOAD | addr=0x%08x | len=0x%08x\n", inst->addr, inst->size);
 741                 /* load header */
 742                 sb->image_size += sizeof(struct sb_instruction_load_t) / BLOCK_SIZE;
 743                 sec->sec_size += sizeof(struct sb_instruction_load_t) / BLOCK_SIZE;
 744                 /* data + alignment */
 745                 sb->image_size += (inst->size + inst->padding_size) / BLOCK_SIZE;
 746                 sec->sec_size += (inst->size + inst->padding_size) / BLOCK_SIZE;
 747             }
 748         }
 749     }
 750     /* final signature */
 751     sb->image_size += 2;
 752 }
 753
 754 static uint64_t generate_timestamp()
 755 {
 756     struct tm tm_base = {0, 0, 0, 1, 0, 100, 0, 0, 1, 0, NULL}; /* 2000/1/1 0:00:00 */
 757     time_t t = time(NULL) - mktime(&tm_base);
 758     return (uint64_t)t * 1000000L;
 759 }
 760
 761 void generate_version(struct sb_version_t *ver)
 762 {
 763     ver->major = 0x999;
 764     ver->pad0 = 0;
 765     ver->minor = 0x999;
 766     ver->pad1 = 0;
 767     ver->revision = 0x999;
 768     ver->pad2 = 0;
 769 }
 770
 771 static void produce_sb_header(struct sb_file_t *sb, struct sb_header_t *sb_hdr)
 772 {
 773     struct sha_1_params_t sha_1_params;
 774
 775     sb_hdr->signature[0] = 'S';
 776     sb_hdr->signature[1] = 'T';
 777     sb_hdr->signature[2] = 'M';
 778     sb_hdr->signature[3] = 'P';
 779     sb_hdr->major_ver = IMAGE_MAJOR_VERSION;
 780     sb_hdr->minor_ver = IMAGE_MINOR_VERSION;
 781     sb_hdr->flags = 0;
 782     sb_hdr->image_size = sb->image_size;
 783     sb_hdr->header_size = sizeof(struct sb_header_t) / BLOCK_SIZE;
 784     sb_hdr->first_boot_sec_id = sb->sections[0].identifier;
 785     sb_hdr->nr_keys = g_nr_keys;
 786     sb_hdr->nr_sections = sb->nr_sections;
 787     sb_hdr->sec_hdr_size = sizeof(struct sb_section_header_t) / BLOCK_SIZE;
 788     sb_hdr->key_dict_off = sb_hdr->header_size +
 789         sb_hdr->sec_hdr_size * sb_hdr->nr_sections;
 790     sb_hdr->first_boot_tag_off = sb_hdr->key_dict_off +
 791         sizeof(struct sb_key_dictionary_entry_t) * sb_hdr->nr_keys / BLOCK_SIZE;
 792     generate_random_data(sb_hdr->rand_pad0, sizeof(sb_hdr->rand_pad0));
 793     generate_random_data(sb_hdr->rand_pad1, sizeof(sb_hdr->rand_pad1));
 794     sb_hdr->timestamp = generate_timestamp();
 795     generate_version(&sb_hdr->product_ver);
 796     generate_version(&sb_hdr->component_ver);
 797     sb_hdr->drive_tag = 0;
 798
 799     sha_1_init(&sha_1_params);
 800     sha_1_update(&sha_1_params, &sb_hdr->signature[0],
 801         sizeof(struct sb_header_t) - sizeof(sb_hdr->sha1_header));
 802     sha_1_finish(&sha_1_params);
 803     sha_1_output(&sha_1_params, sb_hdr->sha1_header);
 804 }
 805
 806 static void produce_sb_section_header(struct sb_section_t *sec,
 807     struct sb_section_header_t *sec_hdr)
 808 {
 809     sec_hdr->identifier = sec->identifier;
 810     sec_hdr->offset = sec->file_offset;
 811     sec_hdr->size = sec->sec_size;
 812     sec_hdr->flags = SECTION_BOOTABLE;
 813 }
 814
 815 static uint8_t instruction_checksum(struct sb_instruction_header_t *hdr)
 816 {
 817     uint8_t sum = 90;
 818     byte *ptr = (byte *)hdr;
 819     for(int i = 1; i < 16; i++)
 820         sum += ptr[i];
 821     return sum;
 822 }
 823
 824 static void produce_section_tag_cmd(struct sb_section_t *sec,
 825     struct sb_instruction_tag_t *tag, bool is_last)
 826 {
 827     tag->hdr.opcode = SB_INST_TAG;
 828     tag->hdr.flags = is_last ? SB_INST_LAST_TAG : 0;
 829     tag->identifier = sec->identifier;
 830     tag->len = sec->sec_size;
 831     tag->flags = SECTION_BOOTABLE;
 832     tag->hdr.checksum = instruction_checksum(&tag->hdr);
 833 }
 834
 835 void produce_sb_instruction(struct sb_inst_t *inst,
 836     struct sb_instruction_common_t *cmd)
 837 {
 838     cmd->hdr.flags = 0;
 839     cmd->hdr.opcode = inst->inst;
 840     cmd->addr = inst->addr;
 841     cmd->len = inst->size;
 842     switch(inst->inst)
 843     {
 844         case SB_INST_CALL:
 845         case SB_INST_JUMP:
 846             cmd->len = 0;
 847             cmd->data = 0;
 848             break;
 849         case SB_INST_FILL:
 850             cmd->data = inst->pattern;
 851             break;
 852         case SB_INST_LOAD:
 853             cmd->data = crc_continue(crc(inst->data, inst->size),
 854                 inst->padding, inst->padding_size);
 855             break;
 856         default:
 857             break;
 858     }
 859     cmd->hdr.checksum = instruction_checksum(&cmd->hdr);
 860 }
 861
 862 static void produce_sb_file(struct sb_file_t *sb, const char *filename)
 863 {
 864     int fd = open(filename, O_WRONLY | O_TRUNC | O_CREAT,
 865         S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 866     if(fd < 0)
 867         bugp("cannot open output file");
 868
 869     byte real_key[16];
 870     byte (*cbc_macs)[16] = xmalloc(16 * g_nr_keys);
 871     /* init CBC-MACs */
 872     for(int i = 0; i < g_nr_keys; i++)
 873         memset(cbc_macs[i], 0, 16);
 874
 875     fill_gaps(sb);
 876     compute_sb_offsets(sb);
 877
 878     generate_random_data(real_key, sizeof(real_key));
 879
 880     /* global SHA-1 */
 881     struct sha_1_params_t file_sha1;
 882     sha_1_init(&file_sha1);
 883     /* produce and write header */
 884     struct sb_header_t sb_hdr;
 885     produce_sb_header(sb, &sb_hdr);
 886     sha_1_update(&file_sha1, (byte *)&sb_hdr, sizeof(sb_hdr));
 887     write(fd, &sb_hdr, sizeof(sb_hdr));
 888     /* update CBC-MACs */
 889     for(int i = 0; i < g_nr_keys; i++)
 890         cbc_mac((byte *)&sb_hdr, NULL, sizeof(sb_hdr) / BLOCK_SIZE, g_key_array[i],
 891             cbc_macs[i], &cbc_macs[i], 1);
 892
 893     /* produce and write section headers */
 894     for(int i = 0; i < sb_hdr.nr_sections; i++)
 895     {
 896         struct sb_section_header_t sb_sec_hdr;
 897         produce_sb_section_header(&sb->sections[i], &sb_sec_hdr);
 898         sha_1_update(&file_sha1, (byte *)&sb_sec_hdr, sizeof(sb_sec_hdr));
 899         write(fd, &sb_sec_hdr, sizeof(sb_sec_hdr));
 900         /* update CBC-MACs */
 901         for(int j = 0; j < g_nr_keys; j++)
 902             cbc_mac((byte *)&sb_sec_hdr, NULL, sizeof(sb_sec_hdr) / BLOCK_SIZE,
 903                 g_key_array[j], cbc_macs[j], &cbc_macs[j], 1);
 904     }
 905     /* produce key dictionary */
 906     for(int i = 0; i < g_nr_keys; i++)
 907     {
 908         struct sb_key_dictionary_entry_t entry;
 909         memcpy(entry.hdr_cbc_mac, cbc_macs[i], 16);
 910         cbc_mac(real_key, entry.key, sizeof(real_key) / BLOCK_SIZE, g_key_array[i],
 911             (byte *)&sb_hdr, NULL, 1);
 912
 913         write(fd, &entry, sizeof(entry));
 914         sha_1_update(&file_sha1, (byte *)&entry, sizeof(entry));
 915     }
 916     /* produce sections data */
 917     for(int i = 0; i< sb_hdr.nr_sections; i++)
 918     {
 919         /* produce tag command */
 920         struct sb_instruction_tag_t tag_cmd;
 921         produce_section_tag_cmd(&sb->sections[i], &tag_cmd, (i + 1) == sb_hdr.nr_sections);
 922         if(g_nr_keys > 0)
 923             cbc_mac((byte *)&tag_cmd, (byte *)&tag_cmd, sizeof(tag_cmd) / BLOCK_SIZE,
 924                 real_key, (byte *)&sb_hdr, NULL, 1);
 925         sha_1_update(&file_sha1, (byte *)&tag_cmd, sizeof(tag_cmd));
 926         write(fd, &tag_cmd, sizeof(tag_cmd));
 927         /* produce other commands */
 928         byte cur_cbc_mac[16];
 929         memcpy(cur_cbc_mac, (byte *)&sb_hdr, 16);
 930         for(int j = 0; j < sb->sections[i].nr_insts; j++)
 931         {
 932             struct sb_inst_t *inst = &sb->sections[i].insts[j];
 933             /* command */
 934             struct sb_instruction_common_t cmd;
 935             produce_sb_instruction(inst, &cmd);
 936             if(g_nr_keys > 0)
 937                 cbc_mac((byte *)&cmd, (byte *)&cmd, sizeof(cmd) / BLOCK_SIZE,
 938                     real_key, cur_cbc_mac, &cur_cbc_mac, 1);
 939             sha_1_update(&file_sha1, (byte *)&cmd, sizeof(cmd));
 940             write(fd, &cmd, sizeof(cmd));
 941             /* data */
 942             if(inst->inst == SB_INST_LOAD)
 943             {
 944                 uint32_t sz = inst->size + inst->padding_size;
 945                 byte *data = xmalloc(sz);
 946                 memcpy(data, inst->data, inst->size);
 947                 memcpy(data + inst->size, inst->padding, inst->padding_size);
 948                 if(g_nr_keys > 0)
 949                     cbc_mac(data, data, sz / BLOCK_SIZE,
 950                         real_key, cur_cbc_mac, &cur_cbc_mac, 1);
 951                 sha_1_update(&file_sha1, data, sz);
 952                 write(fd, data, sz);
 953                 free(data);
 954             }
 955         }
 956     }
 957     /* write file SHA-1 */
 958     byte final_sig[32];
 959     sha_1_finish(&file_sha1);
 960     sha_1_output(&file_sha1, final_sig);
 961     generate_random_data(final_sig + 20, 12);
 962     if(g_nr_keys > 0)
 963         cbc_mac(final_sig, final_sig, 2, real_key, (byte *)&sb_hdr, NULL, 1);
 964     write(fd, final_sig, 32);
 965
 966     close(fd);
 967 }
 968
 969 int main(int argc, const char **argv)
 970 {
 971     if(argc != 4)
 972     {
 973         printf("Usage: %s <cmd file> <key file> <out file>\n",*argv);
 974         printf("To enable debug mode, set environement variable SB_DEBUG to YES\n");
 975         return 1;
 976     }
 977
 978     if(strcasecmp(s_getenv("SB_DEBUG"), "YES") == 0)
 979         g_debug = true;
 980
 981     g_key_array = read_keys(argv[2], &g_nr_keys);
 982     struct cmd_file_t *cmd_file = read_command_file(argv[1]);
 983     struct sb_file_t *sb_file = apply_cmd_file(cmd_file);
 984     produce_sb_file(sb_file, argv[3]);
 985
 986     return 0;
 987 }