From db01557ce493c435db177fda78653697ba2afa51 Mon Sep 17 00:00:00 2001
From: Keith Packard <keithp@keithp.com>
Date: Sat, 23 Mar 2013 02:10:38 -0700
Subject: [PATCH] ao-tools/ao-stmload: Be smarter about ELF parsing. Retry open
 on failure.

Figuring out what goes where is tricky; turns out we want to pull all
of the sections that map inside any program area that is supposed to
be loaded from the file.

So, we walk the program headers, then walk all of the section headers
looking for those that suck data from the same portion of the
file. Compute where in ROM each relevant section goes and build a full
ROM image in memory using that.

This patch also adds code to close and re-open the device if the first
open failed to do what we want. Much nicer to have the computer figure
out when the open succeeded rather than having people re-run the app.

Signed-off-by: Keith Packard <keithp@keithp.com>
---
 ao-tools/ao-stmload/ao-stmload.c | 114 ++++++++++++++++++++++++++-----
 1 file changed, 98 insertions(+), 16 deletions(-)

diff --git a/ao-tools/ao-stmload/ao-stmload.c b/ao-tools/ao-stmload/ao-stmload.c
index a471dcc4..89b818da 100644
--- 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;
@@ -199,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;
@@ -209,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
@@ -218,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();
+			}
 
-		data = elf_getdata(scn, NULL);
+			section_name = elf_strptr(e, shstrndx, shdr.sh_name);
 
-		/* Write the section data into the memory block */
-		load = load_write(load, phdr.p_paddr, phdr.p_filesz, data->d_buf);
+			if (phdr.p_offset <= shdr.sh_offset && shdr.sh_offset < phdr.p_offset + phdr.p_filesz) {
+					
+				if (shdr.sh_size == 0)
+					continue;
+
+				sh_paddr = phdr.p_paddr + shdr.sh_offset - phdr.p_offset;
+
+				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;
 }
@@ -422,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) {
@@ -472,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 ||
-- 
2.30.2