bootwrapper: Import C semihosting code

Import the reimplementation in C of the semihosting support
written by Dave Martin for the ARM big.LITTLE bootwrapper.
The main benefit of this switchover (apart from only having
one implementation rather than two) is the fdt support it
includes.

This commit only imports the sources, it doesn't compile them.

Signed-off-by: Peter Maydell <peter.maydell@linaro.org>

5 files changed, 965 insertions, 0 deletions

diff --git a/semi_loader.c b/semi_loader.c
new file mode 100644
index 0000000..9039445
--- /dev/null
+++ b/semi_loader.c
@@ -0,0 +1,536 @@
+/*
+ * 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.
+ */
+
+#include <string.h>
+#include "libfdt.h"
+#include "semihosting.h"
+#include "semi_loader.h"
+
+static void _print_info(char const **strings)
+{
+	char const *string;
+
+	semi_write0("[bootwrapper] ");
+	while((string = *strings++))
+		semi_write0(string);
+}
+
+#define info(strings...) do {					\
+	char const *__info_strings[] = { strings, NULL };	\
+								\
+	_print_info(__info_strings);				\
+} while(0)
+
+#define warn(strings...) info("WARNING: ", strings)
+#define error(strings...) info("ERROR: ", strings)
+
+#define fatal(strings...) do {			\
+	error(strings);				\
+	semi_fatal("[bootwrapper] BOOT FAILED\n");	\
+} while(0)
+
+#define CMDLINE_KERNEL "--kernel"
+#define CMDLINE_INITRD "--initrd"
+#define CMDLINE_NOINITRD "--no-initrd"
+#define CMDLINE_DTB "--dtb"
+#define CMDLINE_FDT "--fdt"	/* deprecated */
+#define CMDLINE_REST "-- "
+
+static void _usage_fatal(void)
+{
+	info("Usage: [" CMDLINE_KERNEL " <kernel filename>] ["
+		CMDLINE_NOINITRD "|" CMDLINE_INITRD " <initrd filename>] ["
+		CMDLINE_DTB " <dtb filename>] ["
+		CMDLINE_REST "<kernel boot arguments>]\n");
+	fatal("Incorrect bootwrapper command-line.\n");
+}
+
+#define usage_fatal(strings...) do {		\
+	error(strings);				\
+	_usage_fatal();				\
+} while(0)
+
+static void atag_append(void **dest, unsigned tag, void const *data, unsigned size)
+{
+	char *d = *dest;
+	unsigned padded_size = ALIGN_INT(size, 4) + 8;
+	struct atag_header header = {
+		padded_size >> 2,
+		tag
+	};
+
+	if(tag == ATAG_NONE)
+		header.size = 0;
+
+	memcpy(d, &header, sizeof header);
+	memcpy(d + 8, data, size);
+
+	if(padded_size > size + 8)
+		memset(d + 8 + size, 0, padded_size - (size + 8));
+
+	*dest = d + padded_size;
+}
+
+static int _fdt_make_node(void *fdt, int parentoffset, const char *name)
+{
+	int e;
+
+	e = fdt_subnode_offset(fdt, parentoffset, name);
+	if(e != -FDT_ERR_NOTFOUND)
+		return e;
+
+	return fdt_add_subnode(fdt, parentoffset, name);
+}
+
+static void update_fdt(void **dest, struct loader_info *info)
+{
+	int e;
+	int _chosen;
+	void *fdt;
+	uint32_t const *p;
+
+	if(!info->fdt_start)
+		return;
+
+	fdt = ALIGN(*dest, 4);
+	if((e = fdt_open_into((void *)info->fdt_start, fdt, FDT_SIZE_MAX)) < 0)
+		goto libfdt_error;
+
+	/*
+	 * Sanity-check address sizes, since addresses and sizes which do
+	 * not take up exactly 4 bytes are not supported.
+	 */
+	{
+		if(!(p = fdt_getprop(fdt, 0, "#address-cells", &e)))
+			goto libfdt_error;
+		else if(e != 4 || fdt32_to_cpu(*p) != 1)
+			goto size_error;
+
+		if(!(p = fdt_getprop(fdt, 0, "#size-cells", &e)))
+			goto libfdt_error;
+		else if(e != 4 || fdt32_to_cpu(*p) != 1)
+			goto size_error;
+	}
+
+	/*
+	 * Add a memory node, but only if there isn't one already.  If
+	 * there is already a memory node with non-zero size, it was put
+	 * in the DT on purpose and should take precedence over our
+	 * guesses.  Otherwise, make a memory node with the appropriate
+	 * parameters.
+	 */
+	{
+		int offset, depth = 0;
+		int _memory;
+		uint32_t reg[2];
+
+		for(offset = fdt_next_node(fdt, 0, &depth); offset >= 0;
+				offset = fdt_next_node(fdt, offset, &depth)) {
+			char const *name;
+
+			if(depth != 1)
+				continue;
+
+			name = fdt_get_name(fdt, offset, (void *)0);
+			if(!strcmp(name, "memory") ||
+					!strncmp(name, "memory@", 7)) {
+				p = fdt_getprop(fdt, offset, "reg", &e);
+				if(e < 0)
+					goto libfdt_error;
+
+				if(fdt32_to_cpu(p[1]) != 0)
+					goto no_add_memory;
+			}
+		}
+
+		if((e = _fdt_make_node(fdt, 0, "memory")) < 0)
+			goto libfdt_error;
+		_memory = e;
+
+		reg[0] = cpu_to_fdt32(PHYS_OFFSET);
+		reg[1] = cpu_to_fdt32(PHYS_SIZE);
+		if((e = fdt_setprop(fdt, _memory, "reg", &reg, sizeof reg)) < 0)
+			goto libfdt_error;
+
+		if((e = fdt_setprop_string(fdt, _memory, "device_type",
+				"memory")) < 0)
+			goto libfdt_error;
+	}
+
+no_add_memory:
+	/* populate the "chosen" node */
+
+	if((e = _fdt_make_node(fdt, 0, "chosen")) < 0)
+		goto libfdt_error;
+	_chosen = e;
+
+	e = fdt_setprop_string(fdt, _chosen, "bootargs",
+				(char const *)info->cmdline_start);
+	if(e < 0)
+		goto libfdt_error;
+
+	if(info->initrd_start) {
+		uint32_t initrd_end = info->initrd_start + info->initrd_size;
+
+		if((e = fdt_setprop_cell(fdt, _chosen, "linux,initrd-start",
+					info->initrd_start)) < 0)
+			goto libfdt_error;
+
+		if((e = fdt_setprop_cell(fdt, _chosen, "linux,initrd-end",
+					initrd_end)) < 0)
+			goto libfdt_error;
+	}
+
+	/* success */
+
+	/* clean up */
+	fdt_pack(fdt);
+	info->fdt_start = (unsigned)fdt;
+	info->fdt_size = fdt_totalsize(fdt);
+	info("FDT updated.\n");
+
+	return;
+		
+libfdt_error:
+	fatal("libfdt: ", fdt_strerror(e), ", while updating device tree\n");
+
+size_error:
+	fatal("Unexpected/invalid #address-cells/#size-cells in device tree\n");
+}
+
+static int is_space(char c)
+{
+	return c == ' ';
+}
+
+static void skip_space(char **s)
+{
+	char *t = *s;
+
+	for(t = *s; is_space(*t); t++);
+	*s = t;
+}
+
+static void find_space(char **s)
+{
+	char *t = *s;
+
+	for(t = *s; *t && !is_space(*t); t++);
+	*s = t;
+}
+
+static int match_word(char **s, char const *string)
+{
+	unsigned l;
+
+	l = strlen(string);
+	if(strncmp(*s, string, l))
+		return 0;
+
+	*s += l;
+	skip_space(s);
+	return 1;
+}
+
+/*
+ * Match an option with a mandatory argument.
+ * On success, a pointer to the argument is returned, with leading and
+ * trailing whitespace stripped.
+ */
+static char *match_option(char **s, char const *option_string)
+{
+	char *arg;
+
+	if(!match_word(s, option_string))
+		return (void *)0;
+
+	if(!**s)
+		usage_fatal("Option requires as argument: ",
+			option_string, "\n");
+
+	/* otherwise, *s now points to the argument */
+	arg = *s;
+
+	find_space(s);		/* find the end of the argument */
+	if(**s) {
+		*(*s)++ = '\0';	/* null-terminate if necessary */
+		skip_space(s);	/* skip any remaining space */
+	}
+
+	return arg;
+}
+
+static void load_file_essential(void **dest, char const *filename,
+				unsigned *size, char const *failmsg)
+{
+	if(semi_load_file(dest, size, filename))
+		fatal(failmsg, ": \"", filename, "\"\n");
+}
+
+/* Move the kernel if necessary, based on the image type: */
+static void correct_kernel_location(struct loader_info *info)
+{
+	char *const text_start = (char *)(PHYS_OFFSET + TEXT_OFFSET);
+	char *const text_end = text_start + info->kernel_size;
+	char *const uImage_payload = text_start + UIMAGE_HEADER_SIZE;
+	unsigned long *const zImage_magic_p = (unsigned long *)(
+		uImage_payload + ZIMAGE_MAGIC_OFFSET);
+
+	/*
+	 * If the image is not a uImage, then it is a raw Image or zImage,
+	 * and no action is necessary:
+	 */
+	if(info->kernel_size <= UIMAGE_HEADER_SIZE)
+		return;
+
+	if(memcmp(text_start, uImage_magic, sizeof uImage_magic))
+		return;
+
+	warn("Ignoring uImage meta-data\n");
+
+	/*
+	 * If the uImage payload is a zImage, the position-independent
+	 * nature of the zImage header means that no relocation is
+	 * needed.  Instead, just enter at the start of the loaded
+	 * zImage header:
+	 */
+	if(text_end >= (char *)&zImage_magic_p[1]
+			&& *zImage_magic_p == ZIMAGE_MAGIC) {
+		info->kernel_entry += UIMAGE_HEADER_SIZE;
+		return;
+	}
+
+	/*
+	 * Otherwise, move the payload to replace the uImage header, and
+	 * leave the entry point unmodified.
+	 */
+	memmove(text_start, uImage_payload,
+		info->kernel_size - UIMAGE_HEADER_SIZE);
+}
+
+static char semi_cmdline[SEMI_CMDLINE_MAX];
+
+static char *kernel_arg = (void *)0;
+static char *initrd_arg = (void *)0;
+static char *fdt_arg = (void *)0;
+static char *dtb_arg = (void *)0;
+static char *cmdline_arg = (void *)0;
+static char *noinitrd_arg = (void *)0;
+
+static const struct {
+	char const *option_string;
+	char **argp;
+	enum { OPT_ARG, OPT_BOOL, OPT_REST } action;
+} options[] = {
+	{ CMDLINE_KERNEL,	&kernel_arg,	OPT_ARG		},
+	{ CMDLINE_INITRD,	&initrd_arg,	OPT_ARG		},
+	{ CMDLINE_NOINITRD,	&noinitrd_arg,	OPT_BOOL	},
+	{ CMDLINE_FDT,		&fdt_arg,	OPT_ARG		},
+	{ CMDLINE_DTB,		&dtb_arg,	OPT_ARG		},
+	{ CMDLINE_REST,		&cmdline_arg,	OPT_REST	},
+};
+
+void load_kernel(struct loader_info *info)
+{
+	unsigned i;
+	char *cmdline = semi_cmdline;
+	int cmdline_length;
+	void *phys = (char *)(PHYS_OFFSET + TEXT_OFFSET);
+	void *atagp = (char *)(PHYS_OFFSET + ATAGS_OFFSET);
+
+	union {
+		struct atag_core core;
+		struct atag_mem mem;
+		struct atag_initrd2 initrd;
+	} atag;
+
+	/* Fetch the command line: */
+
+	if(semi_get_cmdline(semi_cmdline, sizeof semi_cmdline,
+			    &cmdline_length) ||
+				cmdline_length >= sizeof semi_cmdline) {
+		warn("Failed to get semihosting command line, using built-in defaults\n");
+		cmdline_length = 0;
+	}
+	cmdline[cmdline_length] = '\0';
+
+	/* Parse the arguments (if any): */
+
+	skip_space(&cmdline);
+
+	while(*cmdline) {
+		for(i = 0; i < sizeof options / sizeof *options; i++) {
+			char *arg;
+
+			switch(options[i].action) {
+			case OPT_BOOL:
+				if(!match_word(&cmdline,
+						options[i].option_string))
+					continue;
+
+				*options[i].argp = cmdline; /* non-NULL */
+				goto next_arg;
+
+			case OPT_REST:
+				if(!match_word(&cmdline,
+						options[i].option_string))
+					continue;
+
+			*options[i].argp = cmdline;
+			goto args_done;
+
+			case OPT_ARG:
+				arg = match_option(&cmdline,
+					options[i].option_string);
+				if(!arg)
+					continue;
+
+				if(*options[i].argp)
+					usage_fatal("Duplicate option ",
+						options[i].option_string);
+
+				/* otherwise, option was parsed successfully: */
+				*options[i].argp = arg;
+				goto next_arg;
+			}
+		} /* for(i) */
+
+		/* Failed to match any expected option: */
+		usage_fatal("Invalid option(s): ", cmdline);
+
+	next_arg: ;
+	} /* while(*cmdline) */
+
+args_done:
+	if(initrd_arg && noinitrd_arg)
+		usage_fatal("Option --initrd conflicts with --no-initrd.\n");
+
+	if(fdt_arg) {
+		warn("--fdt is deprecated.  Please use --dtb instead.\n");
+
+		if(dtb_arg)
+			usage_fatal("--fdt conflicts with --dtb.\n");
+		else
+			dtb_arg = fdt_arg;
+	}
+
+	/*
+	 * Now, proceed to load images and set up ATAGs.
+	 * For simplicity, ATAGs are generated even if there is a DTB
+	 */
+
+	/* built-in FDT not supported, for now */
+	info->fdt_start = info->fdt_size = 0;
+
+	info->atags_start = (unsigned)atagp;
+
+	memset(&atag.core, 0, sizeof atag.core);
+	atag_append(&atagp, ATAG_CORE, &atag.core, sizeof atag.core);
+
+	/* create the essential ATAGs */
+
+	atag.mem.start = PHYS_OFFSET;
+	atag.mem.size = PHYS_SIZE;
+	atag_append(&atagp, ATAG_MEM, &atag.mem, sizeof atag.mem);
+
+	/* load the kernel */
+
+	info->kernel_entry = (unsigned)phys;
+
+	if(kernel_arg) {
+		load_file_essential(&phys, kernel_arg, &info->kernel_size,
+			"Failed to load kernel image");
+		info("Loaded kernel: ", kernel_arg, "\n");
+	} else if(info->kernel_size) {
+		info("Using built-in kernel\n");
+		phys += info->kernel_size;
+	} else
+		usage_fatal("Expected " CMDLINE_KERNEL "\n");
+
+	/* move the kernel to the correct place, if necessary */
+
+	correct_kernel_location(info);
+
+	phys = (char *)(PHYS_OFFSET + INITRD_OFFSET);
+
+	/* load the initrd */
+
+	atag.initrd.size = 0;
+
+	if(initrd_arg) {
+		unsigned start = (unsigned)phys;
+
+		load_file_essential(&phys, initrd_arg, NULL,
+			"Failed to load initrd image");
+		info("Loaded initrd: ", initrd_arg, "\n");
+
+		info->initrd_start = atag.initrd.start = start;
+		info->initrd_size = atag.initrd.size = (unsigned)phys - start;
+	} else if(info->initrd_size) {
+		if(noinitrd_arg) {
+			info->initrd_size = 0;
+			info("Built-in initrd discarded, as requested\n");
+		} else {
+			info("Using built-in initrd\n");
+
+			atag.initrd.start = info->initrd_start;
+			atag.initrd.size = info->initrd_size;
+		}
+	} else
+		info->initrd_size = 0;
+
+	if(atag.initrd.size)
+		atag_append(&atagp, ATAG_INITRD2, &atag.initrd, sizeof atag.initrd);
+
+	/* load the FDT, if specified */
+
+	if(fdt_arg) {
+		phys = ALIGN(phys, 4);
+		info->fdt_start = (unsigned)phys;
+
+		load_file_essential(&phys, fdt_arg, NULL,
+			"Failed to load device tree blob");
+		info("Loaded FDT: ", fdt_arg, "\n");
+
+		info->fdt_size = (unsigned)phys - info->fdt_start;
+	}
+
+	/*
+	 * The FDT will get modified to reflect bootargs, initrd and memory
+	 * configuration later.
+	 */
+
+	/* set the command line */
+
+	if(cmdline_arg) {
+		info->cmdline_start = (unsigned)cmdline_arg;
+		info->cmdline_size = strlen(cmdline_arg) + 1;
+	} else if(info->cmdline_size)
+		info("Using built-in kernel bootargs\n");
+
+	/* cmdline_size is presumed to include a NUL terminator: */
+	if(info->cmdline_size) {
+		atag_append(&atagp, ATAG_CMDLINE,
+			(char *)info->cmdline_start, info->cmdline_size);
+		info("Kernel bootargs: ", (char *)info->cmdline_start, "\n");
+	}
+
+	atag_append(&atagp, ATAG_NONE, 0, 0);
+
+	update_fdt(&phys, info);
+}
diff --git a/semi_loader.h b/semi_loader.h
new file mode 100644
index 0000000..8a1a602
--- /dev/null
+++ b/semi_loader.h
@@ -0,0 +1,99 @@
+/*
+ * 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.
+ */
+
+#ifndef SEMI_LOADER_H
+#define SEMI_LOADER_H
+
+#define ATAG_NONE 0x00000000
+#define ATAG_CORE 0x54410001
+#define ATAG_MEM 0x54410002
+#define ATAG_INITRD2 0x54420005
+#define ATAG_CMDLINE 0x54410009
+
+struct atag_header {
+	unsigned size;
+	unsigned tag;
+};
+
+struct atag_core {
+	unsigned flags;
+	unsigned pagesize;
+	unsigned rootdev;
+};
+
+struct atag_mem {
+	unsigned size;
+	unsigned start;
+};
+
+struct atag_initrd2 {
+	unsigned start;
+	unsigned size;
+};
+
+static const char uImage_magic[] = {
+	0x27, 0x05, 0x19, 0x56
+};
+#define UIMAGE_HEADER_SIZE 0x40
+
+#define ZIMAGE_MAGIC_OFFSET 36
+#define ZIMAGE_MAGIC 0x016f2818UL
+
+#define PHYS_OFFSET 0x80000000
+#define PHYS_SIZE 0x80000000 /* can limit on kernel cmdline if necessary */
+#define ATAGS_OFFSET 0x100
+#define TEXT_OFFSET 0x8000
+#define INITRD_OFFSET 0xD00000 /* qemu uses the same random offset */
+
+#define FDT_SIZE_MAX 0x10000	/* maximum size allowed for device tree blob */
+
+#define SEMI_CMDLINE_MAX 0x1000	/* maximum semihosting command line length */
+
+/*
+ * Align number <n> or pointer <p> up to the next boundary of size <size>.
+ * <size> must be a power of two.
+ */
+#define ALIGN_INT(n, size) (((n) + ((size) - 1)) & ~((size) - 1))
+#define ALIGN(p, size) ((void *)ALIGN_INT((unsigned)(p), size))
+
+struct loader_info {
+	unsigned kernel_size;	/* nonzero indicates preloaded kernel size */
+	unsigned initrd_start;	/* start of preloaded initrd, if any */
+	unsigned initrd_size;
+	unsigned cmdline_start;	/* start of cmdline buffer */
+	unsigned cmdline_size;
+
+	/* The remaining fields are set by the loader: */
+
+	/* There could be a built-in FDT, but currently that it not supported */
+	unsigned fdt_start;	/* start of device tree blob, if any */
+	unsigned fdt_size;
+
+	unsigned atags_start;
+	unsigned kernel_entry;	/* kernel entry point */
+};
+
+void load_kernel(struct loader_info *info);
+
+static void boot_kernel(struct loader_info *info,
+		unsigned r0, unsigned r1, unsigned r2, unsigned r3) {
+	((void (*)(unsigned, unsigned, unsigned, unsigned))info->kernel_entry)(
+		r0, r1, r2, r3);
+}
+
+#endif /* ! SEMI_LOADER_H */
diff --git a/semihosting.c b/semihosting.c
new file mode 100644
index 0000000..b5e1c9b
--- /dev/null
+++ b/semihosting.c
@@ -0,0 +1,143 @@
+/*
+ * 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.
+ */
+
+#include <string.h>
+#include "semihosting.h"
+
+int semi_open(char const *filename, int mode)
+{
+	struct {
+		char const *filename;
+		int mode;
+		int filename_length;
+	} args;
+
+	args.filename = filename;
+	args.mode = mode;
+	args.filename_length = strlen(filename);
+
+	return __semi_call(SYS_OPEN, &args);
+}
+
+int semi_close(int fd)
+{
+	return __semi_call(SYS_CLOSE, &fd);
+}
+
+int semi_write0(char const *string)
+{
+	return __semi_call(SYS_WRITE0, string);
+}
+
+int semi_read(int fd, char *buffer, int length)
+{
+	struct {
+		int fd;
+		char *buffer;
+		int length;
+	} args;
+
+	args.fd = fd;
+	args.buffer = buffer;
+	args.length = length;
+
+	return __semi_call(SYS_READ, &args);
+}
+
+int semi_flen(int fd)
+{
+	return __semi_call(SYS_FLEN, &fd);
+}
+
+int semi_get_cmdline(char *buffer, int size, int *length)
+{
+	int result;
+	struct {
+		char *buffer;
+		int size;
+	} args;
+
+	args.buffer = buffer;
+	args.size = size;
+
+	result = __semi_call(SYS_GET_CMDLINE, &args);
+	if(result)
+		return result;
+
+	if(length)
+		*length = args.size;
+
+	return 0;
+}
+
+int semi_reportexc(int reason)
+{
+	return __semi_call(SYS_REPORTEXC, (void *)reason);
+}
+
+void semi_exit(void)
+{
+	semi_reportexc(REPORTEXC_REASON_APP_EXIT);
+	while(1); /* should not be reached */
+}
+
+void semi_fatal(char const *message)
+{
+	semi_write0(message);
+	semi_exit();
+}
+
+int semi_load_file(void **dest, unsigned *size, char const *filename)
+{
+	int result = -1;	/* fail by default */
+	int fd = -1;
+	int filesize;
+
+	fd = semi_open(filename, OPEN_RDONLY);
+	if(fd == -1) {
+		semi_write0("Cannot open file: ");
+		goto out;
+	}
+
+	filesize = semi_flen(fd);
+	if(filesize == -1) {
+		semi_write0("Cannot get file size for: ");
+		goto out;
+	}
+
+	if(semi_read(fd, *dest, filesize)) {
+		semi_write0("Could not read: ");
+		goto out;
+	}
+
+	result = 0;	/* success */
+	*dest = (char *)*dest + filesize;
+
+out:
+	if(fd != -1)
+		semi_close(fd);
+
+	if(result) {	/* print context for the error message */
+		semi_write0(filename);
+		semi_write0("\n");
+	} else
+		if(size)
+			*size = filesize;
+
+	return result;
+}
diff --git a/semihosting.h b/semihosting.h
new file mode 100644
index 0000000..40817c2
--- /dev/null
+++ b/semihosting.h
@@ -0,0 +1,50 @@
+/*
+ * 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.
+ */
+
+#ifndef SEMIHOSTING_H
+#define SEMIHOSTING_H
+
+#define SYS_OPEN	1
+#define OPEN_RDONLY	1
+#define SYS_CLOSE	2
+#define SYS_WRITE0	4
+#define SYS_READ	6
+#define SYS_FLEN	0x0C
+#define SYS_GET_CMDLINE	0x15
+#define SYS_REPORTEXC	0x18
+#define REPORTEXC_REASON_APP_EXIT 0x20026
+#define SEMIHOSTING_SVC	0x123456	/* SVC comment field for semihosting */
+
+#ifndef __ASSEMBLER__
+
+int __semi_call(int id, ...);
+int semi_open(char const *filename, int mode);
+int semi_close(int fd);
+int semi_write0(char const *string);
+int semi_read(int fd, char *buffer, int length);
+int semi_flen(int fd);
+int semi_get_cmdline(char *buffer, int size, int *length);
+int semi_reportexc(int reason);
+void semi_fatal(char const *message);
+void semi_exit(void);
+/* semi_load_file: *dest is advanced to point to the end of the loaded data */
+int semi_load_file(void **dest, unsigned *size, char const *filename);
+
+#endif /* ! __ASSEMBLER__ */
+
+#endif /* ! SEMIHOSTING_H */
diff --git a/string.c b/string.c
new file mode 100644
index 0000000..a807baa
--- /dev/null
+++ b/string.c
@@ -0,0 +1,137 @@
+#include <string.h>
+
+static void *__memmove_down(void *__dest, __const void *__src, size_t __n)
+{
+	unsigned char *d = (unsigned char *)__dest, *s = (unsigned char *)__src;
+
+	while (__n--)
+		*d++ = *s++;
+
+	return __dest;
+}
+
+static void *__memmove_up(void *__dest, __const void *__src, size_t __n)
+{
+	unsigned char *d = (unsigned char *)__dest + __n - 1, *s = (unsigned char *)__src + __n - 1;
+
+	while (__n--)
+		*d-- = *s--;
+
+	return __dest;
+}
+
+void *(memcpy)(void *__dest, __const void *__src, size_t __n)
+{
+	return __memmove_down(__dest, __src, __n);
+}
+
+void *(memmove)(void *__dest, __const void *__src, size_t __n)
+{
+	if(__dest > __src)
+		return __memmove_up(__dest, __src, __n);
+	else
+		return __memmove_down(__dest, __src, __n);
+}
+
+void *(memchr)(void const *s, int c, size_t n)
+{
+	unsigned char const *_s = (unsigned char const *)s;
+
+	while(n && *_s != c) {
+		++_s;
+		--n;
+	}
+
+	if(n)
+		return (void *)_s;	/* the C library casts const away */
+	else
+		return (void *)0;
+}
+
+size_t (strlen)(const char *s)
+{
+	const char *sc = s;
+
+	while (*sc != '\0')
+		sc++;
+	return sc - s;
+}
+
+void *(memset)(void *s, int c, size_t count)
+{
+	char *xs = s;
+	while (count--)
+		*xs++ = c;
+	return s;
+}
+
+int (memcmp)(void const *p1, void const *p2, size_t n)
+{
+	unsigned char const *_p1 = p1;
+	unsigned char const *_p2 = p2;
+
+	while(n--) {
+		if(*_p1 < *_p2)
+			return -1;
+		else if(*_p1 > *_p2)
+			return 1;
+
+		++_p1;
+		++_p2;
+	}
+
+	return 0;
+}
+
+int (strcmp)(char const *s1, char const *s2)
+{
+	while(*s1 && *s2) {
+		if(*s1 < *s2)
+			return -1;
+		else if(*s1 > *s2)
+			return 1;
+
+		++s1;
+		++s2;
+	}
+
+	if(!*s1 && !*s2)
+		return 0;
+	else if(!*s1)
+		return -1;
+	else
+		return 1;
+}
+
+int (strncmp)(char const *s1, char const *s2, size_t n)
+{
+	while(*s1 && *s2 && n--) {
+		if(*s1 < *s2)
+			return -1;
+		else if(*s1 > *s2)
+			return 1;
+
+		++s1;
+		++s2;
+	}
+
+	if(n == 0 || (!*s1 && !*s2))
+		return 0;
+	else if(!*s1)
+		return -1;
+	else
+		return 1;
+}
+
+char *(strchr)(char const *s, int c)
+{
+	unsigned char const *_s = (unsigned char const *)s;
+
+	while(*_s && *_s != c)
+		++_s;
+
+	if(*_s)
+		return (char *)_s;	/* the C library casts const away */
+	else
+		return (char *)0;
+}