3 files changed, 76 insertions, 5 deletions

diff --git a/utils/sbtools/elf.c b/utils/sbtools/elf.c
index fed09cd6ff..57f38b8016 100644
--- a/utils/sbtools/elf.c
+++ b/utils/sbtools/elf.c
@@ -155,6 +155,21 @@ static struct elf_section_t *elf_add_section(struct elf_params_t *params)
     return sec;
 }
 
+static struct elf_segment_t *elf_add_segment(struct elf_params_t *params)
+{
+    struct elf_segment_t *seg = xmalloc(sizeof(struct elf_section_t));
+    if(params->first_segment == NULL)
+        params->first_segment = params->last_segment = seg;
+    else
+    {
+        params->last_segment->next = seg;
+        params->last_segment = seg;
+    }
+    seg->next = NULL;
+
+    return seg;
+}
+
 void elf_add_load_section(struct elf_params_t *params,
     uint32_t load_addr, uint32_t size, const void *section)
 {
@@ -406,7 +421,7 @@ bool elf_read_file(struct elf_params_t *params, elf_read_fn_t read,
     }
     /* run through sections */
     printf(user, false, "ELF file:\n");
-    for(int i = 1; i< ehdr.e_shnum; i++)
+    for(int i = 1; i < ehdr.e_shnum; i++)
     {
         uint32_t off = ehdr.e_shoff + i * ehdr.e_shentsize;
         Elf32_Shdr shdr;
@@ -437,9 +452,51 @@ bool elf_read_file(struct elf_params_t *params, elf_read_fn_t read,
         }
         
     }
+    /* run through segments */
+    for(int i = 1; i < ehdr.e_phnum; i++)
+    {
+        uint32_t off = ehdr.e_phoff + i * ehdr.e_phentsize;
+        Elf32_Phdr phdr;
+        memset(&phdr, 0, sizeof(phdr));
+        if(!read(user, off, &phdr, sizeof(phdr)))
+            error_printf("error reading elf segment header");
+        if(phdr.p_type != PT_LOAD)
+            continue;
+        struct elf_segment_t *seg = elf_add_segment(params);
+        seg->vaddr = phdr.p_vaddr;
+        seg->paddr = phdr.p_paddr;
+        seg->vsize = phdr.p_memsz;
+        seg->psize = phdr.p_filesz;
+        printf(user, false, "create segment [%#x,+%#x[ -> [%#x,+%#x[\n",
+            seg->vaddr, seg->vsize, seg->paddr, seg->psize);
+    }
+    
     return true;
 }
 
+uint32_t elf_translate_virtual_address(struct elf_params_t *params, uint32_t addr)
+{
+    struct elf_segment_t *seg = params->first_segment;
+    while(seg)
+    {
+        if(seg->vaddr <= addr && addr < seg->vaddr + seg->vsize)
+            return addr - seg->vaddr + seg->paddr;
+        seg = seg->next;
+    }
+    return addr;
+}
+
+void elf_translate_addresses(struct elf_params_t *params)
+{
+    struct elf_section_t *sec = params->first_section;
+    while(sec)
+    {
+        sec->addr = elf_translate_virtual_address(params, sec->addr);
+        sec = sec->next;
+    }
+    params->start_addr = elf_translate_virtual_address(params, params->start_addr);
+}
+
 bool elf_is_empty(struct elf_params_t *params)
 {
     return params->first_section == NULL;
diff --git a/utils/sbtools/elf.h b/utils/sbtools/elf.h
index b145bfffc5..d63b9a93b7 100644
--- a/utils/sbtools/elf.h
+++ b/utils/sbtools/elf.h
@@ -16,24 +16,35 @@ enum elf_section_type_t
 
 struct elf_section_t
 {
-    uint32_t addr;
-    uint32_t size;
+    uint32_t addr; /* virtual address */
+    uint32_t size; /* virtual size */
     enum elf_section_type_t type;
     /* <union> */
-    void *section;
-    uint32_t pattern;
+    void *section; /* data */
+    uint32_t pattern; /* fill pattern */
     /* </union> */
     struct elf_section_t *next;
     /* Internal to elf_write_file */
     uint32_t offset;
 };
 
+struct elf_segment_t
+{
+    uint32_t vaddr; /* virtual address */
+    uint32_t paddr; /* physical address */
+    uint32_t vsize; /* virtual size */
+    uint32_t psize; /* physical size */
+    struct elf_segment_t *next;
+};
+
 struct elf_params_t
 {
     bool has_start_addr;
     uint32_t start_addr;
     struct elf_section_t *first_section;
     struct elf_section_t *last_section;
+    struct elf_segment_t *first_segment;
+    struct elf_segment_t *last_segment;
 };
 
 typedef bool (*elf_read_fn_t)(void *user, uint32_t addr, void *buf, size_t count);
@@ -46,6 +57,8 @@ void elf_add_load_section(struct elf_params_t *params,
     uint32_t load_addr, uint32_t size, const void *section);
 void elf_add_fill_section(struct elf_params_t *params,
     uint32_t fill_addr, uint32_t size, uint32_t pattern);
+uint32_t elf_translate_virtual_address(struct elf_params_t *params, uint32_t addr);
+void elf_translate_addresses(struct elf_params_t *params);
 void elf_write_file(struct elf_params_t *params, elf_write_fn_t write, void *user);
 bool elf_read_file(struct elf_params_t *params, elf_read_fn_t read, elf_printf_fn_t printf,
     void *user);
diff --git a/utils/sbtools/elftosb.c b/utils/sbtools/elftosb.c
index 78d2e80bf9..cb5cc4c6db 100644
--- a/utils/sbtools/elftosb.c
+++ b/utils/sbtools/elftosb.c
@@ -817,6 +817,7 @@ static void load_elf_by_id(struct cmd_file_t *cmd_file, const char *id)
     close(fd);
     if(!src->loaded)
         bug("error loading elf file '%s' (id '%s')\n", src->filename, id);
+    elf_translate_addresses(&src->elf);
 }
 
 static void load_bin_by_id(struct cmd_file_t *cmd_file, const char *id)