[2/4] bootwrapper: Import C semihosting code

diff --git a/semi_loader.c b/semi_loader.c
new file mode 100644
index 0000000..8c42f4a
--- /dev/null
+++ b/semi_loader.c
@@ -0,0 +1,525 @@ 
+/*
+ * 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_FDT "--fdt"
+#define CMDLINE_REST "-- "
+
+static void _usage_fatal(void)
+{
+	info("Usage: [" CMDLINE_KERNEL " <kernel filename>] ["
+		CMDLINE_NOINITRD "|" CMDLINE_INITRD " <initrd filename>] ["
+		CMDLINE_FDT " <fdt 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 *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_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");
+
+
+	/*
+	 * 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;
+}