ao-tools/lib: Add loading support for 32-bit ihx files

[fw/altos] / ao-tools / ao-stmload / ao-stmload.c

diff --git a/ao-tools/ao-stmload/ao-stmload.c b/ao-tools/ao-stmload/ao-stmload.c
index e689539bb41c5b3e372c3698d2ae601444fdf30c..89b818dab5082baa9bf57761ea82c1e0bdf2a241 100644 (file)
--- a/ao-tools/ao-stmload/ao-stmload.c
+++ b/ao-tools/ao-stmload/ao-stmload.c
@@ -69,6 +69,11 @@ find_symbols (Elf *e)
        int             i, symbol_count, s;
        int             required = 0;
        char            *symbol_name;
+       char            *section_name;
+       size_t          shstrndx;
+
+       if (elf_getshdrstrndx(e, &shstrndx) < 0)
+               return 0;
 
        /*
         * Find the symbols
@@ -76,9 +81,29 @@ find_symbols (Elf *e)
 
        scn = NULL;
        while ((scn = elf_nextscn(e, scn)) != NULL) {
+
                if (gelf_getshdr(scn, &shdr) != &shdr)
                        return 0;
 
+#if 0
+               section_name = elf_strptr(e, shstrndx, shdr.sh_name);
+
+               printf ("name %s\n", section_name);
+
+               if (shdr.sh_type == SHT_PROGBITS)
+               {
+                       printf ("\ttype %lx\n", shdr.sh_type);
+                       printf ("\tflags %lx\n", shdr.sh_flags);
+                       printf ("\taddr %lx\n", shdr.sh_addr);
+                       printf ("\toffset %lx\n", shdr.sh_offset);
+                       printf ("\tsize %lx\n", shdr.sh_size);
+                       printf ("\tlink %lx\n", shdr.sh_link);
+                       printf ("\tinfo %lx\n", shdr.sh_info);
+                       printf ("\taddralign %lx\n", shdr.sh_addralign);
+                       printf ("\tentsize %lx\n", shdr.sh_entsize);
+               }
+#endif
+
                if (shdr.sh_type == SHT_SYMTAB) {
                        symbol_data = elf_getdata(scn, NULL);
                        symbol_count = shdr.sh_size / shdr.sh_entsize;
@@ -112,10 +137,17 @@ struct load {
        uint8_t         buf[0];
 };
 
+uint32_t round4(uint32_t a) {
+       return (a + 3) & ~3;
+}
+
 struct load *
 new_load (uint32_t addr, uint32_t len)
 {
-       struct load *new = calloc (1, sizeof (struct load) + len);
+       struct load *new;
+
+       len = round4(len);
+       new = calloc (1, sizeof (struct load) + len);
        if (!new)
                abort();
 
@@ -192,9 +224,15 @@ get_load(Elf *e)
        uint8_t         *buf;
        char            *got_name;
        size_t          nphdr;
-       int             p;
+       size_t          p;
        GElf_Phdr       phdr;
+       GElf_Addr       p_paddr;
+       GElf_Off        p_offset;
+       GElf_Addr       sh_paddr;
        struct load     *load = NULL;
+       char            *section_name;
+       size_t          nshdr;
+       size_t          s;
        
        if (elf_getshdrstrndx(e, &shstrndx) < 0)
                return 0;
@@ -202,6 +240,9 @@ get_load(Elf *e)
        if (elf_getphdrnum(e, &nphdr) < 0)
                return 0;
 
+       if (elf_getshdrnum(e, &nshdr) < 0)
+               return 0;
+
        /*
         * As far as I can tell, all of the phdr sections should
         * be flashed to memory
@@ -211,16 +252,54 @@ get_load(Elf *e)
                /* Find this phdr */
                gelf_getphdr(e, p, &phdr);
 
+               if (phdr.p_type != PT_LOAD)
+                       continue;
+
+               p_offset = phdr.p_offset;
                /* Get the associated file section */
-               scn = gelf_offscn(e, phdr.p_offset);
 
-               if (gelf_getshdr(scn, &shdr) != &shdr)
-                       abort();
+#if 0
+               printf ("offset %08x vaddr %08x paddr %08x filesz %08x memsz %08x\n",
+                       (uint32_t) phdr.p_offset,
+                       (uint32_t) phdr.p_vaddr,
+                       (uint32_t) phdr.p_paddr,
+                       (uint32_t) phdr.p_filesz,
+                       (uint32_t) phdr.p_memsz);
+#endif
+               
+               for (s = 0; s < nshdr; s++) {
+                       scn = elf_getscn(e, s);
+
+                       if (!scn) {
+                               printf ("getscn failed\n");
+                               abort();
+                       }
+                       if (gelf_getshdr(scn, &shdr) != &shdr) {
+                               printf ("gelf_getshdr failed\n");
+                               abort();
+                       }
+
+                       section_name = elf_strptr(e, shstrndx, shdr.sh_name);
+
+                       if (phdr.p_offset <= shdr.sh_offset && shdr.sh_offset < phdr.p_offset + phdr.p_filesz) {
+                                       
+                               if (shdr.sh_size == 0)
+                                       continue;
 
-               data = elf_getdata(scn, NULL);
+                               sh_paddr = phdr.p_paddr + shdr.sh_offset - phdr.p_offset;
 
-               /* Write the section data into the memory block */
-               load = load_write(load, phdr.p_paddr, phdr.p_filesz, data->d_buf);
+                               printf ("\tsize %08x rom %08x exec %08x %s\n",
+                                       (uint32_t) shdr.sh_size,
+                                       (uint32_t) sh_paddr,
+                                       (uint32_t) shdr.sh_addr,
+                                       section_name);
+
+                               data = elf_getdata(scn, NULL);
+
+                               /* Write the section data into the memory block */
+                               load = load_write(load, sh_paddr, shdr.sh_size, data->d_buf);
+                       }
+               }
        }
        return load;
 }
@@ -415,6 +494,7 @@ main (int argc, char **argv)
        stlink_t                *sl;
        int                     was_flashed = 0;
        struct load             *load;
+       int                     tries;
 
        while ((c = getopt_long(argc, argv, "D:c:s:", options, NULL)) != -1) {
                switch (c) {
@@ -465,19 +545,28 @@ main (int argc, char **argv)
        /* Connect to the programming dongle
         */
        
-       if (device) {
-               sl = stlink_v1_open(50);
-       } else {
-               sl = stlink_open_usb(50);
+       for (tries = 0; tries < 3; tries++) {
+               if (device) {
+                       sl = stlink_v1_open(50);
+               } else {
+                       sl = stlink_open_usb(50);
                
+               }
+               if (!sl) {
+                       fprintf (stderr, "No STLink devices present\n");
+                       done (sl, 1);
+               }
+
+               if (sl->chip_id != 0)
+                       break;
+               stlink_reset(sl);
+               stlink_close(sl);
        }
-       if (!sl) {
-               fprintf (stderr, "No STLink devices present\n");
-               done (sl, 1);
+       if (sl->chip_id == 0) {
+               fprintf (stderr, "Debugger connection failed\n");
+               done(sl, 1);
        }
 
-       sl->verbose = 50;
-
        /* Verify that the loaded image fits entirely within device flash
         */
        if (load->addr < sl->flash_base ||