diff --git a/common/cmd_elf.c b/common/cmd_elf.c
index 1d92bb3..104bf75 100644
--- a/common/cmd_elf.c
+++ b/common/cmd_elf.c
@@ -31,6 +31,7 @@ #endif
 
 int valid_elf_image (unsigned long addr);
 unsigned long load_elf_image (unsigned long addr);
+void safememcpy(void *d, void *s, size_t n);
 
 /* ======================================================================
  * Interpreter command to boot an arbitrary ELF image from memory.
@@ -263,54 +264,123 @@ #endif
  * ====================================================================== */
 unsigned long load_elf_image (unsigned long addr)
 {
-	Elf32_Ehdr *ehdr;		/* Elf header structure pointer     */
-	Elf32_Shdr *shdr;		/* Section header structure pointer */
-	unsigned char *strtab = 0;	/* String table pointer             */
-	unsigned char *image;		/* Binary image pointer             */
-	int i;				/* Loop counter                     */
-
+	Elf32_Ehdr *ehdr;				/* Elf header structure pointer     */
+	Elf32_Shdr *shdr;				/* Section header structure pointer */
+	int j;							/* Loop counter				     */
+	Elf32_Phdr *phdr,*phdr_last,*phdr_first; /* Program header struct pointer    */
+	unsigned long elfimage_moveaddr; /* If overlap, position to move ELF image */
 	/* -------------------------------------------------- */
 
 	ehdr = (Elf32_Ehdr *) addr;
+	printf ("## Attemping to load ELF image at 0x%08lx\n", addr);
+	/* Read program headers */
+	printf("## Extracting (%ld) program headers at offset 0x%08lx (%ld bytes)\n",
+		ehdr->e_phnum, ehdr->e_phoff, ehdr->e_phentsize);
+	if (ehdr->e_phnum == 0) {
+		printf("## ERROR: No program headers found. This is not an executable ELF.\n");
+		return 0;
+	}
 
-	/* Find the section header string table for output info */
-	shdr = (Elf32_Shdr *) (addr + ehdr->e_shoff +
-			       (ehdr->e_shstrndx * sizeof (Elf32_Shdr)));
-
-	if (shdr->sh_type == SHT_STRTAB)
-		strtab = (unsigned char *) (addr + shdr->sh_offset);
-
-	/* Load each appropriate section */
-	for (i = 0; i < ehdr->e_shnum; ++i) {
-		shdr = (Elf32_Shdr *) (addr + ehdr->e_shoff +
-				       (i * sizeof (Elf32_Shdr)));
-
-		if (!(shdr->sh_flags & SHF_ALLOC)
-		   || shdr->sh_addr == 0 || shdr->sh_size == 0) {
-			continue;
-		}
-
-		if (strtab) {
-			printf ("%sing %s @ 0x%08lx (%ld bytes)\n",
-				(shdr->sh_type == SHT_NOBITS) ?
-					"Clear" : "Load",
-				&strtab[shdr->sh_name],
-				(unsigned long) shdr->sh_addr,
-				(long) shdr->sh_size);
+	/* Test for overlap */
+	/* Find the first and last program header */
+	phdr_last = (Elf32_Phdr *) (addr + ehdr->e_phoff + (((int)ehdr->e_phnum-1)*ehdr->e_phentsize));
+	phdr_first = (Elf32_Phdr *) (addr + ehdr->e_phoff);
+#ifdef DEBUG
+	printf("  -> Checking for overlap\n");
+	printf("    - First Program header %ld:\n     offset=0x%x\n     vaddr=0x%08lx\n     paddr=0x%08lx\n     filesz=0x%x\n     memsz=0x%x\n\n",
+			ehdr->e_phnum-1, phdr_first->p_offset, phdr_first->p_vaddr, phdr_first->p_paddr, phdr_first->p_filesz, phdr_first->p_memsz);
+	printf("    -  Last Program header %ld:\n     offset=0x%x\n     vaddr=0x%08lx\n     paddr=0x%08lx\n     filesz=0x%x\n     memsz=0x%x\n\n",
+			ehdr->e_phnum-1, phdr_last->p_offset, phdr_last->p_vaddr, phdr_last->p_paddr, phdr_last->p_filesz, phdr_last->p_memsz);
+#endif
+	shdr = (Elf32_Shdr *)((unsigned long)(ehdr->e_shnum-1) * (unsigned long)sizeof(Elf32_Shdr) + (unsigned long)addr + (unsigned long)ehdr->e_shoff);
+	/* Test to see if the ELF image will overwrite the ELF header when 'extracted'. */
+	if (
+		((addr >= phdr_first->p_vaddr) && (addr <= (phdr_last->p_vaddr + phdr_last->p_memsz)))
+		||
+		((addr + (unsigned long)shdr->sh_offset + (unsigned long)shdr->sh_size >= phdr_first->p_vaddr) &&
+		(addr + (unsigned long)shdr->sh_offset + (unsigned long)shdr->sh_size <= (phdr_last->p_vaddr + phdr_last->p_memsz)))
+		) {
+		printf("## WARNING: VADDR WITHIN ELF HEADER MEMORY SPACE.\n");
+		/* Calculate where to move the ELF image - the end of the last
+		 * program header virtual address + memory size				*/
+		elfimage_moveaddr = (unsigned long)phdr_last->p_vaddr + (unsigned long)phdr_last->p_memsz + 1;
+		printf("## ATTEMPTING TO MOVE ELF IMAGE.\n");
+		printf("##  -> shdr=0x%08lx\n",shdr);
+		printf("##  -> position=0x%08lx (vaddr+memsz)\n",
+				elfimage_moveaddr);
+		printf("##  -> size=0x%08lx (shdr[%ld]->offs+sz=%x+%x)\n",
+				(unsigned long)shdr->sh_offset + (unsigned long)shdr->sh_size, shdr->sh_offset, shdr->sh_size );
+		safememcpy((void *)elfimage_moveaddr, (void *)addr, (unsigned long)shdr->sh_offset + (unsigned long)shdr->sh_size );
+		printf("## Retrying load_elf_image(0x%08lx)\n",elfimage_moveaddr);
+		return load_elf_image(elfimage_moveaddr);
+	}
+	
+	/* Process program headers */
+	for (j = 0; j < (unsigned int)ehdr->e_phnum; j++) {
+		phdr = (Elf32_Phdr *) (addr + ehdr->e_phoff + (j*ehdr->e_phentsize));
+#ifdef DEBUG
+		printf("  -> Program header %ld:\n     type=0x%x\n     offset=0x%x\n     vaddr=0x%08lx\n     paddr=0x%08lx\n     filesz=0x%x\n     memsz=0x%x\n     flags=0x%x\n     align=0x%x\n",
+				j,phdr->p_type, phdr->p_offset, phdr->p_vaddr, phdr->p_paddr, phdr->p_filesz, phdr->p_memsz, phdr->p_flags, phdr->p_align);
+#endif
+		if (phdr->p_type==PT_LOAD) {
+			printf("  -> TYPE:LOAD program header. Attempting to allocate memory.\n");
+			/* Move/clear sections */
+			if ((unsigned long)phdr->p_filesz != 0) {
+				printf("  -> Moving program section %ld from 0x%08lx to 0x%08lx (%ld bytes)\n",
+					j, (unsigned long)ehdr+(unsigned long)phdr->p_offset, phdr->p_vaddr, phdr->p_filesz);
+				safememcpy((void *)phdr->p_vaddr, (void *)( (unsigned long)ehdr+(unsigned long)phdr->p_offset), phdr->p_filesz);
+			}
+			if (phdr->p_memsz > phdr->p_filesz) { /* Zero BSS section */
+				printf("  -> Clearing .BSS section from 0x%08lx to 0x%08lx (%ld bytes)\n",
+				(unsigned long)phdr->p_vaddr + (unsigned long)phdr->p_filesz, (unsigned long)phdr->p_vaddr + (unsigned long)phdr->p_memsz,  (unsigned long)phdr->p_memsz - (unsigned long)phdr->p_filesz);
+				memset ((void *)(unsigned long)phdr->p_vaddr + (unsigned long)phdr->p_filesz, 0, (unsigned long)phdr->p_memsz - (unsigned long)phdr->p_filesz);
+			}
 		}
+#ifdef DEBUG
+		printf("  -> Program segment %ld processing complete.\n",j);
+#endif
+	}
+	printf("## Returning entry point at address 0x%08lx\n",
+		(unsigned long) ehdr->e_entry);
+	return ehdr->e_entry;
+}
 
-		if (shdr->sh_type == SHT_NOBITS) {
-			memset ((void *)shdr->sh_addr, 0, shdr->sh_size);
-		} else {
-			image = (unsigned char *) addr + shdr->sh_offset;
-			memcpy ((void *) shdr->sh_addr,
-				(const void *) image,
-				shdr->sh_size);
+/* ======================================================================
+ * A modified version of memcpy. This version will check if the memory
+ * segments overlap before performing the copy, and act accordingly.
+ * ====================================================================== */
+void safememcpy(void *d, void *s, size_t n) {
+#ifdef DEBUG
+	printf("## MEMCPY: Moving %ld bytes from 0x%08lx to 0x%08lx\n",n,s,d);
+#endif
+	if (n != 0) {
+		int addi = 1;
+		unsigned char *s1 = d;
+		unsigned char *s2 = s;
+		/* Check for overlap */
+		if ((d <= s+n) && (d >= s)) {
+#ifdef DEBUG
+			printf("## MEMCPY: OVERLAP DETECTED. Performing copy in descending order\n");
+#endif
+			/* Switch the order the copy should execute */
+			addi = -1;
+			s1 = s1+n;
+			s2 = s2+n;
 		}
-		flush_cache (shdr->sh_addr, shdr->sh_size);
+#ifdef DEBUG
+		if (((d+n) >= s) && ((d+n) <= (s+n)))
+			printf("## MEMCPY: OVERLAP DETECTED. Performing copy in ascending order\n");
+#endif
+		/* Perform memory copy */
+		do {
+			*s1 = *s2;
+			s1 = s1 + addi;
+			s2 = s2 + addi;
+		} while (--n != 0);
 	}
-
-	return ehdr->e_entry;
+#ifdef DEBUG
+	printf("## MEMCPY: Finished.\n");
+#endif
 }
 
 /* ====================================================================== */
