#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();
}