diff --git a/Makefile b/Makefile
index 310b2dd..995fd8f 100644
--- a/Makefile
+++ b/Makefile
@@ -13,8 +13,11 @@ else
 include config-default.mk
 endif
 
+LIBFDTOBJS      = libfdt/fdt.o libfdt/fdt_ro.o libfdt/fdt_wip.o \
+		  libfdt/fdt_sw.o libfdt/fdt_rw.o libfdt/fdt_strerror.o
 MONITOR		= monitor.S
 BOOTLOADER	= boot.S
+OBJS 		= boot.o c_start.o monitor.o semihosting.o string.o semi_loader.o $(LIBFDTOBJS)
 KERNEL		= uImage
 
 IMAGE		= linux-system.axf
@@ -36,27 +39,26 @@ semi: $(SEMIIMG)
 
 clean distclean:
 	rm -f $(IMAGE) $(SEMIIMG) \
-	boot.o model.lds monitor.o $(KERNEL) \
-	bootsemi.o modelsemi.lds
+	model.lds modelsemi.lds $(OBJS) $(KERNEL)
 
 $(KERNEL): $(KERNEL_SRC)/arch/arm/boot/uImage
 	cp $< $@
 
-$(IMAGE): boot.o monitor.o model.lds $(KERNEL) $(FILESYSTEM) Makefile
-	$(LD) -o $@ --script=model.lds
+$(IMAGE): $(OBJS) model.lds $(KERNEL) $(FILESYSTEM) Makefile
+	$(LD) -o $@ $(OBJS) --script=model.lds
 
-$(SEMIIMG): bootsemi.o monitor.o modelsemi.lds
-	$(LD) -o $@ --script=modelsemi.lds
+$(SEMIIMG): $(OBJS) modelsemi.lds
+	$(LD) -o $@ $(OBJS) --script=modelsemi.lds
 
 boot.o: $(BOOTLOADER)
 	$(CC) $(CPPFLAGS) -DKCMD='$(KCMD)' -c -o $@ $<
 
-bootsemi.o: $(BOOTLOADER)
-	$(CC) $(CPPFLAGS) -DSEMIHOSTING=1 -c -o $@ $<
-
 monitor.o: $(MONITOR)
 	$(CC) $(CPPFLAGS) -c -o $@ $<
 
+%.o: %.c
+	$(CC) $(CPPFLAGS) -O2 -ffreestanding -Ilibfdt -c -o $@ $<
+
 model.lds: $(LD_SCRIPT) Makefile
 	$(CC) $(CPPFLAGS) -E -P -C -o $@ $<
 
diff --git a/boot.S b/boot.S
index 823def8..61cd93f 100644
--- a/boot.S
+++ b/boot.S
@@ -12,51 +12,9 @@
 	.arch_extension virt
 	.text
 
-#ifdef SEMIHOSTING
-@ Helper definitions and macros for semihosting
-SYS_OPEN = 0x01
-SYS_CLOSE = 0x02
-SYS_WRITE0 = 0x04
-SYS_READ = 0x06
-SYS_FLEN = 0x0C
-SYS_GET_CMDLINE = 0x15
-SYS_REPORTEXC = 0x18
 
-.macro semihost what
-	@ Make a semihosting call. Note that on return r0 is always
-	@ either the return value or is trashed.
-	mov     r0, \what
-#if defined(MACH_MPS)
-	@ M profile semihosting is via bpkt
-	bkpt    0xab
-#elif defined(__thumb__)
-	@ Otherwise, different SVC numbers for ARM or Thumb mode
-	svc    0xab
-#else
-	svc     0x123456
-#endif
-.endm
-
-.macro exit
-	@ Exit via semihosting call
-	@ REASON_APP_EXIT = 0x20026
-	mov r1, 0x20000
-	orr r1, r1, 0x26
-	semihost SYS_REPORTEXC
-	@ This point is never reached.
-.endm
-
-.macro fail string
-	@ Print the message pointed to by string out via semihosting,
-	@ and then exit.
-	ldr r1, =\string
-	semihost SYS_WRITE0
-	exit
-.endm
-#endif
-
-	.globl	_start
-_start:
+	.globl	start
+start:
 #ifdef SMP
 #ifdef VEXPRESS
 	@
@@ -143,7 +101,7 @@ into_hyp_mode:
 	@ Secondary CPUs (following the RealView SMP booting protocol)
 	@
 
-	ldr	r1, =filesystem - 0x100
+	ldr	r1, =fs_start - 0x100
 	adr	r2, 1f
 	ldmia	r2, {r3 - r7}			@ move the code to a location
 	stmia	r1, {r3 - r7}			@ less likely to be overridden
@@ -180,359 +138,39 @@ into_hyp_mode:
 	orr	r1, #0x0001			@ cr
 	str	r1, [r0, #0x30]
 
-#ifdef SEMIHOSTING
-	@
-	@ Get kernel, initrd, command line via semihosting
-	@
-
-	mov     r0, SYS_GET_CMDLINE
-	ldr     r1, =semiblk
-	ldr     r2, =sh_cmdline
-	str     r2, [r1]
-	@ We have huge amounts of space between us and where we're
-	@ going to put the kernel, so we can allow for an enormous
-	@ command line length.
-	mov     r3, 0x2000
-	str     r3, [r1, #4]
-	semihost SYS_GET_CMDLINE
-	cmp     r0, 0
-	bne     1f
-	ldr     r3, [r1, #4]
-	cmp     r3, 0x2000
-	blt     2f
-1:
-	@ Failed to get command line, or truncated
-	fail get_cmd_failed
-
-2:
-	@ That gives us one big NUL terminated string.
-	@ We expect:
-	@ --kernel kernelfilename
-	@ --initrd initrdfilename
-	@ --
-	@ and the remainder of the string is kernel args
-
-	@ r2 is still the command line here
-1:
-	@ Skip any leading whitespace, stop if we hit the end
-	ldrb r0, [r2]
-	cmp r0, 32
-	bne 2f
-	add r2, r2, 1
-	b 1b
-2:
-	cmp r0, 0
-	bne 3f
-	@ If we hit the end of the string before '--' it's invalid
-	fail bad_cmd_line
-3:
-	ldr r1, =dashdashkernel
-	bl matchword
-	cmp r0, 0
-	beq 2f
-	@ found --kernel, r2 points at next word
-	@ load at offset -0x40 to allow for uImage header
-	ldr r1, =kernel
-	sub r1, r1, 0x40
-	bl loadfile
-	b 1b
-
-2:
-	adr r1, dashdashinitrd
-	bl matchword
-	cmp r0, 0
-	beq 2f
-	@ found --initrd, r2 points at next word
-	ldr r1, =filesystem
-	bl loadfile
-	@ now r3 is length of the initrd, fix up our ATAGS
-	ldr r1, =atag_initrd_sz
-	str r3, [r1]
-	b 1b
-
-2:
-	adr r1, dashdash
-	bl matchword
-	cmp r0, 0
-	beq 2f
-	@ found --, r2 points at next word
-	@ handle rest of string (if any) as kernel args
-	@ If we didn't have an initrd, write the ATAG_CMDLINE
-	@ over the top of the ATAG_INITRD2
-	ldr r0, =atag_cmdline
-	ldr r4, [r0, #4]   @ ATAG_CMDLINE magic number
-	ldr r1, =atag_initrd_sz
-	ldr r1, [r1]
-	cmp r1, 0
-        itt eq
-	ldreq r0, =atag_initrd
-	streq r4, [r0, #4]
-	add r3, r0, #8
-	@ copy string from r2 to r3
-1:      ldrb r1, [r2],#1
-	strb r1, [r3],#1
-	cmp r1, 0
-	bne 1b
-	@ zero-pad to word boundary
-1:      tst r3, #3
-	beq 1f
-	strb r1, [r3],#1
-	b 1b
-1:      @ now write the length word
-	sub r4, r3, r0
-	lsr r4, r4, #2
-	str r4, [r0]
-	@ and terminate the ATAGS list with an ATAG_NONE node
-	str r1, [r3, #0]
-	str r1, [r3, #4]
-
-	b run_kernel
-
-2:
-	@ unrecognised option
-	fail bad_cmd_line
-
-matchword:
-	@ Subroutine: if the first word (up to space or end)
-	@ in the string r2 matches the word pointed at by r1
-	@ then return with r0 != 0 and r2 pointing at the
-	@ space/end. Otherwise return with r0 = 0, r2 unchanged.
-	mov r3, r2
-1:
-	ldrb r0, [r2]
-	ldrb r4, [r1]
-	cmp r4, 0
-	beq 1f
-	cmp r4, r0
-	bne matchfail
-	add r2, r2, 1
-	add r1, r1, 1
-	b 1b
-1:
-	@ end of matched string, is r2 at end of word?
-	cmp r0, 32
-        itt ne
-	cmpne r0, 0
-	bne matchfail
-	@ Success
-	mov r0, 1
-	bx lr
-1:      @ not end of string, match?
-matchfail:
-	mov r2, r3
-	mov r0, 0
-	bx lr
+	@ Now we've got rid of the secondary CPUs, set up a stack
+	@ for CPU 0 so we can write most of this in C.
+	ldr     sp, =stacktop
 
-loadfile:
-	@ Subroutine: r2 points to a filename argument (possibly with
-	@ leading spaces, space or NUL terminated), r1 is the address
-	@ to load it at. Load the file via semihosting to the
-	@ specified address. On exit r2 points to the space/NUL
-	@ after the filename, and r3 is the length of the file in bytes
-	@ We modify the filename string in place, temporarily
-	mov r5, r1
-	adr r1, loading_str
-	semihost SYS_WRITE0
-	@ skip leading spaces
-1:      ldrb r0, [r2]
-	cmp r0, 32
-	bne 1f
-	add r2, r2, 1
-	b 1b
-1:      mov r3, r2
-	@ advance until next space or NUL
-1:      ldrb r0, [r2]
-	cmp r0, 32
-        ite ne
-	cmpne r0, 0
-	beq 1f
-	add r2, r2, 1
-	b 1b
-1:      @ filename is r3 to r2, save terminating byte and nul terminate
-	mov r4, r0
-	mov r0, 0
-	strb r0, [r2]
-	mov r1, r3
-	semihost SYS_WRITE0
-	adr r1, colonspace_str
-	semihost SYS_WRITE0
-	adr r1, semiblk
-	str r3, [r1]     @ filename
-	mov r0, 1
-	str r0, [r1, #4] @ file mode: "rb"
-	subs r0, r2, r3
-	bne 1f
-	fail nofilename
-1:      str r0, [r1, #8] @ file name length
-	semihost SYS_OPEN
-	cmp r0, -1
-	bne 1f
-	fail openfailed
-1:      @ now we can restore the terminating byte
-	strb r4, [r2]
-	mov r4, r0
-	str r0, [r1]     @ filehandle
-	semihost SYS_FLEN
-	cmp r0, -1
-	bne 1f
-	fail flenfailed
-1:      adr r1, semiblk
-	mov r3, r0
-	str r4, [r1]     @ filehandle
-	str r5, [r1, #4] @ buffer
-	str r0, [r1, #8] @ length
-	semihost SYS_READ
-	cmp r0, 0
-	beq 1f
-	fail readfailed
-1:      adr r1, semiblk
-	str r4, [r1]
-	semihost SYS_CLOSE
-	cmp r0, 0
-	beq 1f
-	fail closefailed
-1:      @ Success! r2 is pointing to the space/NUL after the filename,
-	@ r3 is the length of the file in bytes
-	adr r1, ok_str
-	semihost SYS_WRITE0
-	bx lr
+	@ And call the C entrypoint
+	bl      c_start
+	@ Never reached
+1:	b 1b
 
-#endif
-run_kernel:
 	@
-	@ Kernel parameters
+	@ Function for C code to make semihosting calls:
 	@
-	mov	r0, #0
-#ifdef MACH_MPS
-	ldr	r1, =10000			@ MPS (temporary)
-#elif defined (VEXPRESS)
-	ldr	r1, =2272			@ Versatile Express
+	.globl __semi_call
+__semi_call:
+#if defined(MACH_MPS)
+	@ M profile semihosting is via bpkt
+	bkpt    0xab
+#elif defined(__thumb__)
+	@ Otherwise, different SVC numbers for ARM or Thumb mode
+	svc    0xab
 #else
-	ldr	r1, =827			@ RealView/EB
-#endif
-	adr	r2, atags
-	mov	r3, #0
-	ldr	lr, =kernel
-#ifdef THUMB2_KERNEL
-	orr	lr, lr, #1			@ Thumb-2 kernel
+	svc     0x123456
 #endif
-	mov     pc, lr				@ jump to the kernel
-
+	mov pc, lr
 
 	@
 	@ Data
 	@
-
-#ifdef SEMIHOSTING
-	.org 0x500
-	@ Put the constant pool here as otherwise the assembler will
-	@ put it immediately after our atags and it will be overwritten
-	@ by the semihosting command line.
-	.ltorg
-
-	@ block of four words used to pass/return values in semihosting calls
-	.align 2
-semiblk:
-	.long 0,0,0,0
-
-dashdashkernel:
-	.asciz "--kernel"
-	.align 2
-dashdashinitrd:
-	.asciz "--initrd"
-	.align 2
-dashdash:
-	.asciz "--"
-	.align 2
-
-get_cmd_failed:
-	.asciz "Failed to get semihosting command line\n"
-	.align 2
-bad_cmd_line:
-	.asciz "Bad command line format (unknown option?)\n"
-	.align 2
-nofilename:
-	.asciz "Expected filename argument\n"
-	.align 2
-openfailed:
-	.asciz "open failed!\n"
-	.align 2
-flenfailed:
-	.asciz "could not find length of file!\n"
-	.align 2
-readfailed:
-	.asciz "could not read file!\n"
-	.align 2
-closefailed:
-	.asciz "could not close file!\n"
-	.align 2
-loading_str:
-	.asciz "Loading: "
-	.align 2
-colonspace_str:
-	.asciz ": "
-	.align 2
-ok_str:
-	.asciz "OK\n"
-	.align 2
-atags:
-	@ Template for our ATAGs: we will edit these: the INITRD2
-	@ is optional and the CMDLINE will be expanded to include
-	@ the actual command line.
-	@ The important thing here is that any editing we do
-	@ to the template should only make it shorter, so we
-	@ don't accidentally overwrite the semihosting commandline
-	@ until the very end when we copy the tail end of the
-	@ semihosting command line into the ATAG_CMDLINE node
-	@ as the kernel parameters.
-
-	@ ATAG_CORE
-	.long 2
-	.long 0x54410001
-
-	@ ATAG_INITRD2
-atag_initrd:
-	.long	4
-	.long	0x54420005
-	.long	filesystem    @ address
-atag_initrd_sz:
-	.long	0        @ size
-
-	@ ATAG_CMDLINE
-atag_cmdline:
-	.long 0          @ length
-	.long 0x54410009
-	@ command line string will start here
-
-sh_cmdline:
-	@ Semihosting command line will be written here
-
-#else /* not SEMIHOSTING */
-	.org	0x200
-	@ Static ATAGS for when kernel/etc are compiled into the ELF file
-atags:
-	@ ATAG_CORE
-	.long	2
-	.long	0x54410001
-
-	@ ATAG_CMDLINE
-	.long	(1f - .) >> 2
-	.long	0x54410009
-        /* The kernel boot command line is defined in the Make system */
-        .asciz KCMD
-	.align	2
-1:
-
-#ifdef USE_INITRD
-	@ ATAG_INITRD2
-	.long	4
-	.long	0x54420005
-	.long	filesystem
-	.long	fs_size
+	/* The kernel boot command line for builtin kernels is defined in the Make system */
+	.globl kernel_cmd
+	.globl kernel_cmd_end
+kernel_cmd:
+#ifdef KCMD
+	.asciz KCMD
 #endif
-
-	@ ATAG_NONE
-	.long	0
-	.long	0x00000000
-
-#endif /* not SEMIHOSTING */
+kernel_cmd_end:
diff --git a/c_start.c b/c_start.c
new file mode 100644
index 0000000..dbdcc62
--- /dev/null
+++ b/c_start.c
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2012 Linaro Limited
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name of Linaro Limited nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ */
+
+/* This file just contains a small glue function which fishes the
+ * location of kernel etc out of linker script defined symbols, and
+ * calls semi_loader functions to do the actual work of loading
+ * and booting the kernel.
+ */
+
+#include <stdint.h>
+#include "semihosting.h"
+#include "semi_loader.h"
+
+/* Linker script defined symbols for any preloaded kernel/initrd */
+extern uint8_t fs_start, fs_end, kernel_entry, kernel_start, kernel_end;
+/* Symbols defined by boot.S */
+extern uint8_t kernel_cmd, kernel_cmd_end;
+
+static struct loader_info loader;
+
+#ifdef MACH_MPS
+#define PLAT_ID 10000 /* MPS (temporary) */
+#elif defined (VEXPRESS)
+#define PLAT_ID 2272 /* Versatile Express */
+#else
+#define PLAT_ID 827 /* RealView/EB */
+#endif
+
+void c_start(void)
+{
+	/* Main C entry point */
+	loader.kernel_size = (uint32_t)&kernel_end - (uint32_t)&kernel_start;
+	loader.initrd_start = (uint32_t)&fs_start;
+	loader.initrd_size = (uint32_t)&fs_end - (uint32_t)&fs_start;
+	loader.kernel_entry = (uint32_t)&kernel_entry;
+	if (loader.kernel_size) {
+		loader.cmdline_start = (uint32_t)&kernel_cmd;
+		loader.cmdline_size = &kernel_cmd_end - &kernel_cmd;
+	}
+	load_kernel(&loader);
+
+	/* Start the kernel */
+	if(loader.fdt_start) {
+		boot_kernel(&loader, 0, -1, loader.fdt_start, 0);
+	} else {
+		boot_kernel(&loader, 0, PLAT_ID, loader.atags_start, 0);
+	}
+
+	semi_write0("[bootwrapper] ERROR: returned from boot_kernel\n");
+}
diff --git a/model.lds.S b/model.lds.S
index e6bf960..07fae8c 100644
--- a/model.lds.S
+++ b/model.lds.S
@@ -22,36 +22,40 @@ INPUT(./uImage)
 
 PHYS_OFFSET = 0x80000000;
 MON_OFFSET  = 0xf0000000;
-
-
+STACKTOP = 0xff000000;
 
 
 SECTIONS
 {
  . = PHYS_OFFSET;
 
-#ifdef SEMIHOSTING
- .text : { bootsemi.o }
-#else
- .text : { boot.o }
-#endif
-
  . = PHYS_OFFSET + 0x8000 - 0x40;
- kernel = . + 0x40;
+
+ kernel_start = .;
+ kernel_entry = . + 0x40;
 #ifndef SEMIHOSTING
  .kernel : { ./uImage }
 #endif
+ kernel_end = .;
 
  . = PHYS_OFFSET + 0x00d00000;
- filesystem = .;
+ fs_start = .;
 #if defined(USE_INITRD) && !defined(SEMIHOSTING)
  .filesystem : { ./filesystem.cpio.gz }
- fs_size = . - filesystem;
 #endif
+ fs_end = .;
 
- .data : { *(.data) }
- .bss : { *(.bss) }
 
  . = MON_OFFSET;
  .monitor : { monitor.o }
+
+ /* Put most of the actual boot loader code up in high memory
+  * where it won't get overwritten by kernel, initrd or atags.
+  */
+ .text : { *(.text) }
+ .data : { *(.data) }
+ .bss : { *(.bss) }
+
+ . = STACKTOP;
+ stacktop = .;
 }
