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