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
  73         /*
  74          * Find the symbols
  75          */
  76
  77         scn = NULL;
  78         while ((scn = elf_nextscn(e, scn)) != NULL) {
  79                 if (gelf_getshdr(scn, &shdr) != &shdr)
  80                         return 0;
  81
  82                 if (shdr.sh_type == SHT_SYMTAB) {
  83                         symbol_data = elf_getdata(scn, NULL);
  84                         symbol_count = shdr.sh_size / shdr.sh_entsize;
  85                         break;
  86                 }
  87         }
  88
  89         if (!symbol_data)
  90                 return 0;
  91
  92         for (i = 0; i < symbol_count; i++) {
  93                 gelf_getsym(symbol_data, i, &sym);
  94
  95                 symbol_name = elf_strptr(e, shdr.sh_link, sym.st_name);
  96
  97                 for (s = 0; s < NUM_SYMBOLS; s++)
  98                         if (!strcmp (ao_symbols[s].name, symbol_name)) {
  99                                 int     t;
 100                                 ao_symbols[s].addr = sym.st_value;
 101                                 if (ao_symbols[s].required)
 102                                         ++required;
 103                         }
 104         }
 105
 106         return required >= NUM_REQUIRED_SYMBOLS;
 107 }
 108
 109 struct load {
 110         uint32_t        addr;
 111         uint32_t        len;
 112         uint8_t         buf[0];
 113 };
 114
 115 struct load *
 116 new_load (uint32_t addr, uint32_t len)
 117 {
 118         struct load *new = calloc (1, sizeof (struct load) + len);
 119         if (!new)
 120                 abort();
 121
 122         new->addr = addr;
 123         new->len = len;
 124         return new;
 125 }
 126
 127 void
 128 load_paste(struct load *into, struct load *from)
 129 {
 130         if (from->addr < into->addr || into->addr + into->len < from->addr + from->len)
 131                 abort();
 132
 133         memcpy(into->buf + from->addr - into->addr, from->buf, from->len);
 134 }
 135
 136 /*
 137  * Make a new load structure large enough to hold the old one and
 138  * the new data
 139  */
 140 struct load *
 141 expand_load(struct load *from, uint32_t addr, uint32_t len)
 142 {
 143         struct load     *new;
 144
 145         if (from) {
 146                 uint32_t        from_last = from->addr + from->len;
 147                 uint32_t        last = addr + len;
 148
 149                 if (addr > from->addr)
 150                         addr = from->addr;
 151                 if (last < from_last)
 152                         last = from_last;
 153
 154                 len = last - addr;
 155
 156                 if (addr == from->addr && len == from->len)
 157                         return from;
 158         }
 159         new = new_load(addr, len);
 160         if (from) {
 161                 load_paste(new, from);
 162                 free (from);
 163         }
 164         return new;
 165 }
 166
 167 /*
 168  * Create a new load structure with data from the existing one
 169  * and the new data
 170  */
 171 struct load *
 172 load_write(struct load *from, uint32_t addr, uint32_t len, void *data)
 173 {
 174         struct load     *new;
 175
 176         new = expand_load(from, addr, len);
 177         memcpy(new->buf + addr - new->addr, data, len);
 178         return new;
 179 }
 180
 181 /*
 182  * Construct a large in-memory block for all
 183  * of the loaded sections of the program
 184  */
 185 static struct load *
 186 get_load(Elf *e)
 187 {
 188         Elf_Scn         *scn;
 189         size_t          shstrndx;
 190         GElf_Shdr       shdr;
 191         Elf_Data        *data;
 192         uint8_t         *buf;
 193         char            *got_name;
 194         size_t          nphdr;
 195         int             p;
 196         GElf_Phdr       phdr;
 197         struct load     *load = NULL;
 198
 199         if (elf_getshdrstrndx(e, &shstrndx) < 0)
 200                 return 0;
 201
 202         if (elf_getphdrnum(e, &nphdr) < 0)
 203                 return 0;
 204
 205         /*
 206          * As far as I can tell, all of the phdr sections should
 207          * be flashed to memory
 208          */
 209         for (p = 0; p < nphdr; p++) {
 210
 211                 /* Find this phdr */
 212                 gelf_getphdr(e, p, &phdr);
 213
 214                 /* Get the associated file section */
 215                 scn = gelf_offscn(e, phdr.p_offset);
 216
 217                 if (gelf_getshdr(scn, &shdr) != &shdr)
 218                         abort();
 219
 220                 data = elf_getdata(scn, NULL);
 221
 222                 /* Write the section data into the memory block */
 223                 load = load_write(load, phdr.p_paddr, phdr.p_filesz, data->d_buf);
 224         }
 225         return load;
 226 }
 227
 228 /*
 229  * Edit the to-be-written memory block
 230  */
 231 static int
 232 rewrite(struct load *load, unsigned addr, uint8_t *data, int len)
 233 {
 234         int             i;
 235
 236         if (addr < load->addr || load->addr + load->len < addr + len)
 237                 return 0;
 238
 239         printf("rewrite %04x:", addr);
 240         for (i = 0; i < len; i++)
 241                 printf (" %02x", load->buf[addr - load->addr + i]);
 242         printf(" ->");
 243         for (i = 0; i < len; i++)
 244                 printf (" %02x", data[i]);
 245         printf("\n");
 246         memcpy(load->buf + addr - load->addr, data, len);
 247 }
 248
 249 /*
 250  * Open the specified ELF file and
 251  * check for the symbols we need
 252  */
 253
 254 Elf *
 255 ao_open_elf(char *name)
 256 {
 257         int             fd;
 258         Elf             *e;
 259         Elf_Scn         *scn;
 260         Elf_Data        *symbol_data = NULL;
 261         GElf_Shdr       shdr;
 262         GElf_Sym        sym;
 263         size_t          n, shstrndx, sz;
 264         int             i, symbol_count, s;
 265         int             required = 0;
 266
 267         if (elf_version(EV_CURRENT) == EV_NONE)
 268                 return NULL;
 269
 270         fd = open(name, O_RDONLY, 0);
 271
 272         if (fd < 0)
 273                 return NULL;
 274
 275         e = elf_begin(fd, ELF_C_READ, NULL);
 276
 277         if (!e)
 278                 return NULL;
 279
 280         if (elf_kind(e) != ELF_K_ELF)
 281                 return NULL;
 282
 283         if (elf_getshdrstrndx(e, &shstrndx) != 0)
 284                 return NULL;
 285
 286         if (!find_symbols(e)) {
 287                 fprintf (stderr, "Cannot find required symbols\n");
 288                 return NULL;
 289         }
 290
 291         return e;
 292 }
 293
 294 /*
 295  * Read a 32-bit value from the target device with arbitrary
 296  * alignment
 297  */
 298 static uint32_t
 299 get_uint32(stlink_t *sl, uint32_t addr)
 300 {
 301         const           uint8_t *data = sl->q_buf;
 302         uint32_t        actual_addr;
 303         int             off;
 304         uint32_t        result;
 305
 306         sl->q_len = 0;
 307
 308         printf ("read 0x%x\n", addr);
 309
 310         actual_addr = addr & ~3;
 311
 312         stlink_read_mem32(sl, actual_addr, 8);
 313
 314         if (sl->q_len != 8)
 315                 abort();
 316
 317         off = addr & 3;
 318         result = data[off] | (data[off + 1] << 8) | (data[off+2] << 16) | (data[off+3] << 24);
 319         printf ("read 0x%08x = 0x%08x\n", addr, result);
 320         return result;
 321 }
 322
 323 /*
 324  * Read a 16-bit value from the target device with arbitrary
 325  * alignment
 326  */
 327 static uint16_t
 328 get_uint16(stlink_t *sl, uint32_t addr)
 329 {
 330         const           uint8_t *data = sl->q_buf;
 331         uint32_t        actual_addr;
 332         int             off;
 333         uint16_t        result;
 334
 335         sl->q_len = 0;
 336
 337
 338         actual_addr = addr & ~3;
 339
 340         stlink_read_mem32(sl, actual_addr, 8);
 341
 342         if (sl->q_len != 8)
 343                 abort();
 344
 345         off = addr & 3;
 346         result = data[off] | (data[off + 1] << 8);
 347         printf ("read 0x%08x = 0x%04x\n", addr, result);
 348         return result;
 349 }
 350
 351 /*
 352  * Check to see if the target device has been
 353  * flashed with a similar firmware image before
 354  *
 355  * This is done by looking for the same romconfig version,
 356  * which should be at the same location as the linker script
 357  * places this at 0x100 from the start of the rom section
 358  */
 359 static int
 360 check_flashed(stlink_t *sl)
 361 {
 362         uint16_t        romconfig_version = get_uint16(sl, AO_ROMCONFIG_VERSION);
 363         uint16_t        romconfig_check = get_uint16(sl, AO_ROMCONFIG_CHECK);
 364
 365         if (romconfig_version != (uint16_t) ~romconfig_check) {
 366                 fprintf (stderr, "Device has not been flashed before\n");
 367                 return 0;
 368         }
 369         return 1;
 370 }
 371
 372 static const struct option options[] = {
 373         { .name = "device", .has_arg = 1, .val = 'D' },
 374         { .name = "cal", .has_arg = 1, .val = 'c' },
 375         { .name = "serial", .has_arg = 1, .val = 's' },
 376         { 0, 0, 0, 0},
 377 };
 378
 379 static void usage(char *program)
 380 {
 381         fprintf(stderr, "usage: %s [--cal=<radio-cal>] [--serial=<serial>] file.elf\n", program);
 382         exit(1);
 383 }
 384
 385 void
 386 done(stlink_t *sl, int code)
 387 {
 388         if (sl) {
 389                 stlink_reset(sl);
 390                 stlink_run(sl);
 391                 stlink_exit_debug_mode(sl);
 392                 stlink_close(sl);
 393         }
 394         exit (code);
 395 }
 396
 397 int
 398 main (int argc, char **argv)
 399 {
 400         char                    *device = NULL;
 401         char                    *filename;
 402         Elf                     *e;
 403         char                    *serial_end;
 404         unsigned int            serial = 0;
 405         char                    *serial_ucs2;
 406         int                     serial_ucs2_len;
 407         char                    serial_int[2];
 408         unsigned int            s;
 409         int                     i;
 410         int                     string_num;
 411         uint32_t                cal = 0;
 412         char                    cal_int[4];
 413         char                    *cal_end;
 414         int                     c;
 415         stlink_t                *sl;
 416         int                     was_flashed = 0;
 417         struct load             *load;
 418
 419         while ((c = getopt_long(argc, argv, "D:c:s:", options, NULL)) != -1) {
 420                 switch (c) {
 421                 case 'D':
 422                         device = optarg;
 423                         break;
 424                 case 'c':
 425                         cal = strtoul(optarg, &cal_end, 10);
 426                         if (cal_end == optarg || *cal_end != '\0')
 427                                 usage(argv[0]);
 428                         break;
 429                 case 's':
 430                         serial = strtoul(optarg, &serial_end, 10);
 431                         if (serial_end == optarg || *serial_end != '\0')
 432                                 usage(argv[0]);
 433                         break;
 434                 default:
 435                         usage(argv[0]);
 436                         break;
 437                 }
 438         }
 439
 440         filename = argv[optind];
 441         if (filename == NULL)
 442                 usage(argv[0]);
 443
 444         /*
 445          * Open the source file and load the symbols and
 446          * flash data
 447          */
 448
 449         e = ao_open_elf(filename);
 450         if (!e) {
 451                 fprintf(stderr, "Cannot open file \"%s\"\n", filename);
 452                 exit(1);
 453         }
 454
 455         if (!find_symbols(e)) {
 456                 fprintf(stderr, "Cannot find symbols in \"%s\"\n", filename);
 457                 exit(1);
 458         }
 459
 460         if (!(load = get_load(e))) {
 461                 fprintf(stderr, "Cannot find program data in \"%s\"\n", filename);
 462                 exit(1);
 463         }
 464
 465         /* Connect to the programming dongle
 466          */
 467
 468         if (device) {
 469                 sl = stlink_v1_open(50);
 470         } else {
 471                 sl = stlink_open_usb(50);
 472
 473         }
 474         if (!sl) {
 475                 fprintf (stderr, "No STLink devices present\n");
 476                 done (sl, 1);
 477         }
 478
 479         sl->verbose = 50;
 480
 481         /* Verify that the loaded image fits entirely within device flash
 482          */
 483         if (load->addr < sl->flash_base ||
 484             sl->flash_base + sl->flash_size < load->addr + load->len) {
 485                 fprintf (stderr, "\%s\": Invalid memory range 0x%08x - 0x%08x\n", filename,
 486                          load->addr, load->addr + load->len);
 487                 done(sl, 1);
 488         }
 489
 490         /* Enter debugging mode
 491          */
 492         if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE)
 493                 stlink_exit_dfu_mode(sl);
 494
 495         if (stlink_current_mode(sl) != STLINK_DEV_DEBUG_MODE)
 496                 stlink_enter_swd_mode(sl);
 497
 498         /* Go fetch existing config values
 499          * if available
 500          */
 501         was_flashed = check_flashed(sl);
 502
 503         if (!serial) {
 504                 if (!was_flashed) {
 505                         fprintf (stderr, "Must provide serial number\n");
 506                         done(sl, 1);
 507                 }
 508                 serial = get_uint16(sl, AO_SERIAL_NUMBER);
 509                 if (!serial || serial == 0xffff) {
 510                         fprintf (stderr, "Invalid existing serial %d\n", serial);
 511                         done(sl, 1);
 512                 }
 513         }
 514
 515         if (!cal && AO_RADIO_CAL && was_flashed) {
 516                 cal = get_uint32(sl, AO_RADIO_CAL);
 517                 if (!cal || cal == 0xffffffff) {
 518                         fprintf (stderr, "Invalid existing rf cal %d\n", cal);
 519                         done(sl, 1);
 520                 }
 521         }
 522
 523         /* Write the config values into the flash image
 524          */
 525
 526         serial_int[0] = serial & 0xff;
 527         serial_int[1] = (serial >> 8) & 0xff;
 528
 529         if (!rewrite(load, AO_SERIAL_NUMBER, serial_int, sizeof (serial_int))) {
 530                 fprintf(stderr, "Cannot rewrite serial integer at %08x\n",
 531                         AO_SERIAL_NUMBER);
 532                 done(sl, 1);
 533         }
 534
 535         if (AO_USB_DESCRIPTORS) {
 536                 unsigned        usb_descriptors;
 537                 usb_descriptors = AO_USB_DESCRIPTORS - load->addr;
 538                 string_num = 0;
 539
 540                 while (load->buf[usb_descriptors] != 0 && usb_descriptors < load->len) {
 541                         if (load->buf[usb_descriptors+1] == AO_USB_DESC_STRING) {
 542                                 ++string_num;
 543                                 if (string_num == 4)
 544                                         break;
 545                         }
 546                         usb_descriptors += load->buf[usb_descriptors];
 547                 }
 548                 if (usb_descriptors >= load->len || load->buf[usb_descriptors] == 0 ) {
 549                         fprintf(stderr, "Cannot rewrite serial string at %08x\n", AO_USB_DESCRIPTORS);
 550                         done(sl, 1);
 551                 }
 552
 553                 serial_ucs2_len = load->buf[usb_descriptors] - 2;
 554                 serial_ucs2 = malloc(serial_ucs2_len);
 555                 if (!serial_ucs2) {
 556                         fprintf(stderr, "Malloc(%d) failed\n", serial_ucs2_len);
 557                         done(sl, 1);
 558                 }
 559                 s = serial;
 560                 for (i = serial_ucs2_len / 2; i; i--) {
 561                         serial_ucs2[i * 2 - 1] = 0;
 562                         serial_ucs2[i * 2 - 2] = (s % 10) + '0';
 563                         s /= 10;
 564                 }
 565                 if (!rewrite(load, usb_descriptors + 2 + load->addr, serial_ucs2, serial_ucs2_len)) {
 566                         fprintf (stderr, "Cannot rewrite USB descriptor at %08x\n", AO_USB_DESCRIPTORS);
 567                         done(sl, 1);
 568                 }
 569         }
 570
 571         if (cal && AO_RADIO_CAL) {
 572                 cal_int[0] = cal & 0xff;
 573                 cal_int[1] = (cal >> 8) & 0xff;
 574                 cal_int[2] = (cal >> 16) & 0xff;
 575                 cal_int[3] = (cal >> 24) & 0xff;
 576
 577                 if (!rewrite(load, AO_RADIO_CAL, cal_int, sizeof (cal_int))) {
 578                         fprintf(stderr, "Cannot rewrite radio calibration at %08x\n", AO_RADIO_CAL);
 579                         exit(1);
 580                 }
 581         }
 582
 583         /* And flash the resulting image to the device
 584          */
 585         if (stlink_write_flash(sl, load->addr, load->buf, load->len) < 0) {
 586                 fprintf (stderr, "\"%s\": Write failed\n", filename);
 587                 done(sl, 1);
 588         }
 589
 590         done(sl, 0);
 591 }