Tiny Bootblock, step 1/n.
authorPatrick Georgi <patrick.georgi@coresystems.de>
Wed, 23 Dec 2009 12:52:56 +0000 (12:52 +0000)
committerPatrick Georgi <patrick.georgi@coresystems.de>
Wed, 23 Dec 2009 12:52:56 +0000 (12:52 +0000)
Introduce the tiny bootblock infrastructure and use it on QEmu.

Signed-off-by: Patrick Georgi <patrick.georgi@coresystems.de>
Acked-by: Myles Watson <mylesgw@gmail.com>
Acked-by: Ronald G. Minnich <rminnich@gmail.com>
git-svn-id: svn://svn.coreboot.org/coreboot/trunk@4989 2b7e53f0-3cfb-0310-b3e9-8179ed1497e1

12 files changed:
Makefile
src/arch/i386/Kconfig
src/arch/i386/Makefile.bigbootblock.inc [new file with mode: 0644]
src/arch/i386/Makefile.inc
src/arch/i386/Makefile.tinybootblock.inc [new file with mode: 0644]
src/arch/i386/init/bootblock.c [new file with mode: 0644]
src/arch/i386/init/bootblock_prologue.c [new file with mode: 0644]
src/arch/i386/lib/walkcbfs.S [new file with mode: 0644]
src/cpu/x86/32bit/entry32.lds
src/mainboard/emulation/qemu-x86/Kconfig
src/mainboard/emulation/qemu-x86/Makefile.inc
util/cbfstool/cbfs-mkstage.c

index 8216146debe8b5a98d6b9ca09663236959c7cfdf..3b964b81f8bf8508063c2e5b26a941369e7f9a7c 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -293,6 +293,7 @@ doxygen-clean:
 
 clean: doxygen-clean
        rm -f $(allobjs) $(obj)/coreboot*
+       rm -rf $(obj)/bootblock* $(obj)/romstage* $(obj)/location.*
        rm -f $(obj)/option_table.* $(obj)/crt0_includes.h $(obj)/ldscript
        rm -f $(obj)/mainboard/$(MAINBOARDDIR)/static.c $(obj)/mainboard/$(MAINBOARDDIR)/config.py $(obj)/mainboard/$(MAINBOARDDIR)/static.dot
        rm -f $(obj)/mainboard/$(MAINBOARDDIR)/auto.inc $(obj)/mainboard/$(MAINBOARDDIR)/crt0.s $(obj)/mainboard/$(MAINBOARDDIR)/crt0.disasm
index c2f023f2b11ebb785d5e56e1d52aaab7455ade69..19bd63a0d490d60c85ded27cd8e3eb25ae87a03e 100644 (file)
@@ -40,3 +40,6 @@ config MAX_REBOOT_CNT
        int
        default 3
 
+config TINY_BOOTBLOCK
+       bool
+       default n
diff --git a/src/arch/i386/Makefile.bigbootblock.inc b/src/arch/i386/Makefile.bigbootblock.inc
new file mode 100644 (file)
index 0000000..06af5d2
--- /dev/null
@@ -0,0 +1,35 @@
+ifdef POST_EVALUATION
+
+#######################################################################
+# Build the final rom image
+
+$(obj)/coreboot.pre: $(obj)/coreboot.bootblock $(CBFSTOOL)
+       rm -f $@
+       $(CBFSTOOL) $@ create $(CONFIG_COREBOOT_ROMSIZE_KB)K $(obj)/coreboot.bootblock
+
+#######################################################################
+# Build the bootblock
+
+$(obj)/coreboot.bootblock: $(obj)/coreboot
+       @printf "    OBJCOPY    $(subst $(obj)/,,$(@))\n"
+       $(OBJCOPY) -O binary $< $@
+
+$(obj)/ldscript.ld: $(ldscripts) $(obj)/ldoptions
+       printf 'INCLUDE "ldoptions"\n' > $@
+       printf '$(foreach ldscript,$(ldscripts),INCLUDE "$(ldscript:$(obj)/%=%)"\n)' >> $@
+
+$(obj)/crt0_includes.h: $(crt0s)
+       printf '$(foreach crt0,config.h $(crt0s),#include "$(crt0:$(obj)/%=%)"\n)' > $@
+
+$(obj)/mainboard/$(MAINBOARDDIR)/crt0.o: $(obj)/mainboard/$(MAINBOARDDIR)/crt0.s
+       $(CC) -I$(obj) -Wa,-acdlns -c -o $@ $<  > $(dir $@)/crt0.disasm
+
+$(obj)/mainboard/$(MAINBOARDDIR)/crt0.s: $(src)/arch/i386/init/crt0.S.lb $(obj)/crt0_includes.h
+       $(CC) -x assembler-with-cpp -DASSEMBLY -E -I$(src)/include -I$(src)/arch/i386/include -I$(obj) -include $(obj)/config.h -I. -I$(src) $< > $@.new && mv $@.new $@
+
+$(obj)/coreboot: $(initobjs) $(obj)/ldscript.ld
+       @printf "    LINK       $(subst $(obj)/,,$(@))\n"
+       $(CC) -nostdlib -nostartfiles -static -o $@ -L$(obj) -T $(obj)/ldscript.ld $(initobjs)
+       $(NM) -n $(obj)/coreboot | sort > $(obj)/coreboot.map
+
+endif
index e2f464d2853642e0a689cba20cb7670073b6f67f..ef4b90af0c28615809326845ef68ea64d12f25d9 100644 (file)
@@ -11,10 +11,8 @@ ifdef POST_EVALUATION
 
 #######################################################################
 # Build the final rom image
-
-$(obj)/coreboot.rom: $(obj)/coreboot.bootblock $(obj)/coreboot_ram $(CBFSTOOL)
-       rm -f $@
-       $(CBFSTOOL) $@ create $(CONFIG_COREBOOT_ROMSIZE_KB)K $(obj)/coreboot.bootblock
+$(obj)/coreboot.rom: $(obj)/coreboot.pre $(obj)/coreboot_ram $(CBFSTOOL)
+       cp $(obj)/coreboot.pre $@
        if [ -f fallback/coreboot_apc ]; \
        then \
                $(CBFSTOOL) $@ add-stage fallback/coreboot_apc fallback/coreboot_apc $(CBFS_COMPRESS_FLAG); \
@@ -33,36 +31,10 @@ endif
        @printf "    CBFSPRINT  $(subst $(obj)/,,$(@))\n\n"
        $(CBFSTOOL) $(obj)/coreboot.rom print
 
-
-#######################################################################
-# Build the bootblock
-
-$(obj)/coreboot.bootblock: $(obj)/coreboot
-       @printf "    OBJCOPY    $(subst $(obj)/,,$(@))\n"
-       $(OBJCOPY) -O binary $< $@
-
-$(obj)/ldscript.ld: $(ldscripts) $(obj)/ldoptions
-       printf 'INCLUDE "ldoptions"\n' > $@
-       printf '$(foreach ldscript,$(ldscripts),INCLUDE "$(ldscript:$(obj)/%=%)"\n)' >> $@
-
-$(obj)/crt0_includes.h: $(crt0s)
-       printf '$(foreach crt0,config.h $(crt0s),#include "$(crt0:$(obj)/%=%)"\n)' > $@
-
-$(obj)/mainboard/$(MAINBOARDDIR)/crt0.o: $(obj)/mainboard/$(MAINBOARDDIR)/crt0.s
-       $(CC) -I$(obj) -Wa,-acdlns -c -o $@ $<  > $(dir $@)/crt0.disasm
-
-$(obj)/mainboard/$(MAINBOARDDIR)/crt0.s: $(src)/arch/i386/init/crt0.S.lb $(obj)/crt0_includes.h
-       $(CC) -x assembler-with-cpp -DASSEMBLY -E -I$(src)/include -I$(src)/arch/i386/include -I$(obj) -include $(obj)/config.h -I. -I$(src) $< > $@.new && mv $@.new $@
-
-$(obj)/coreboot: $(initobjs) $(obj)/ldscript.ld
-       @printf "    LINK       $(subst $(obj)/,,$(@))\n"
-       $(CC) -nostdlib -nostartfiles -static -o $@ -L$(obj) -T $(obj)/ldscript.ld $(initobjs)
-       $(NM) -n $(obj)/coreboot | sort > $(obj)/coreboot.map
-
 #######################################################################
 # i386 specific tools
 
-$(obj)/option_table.h $(obj)/option_table.c: $(obj)/build_opt_tbl $(top)/src/mainboard/$(MAINBOARDDIR)/cmos.layout
+$(obj)/option_table.h $(obj)/option_table.c $(obj)/arch/i386/../../option_table.c: $(obj)/build_opt_tbl $(top)/src/mainboard/$(MAINBOARDDIR)/cmos.layout
        @printf "    OPTION     $(subst $(obj)/,,$(@))\n"
        $(obj)/build_opt_tbl --config $(top)/src/mainboard/$(MAINBOARDDIR)/cmos.layout --header $(obj)/option_table.h --option $(obj)/option_table.c
 
@@ -91,3 +63,9 @@ $(obj)/coreboot.a: $(objs)
 # done
 
 endif
+
+ifeq ($(CONFIG_TINY_BOOTBLOCK),y)
+include $(src)/arch/i386/Makefile.tinybootblock.inc
+else
+include $(src)/arch/i386/Makefile.bigbootblock.inc
+endif
diff --git a/src/arch/i386/Makefile.tinybootblock.inc b/src/arch/i386/Makefile.tinybootblock.inc
new file mode 100644 (file)
index 0000000..86a71a8
--- /dev/null
@@ -0,0 +1,87 @@
+ifdef POST_EVALUATION
+
+#######################################################################
+# Build the final rom image
+
+$(obj)/coreboot.pre1: $(obj)/coreboot.bootblock $(CBFSTOOL)
+       rm -f $@
+       $(CBFSTOOL) $@ create $(CONFIG_COREBOOT_ROMSIZE_KB)K $(obj)/coreboot.bootblock
+
+$(obj)/coreboot.pre: $(obj)/coreboot.romstage $(obj)/coreboot.pre1 $(CBFSTOOL)
+       rm -f $@
+       cp $(obj)/coreboot.pre1 $@
+       $(CBFSTOOL) $@ add-stage $(obj)/romstage.elf fallback/romstage x 0x$(shell cat $(obj)/location.txt)
+#FIXME: location.txt might require an offset of header size
+
+#######################################################################
+# Build the bootblock
+
+$(obj)/coreboot.bootblock: $(obj)/bootblock.elf
+       @printf "    OBJCOPY    $(subst $(obj)/,,$(@))\n"
+       $(OBJCOPY) -O binary $< $@
+
+bootblock_lds := $(src)/arch/i386/init/ldscript_failover.lb
+bootblock_lds += $(src)/cpu/x86/16bit/entry16.lds
+bootblock_lds += $(src)/cpu/x86/16bit/reset16.lds
+bootblock_lds += $(src)/arch/i386/lib/id.lds
+
+bootblock_inc := $(src)/arch/i386/init/bootblock_prologue.c
+bootblock_inc += $(src)/cpu/x86/16bit/entry16.inc
+bootblock_inc += $(src)/cpu/x86/16bit/reset16.inc
+bootblock_inc += $(src)/cpu/x86/32bit/entry32.inc
+bootblock_inc += $(src)/arch/i386/lib/id.inc
+bootblock_inc += $(obj)/mainboard/$(MAINBOARDDIR)/bootblock.inc
+bootblock_inc += $(src)/arch/i386/lib/walkcbfs.S
+
+$(obj)/bootblock/ldscript.ld: $(bootblock_ldscripts) $(obj)/ldoptions
+       mkdir -p $(obj)/bootblock
+       printf '$(foreach ldscript,ldoptions $(bootblock_lds),INCLUDE "$(ldscript)"\n)' > $@
+
+$(obj)/bootblock/bootblock.c: $(bootblock_inc)
+       mkdir -p $(obj)/bootblock
+       printf '$(foreach crt0,config.h $(bootblock_inc),#include "$(crt0)"\n)' > $@
+
+$(obj)/mainboard/$(MAINBOARDDIR)/bootblock.o: $(obj)/mainboard/$(MAINBOARDDIR)/bootblock.s
+       $(CC) -I$(obj) -Wa,-acdlns -c -o $@ $<  > $(dir $@)/crt0.disasm
+
+$(obj)/mainboard/$(MAINBOARDDIR)/bootblock.s: $(obj)/bootblock/bootblock.c
+       $(CC) -x assembler-with-cpp -DASSEMBLY -E -I$(src)/include -I$(src)/arch/i386/include -I$(obj) -I$(obj)/bootblock -include $(obj)/config.h -I. -I$(src) $< > $@.new && mv $@.new $@
+
+$(obj)/mainboard/$(MAINBOARDDIR)/bootblock.inc: $(obj)/romcc $(src)/arch/i386/init/bootblock.c
+       $(obj)/romcc $(ROMCCFLAGS) $(INCLUDES) $(src)/arch/i386/init/bootblock.c -o $@
+
+$(obj)/bootblock.elf: $(obj)/mainboard/$(MAINBOARDDIR)/bootblock.o $(obj)/bootblock/ldscript.ld
+       @printf "    LINK       $(subst $(obj)/,,$(@))\n"
+       $(CC) -nostdlib -nostartfiles -static -o $@ -L$(obj) -T $(obj)/bootblock/ldscript.ld $<
+       $(NM) -n $(obj)/bootblock.elf | sort > $(obj)/bootblock.map
+
+#######################################################################
+# Build the romstage
+$(obj)/coreboot.romstage: $(obj)/coreboot.pre1 $(initobjs) $(obj)/romstage/ldscript.ld
+       @printf "    LINK       $(subst $(obj)/,,$(@))\n"
+       printf "CONFIG_ROMBASE = 0x0;\n" > $(obj)/location.ld
+       $(CC) -nostdlib -nostartfiles -static -o $(obj)/romstage.elf -L$(obj) -T $(obj)/romstage/ldscript.ld $(initobjs)
+       $(OBJCOPY) -O binary $(obj)/romstage.elf $(obj)/romstage.bin
+       printf "CONFIG_ROMBASE = 0x" > $(obj)/location.ld
+       $(CBFSTOOL) $(obj)/coreboot.pre1 locate $(obj)/romstage.bin fallback/romstage $(CONFIG_XIP_ROM_SIZE) > $(obj)/location.txt
+       cat $(obj)/location.txt >> $(obj)/location.ld
+       printf ";\n" >> $(obj)/location.ld
+       $(CC) -nostdlib -nostartfiles -static -o $(obj)/romstage.elf -L$(obj) -T $(obj)/romstage/ldscript.ld $(initobjs)
+       $(NM) -n $(obj)/romstage.elf | sort > $(obj)/romstage.map
+       $(OBJCOPY) -O binary $(obj)/romstage.elf $@
+
+$(obj)/romstage/ldscript.ld: $(ldscripts) $(obj)/ldoptions
+       mkdir -p $(obj)/romstage
+       printf '$(foreach ldscript,ldoptions location.ld $(ldscripts),INCLUDE "$(ldscript:$(obj)/%=%)"\n)' > $@
+
+$(obj)/romstage/crt0_includes.h: $(crt0s)
+       mkdir -p $(obj)/romstage
+       printf '$(foreach crt0,config.h $(crt0s),#include "$(crt0:$(obj)/%=%)"\n)' > $@
+
+$(obj)/mainboard/$(MAINBOARDDIR)/crt0.o: $(obj)/mainboard/$(MAINBOARDDIR)/crt0.s
+       $(CC) -I$(obj) -Wa,-acdlns -c -o $@ $<  > $(dir $@)/crt0.disasm
+
+$(obj)/mainboard/$(MAINBOARDDIR)/crt0.s: $(src)/arch/i386/init/crt0.S.lb $(obj)/romstage/crt0_includes.h
+       $(CC) -x assembler-with-cpp -DASSEMBLY -E -I$(src)/include -I$(src)/arch/i386/include -I$(obj) -I$(obj)/romstage -include $(obj)/config.h -I. -I$(src) $< > $@.new && mv $@.new $@
+
+endif
diff --git a/src/arch/i386/init/bootblock.c b/src/arch/i386/init/bootblock.c
new file mode 100644 (file)
index 0000000..eea0198
--- /dev/null
@@ -0,0 +1,24 @@
+static unsigned long findstage(char* target)
+{
+       unsigned long entry;
+       asm volatile (
+               "mov $1f, %%esp\n\t"
+               "jmp walkcbfs\n\t"
+               "1:\n\t" : "=a" (entry) : "S" (target) : "ebx", "ecx", "edx", "edi", "ebp", "esp");
+       return entry;
+}
+
+static void call(unsigned long addr)
+{
+       asm volatile ("jmp %0\n\t" : : "r" (addr));
+}
+
+static void main(void)
+{
+       const char* target1 = "fallback/romstage";
+       unsigned long entry;
+       entry = findstage(target1);
+       if (entry) call(entry);
+       asm volatile ("1:\n\thlt\n\tjmp 1b\n\t");
+}
+
diff --git a/src/arch/i386/init/bootblock_prologue.c b/src/arch/i386/init/bootblock_prologue.c
new file mode 100644 (file)
index 0000000..f921c3f
--- /dev/null
@@ -0,0 +1,37 @@
+/* -*- asm -*-
+ * $ $
+ *
+ */
+
+/* 
+ * Copyright (C) 1996-2002 Markus Franz Xaver Johannes Oberhumer
+ *
+ * This file is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * Originally this code was part of ucl the data compression library
+ * for upx the ``Ultimate Packer of eXecutables''.
+ *
+ * - Converted to gas assembly, and refitted to work with etherboot.
+ *   Eric Biederman 20 Aug 2002
+ * - Merged the nrv2b decompressor into crt0.base of coreboot
+ *   Eric Biederman 26 Sept 2002
+ */
+
+
+#include <arch/asm.h>
+#include <arch/intel.h>
+#include <console/loglevel.h>  
+
+/*
+ * This is the entry code the code in .reset section
+ * jumps to this address.
+ *
+ */
+.section ".rom.data", "a", @progbits
+.section ".rom.text", "ax", @progbits
+
+       intel_chip_post_macro(0x01)             /* delay for chipsets */
+
diff --git a/src/arch/i386/lib/walkcbfs.S b/src/arch/i386/lib/walkcbfs.S
new file mode 100644 (file)
index 0000000..aba0453
--- /dev/null
@@ -0,0 +1,94 @@
+#define CBFS_HEADER_PTR 0xfffffffc
+
+#define CBFS_HEADER_MAGIC 0
+#define CBFS_HEADER_VERSION (CBFS_HEADER_MAGIC + 4)
+#define CBFS_HEADER_ROMSIZE (CBFS_HEADER_VERSION + 4)
+#define CBFS_HEADER_BOOTBLOCKSIZE (CBFS_HEADER_ROMSIZE + 4)
+#define CBFS_HEADER_ALIGN (CBFS_HEADER_BOOTBLOCKSIZE + 4)
+#define CBFS_HEADER_OFFSET (CBFS_HEADER_ALIGN + 4)
+
+#define CBFS_FILE_MAGIC 0
+#define CBFS_FILE_LEN (CBFS_FILE_MAGIC + 8)
+#define CBFS_FILE_TYPE (CBFS_FILE_LEN + 4)
+#define CBFS_FILE_CHECKSUM (CBFS_FILE_TYPE + 4)
+#define CBFS_FILE_OFFSET (CBFS_FILE_CHECKSUM + 4)
+
+#define CBFS_FILE_STRUCTSIZE (CBFS_FILE_OFFSET + 4)
+
+#define CBFS_STAGE_COMPRESSION 0
+#define CBFS_STAGE_ENTRY (CBFS_STAGE_COMPRESSION + 4)
+#define CBFS_STAGE_LOAD (CBFS_STAGE_ENTRY + 8)
+#define CBFS_STAGE_LEN (CBFS_STAGE_LOAD + 8)
+#define CBFS_STAGE_MEMLEN (CBFS_STAGE_LEN + 4)
+
+/*
+  input %esi: filename
+  input %esp: return address (not pointer to return address!)
+  output %eax: entry point
+  clobbers %ebx, %ecx, %edx, %edi, %ebp
+*/
+walkcbfs:
+       mov %esi, %ebp /* stash away filename pointer */
+       mov $0, %edx
+1:
+       cmpb $0, (%edx,%esi)
+       jz 2f
+       add $1, %edx
+       jmp 1b
+2:
+       add $1, %edx
+       mov CBFS_HEADER_PTR, %eax
+       mov CBFS_HEADER_ROMSIZE(%eax), %ecx
+       bswap %ecx
+       mov $0, %ebx
+       sub %ecx, %ebx
+       mov CBFS_HEADER_OFFSET(%eax), %ecx
+       bswap %ecx
+       add %ecx, %ebx
+       mov CBFS_HEADER_ALIGN(%eax), %eax
+       bswap %eax
+       sub $1, %eax
+
+walker:
+       mov %ebp, %esi
+       mov %ebx, %edi
+       add $CBFS_FILE_STRUCTSIZE, %edi /* edi = address of first byte after struct cbfs_file */
+       mov %edx, %ecx
+       repe cmpsb
+       # zero flag set if strings are equal
+       jnz tryharder
+
+       # we found it!
+       mov CBFS_FILE_OFFSET(%ebx), %eax
+       bswap %eax
+       add %ebx, %eax
+       add $CBFS_STAGE_ENTRY, %eax /* eax = ((cbfs_stage* (cbfs_file* ebx)->offset)->entry) */
+       mov 0(%eax), %eax
+       jmp *%esp
+
+tryharder:
+       mov CBFS_FILE_OFFSET(%ebx), %ecx
+       bswap %ecx
+       add %ebx, %ecx
+       mov CBFS_FILE_LEN(%ebx), %edi
+       bswap %edi
+       add %edi, %ecx
+       add %eax, %ecx
+       mov %eax, %edi
+       not %edi
+       and %edi, %ecx
+       mov %ecx, %ebx
+
+       /* look if we should exit */
+       mov CBFS_HEADER_PTR, %esi
+       mov CBFS_HEADER_ROMSIZE(%esi), %ecx
+       bswap %ecx
+       not %ecx
+       add $1, %ecx
+
+       cmp %ebx, %ecx
+       /* if we're still inside the ROM area, jump back */
+       jbe walker
+
+       mov $0, %eax
+       jmp *%esp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..471b5f7005b0a09ee64c5f3d4d1f0c261c2da703 100644 (file)
@@ -0,0 +1 @@
+ENTRY(protected_start)
index 38d460df76038222713186bcdd539a7cba525622..5200e269bc0ed2a4cdf83483471169a91e278185 100644 (file)
@@ -6,6 +6,7 @@ config BOARD_EMULATION_QEMU_X86
        select HAVE_PIRQ_TABLE
        select BOARD_ROMSIZE_KB_256
        select WARNINGS_ARE_ERRORS
+       select TINY_BOOTBLOCK
 
 config MAINBOARD_DIR
        string
index 977cb4ad501e180fce6e2a9849608e56f0a77133..0f4aedff98c2f7d3c6222835dd6ab3dfbde3c793 100644 (file)
@@ -1,18 +1,12 @@
 initobj-y += crt0.o
-crt0-y += ../../../../src/cpu/x86/16bit/entry16.inc
 crt0-y += ../../../../src/cpu/x86/32bit/entry32.inc
-crt0-y += ../../../../src/cpu/x86/16bit/reset16.inc
-crt0-y += ../../../../src/arch/i386/lib/id.inc
 crt0-y += auto.inc
 
 obj-y += mainboard.o
 obj-$(CONFIG_GENERATE_PIRQ_TABLE) += irq_tables.o
 
 ldscript-y += ../../../../src/arch/i386/init/ldscript_fallback_cbfs.lb
-ldscript-y += ../../../../src/cpu/x86/16bit/entry16.lds
 ldscript-y += ../../../../src/cpu/x86/32bit/entry32.lds
-ldscript-y += ../../../../src/cpu/x86/16bit/reset16.lds
-ldscript-y += ../../../../src/arch/i386/lib/id.lds
 
 ifdef POST_EVALUATION
 
index 693d7cc0990e9725bb67965e02dc3d4537d0bb96..6e8daef508729c950dca28c50d99c04e7606072b 100644 (file)
@@ -122,6 +122,10 @@ int parse_elf_to_stage(unsigned char *input, unsigned char **output,
                        mem_end = mend;
        }
 
+       if (data_start < *location) {
+               data_start = *location;
+       }
+
        /* allocate an intermediate buffer for the data */
        buffer = calloc(data_end - data_start, 1);
 
@@ -133,6 +137,7 @@ int parse_elf_to_stage(unsigned char *input, unsigned char **output,
        /* Copy the file data into the buffer */
 
        for (i = 0; i < headers; i++) {
+               unsigned int l_start, l_offset = 0;
 
                if (elf32_to_native(phdr[i].p_type) != PT_LOAD)
                        continue;
@@ -140,9 +145,15 @@ int parse_elf_to_stage(unsigned char *input, unsigned char **output,
                if (elf32_to_native(phdr[i].p_memsz) == 0)
                        continue;
 
-               memcpy(buffer + (elf32_to_native(phdr[i].p_paddr) - data_start),
-                      &header[elf32_to_native(phdr[i].p_offset)],
-                      elf32_to_native(phdr[i].p_filesz));
+               l_start = elf32_to_native(phdr[i].p_paddr);
+               if (l_start < *location) {
+                       l_offset = *location - l_start;
+                       l_start = *location;
+               }
+
+               memcpy(buffer + (l_start - data_start),
+                      &header[elf32_to_native(phdr[i].p_offset)+l_offset],
+                      elf32_to_native(phdr[i].p_filesz)-l_offset);
        }
 
        /* Now make the output buffer */