1 files changed, 315 insertions, 179 deletions

diff --git a/src/model/reladd.c b/src/model/reladd.c
index 549afcf..bc13b97 100644
--- a/src/model/reladd.c
+++ b/src/model/reladd.c
@@ -1,177 +1,292 @@
 #include <assert.h>
 #include <stdlib.h>
+#include <string.h>
 #include <sys/types.h>
 #include <libelf/gelf.h>
 #include <libelfu/libelfu.h>
 
-typedef enum Destsegment {
-  DS_UNKNOWN,
-  DS_TEXT,
-  DS_DATA,
-} Destsegment;
 
 
-static Destsegment destsegment(ElfuScn *ms)
+static GElf_Word makeSpaceAtOffset(ElfuElf *me, GElf_Off offset, GElf_Word size)
 {
-  if (!(ms->shdr.sh_flags & SHF_ALLOC)) {
-    return DS_UNKNOWN;
+  ElfuPhdr *mp;
+  ElfuScn *ms;
+  /* Force a minimum alignment, just to be sure. */
+  GElf_Word align = 64;
+
+  /* Find maximum alignment size by which we have to shift.
+   * Assumes alignment sizes are always 2^x. */
+  CIRCLEQ_FOREACH(mp, &me->phdrList, elem) {
+    if (mp->phdr.p_offset >= offset) {
+      if (mp->phdr.p_align > align) {
+        align = mp->phdr.p_align;
+      }
+    }
   }
 
-  if (!(ms->shdr.sh_flags & SHF_WRITE)
-      && (ms->shdr.sh_flags & SHF_EXECINSTR)) {
-    return DS_TEXT;
-  } else if ((ms->shdr.sh_flags & SHF_WRITE)
-             && !(ms->shdr.sh_flags & SHF_EXECINSTR)) {
-    return DS_DATA;
+  CIRCLEQ_FOREACH(ms, &me->orphanScnList, elemChildScn) {
+    if (ms->shdr.sh_offset >= offset) {
+      if (ms->shdr.sh_addralign > align) {
+        align = ms->shdr.sh_addralign;
+      }
+    }
   }
 
-  return DS_UNKNOWN;
+  size = ROUNDUP(size, align);
+
+  /* Shift stuff */
+  CIRCLEQ_FOREACH(mp, &me->phdrList, elem) {
+    if (mp->phdr.p_type != PT_LOAD) {
+      continue;
+    }
+
+    if (mp->phdr.p_offset >= offset) {
+      ElfuScn *ms;
+      ElfuPhdr *mpc;
+
+      mp->phdr.p_offset += size;
+      CIRCLEQ_FOREACH(mpc, &mp->childPhdrList, elemChildPhdr) {
+        mpc->phdr.p_offset += size;
+      }
+      CIRCLEQ_FOREACH(ms, &mp->childScnList, elemChildScn) {
+        ms->shdr.sh_offset += size;
+      }
+    }
+  }
+
+  CIRCLEQ_FOREACH(ms, &me->orphanScnList, elemChildScn) {
+    if (ms->shdr.sh_offset >= offset) {
+      ms->shdr.sh_offset += size;
+    }
+  }
+
+  if (me->ehdr.e_phoff >= offset) {
+    me->ehdr.e_phoff += size;
+  }
+
+  if (me->ehdr.e_shoff >= offset) {
+    me->ehdr.e_shoff += size;
+  }
+
+  return size;
 }
 
 
 
-static ElfuScn* insertSection(ElfuElf *me, ElfuElf *mrel, ElfuScn *ms)
+/* Finds a suitable PHDR to insert a hole into and expands it
+ * if necessary.
+ * Returns memory address the hole will be mapped to, or 0 if
+ * the operation failed. */
+static GElf_Addr getSpaceInPhdr(ElfuElf *me, GElf_Word size,
+                                GElf_Word align, int w, int x,
+                                ElfuPhdr **injPhdr)
 {
-  ElfuPhdr *mp;
   ElfuPhdr *first = NULL;
   ElfuPhdr *last = NULL;
-  ElfuScn *newscn = NULL;
-  ElfuPhdr *injAnchor;
-  int searchForCode = 0;
-
-  switch (destsegment(ms)) {
-    case DS_TEXT:
-      searchForCode = 1;
-    case DS_DATA:
-      newscn = elfu_mCloneScn(ms);
-      if (!newscn) {
-        return NULL;
-      }
+  ElfuPhdr *mp;
 
-      /* Find first and last LOAD PHDRs. */
-      CIRCLEQ_FOREACH(mp, &me->phdrList, elem) {
-        if (mp->phdr.p_type != PT_LOAD) {
-          continue;
-        }
+  assert(!(w && x));
 
-        if (!first || mp->phdr.p_vaddr < first->phdr.p_vaddr) {
-          first = mp;
-        }
-        if (!last || mp->phdr.p_vaddr > last->phdr.p_vaddr) {
-          /* No need to check p_memsz as segments may not overlap in memory. */
-          last = mp;
+  /* Find first and last LOAD PHDRs.
+   * Don't compare p_memsz - segments don't overlap in memory. */
+  CIRCLEQ_FOREACH(mp, &me->phdrList, elem) {
+    if (mp->phdr.p_type != PT_LOAD) {
+      continue;
+    }
+    if (!first || mp->phdr.p_vaddr < first->phdr.p_vaddr) {
+      first = mp;
+    }
+    if (!last || mp->phdr.p_vaddr > last->phdr.p_vaddr) {
+      last = mp;
+    }
+  }
+
+  if ((w && (last->phdr.p_flags & PF_W))
+      || (x && (last->phdr.p_flags & PF_X))) {
+    /* Need to append. */
+    GElf_Off injOffset = OFFS_END(last->phdr.p_offset, last->phdr.p_filesz);
+    GElf_Word injSpace = 0;
+    GElf_Word nobitsize = last->phdr.p_memsz - last->phdr.p_filesz;
+
+    /* Expand NOBITS if any */
+    if (nobitsize > 0) {
+      GElf_Off endOff = OFFS_END(last->phdr.p_offset, last->phdr.p_filesz);
+      GElf_Off endAddr = OFFS_END(last->phdr.p_vaddr, last->phdr.p_filesz);
+      ElfuScn *ms;
+
+      ELFU_INFO("Expanding NOBITS at address 0x%jx...\n", endAddr);
+
+      CIRCLEQ_FOREACH(ms, &last->childScnList, elemChildScn) {
+        if (ms->shdr.sh_offset == endOff) {
+          assert(ms->shdr.sh_type == SHT_NOBITS);
+          assert(ms->shdr.sh_size == nobitsize);
+          ms->data.d_buf = malloc(ms->shdr.sh_size);
+          memset(ms->data.d_buf, '\0', ms->shdr.sh_size);
+          if (!ms->data.d_buf) {
+            ELFU_WARN("mExpandNobits: Could not allocate %jd bytes for NOBITS expansion. Data may be inconsistent.\n", ms->shdr.sh_size);
+            assert(0);
+            goto ERROR;
+          }
+
+          ms->data.d_align = 1;
+          ms->data.d_off  = 0;
+          ms->data.d_type = ELF_T_BYTE;
+          ms->data.d_size = ms->shdr.sh_size;
+          ms->data.d_version = elf_version(EV_NONE);
+
+          ms->shdr.sh_type = SHT_PROGBITS;
+          ms->shdr.sh_addr = endAddr;
         }
       }
 
-      if (searchForCode) {
-        if ((first->phdr.p_flags & PF_X) && !(first->phdr.p_flags & PF_W)) {
-          injAnchor = first;
-        } else if ((last->phdr.p_flags & PF_X) && !(last->phdr.p_flags & PF_W)) {
-          injAnchor = last;
-        } else {
-          injAnchor = NULL;
-        }
-      } else {
-        if ((first->phdr.p_flags & PF_W) && !(first->phdr.p_flags & PF_X)) {
-          injAnchor = first;
-        } else if ((last->phdr.p_flags & PF_W) && !(last->phdr.p_flags & PF_X)) {
-          injAnchor = last;
-        } else {
-          injAnchor = NULL;
-        }
+      injSpace += makeSpaceAtOffset(me, endOff, nobitsize);
+      injSpace -= nobitsize;
+      injOffset += nobitsize;
+      last->phdr.p_filesz += nobitsize;
+      assert(last->phdr.p_filesz == last->phdr.p_memsz);
+    }
+
+    /* Calculate how much space we need, taking alignment into account */
+    size += ROUNDUP(injOffset, align) - injOffset;
+
+    /* If there is not enough space left, create even more. */
+    if (injSpace < size) {
+      injSpace += makeSpaceAtOffset(me, injOffset, size - injSpace);
+    }
+    assert(injSpace >= size);
+
+    /* Remap ourselves */
+    last->phdr.p_filesz += size;
+    last->phdr.p_memsz += size;
+
+    injOffset = ROUNDUP(injOffset, align);
+
+    if (injPhdr) {
+      *injPhdr = last;
+    }
+    return last->phdr.p_vaddr + (injOffset - last->phdr.p_offset);
+  } else if ((w && (first->phdr.p_flags & PF_W))
+             || (x && (first->phdr.p_flags & PF_X))) {
+    /* Need to prepend or split up the PHDR. */
+    GElf_Off injOffset = OFFS_END(first->phdr.p_offset, first->phdr.p_filesz);
+    ElfuScn *ms;
+
+    /* Round up size to take PHDR alignment into account.
+     * We assume that this is a multiple of the alignment asked for. */
+    assert(first->phdr.p_align >= align);
+    size = ROUNDUP(size, first->phdr.p_align);
+
+    /* Find first section. We assume there is at least one. */
+    assert(!CIRCLEQ_EMPTY(&first->childScnList));
+    injOffset = CIRCLEQ_FIRST(&first->childScnList)->shdr.sh_offset;
+
+    /* Move our sections */
+    CIRCLEQ_FOREACH(ms, &first->childScnList, elemChildScn) {
+      if (ms->shdr.sh_offset >= injOffset) {
+        ms->shdr.sh_offset += size;
       }
+    }
 
-      if (!injAnchor) {
-        ELFU_WARN("insertSection: Could not find injection anchor.\n"
-                  "               It has to be the first or last segment in the memory image.\n");
+    /* Move our PHDRs */
+    CIRCLEQ_FOREACH(mp, &first->childPhdrList, elemChildPhdr) {
+      if (mp->phdr.p_offset >= injOffset) {
+        mp->phdr.p_offset += size;
       } else {
-        GElf_Off injOffset;
-
-        /* If the anchor is first or last, insert before or after */
-        if (injAnchor == first) {
-          /* Find first section and inject before it */
-          ElfuScn *firstScn = elfu_mScnFirstInSegment(me, injAnchor);
-          if (!firstScn) {
-            ELFU_WARN("insertSection: mScnFirstInSegment failed.\n");
+        mp->phdr.p_vaddr -= size;
+        mp->phdr.p_paddr -= size;
+      }
+    }
 
-            // TODO: Error handling
-          } else {
-            injOffset = firstScn->shdr.sh_offset;
+    /* Move other PHDRs and sections */
+    assert(size <= makeSpaceAtOffset(me, injOffset, size));
 
-            ELFU_INFO("Inserting %s at offset 0x%jx...\n",
-                      elfu_mScnName(mrel, ms),
-                      injOffset);
+    /* Remap ourselves */
+    first->phdr.p_vaddr -= size;
+    first->phdr.p_paddr -= size;
+    first->phdr.p_filesz += size;
+    first->phdr.p_memsz += size;
 
-            /* Make space */
-            elfu_mInsertSpaceBefore(me, injOffset, ms->shdr.sh_size);
+    injOffset = ROUNDUP(injOffset, align);
 
-            /* Update memory offset */
-            newscn->shdr.sh_addr = injAnchor->phdr.p_vaddr;
+    if (injPhdr) {
+      *injPhdr = first;
+    }
+    return first->phdr.p_vaddr + (injOffset - first->phdr.p_offset);
+  }
 
-            /* Insert into chain of sections */
-            elfu_mInsertScnInChainBefore(me, firstScn, newscn);
-          }
-        } else {
-          /* Find last section and inject after it */
-          ElfuScn *lastScn = elfu_mScnLastInSegment(me, injAnchor);
-          if (!lastScn) {
-            ELFU_WARN("insertSection: mScnLastInSegment failed.\n");
+  ERROR:
+  if (injPhdr) {
+    *injPhdr = NULL;
+  }
+  return 0;
+}
 
-            // TODO: Error handling
-          } else {
-            injOffset = lastScn->shdr.sh_offset + SCNFILESIZE(&lastScn->shdr);
 
-            ELFU_INFO("Expanding at offset 0x%jx...\n",
-                      injOffset);
 
-            /* Expand NOBITS sections at injection site, if any. */
-            elfu_mExpandNobits(me, injOffset);
 
-            /* Recalculate injOffset in case we expanded a NOBITS section */
-            lastScn = elfu_mScnLastInSegment(me, injAnchor);
-            injOffset = lastScn->shdr.sh_offset + SCNFILESIZE(&lastScn->shdr);
+static ElfuScn* insertSection(ElfuElf *me, ElfuElf *mrel, ElfuScn *oldscn)
+{
+  ElfuScn *newscn = NULL;
+  GElf_Addr injAddr;
+  GElf_Off injOffset;
+  ElfuPhdr *injPhdr;
 
-            ELFU_INFO("Inserting %s at offset 0x%jx...\n",
-                      elfu_mScnName(mrel, ms),
-                      injOffset);
+  if (oldscn->shdr.sh_flags & SHF_ALLOC) {
+    newscn = elfu_mCloneScn(oldscn);
+    if (!newscn) {
+      return NULL;
+    }
 
-            /* Make space */
-            elfu_mInsertSpaceAfter(me, injOffset, ms->shdr.sh_size);
 
-            /* Update memory offset */
-            newscn->shdr.sh_addr = injAnchor->phdr.p_vaddr + (injOffset - injAnchor->phdr.p_offset);
+    injAddr = getSpaceInPhdr(me, newscn->shdr.sh_size,
+                             newscn->shdr.sh_addralign,
+                             newscn->shdr.sh_flags & SHF_WRITE,
+                             newscn->shdr.sh_flags & SHF_EXECINSTR,
+                             &injPhdr);
 
-            /* Insert into chain of sections */
-            elfu_mInsertScnInChainAfter(me, lastScn, newscn);
-          }
-        }
+    if (!injPhdr) {
+      ELFU_WARN("insertSection: Could not find a place to insert section.\n");
+      goto ERROR;
+    }
 
-        /* Update file offset in new section BEFORE we do anything else */
-        newscn->shdr.sh_offset = injOffset;
+    ELFU_INFO("Inserting %s at address 0x%jx...\n",
+              elfu_mScnName(mrel, oldscn),
+              injAddr);
 
-        /* Inject name */
-        // TODO
-        newscn->shdr.sh_name = 0;
+    injOffset = injAddr - injPhdr->phdr.p_vaddr + injPhdr->phdr.p_offset;
 
-        // TODO: Relocate
+    newscn->shdr.sh_addr = injAddr;
+    newscn->shdr.sh_offset = injOffset;
 
-        return newscn;
+    if (CIRCLEQ_EMPTY(&injPhdr->childScnList)
+        || CIRCLEQ_LAST(&injPhdr->childScnList)->shdr.sh_offset < injOffset) {
+      CIRCLEQ_INSERT_TAIL(&injPhdr->childScnList, newscn, elemChildScn);
+    } else {
+      ElfuScn *ms;
+      CIRCLEQ_FOREACH(ms, &injPhdr->childScnList, elemChildScn) {
+        if (injOffset < ms->shdr.sh_offset) {
+          CIRCLEQ_INSERT_BEFORE(&injPhdr->childScnList, ms, newscn, elemChildScn);
+          break;
+        }
       }
-      break;
+    }
 
-    case DS_UNKNOWN:
-      ELFU_WARN("insertSection: Don't know where to insert ' %s with flags %jd (type %d).\n",
-                elfu_mScnName(mrel, ms),
-                ms->shdr.sh_flags,
-                ms->shdr.sh_type);
-    default:
+    /* Inject name */
+    // TODO
+    newscn->shdr.sh_name = 0;
+
+    // TODO: Relocate
+
+    return newscn;
+  } else {
       ELFU_WARN("insertSection: Skipping section %s with flags %jd (type %d).\n",
-                elfu_mScnName(mrel, ms),
-                ms->shdr.sh_flags,
-                ms->shdr.sh_type);
-      return NULL;
+                elfu_mScnName(mrel, oldscn),
+                oldscn->shdr.sh_flags,
+                oldscn->shdr.sh_type);
+      goto ERROR;
   }
 
+  ERROR:
   if (newscn) {
     // TODO: Destroy newscn
   }
@@ -179,66 +294,87 @@ static ElfuScn* insertSection(ElfuElf *me, ElfuElf *mrel, ElfuScn *ms)
 }
 
 
+int subScnAdd1(ElfuElf *mrel, ElfuScn *ms, void *aux1, void *aux2)
+{
+  (void)aux2;
+  ElfuElf *me = (ElfuElf*)aux1;
 
-void elfu_mReladd(ElfuElf *me, ElfuElf *mrel)
+  ElfuScn *newscn;
+
+  switch(ms->shdr.sh_type) {
+    case SHT_PROGBITS: /* 1 */
+      /* Find a place where it belongs and shove it in. */
+      newscn = insertSection(me, mrel, ms);
+      if (!newscn) {
+        ELFU_WARN("mReladd: Could not insert section %s (type %d), skipping.\n",
+                  elfu_mScnName(mrel, ms),
+                  ms->shdr.sh_type);
+      }
+      break;
+  }
+
+  return 0;
+}
+
+
+int subScnAdd2(ElfuElf *mrel, ElfuScn *ms, void *aux1, void *aux2)
 {
-  ElfuScn *ms;
+  (void)aux2;
+  ElfuElf *me = (ElfuElf*)aux1;
+  (void)me;
+
+  switch(ms->shdr.sh_type) {
+    case SHT_NULL: /* 0 */
+    case SHT_PROGBITS: /* 1 */
+      break;
 
+    case SHT_SYMTAB: /* 2 */
+    case SHT_DYNSYM: /* 11 */
+      /* Merge with the existing table. Take care of string tables also. */
+
+    case SHT_STRTAB: /* 3 */
+      /* May have to be merged with the existing string table for
+       * the symbol table. */
+
+    case SHT_RELA: /* 4 */
+    case SHT_REL: /* 9 */
+      /* Possibly append this in memory to the section model
+       * that it describes. */
+
+    case SHT_NOBITS: /* 8 */
+      /* Expand this to SHT_PROGBITS, then insert as such. */
+
+    case SHT_HASH: /* 5 */
+    case SHT_DYNAMIC: /* 6 */
+    case SHT_SHLIB: /* 10 */
+    case SHT_SYMTAB_SHNDX: /* 18 */
+
+    /* Don't care about the next ones yet. I've never seen
+     * them and they can be implemented when necessary. */
+    case SHT_NOTE: /* 7 */
+    case SHT_INIT_ARRAY: /* 14 */
+    case SHT_FINI_ARRAY: /* 15 */
+    case SHT_PREINIT_ARRAY: /* 16 */
+    case SHT_GROUP: /* 17 */
+    case SHT_NUM: /* 19 */
+    default:
+      ELFU_WARN("mReladd: Skipping section %s (type %d).\n",
+                elfu_mScnName(mrel, ms),
+                ms->shdr.sh_type);
+  }
+
+  return 0;
+}
+
+
+void elfu_mReladd(ElfuElf *me, ElfuElf *mrel)
+{
   assert(me);
   assert(mrel);
 
-
   /* For each section in object file, guess how to insert it */
-  CIRCLEQ_FOREACH(ms, &mrel->scnList, elem) {
-    ElfuScn *newscn;
-
-    switch(ms->shdr.sh_type) {
-      case SHT_NULL: /* 0 */
-        continue;
-
-      case SHT_PROGBITS: /* 1 */
-        /* Find a place where it belongs and shove it in. */
-        newscn = insertSection(me, mrel, ms);
-        if (!newscn) {
-          ELFU_WARN("mReladd: Could not insert section %s (type %d), skipping.\n",
-                    elfu_mScnName(mrel, ms),
-                    ms->shdr.sh_type);
-        }
-        break;
-
-      case SHT_SYMTAB: /* 2 */
-      case SHT_DYNSYM: /* 11 */
-        /* Merge with the existing table. Take care of string tables also. */
-
-      case SHT_STRTAB: /* 3 */
-        /* May have to be merged with the existing string table for
-         * the symbol table. */
-
-      case SHT_RELA: /* 4 */
-      case SHT_REL: /* 9 */
-        /* Possibly append this in memory to the section model
-         * that it describes. */
-
-      case SHT_NOBITS: /* 8 */
-        /* Expand this to SHT_PROGBITS, then insert as such. */
-
-      case SHT_HASH: /* 5 */
-      case SHT_DYNAMIC: /* 6 */
-      case SHT_SHLIB: /* 10 */
-      case SHT_SYMTAB_SHNDX: /* 18 */
-
-      /* Don't care about the next ones yet. I've never seen
-       * them and they can be implemented when necessary. */
-      case SHT_NOTE: /* 7 */
-      case SHT_INIT_ARRAY: /* 14 */
-      case SHT_FINI_ARRAY: /* 15 */
-      case SHT_PREINIT_ARRAY: /* 16 */
-      case SHT_GROUP: /* 17 */
-      case SHT_NUM: /* 19 */
-      default:
-        ELFU_WARN("mReladd: Skipping section %s (type %d).\n",
-                  elfu_mScnName(mrel, ms),
-                  ms->shdr.sh_type);
-    }
-  }
+  elfu_mScnForall(mrel, subScnAdd1, me, NULL);
+
+  /* Do relocations and other stuff */
+  elfu_mScnForall(mrel, subScnAdd2, me, NULL);
 }