#include	"stddef.h"
#include	"string.h"
#include	"linux-asm-io.h"
#include	"etherboot.h"
#include	"start32.h"
#include	"elf_boot.h"

#ifndef	FIRST32DOS
#define	FIRST32LINUX	1
#endif

#ifdef	FIRST32LINUX
#define SERIAL_CONSOLE 0
/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2, or (at
 * your option) any later version.
 */

/*

Memory layout assumed by mknbi and this program

0x07C00-0x07FFF   0.5 kB	floppy boot sector if loaded from floppy
0x0F???-0x0FFFF     ? kB	large Etherboot data buffers (deprecated)
0x10000-0x8FFFF 512.0 kB	kernel (from tagged image)
0x90000-0x901FF	  0.5 kB	Linux floppy boot sector (from Linux image)
0x90200-0x921FF	  8.0 kB	kernel setup (from Linux image)
0x92200-0x923FF	  0.5 kB	tagged image header ("directory")
0x92400-0x927FF	  1.0 kB	kernel parameters (generated by mknbi)
0x92800-0x93FFF	  6.0 kB	this program (generated by mknbi)
0x94000-0x9FFFF	 48.0 kB	Etherboot (top few kB may be used by BIOS)
				Normally Etherboot starts at 0x94000
0x100000-			kernel (if bzImage) (from tagged image)
after bzImage kernel		ramdisk (optional) (from tagged image)
				moved to below top of memory by this program
				but not higher than 896kB or what the
				limit in setup.S says

*/

#define		PARAMSIZE	512

extern void printf(const char *, ...);
extern int sprintf(char *, const char *, ...);
extern void xstartlinux(unsigned long);
extern void exit(int);

#ifdef	FIRST32ELF
static Elf32_Phdr	*seg[S_END] = { 0 };
#else
static struct segment	*seg[S_END] = { 0 };
#endif
static unsigned char 	*ip, *op;
static short		*vgamode;
static struct bootp_t	*bp;
static unsigned char	*vendortags;
unsigned long		top_of_initrd = 0;
static enum { RD_TOP, RD_ASIS, RD_HEXADDR } rdmode = RD_TOP;
static unsigned long	rdaddr;


#if SERIAL_CONSOLE
/* Base Address */
#define TTYS0 0x3f8
/* Data */
#define TTYS0_RBR (TTYS0+0x00)
#define TTYS0_TBR (TTYS0+0x00)
/* Control */
#define TTYS0_IER (TTYS0+0x01)
#define TTYS0_IIR (TTYS0+0x02)
#define TTYS0_FCR (TTYS0+0x02)
#define TTYS0_LCR (TTYS0+0x03)
#define TTYS0_MCR (TTYS0+0x04)

#define TTYS0_DLL (TTYS0+0x00)
#define TTYS0_DLM (TTYS0+0x01)
/* Status */
#define TTYS0_LSR (TTYS0+0x05)
#define TTYS0_MSR (TTYS0+0x06)
#define TTYS0_SCR (TTYS0+0x07)

static void ttys0_tx_byte(unsigned byte)
{
	while((inb(TTYS0_LSR) & 0x20) == 0)
		;
	outb(byte, TTYS0_TBR);
}

#endif
void putchar(int c)
{
	if (c == '\n')
		putchar('\r');
#if SERIAL_CONSOLE
	ttys0_tx_byte(c);
#endif
	console_putc(c);
}

static inline void quit(void)
{
	printf("Bad argument\n");
	exit(0);
}

static void nomem(void)
{
	printf("Out of parameter space\n");
	exit(0);
}

static inline void checkvendor(void)
{
	union {
		unsigned long	l;
		unsigned char	c[4];
	} u;

	memcpy(u.c, vendortags, sizeof(u));
	if (u.l == RFC_1048 || u.l == VEND_CMU || u.l == VEND_STAN)
		vendortags += 4;
	else
		vendortags = 0;
}

#ifdef	FIRST32ELF
static inline void locate_segs(union infoblock *header)
{
	int		i;
	Elf32_Phdr	*s;

	s = (Elf32_Phdr *)((char *)header + header->ehdr.e_phoff);
	for (i = 0; i < S_END && i < header->ehdr.e_phnum; i++, s++) {
		seg[i] = s;
#if	DEBUG > 1
		printf("%d %#X\n", i, s->p_paddr);
#endif
	}
}

#else

static inline void locate_segs(union infoblock *header)
{
	int		i;
	struct segment	*s;

	s = (struct segment *)((char *)header + sizeof(struct imgheader)
		+ ((header->img.length & 0xF0) >> 2));
	for (i = 0; i < S_END; i++, s++) {
		seg[i] = s;
#if	DEBUG > 1
		printf("%d %#X\n", i, s->p_paddr);
#endif
		if (s->flags & F_FINAL)
			break;
		s = (struct segment *)((char *)s + ((s->lengths & 0xF0) >> 2));
	}
}
#endif	/* !FIRST32ELF */

/*
 *	Find DHCP vendor tag, return pointer to tag length
 */
static unsigned char *gettag(unsigned int tag)
{
	unsigned char		*p;
	unsigned char		c;
	static unsigned char	emptytag[] = { 0, 0 };

	if (vendortags == 0)
		return (emptytag);
	for (p = vendortags; (c = *p) != RFC1533_END; ) {
		if (c == RFC1533_PAD)
			p++;
		else if (c == tag)
			return (p + 1);
		else
			p += p[1] + 2;
	}
	return (emptytag);
}

static void outtag(unsigned char *value)
{
	int			len;

	len = *value++;
	if (op + len > ip)
		nomem();
	while (len-- > 0)
		*op++ = *value++;
}

/* Return 1 if s2 is a prefix of s1 */
static int strprefix(const unsigned char *s1, const unsigned char *s2)
{
	while (*s1 != '\0' && *s2 != '\0' && *s1++ == *s2++)
		;
	/* Have we reached the end of s2? */
	return (*s2 == '\0');
}

enum keyword { K_VGA, K_NFSROOT, K_IP, K_RDBASE, K_MEM };

static inline int match_keyword(const unsigned char *start)
{
	if (strprefix(start, "vga"))
		return (K_VGA);
	if (strprefix(start, "nfsroot"))
		return (K_NFSROOT);
	if (strprefix(start, "ip"))
		return (K_IP);
	if (strprefix(start, "rdbase"))
		return (K_RDBASE);
	if (strprefix(start, "mem"))
		return (K_MEM);
	return (-1);
}

#define	isws(c)	((c) == ' ' || (c) == '\t')

static inline int copy_and_match(void)
{
	int			c;
	unsigned char		*start;

	start = ip;
	/* Stop copying at = if it exists */
	while ((c = *ip) != '\0' && !isws(c) && c != '=') {
		*op++ = *ip++;
	}
	if (c == '=') {
		ip++;
		*op++ = '=';
		return (match_keyword(start));
	}
	return (-1);
}

static unsigned long gethex(const unsigned char *p)
{
	unsigned long		value = 0;

	for (;;) {
		int c = *p++;
		if (c >= '0' && c <= '9')
			c -= '0';
		else if (c >= 'a' && c <= 'f')
			c -= 'a' - 10;
		else if (c >= 'A' && c <= 'F')
			c -= 'A' - 10;
		else
			break;
		value <<= 4;
		value |= c;
	}
	return (value);
}

static int getdec(const unsigned char *p)
{
	int			value = 0, sign = 0;

	if (*p == '-') {
		sign = 1;
		p++;
	}
	for (;;) {
		int c = *p++;
		if (c >= '0' && c <= '9')
			c -= '0';
		else
			break;
		value *= 10;
		value += c;
	}
	return (sign ? -value : value);
}

static void copy_nonws(void)
{
	int			c;

	/* Copy up to next whitespace */
	while ((c = *ip) != '\0' && !isws(c))
		*op++ = *ip++;
}

static void discard_arg(void)
{
	int			c;

	/* Discard up to next whitespace */
	while ((c = *ip) != '\0' && !isws(c))
		ip++;
}

static int outip(const unsigned char *p)
{
	long		ip;

	if (*p == 0)
		return (0);
	memcpy(&ip, p + 1, sizeof(ip));
	return sprintf(op, "%@", ip);
}

static inline void subst_value(int kwindex)
{
	int			c;
	unsigned char		*p;

	if (kwindex == K_VGA) {
		/* backup over "vga=" */
		op -= sizeof("vga=") - 1;
		if (strprefix(ip, "ask"))
			c = -3;
		else if (strprefix(ip, "extended"))
			c = -2;
		else if (strprefix(ip, "normal"))
			c = -1;
		else if (strprefix(ip, "0x"))
			c = gethex(ip+2);
		else	/* assume decimal mode number */
			c = getdec(ip);
		*vgamode = c;
		discard_arg();
	} else if (kwindex == K_NFSROOT && strprefix(ip, "rom") &&
		(ip[3] == '\0' || isws(ip[3]))) {
		outtag(gettag(RFC1533_ROOTPATH));
		discard_arg();
	} else if (kwindex == K_IP && strprefix(ip, "rom") &&
		(ip[3] == '\0' || isws(ip[3]))) {
		long		ip;
		op += sprintf(op, "%@:%@:", bp->bp_yiaddr, bp->bp_siaddr);
		p = gettag(RFC1533_GATEWAY);
		op += outip(p);
		*op++ = ':';
		p = gettag(RFC1533_NETMASK);
		op += outip(p);
		*op++ = ':';
		outtag(gettag(RFC1533_HOSTNAME));
		p = gettag(RFC1533_VENDOR_ETHDEV);
		if (*p)
			*op++ = ':';
		outtag(p);
		discard_arg();
	} else if (kwindex == K_RDBASE) {
		if (strprefix(ip, "top"))
			rdmode = RD_TOP;
		else if (strprefix(ip, "asis"))
			rdmode = RD_ASIS;
		else if (strprefix(ip, "0x")) {
			rdmode = RD_HEXADDR;
			rdaddr = gethex(ip+2);
		}
		discard_arg();
	} else if (kwindex == K_MEM) {
		unsigned char 	*p;
		unsigned long	memsize;
		memsize = getdec(p = ip);
		while (*p >= '0' && *p <= '9')
			++p;
		if (*p == 'G')
			memsize <<= 30;
		else if (*p == 'M')
			memsize <<= 20;
		else if (*p == 'K')
			memsize <<= 10;
		top_of_initrd = memsize;
		copy_nonws();
	} else
		copy_nonws();
}

static inline int skipws(void)
{
	int			c;

	while ((c = *ip) != '\0' && isws(c))
		ip++;
	return (c);
}

/*
 *	The parameters are copied from the input area to the output
 *	area, looking out for keyword=value pairs while doing so.
 *	If a possible keyword is found, indicated by an =,
 *	it is matched against a small list.
 *	If it matches none of the keywords on the list,
 *	the value is copied unchanged.
 *	If it matches a keyword, then the appropriate substitutions
 *	are made.
 *	While doing the substitution, a check is made that the output
 *	pointer doesn't overrun the input pointer. This is the only
 *	place it could happen, as the substitution may be longer than
 *	the original.
 */
static inline void process_params(void)
{
	int			i;

	while (skipws() != '\0') {
		if ((i = copy_and_match()) >= 0)
			subst_value(i);
		else
			copy_nonws();
		*op++ = ' ';
	}
	/* There may be a space after the last arg, probably does not matter
           but this is a reminder */
	*op = '\0';
}

/*
 * String is not null terminated, count of chars following is in first element
 * If there are 6 colons, returns char position after 6th colon
 * Else returns one position after string
 * which forces the length calculated below to be negative
 * Length of 7th argument can be calculated by subtracting the
 * length of the string preceding from the total length.
 */
static inline unsigned char *skip6colons(unsigned char *p)
{
	int	len, coloncount;

	for (len = *p++, coloncount = 6; len > 0 && coloncount > 0; p++, len--)
		if (*p == ':')
			coloncount--;
	return (p + (coloncount > 0));
}

static void parse_elf_boot_notes(
	void *notes, union infoblock **rheader, struct bootp_t **rbootp)
{
	unsigned char *note, *end;
	Elf_Bhdr *bhdr;
	Elf_Nhdr *hdr;

	bhdr = notes;
	if (bhdr->b_signature != ELF_BHDR_MAGIC) {
		return;
	}

	note = ((char *)bhdr) + sizeof(*bhdr);
	end  = ((char *)bhdr) + bhdr->b_size;
	while (note < end) {
		unsigned char *n_name, *n_desc, *next;
		hdr = (Elf_Nhdr *)note;
		n_name = note + sizeof(*hdr);
		n_desc = n_name + ((hdr->n_namesz + 3) & ~3);
		next = n_desc + ((hdr->n_descsz + 3) & ~3);
		if (next > end) 
			break;
#if 0
		printf("n_type: %x n_name(%d): n_desc(%d): \n", 
			hdr->n_type, hdr->n_namesz, hdr->n_descsz);
#endif

		if ((hdr->n_namesz == 10) &&
			(memcmp(n_name, "Etherboot", 10) == 0)) {
			switch(hdr->n_type) {
			case EB_BOOTP_DATA:
				*rbootp = *((void **)n_desc);
				break;
			case EB_HEADER:
				*rheader = *((void **)n_desc);
				break;
			default:
				break;
			}
		}
		note = next;
	}
}

int first(struct ebinfo *eb, union infoblock *header, struct bootp_t *bootp)
{
	int			i;
	unsigned char		*p, *q, *params;
	struct bootblock	*boot;
	struct setupblock	*setup;
	union {
		unsigned long	l;
		unsigned char	c[4];
	} u;

#if DEBUG > 1
	printf("&eb = %#X\n", &eb);
#endif
	printf(MKNBI_VERSION "/" __FILE__
#ifdef	FIRST32ELF
		" (ELF)"
#endif
		" (GPL)\n");
#if DEBUG > 1
	printf("eb = %#X, header = %#X, bootp = %#X\n", eb, header, bootp);
#endif
	/* Sanity checks */
#ifdef	FIRST32ELF
	parse_elf_boot_notes(eb, &header, &bootp);
	if (header->img.magic != ELF_MAGIC
#else
	if (header->img.magic != TAG_MAGIC
#endif
		|| bootp->bp_op != BOOTP_REPLY)
		quit();
	bp = bootp;
	vendortags = (unsigned char *)bootp->bp_vend;
	checkvendor();
	locate_segs(header);
	/* Locate boot block */
	boot = (struct bootblock *)seg[S_BOOT]->p_paddr;
	/* Point to word to alter if vga=... specified */
	vgamode = &boot->vgamode;
	/* Locate setup block */
	setup = (struct setupblock *)seg[S_SETUP]->p_paddr;
	/* Adjust loader type byte */
	setup->su_type = SU_MY_LOADER_TYPE;
	/* If setup version >= 0x202, use new command line protocol.
	   This frees setup.S from being tied to 0x90000 */
	if (setup->su_version >= 0x202)
		setup->su_cmd_line_ptr = params = (unsigned char *)seg[S_PARAMS]->p_paddr;
	else {	/* Use old protocol */
		/* Adjust boot block pointers to point to command line */
		boot->cl_magic = CL_MAGIC;
		boot->cl_offset = (params = (unsigned char *)seg[S_PARAMS]->p_paddr) - ((unsigned char *)boot);
	}
	p = params + (i = strlen(params) + 1);
	/* Append T129 if present */
	q = gettag(RFC1533_VENDOR_MAGIC);
	/* Check T128 present and correct */
	if (*q == 6 && (memcpy(u.c, q + 1, sizeof(u)), u.l == VEND_EB)) {
		q = gettag(RFC1533_VENDOR_ADDPARM);
		i = PARAMSIZE - 1 - i;		/*  +1 for SPACE */
		if (i > *q)			/* enough space? */
			i = *q;
		q++;
		if (i > 0)
			p[-1] = ' ';		/* NUL -> space */
		while (i-- > 0)
			*p++ = *q++;
		*p++ = '\0';			/* position past NUL */
		if (*(q = gettag(RFC1533_VENDOR_SELECTION)) == 1
		  && *(q = gettag(q[1])) > 0) {
			unsigned char	*r = skip6colons(q);
			/* If we have an argument and enough space, copy it */
			if ((i = *q - (r - q) + 1) > 0
			  && i < PARAMSIZE - 2 - strlen(params)) {
			/* +2 for SPACE and final NUL */
				p[-1] = ' ';
				while (i-- > 0) {
					/* escapes: ~b -> \\, ~c -> : */
					if (i > 0 && r[0] == '~' && r[1] == 'b')
						*p++ = '\\', r += 2, --i;
					else if (i > 0 && r[0] == '~' && r[1] == 'c')
						*p++ = ':', r += 2, --i;
					else
						*p++ = *r++;
				}
				*p++ = '\0';
			}
		}
	}
	/* Move parameters to end of parameter area,
	   tail first to avoid overwriting */
	q = params + PARAMSIZE;
	/* At least 1 byte is copied, the NUL */
	do {
		*--q = *--p;
	} while (p != params);
	ip = q;
	op = params;
#ifdef	DEBUG
	printf("Parameters: %s\n", p);
#endif
	/* mem= param affects top_of_initrd */
	process_params();
	if (seg[S_RAMDISK] != 0) {
		unsigned long		max;

		get_memsizes();
		max = (setup->su_version >= 0x203) ? setup->ramdisk_max : 0x37FFFFFF;
		/* compute top of initrd only if user has not overridden it */
		if (top_of_initrd == 0) {
			struct e820entry	*e;
			/* look for highest E820_RAM that is under ramdisk_max
			   strictly speaking we should also check that
			   we have room for the ramdisk in the memory segment */
			for (i = 0; i < meminfo.map_count; i++) {
				e = &meminfo.map[i];
				if (e->type == E820_RAM
					&& e->addr < max
					&& (e->addr + e->size) > top_of_initrd)
					top_of_initrd = e->addr + e->size;
			}
			/*2004/05/08: memdisk needs 64k between initrd and the computed top */
			top_of_initrd -= 0x10000;
		}
		if (top_of_initrd > max)
			top_of_initrd = max;
		/* Round down to next lower 4k boundary */
		top_of_initrd &= ~0xFFF;
		printf("Top of ramdisk is %#X\n", top_of_initrd);
		if (rdmode == RD_TOP || rdmode == RD_HEXADDR) {
			long			*dp, *sp;

                       /*2004/05/08: now we have a true size rd, hence
                               need to align tail pointer to 4k boundary up */
			sp = (long *)((p = (unsigned char *)seg[S_RAMDISK]->p_paddr) +
				(i = ((seg[S_RAMDISK]->p_filesz + 0xfff) & ~0xfff)));
			/*
			 * If user specified address, align dest tail pointer to 4k boundary below
			 */
			if (rdmode == RD_HEXADDR)
				dp = (long *)((rdaddr & ~0xfff) + i);
			else
				dp = (long *)top_of_initrd;
			/* Copy to destination by longwords, tail first;
				seg[] might get overwritten, save rd size first */
			i = seg[S_RAMDISK]->p_filesz;
			while (sp > (long *)p)
				*--dp = *--sp;
			printf("Ramdisk at %#X, size %#X\n",
				(setup->su_ramdisk_start = (unsigned long)dp),
				(setup->su_ramdisk_size = i));
		} else {	/* leave ramdisk as loaded, just report */
			printf("Ramdisk at %#X, size %#X\n",
				(setup->su_ramdisk_start = (unsigned long)seg[S_RAMDISK]->p_paddr),
				(setup->su_ramdisk_size = seg[S_RAMDISK]->p_filesz));
		}
	}
#ifdef	DEBUG
	printf("Ready\n");
#endif
#if DEBUG > 3
	/* Delay so we can read display */
	for (i = 0; i < 0x7ffffff; i++)
		;
#endif
	xstartlinux((unsigned long)setup);
	return (0);
}
#endif	/* FIRST32LINUX */

#ifdef	FIRST32DOS
extern void printf(const char *, ...);
extern void xstart(unsigned long, union infoblock *, struct bootp_t *);
extern void exit(int);

struct bootp_t	bpcopy;

void putchar(int c)
{
	if (c == '\n')
		putchar('\r');
	console_putc(c);
}

static inline void quit(void)
{
	printf("Bad argument\n");
	exit(0);
}

static void parse_elf_boot_notes(
	void *notes, union infoblock **rheader, struct bootp_t **rbootp)
{
	unsigned char *note, *end;
	Elf_Bhdr *bhdr;
	Elf_Nhdr *hdr;

	bhdr = notes;
	if (bhdr->b_signature != ELF_BHDR_MAGIC) {
		return;
	}

	note = ((char *)bhdr) + sizeof(*bhdr);
	end  = ((char *)bhdr) + bhdr->b_size;
	while (note < end) {
		unsigned char *n_name, *n_desc, *next;
		hdr = (Elf_Nhdr *)note;
		n_name = note + sizeof(*hdr);
		n_desc = n_name + ((hdr->n_namesz + 3) & ~3);
		next = n_desc + ((hdr->n_descsz + 3) & ~3);
		if (next > end) 
			break;
#if 0
		printf("n_type: %x n_name(%d): n_desc(%d): \n", 
			hdr->n_type, hdr->n_namesz, hdr->n_descsz);
#endif

		if ((hdr->n_namesz == 10) &&
			(memcmp(n_name, "Etherboot", 10) == 0)) {
			switch(hdr->n_type) {
			case EB_BOOTP_DATA:
				*rbootp = *((void **)n_desc);
				break;
			case EB_HEADER:
				*rheader = *((void **)n_desc);
				break;
			default:
				break;
			}
		}
		note = next;
	}
}

int first(struct ebinfo *eb, union infoblock *header, struct bootp_t *bootp)
{
#if DEBUG > 1
	printf("&eb = %#X\n", &eb);
#endif
	printf(MKNBI_VERSION "/" __FILE__ " (ELF)" " (GPL)\n");
#if DEBUG > 1
	printf("eb = %#X, header = %#X, bootp = %#X\n", eb, header, bootp);
#endif
	/* Sanity checks */
	parse_elf_boot_notes(eb, &header, &bootp);
	if (header->img.magic != ELF_MAGIC || bootp->bp_op != BOOTP_REPLY)
		quit();
	memcpy(&bpcopy, bootp, sizeof(bpcopy));
	xstart(RELOC - 0x1000, header, &bpcopy);
	return (0);
}
#endif	/* FIRST32DOS */