X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=ao-tools%2Fao-stmload%2Fao-stmload.c;h=89b818dab5082baa9bf57761ea82c1e0bdf2a241;hp=e689539bb41c5b3e372c3698d2ae601444fdf30c;hb=1f30b1f14dbab6e6ea94177e459c80732e31e433;hpb=20877ae9de8bb5d3a29e2a96024e53afbd396f55

diff --git a/ao-tools/ao-stmload/ao-stmload.c b/ao-tools/ao-stmload/ao-stmload.c
index e689539b..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;
@@ -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 ||