1 files changed, 98 insertions, 0 deletions
diff --git a/boot/elf.c b/boot/elf.c
new file mode 100644
index 0000000..b8328b0
--- /dev/null
+++ b/boot/elf.c
@@ -0,0 +1,98 @@
+#include <elf.h>
+#include <stddef.h>
+
+#include "bootloader.h"
+
+static int cmp(const char *a, const char *b, size_t len)
+{
+	while (len--) if (*a++ != *b++) return 1;
+	return 0;
+}
+
+static void load_segment(char *dst, const char *src, size_t outlen, size_t inlen)
+{
+	outlen -= inlen;
+	while (inlen--) *dst++ = *src++;
+	while (outlen--) *dst++ = 0;
+}
+
+static const char e_ident[] = {
+	0x7f, 'E', 'L', 'F',  /* magic number  */
+	ELFCLASS32,           /* 32-bit        */
+	ELFDATA2MSB,          /* big endian    */
+	1,                    /* ELF version 1 */
+};
+
+_Noreturn void bootloader(struct systemid *boot)
+{
+	if (!boot->load_addr || boot->load_addr & 3) fail();
+	if (boot->stack_master & 3) fail();
+
+	char *base  = (void*) boot->load_addr;
+	long *stack = (long*)(boot->stack_master ? boot->stack_master : 0x06002000);
+
+	Elf32_Ehdr *ehdr = (void*)base;
+	if (cmp(ehdr->e_ident, e_ident, sizeof e_ident)
+		|| ehdr->e_type    != ET_EXEC
+		|| ehdr->e_machine != EM_SH
+		|| ehdr->e_version != EV_CURRENT
+		|| !ehdr->e_entry)
+		fail();
+
+	void  *entry     = (void*)ehdr->e_entry;
+	char  *phdrs     = base + ehdr->e_phoff;
+	size_t phentsize = ehdr->e_phentsize;
+	size_t phnum     = ehdr->e_phnum;
+
+	/* find load address for program headers, immediately before first segment */
+	char *phdr_out = NULL, *p;
+	size_t n;
+	for (p = phdrs, n = phnum; n--; p += phentsize) {
+		Elf32_Phdr *ph = (void*)p;
+		if (ph->p_type == PT_LOAD) {
+			phdr_out = (char*)ph->p_vaddr - phentsize * phnum;
+			break;
+		}
+	}
+	if (!phdr_out) fail();
+
+	/* fixup PT_PHDR program header (if present) to point to where we're loading it */
+	for (p = phdrs, n = phnum; n--; p += phentsize) {
+		Elf32_Phdr *ph = (void*)p;
+		if (ph->p_type == PT_PHDR) {
+			ph->p_vaddr = (Elf32_Word)phdr_out;
+			break;
+		}
+	}
+
+	/* load program headers */
+	load_segment(phdr_out, phdrs, phentsize * phnum, phentsize * phnum);
+
+	/* load all the PT_LOAD segments */
+	for (p = phdrs, n = phnum; n--; p += phentsize) {
+		Elf32_Phdr *ph = (void*)p;
+		if (ph->p_type != PT_LOAD) continue;
+		load_segment((void*)ph->p_vaddr, base + ph->p_offset, ph->p_memsz, ph->p_filesz);
+	}
+
+#define PUSH(v) do { *--stack = (long)(v); } while(0)
+#define PUSH_AUX(a,v) do { PUSH(v); PUSH(a); } while(0)
+
+	/* push aux vector onto target process stack */
+	long *emptystr = stack - 1;
+	PUSH_AUX(AT_NULL,  0);
+	PUSH_AUX(AT_PHDR,  phdr_out);
+	PUSH_AUX(AT_PHENT, phentsize);
+	PUSH_AUX(AT_PHNUM, phnum);
+	PUSH_AUX(AT_ENTRY, entry);
+
+	/* push environment and arguments */
+	PUSH(0);         /* envp[0] */
+	PUSH(0);         /* argv[1] */
+	PUSH(emptystr);  /* argv[0] */
+	PUSH(1);         /* argc    */
+
+	jump(entry, stack);
+	fail();
+}
+