git.gag.com Git - fw/altos/blob - ao-tools/ao-stmload/ao-stmload.c

   1 /*
   2  * Copyright © 2012 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #include <err.h>
  19 #include <fcntl.h>
  20 #include <gelf.h>
  21 #include <stdio.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <sysexits.h>
  25 #include <unistd.h>
  26 #include <getopt.h>
  27 #include <string.h>
  28 #include "stlink-common.h"
  29
  30 #define AO_USB_DESC_STRING              3
  31
  32 struct sym {
  33         unsigned        addr;
  34         char            *name;
  35         int             required;
  36 } ao_symbols[] = {
  37
  38         { 0,    "ao_romconfig_version", 1 },
  39 #define AO_ROMCONFIG_VERSION    (ao_symbols[0].addr)
  40
  41         { 0,    "ao_romconfig_check",   1 },
  42 #define AO_ROMCONFIG_CHECK      (ao_symbols[1].addr)
  43
  44         { 0,    "ao_serial_number", 1 },
  45 #define AO_SERIAL_NUMBER        (ao_symbols[2].addr)
  46
  47         { 0,    "ao_usb_descriptors", 0 },
  48 #define AO_USB_DESCRIPTORS      (ao_symbols[3].addr)
  49
  50         { 0,    "ao_radio_cal", 0 },
  51 #define AO_RADIO_CAL            (ao_symbols[4].addr)
  52 };
  53
  54 #define NUM_SYMBOLS             5
  55 #define NUM_REQUIRED_SYMBOLS    3
  56
  57 /*
  58  * Look through the Elf file for the AltOS symbols
  59  * that can be adjusted before the image is written
  60  * to the device
  61  */
  62 static int
  63 find_symbols (Elf *e)
  64 {
  65         Elf_Scn         *scn;
  66         Elf_Data        *symbol_data = NULL;
  67         GElf_Shdr       shdr;
  68         GElf_Sym        sym;
  69         int             i, symbol_count, s;
  70         int             required = 0;
  71         char            *symbol_name;
  72         char            *section_name;
  73         size_t          shstrndx;
  74
  75         if (elf_getshdrstrndx(e, &shstrndx) < 0)
  76                 return 0;
  77
  78         /*
  79          * Find the symbols
  80          */
  81
  82         scn = NULL;
  83         while ((scn = elf_nextscn(e, scn)) != NULL) {
  84
  85                 if (gelf_getshdr(scn, &shdr) != &shdr)
  86                         return 0;
  87
  88 #if 0
  89                 section_name = elf_strptr(e, shstrndx, shdr.sh_name);
  90
  91                 printf ("name %s\n", section_name);
  92
  93                 if (shdr.sh_type == SHT_PROGBITS)
  94                 {
  95                         printf ("\ttype %lx\n", shdr.sh_type);
  96                         printf ("\tflags %lx\n", shdr.sh_flags);
  97                         printf ("\taddr %lx\n", shdr.sh_addr);
  98                         printf ("\toffset %lx\n", shdr.sh_offset);
  99                         printf ("\tsize %lx\n", shdr.sh_size);
 100                         printf ("\tlink %lx\n", shdr.sh_link);
 101                         printf ("\tinfo %lx\n", shdr.sh_info);
 102                         printf ("\taddralign %lx\n", shdr.sh_addralign);
 103                         printf ("\tentsize %lx\n", shdr.sh_entsize);
 104                 }
 105 #endif
 106
 107                 if (shdr.sh_type == SHT_SYMTAB) {
 108                         symbol_data = elf_getdata(scn, NULL);
 109                         symbol_count = shdr.sh_size / shdr.sh_entsize;
 110                         break;
 111                 }
 112         }
 113
 114         if (!symbol_data)
 115                 return 0;
 116
 117         for (i = 0; i < symbol_count; i++) {
 118                 gelf_getsym(symbol_data, i, &sym);
 119
 120                 symbol_name = elf_strptr(e, shdr.sh_link, sym.st_name);
 121
 122                 for (s = 0; s < NUM_SYMBOLS; s++)
 123                         if (!strcmp (ao_symbols[s].name, symbol_name)) {
 124                                 int     t;
 125                                 ao_symbols[s].addr = sym.st_value;
 126                                 if (ao_symbols[s].required)
 127                                         ++required;
 128                         }
 129         }
 130
 131         return required >= NUM_REQUIRED_SYMBOLS;
 132 }
 133
 134 struct load {
 135         uint32_t        addr;
 136         uint32_t        len;
 137         uint8_t         buf[0];
 138 };
 139
 140 uint32_t round4(uint32_t a) {
 141         return (a + 3) & ~3;
 142 }
 143
 144 struct load *
 145 new_load (uint32_t addr, uint32_t len)
 146 {
 147         struct load *new;
 148
 149         len = round4(len);
 150         new = calloc (1, sizeof (struct load) + len);
 151         if (!new)
 152                 abort();
 153
 154         new->addr = addr;
 155         new->len = len;
 156         return new;
 157 }
 158
 159 void
 160 load_paste(struct load *into, struct load *from)
 161 {
 162         if (from->addr < into->addr || into->addr + into->len < from->addr + from->len)
 163                 abort();
 164
 165         memcpy(into->buf + from->addr - into->addr, from->buf, from->len);
 166 }
 167
 168 /*
 169  * Make a new load structure large enough to hold the old one and
 170  * the new data
 171  */
 172 struct load *
 173 expand_load(struct load *from, uint32_t addr, uint32_t len)
 174 {
 175         struct load     *new;
 176
 177         if (from) {
 178                 uint32_t        from_last = from->addr + from->len;
 179                 uint32_t        last = addr + len;
 180
 181                 if (addr > from->addr)
 182                         addr = from->addr;
 183                 if (last < from_last)
 184                         last = from_last;
 185
 186                 len = last - addr;
 187
 188                 if (addr == from->addr && len == from->len)
 189                         return from;
 190         }
 191         new = new_load(addr, len);
 192         if (from) {
 193                 load_paste(new, from);
 194                 free (from);
 195         }
 196         return new;
 197 }
 198
 199 /*
 200  * Create a new load structure with data from the existing one
 201  * and the new data
 202  */
 203 struct load *
 204 load_write(struct load *from, uint32_t addr, uint32_t len, void *data)
 205 {
 206         struct load     *new;
 207
 208         new = expand_load(from, addr, len);
 209         memcpy(new->buf + addr - new->addr, data, len);
 210         return new;
 211 }
 212
 213 /*
 214  * Construct a large in-memory block for all
 215  * of the loaded sections of the program
 216  */
 217 static struct load *
 218 get_load(Elf *e)
 219 {
 220         Elf_Scn         *scn;
 221         size_t          shstrndx;
 222         GElf_Shdr       shdr;
 223         Elf_Data        *data;
 224         uint8_t         *buf;
 225         char            *got_name;
 226         size_t          nphdr;
 227         size_t          p;
 228         GElf_Phdr       phdr;
 229         GElf_Addr       p_paddr;
 230         GElf_Off        p_offset;
 231         GElf_Addr       sh_paddr;
 232         struct load     *load = NULL;
 233         char            *section_name;
 234         size_t          nshdr;
 235         size_t          s;
 236
 237         if (elf_getshdrstrndx(e, &shstrndx) < 0)
 238                 return 0;
 239
 240         if (elf_getphdrnum(e, &nphdr) < 0)
 241                 return 0;
 242
 243         if (elf_getshdrnum(e, &nshdr) < 0)
 244                 return 0;
 245
 246         /*
 247          * As far as I can tell, all of the phdr sections should
 248          * be flashed to memory
 249          */
 250         for (p = 0; p < nphdr; p++) {
 251
 252                 /* Find this phdr */
 253                 gelf_getphdr(e, p, &phdr);
 254
 255                 if (phdr.p_type != PT_LOAD)
 256                         continue;
 257
 258                 p_offset = phdr.p_offset;
 259                 /* Get the associated file section */
 260
 261 #if 0
 262                 printf ("offset %08x vaddr %08x paddr %08x filesz %08x memsz %08x\n",
 263                         (uint32_t) phdr.p_offset,
 264                         (uint32_t) phdr.p_vaddr,
 265                         (uint32_t) phdr.p_paddr,
 266                         (uint32_t) phdr.p_filesz,
 267                         (uint32_t) phdr.p_memsz);
 268 #endif
 269
 270                 for (s = 0; s < nshdr; s++) {
 271                         scn = elf_getscn(e, s);
 272
 273                         if (!scn) {
 274                                 printf ("getscn failed\n");
 275                                 abort();
 276                         }
 277                         if (gelf_getshdr(scn, &shdr) != &shdr) {
 278                                 printf ("gelf_getshdr failed\n");
 279                                 abort();
 280                         }
 281
 282                         section_name = elf_strptr(e, shstrndx, shdr.sh_name);
 283
 284                         if (phdr.p_offset <= shdr.sh_offset && shdr.sh_offset < phdr.p_offset + phdr.p_filesz) {
 285
 286                                 if (shdr.sh_size == 0)
 287                                         continue;
 288
 289                                 sh_paddr = phdr.p_paddr + shdr.sh_offset - phdr.p_offset;
 290
 291                                 printf ("\tsize %08x rom %08x exec %08x %s\n",
 292                                         (uint32_t) shdr.sh_size,
 293                                         (uint32_t) sh_paddr,
 294                                         (uint32_t) shdr.sh_addr,
 295                                         section_name);
 296
 297                                 data = elf_getdata(scn, NULL);
 298
 299                                 /* Write the section data into the memory block */
 300                                 load = load_write(load, sh_paddr, shdr.sh_size, data->d_buf);
 301                         }
 302                 }
 303         }
 304         return load;
 305 }
 306
 307 /*
 308  * Edit the to-be-written memory block
 309  */
 310 static int
 311 rewrite(struct load *load, unsigned addr, uint8_t *data, int len)
 312 {
 313         int             i;
 314
 315         if (addr < load->addr || load->addr + load->len < addr + len)
 316                 return 0;
 317
 318         printf("rewrite %04x:", addr);
 319         for (i = 0; i < len; i++)
 320                 printf (" %02x", load->buf[addr - load->addr + i]);
 321         printf(" ->");
 322         for (i = 0; i < len; i++)
 323                 printf (" %02x", data[i]);
 324         printf("\n");
 325         memcpy(load->buf + addr - load->addr, data, len);
 326 }
 327
 328 /*
 329  * Open the specified ELF file and
 330  * check for the symbols we need
 331  */
 332
 333 Elf *
 334 ao_open_elf(char *name)
 335 {
 336         int             fd;
 337         Elf             *e;
 338         Elf_Scn         *scn;
 339         Elf_Data        *symbol_data = NULL;
 340         GElf_Shdr       shdr;
 341         GElf_Sym        sym;
 342         size_t          n, shstrndx, sz;
 343         int             i, symbol_count, s;
 344         int             required = 0;
 345
 346         if (elf_version(EV_CURRENT) == EV_NONE)
 347                 return NULL;
 348
 349         fd = open(name, O_RDONLY, 0);
 350
 351         if (fd < 0)
 352                 return NULL;
 353
 354         e = elf_begin(fd, ELF_C_READ, NULL);
 355
 356         if (!e)
 357                 return NULL;
 358
 359         if (elf_kind(e) != ELF_K_ELF)
 360                 return NULL;
 361
 362         if (elf_getshdrstrndx(e, &shstrndx) != 0)
 363                 return NULL;
 364
 365         if (!find_symbols(e)) {
 366                 fprintf (stderr, "Cannot find required symbols\n");
 367                 return NULL;
 368         }
 369
 370         return e;
 371 }
 372
 373 /*
 374  * Read a 32-bit value from the target device with arbitrary
 375  * alignment
 376  */
 377 static uint32_t
 378 get_uint32(stlink_t *sl, uint32_t addr)
 379 {
 380         const           uint8_t *data = sl->q_buf;
 381         uint32_t        actual_addr;
 382         int             off;
 383         uint32_t        result;
 384
 385         sl->q_len = 0;
 386
 387         printf ("read 0x%x\n", addr);
 388
 389         actual_addr = addr & ~3;
 390
 391         stlink_read_mem32(sl, actual_addr, 8);
 392
 393         if (sl->q_len != 8)
 394                 abort();
 395
 396         off = addr & 3;
 397         result = data[off] | (data[off + 1] << 8) | (data[off+2] << 16) | (data[off+3] << 24);
 398         printf ("read 0x%08x = 0x%08x\n", addr, result);
 399         return result;
 400 }
 401
 402 /*
 403  * Read a 16-bit value from the target device with arbitrary
 404  * alignment
 405  */
 406 static uint16_t
 407 get_uint16(stlink_t *sl, uint32_t addr)
 408 {
 409         const           uint8_t *data = sl->q_buf;
 410         uint32_t        actual_addr;
 411         int             off;
 412         uint16_t        result;
 413
 414         sl->q_len = 0;
 415
 416
 417         actual_addr = addr & ~3;
 418
 419         stlink_read_mem32(sl, actual_addr, 8);
 420
 421         if (sl->q_len != 8)
 422                 abort();
 423
 424         off = addr & 3;
 425         result = data[off] | (data[off + 1] << 8);
 426         printf ("read 0x%08x = 0x%04x\n", addr, result);
 427         return result;
 428 }
 429
 430 /*
 431  * Check to see if the target device has been
 432  * flashed with a similar firmware image before
 433  *
 434  * This is done by looking for the same romconfig version,
 435  * which should be at the same location as the linker script
 436  * places this at 0x100 from the start of the rom section
 437  */
 438 static int
 439 check_flashed(stlink_t *sl)
 440 {
 441         uint16_t        romconfig_version = get_uint16(sl, AO_ROMCONFIG_VERSION);
 442         uint16_t        romconfig_check = get_uint16(sl, AO_ROMCONFIG_CHECK);
 443
 444         if (romconfig_version != (uint16_t) ~romconfig_check) {
 445                 fprintf (stderr, "Device has not been flashed before\n");
 446                 return 0;
 447         }
 448         return 1;
 449 }
 450
 451 static const struct option options[] = {
 452         { .name = "device", .has_arg = 1, .val = 'D' },
 453         { .name = "cal", .has_arg = 1, .val = 'c' },
 454         { .name = "serial", .has_arg = 1, .val = 's' },
 455         { 0, 0, 0, 0},
 456 };
 457
 458 static void usage(char *program)
 459 {
 460         fprintf(stderr, "usage: %s [--cal=<radio-cal>] [--serial=<serial>] file.elf\n", program);
 461         exit(1);
 462 }
 463
 464 void
 465 done(stlink_t *sl, int code)
 466 {
 467         if (sl) {
 468                 stlink_reset(sl);
 469                 stlink_run(sl);
 470                 stlink_exit_debug_mode(sl);
 471                 stlink_close(sl);
 472         }
 473         exit (code);
 474 }
 475
 476 int
 477 main (int argc, char **argv)
 478 {
 479         char                    *device = NULL;
 480         char                    *filename;
 481         Elf                     *e;
 482         char                    *serial_end;
 483         unsigned int            serial = 0;
 484         char                    *serial_ucs2;
 485         int                     serial_ucs2_len;
 486         char                    serial_int[2];
 487         unsigned int            s;
 488         int                     i;
 489         int                     string_num;
 490         uint32_t                cal = 0;
 491         char                    cal_int[4];
 492         char                    *cal_end;
 493         int                     c;
 494         stlink_t                *sl;
 495         int                     was_flashed = 0;
 496         struct load             *load;
 497         int                     tries;
 498
 499         while ((c = getopt_long(argc, argv, "D:c:s:", options, NULL)) != -1) {
 500                 switch (c) {
 501                 case 'D':
 502                         device = optarg;
 503                         break;
 504                 case 'c':
 505                         cal = strtoul(optarg, &cal_end, 10);
 506                         if (cal_end == optarg || *cal_end != '\0')
 507                                 usage(argv[0]);
 508                         break;
 509                 case 's':
 510                         serial = strtoul(optarg, &serial_end, 10);
 511                         if (serial_end == optarg || *serial_end != '\0')
 512                                 usage(argv[0]);
 513                         break;
 514                 default:
 515                         usage(argv[0]);
 516                         break;
 517                 }
 518         }
 519
 520         filename = argv[optind];
 521         if (filename == NULL)
 522                 usage(argv[0]);
 523
 524         /*
 525          * Open the source file and load the symbols and
 526          * flash data
 527          */
 528
 529         e = ao_open_elf(filename);
 530         if (!e) {
 531                 fprintf(stderr, "Cannot open file \"%s\"\n", filename);
 532                 exit(1);
 533         }
 534
 535         if (!find_symbols(e)) {
 536                 fprintf(stderr, "Cannot find symbols in \"%s\"\n", filename);
 537                 exit(1);
 538         }
 539
 540         if (!(load = get_load(e))) {
 541                 fprintf(stderr, "Cannot find program data in \"%s\"\n", filename);
 542                 exit(1);
 543         }
 544
 545         /* Connect to the programming dongle
 546          */
 547
 548         for (tries = 0; tries < 3; tries++) {
 549                 if (device) {
 550                         sl = stlink_v1_open(50);
 551                 } else {
 552                         sl = stlink_open_usb(50);
 553
 554                 }
 555                 if (!sl) {
 556                         fprintf (stderr, "No STLink devices present\n");
 557                         done (sl, 1);
 558                 }
 559
 560                 if (sl->chip_id != 0)
 561                         break;
 562                 stlink_reset(sl);
 563                 stlink_close(sl);
 564         }
 565         if (sl->chip_id == 0) {
 566                 fprintf (stderr, "Debugger connection failed\n");
 567                 done(sl, 1);
 568         }
 569
 570         /* Verify that the loaded image fits entirely within device flash
 571          */
 572         if (load->addr < sl->flash_base ||
 573             sl->flash_base + sl->flash_size < load->addr + load->len) {
 574                 fprintf (stderr, "\%s\": Invalid memory range 0x%08x - 0x%08x\n", filename,
 575                          load->addr, load->addr + load->len);
 576                 done(sl, 1);
 577         }
 578
 579         /* Enter debugging mode
 580          */
 581         if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE)
 582                 stlink_exit_dfu_mode(sl);
 583
 584         if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
 585                 stlink_enter_swd_mode(sl);
 586
 587         /* Go fetch existing config values
 588          * if available
 589          */
 590         was_flashed = check_flashed(sl);
 591
 592         if (!serial) {
 593                 if (!was_flashed) {
 594                         fprintf (stderr, "Must provide serial number\n");
 595                         done(sl, 1);
 596                 }
 597                 serial = get_uint16(sl, AO_SERIAL_NUMBER);
 598                 if (!serial || serial == 0xffff) {
 599                         fprintf (stderr, "Invalid existing serial %d\n", serial);
 600                         done(sl, 1);
 601                 }
 602         }
 603
 604         if (!cal && AO_RADIO_CAL && was_flashed) {
 605                 cal = get_uint32(sl, AO_RADIO_CAL);
 606                 if (!cal || cal == 0xffffffff) {
 607                         fprintf (stderr, "Invalid existing rf cal %d\n", cal);
 608                         done(sl, 1);
 609                 }
 610         }
 611
 612         /* Write the config values into the flash image
 613          */
 614
 615         serial_int[0] = serial & 0xff;
 616         serial_int[1] = (serial >> 8) & 0xff;
 617
 618         if (!rewrite(load, AO_SERIAL_NUMBER, serial_int, sizeof (serial_int))) {
 619                 fprintf(stderr, "Cannot rewrite serial integer at %08x\n",
 620                         AO_SERIAL_NUMBER);
 621                 done(sl, 1);
 622         }
 623
 624         if (AO_USB_DESCRIPTORS) {
 625                 unsigned        usb_descriptors;
 626                 usb_descriptors = AO_USB_DESCRIPTORS - load->addr;
 627                 string_num = 0;
 628
 629                 while (load->buf[usb_descriptors] != 0 && usb_descriptors < load->len) {
 630                         if (load->buf[usb_descriptors+1] == AO_USB_DESC_STRING) {
 631                                 ++string_num;
 632                                 if (string_num == 4)
 633                                         break;
 634                         }
 635                         usb_descriptors += load->buf[usb_descriptors];
 636                 }
 637                 if (usb_descriptors >= load->len || load->buf[usb_descriptors] == 0 ) {
 638                         fprintf(stderr, "Cannot rewrite serial string at %08x\n", AO_USB_DESCRIPTORS);
 639                         done(sl, 1);
 640                 }
 641
 642                 serial_ucs2_len = load->buf[usb_descriptors] - 2;
 643                 serial_ucs2 = malloc(serial_ucs2_len);
 644                 if (!serial_ucs2) {
 645                         fprintf(stderr, "Malloc(%d) failed\n", serial_ucs2_len);
 646                         done(sl, 1);
 647                 }
 648                 s = serial;
 649                 for (i = serial_ucs2_len / 2; i; i--) {
 650                         serial_ucs2[i * 2 - 1] = 0;
 651                         serial_ucs2[i * 2 - 2] = (s % 10) + '0';
 652                         s /= 10;
 653                 }
 654                 if (!rewrite(load, usb_descriptors + 2 + load->addr, serial_ucs2, serial_ucs2_len)) {
 655                         fprintf (stderr, "Cannot rewrite USB descriptor at %08x\n", AO_USB_DESCRIPTORS);
 656                         done(sl, 1);
 657                 }
 658         }
 659
 660         if (cal && AO_RADIO_CAL) {
 661                 cal_int[0] = cal & 0xff;
 662                 cal_int[1] = (cal >> 8) & 0xff;
 663                 cal_int[2] = (cal >> 16) & 0xff;
 664                 cal_int[3] = (cal >> 24) & 0xff;
 665
 666                 if (!rewrite(load, AO_RADIO_CAL, cal_int, sizeof (cal_int))) {
 667                         fprintf(stderr, "Cannot rewrite radio calibration at %08x\n", AO_RADIO_CAL);
 668                         exit(1);
 669                 }
 670         }
 671
 672         /* And flash the resulting image to the device
 673          */
 674         if (stlink_write_flash(sl, load->addr, load->buf, load->len) < 0) {
 675                 fprintf (stderr, "\"%s\": Write failed\n", filename);
 676                 done(sl, 1);
 677         }
 678
 679         done(sl, 0);
 680 }