[PORT FROM XEN_SANDBOX]Bootstub modification for Xen, ported from R42TW branch

BZ: 136642

Deeply modified port from Xen branch

Change-Id: Icde718b22f8519ab33cc0dfb78a057bacfc9dda0
Orig-Change-Id: I0b56738d9532c268582b6a7a3d3fb1974cf28324
Signed-off-by: Evgeny Kalugin <evgeny.kalugin@intel.com>

9 files changed, 694 insertions, 140 deletions

diff --git a/Android.mk b/Android.mk
index 0c68321..73f89d6 100644
--- a/Android.mk
+++ b/Android.mk
@@ -4,18 +4,21 @@ include $(CLEAR_VARS)
 # First compile bootstub.bin
 
 CMDLINE_SIZE ?= 0x400
-BOOTSTUB_SIZE ?= 4096
+BOOTSTUB_SIZE ?= 8192
 
-LOCAL_SRC_FILES := bootstub.c spi-uart.c head.S sfi.c e820_bios.S ssp-uart.c
-ANDROID_TOOLCHAIN_FLAGS := -m32 -mno-android -ffreestanding
+LOCAL_CC := gcc
+LOCAL_SRC_FILES := bootstub.c head.S e820_bios.S sfi.c ssp-uart.c imr_toc.c spi-uart.c
+ANDROID_TOOLCHAIN_FLAGS := -m32 -ffreestanding
 LOCAL_CFLAGS := $(ANDROID_TOOLCHAIN_FLAGS) -Wall -O1 -DCMDLINE_SIZE=${CMDLINE_SIZE}
 LOCAL_MODULE := bootstub.bin
 LOCAL_MODULE_TAGS := optional
 LOCAL_MODULE_PATH := $(PRODUCT_OUT)
-
 LOCAL_MODULE_CLASS := EXECUTABLES
 LOCAL_FORCE_STATIC_EXECUTABLE := true
 
+
+head.o : PRIVATE_CFLAGS := -D__ASSEMBLY__
+
 include $(BUILD_SYSTEM)/binary.mk
 
 $(LOCAL_INTERMEDIATE_TARGETS): PRIVATE_TARGET_GLOBAL_CFLAGS := $(LOCAL_CFLAGS)
diff --git a/bootstub.c b/bootstub.c
index 435910d..4d3b56b 100644
--- a/bootstub.c
+++ b/bootstub.c
@@ -24,10 +24,43 @@
 #include "bootparam.h"
 #include "spi-uart.h"
 #include "ssp-uart.h"
+#include "mb.h"
 #include "sfi.h"
 
+#include <stdint.h>
+#include <stddef.h>
+#include "imr_toc.h"
+
+#define PAGE_SIZE_MASK	0xFFF
+#define MASK_1K		0x3FF
+#define PAGE_ALIGN_FWD(x)       ((x + PAGE_SIZE_MASK) & ~PAGE_SIZE_MASK)
+#define PAGE_ALIGN_BACK(x)      ((x) & ~PAGE_SIZE_MASK)
+
+#define IMR_START_ADDRESS(x)	(((x) & 0xFFFFFFFC) << 8)
+#define IMR_END_ADDRESS(x)	((x == 0) ? (x) : ((((x) & 0xFFFFFFFC) << 8) | MASK_1K))
+
+#define	IMR6_START_ADDRESS	IMR_START_ADDRESS(*((u32 *)0xff108160))
+#define	IMR6_END_ADDRESS	IMR_END_ADDRESS(*((u32 *)0xff108164))
+#define	IMR7_START_ADDRESS	IMR_START_ADDRESS(*((u32 *)0xff108170))
+#define	IMR7_END_ADDRESS	IMR_END_ADDRESS(*((u32 *)0xff108174))
+
+#define FATAL_HANG()  { asm("cli"); while (1) { asm("nop"); } }
+
 extern int no_uart_used;
 
+extern imr_toc_t imr6_toc;
+static u32 imr7_size;
+
+static u32 sps_load_adrs;
+
+static memory_map_t mb_mmap[E820MAX];
+u32 mb_magic, mb_info;
+
+struct gdt_ptr {
+        u16 len;
+        u32 ptr;
+} __attribute__((packed));
+
 static void *memcpy(void *dest, const void *src, size_t count)
 {
         char *tmp = dest;
@@ -72,23 +105,62 @@ static size_t strnlen(const char *s, size_t maxlen)
         return (es - s);
 }
 
-static void setup_boot_params(struct boot_params *bp, struct setup_header *sh)
+static const char *strnchr(const char *s, int c, size_t maxlen)
 {
-	u8 *initramfs;
-	int nr_entries;
+    int i;
+    for (i = 0; i < maxlen && *s != c; s++, i++)
+        ;
+    return s;
+}
 
-	memset(bp, 0, sizeof (struct boot_params));
+int strncmp(const char *cs, const char *ct, size_t count)
+{
+	unsigned char c1, c2;
+
+	while (count) {
+		c1 = *cs++;
+		c2 = *ct++;
+		if (c1 != c2)
+			return c1 < c2 ? -1 : 1;
+		if (!c1)
+			break;
+		count--;
+	}
+	return 0;
+}
+
+static void setup_boot_params(struct boot_params *bp, struct setup_header *sh)
+{
 	bp->screen_info.orig_video_mode = 0;
 	bp->screen_info.orig_video_lines = 0;
 	bp->screen_info.orig_video_cols = 0;
 	bp->alt_mem_k = 128*1024; // hard coded 128M mem here, since SFI will override it
 	memcpy(&bp->hdr, sh, sizeof (struct setup_header));
-	bp->hdr.cmd_line_ptr = CMDLINE_OFFSET;
-	bp->hdr.cmdline_size = strnlen((const char *)CMDLINE_OFFSET, CMDLINE_SIZE);
 	bp->hdr.type_of_loader = 0xff; //bootstub is unknown bootloader for kernel :)
+	bp->hdr.hardware_subarch = X86_SUBARCH_MRST;
+}
+
+static u32 bzImage_setup(struct boot_params *bp, struct setup_header *sh)
+{
+	void *cmdline = (void *)BOOT_CMDLINE_OFFSET;
+	size_t cmdline_len;
+	u8 *initramfs, *ptr = (u8*)BZIMAGE_OFFSET;
+
+
+	cmdline_len = strnlen((const char *)CMDLINE_OFFSET, CMDLINE_SIZE);
+
+	/*
+	 * Copy the command line to be after bootparams so that it won't be
+	 * overwritten by the kernel executable.
+	 */
+	memset(cmdline, 0, CMDLINE_SIZE);
+	memcpy(cmdline, (const void *)CMDLINE_OFFSET, cmdline_len);
+
+	bp->hdr.cmd_line_ptr = BOOT_CMDLINE_OFFSET;
+	bp->hdr.cmdline_size = cmdline_len;
 	bp->hdr.ramdisk_size = *(u32 *)INITRD_SIZE_OFFSET;
 	bp->hdr.ramdisk_image = (bp->alt_mem_k*1024 - bp->hdr.ramdisk_size) & 0xFFFFF000;
-	bp->hdr.hardware_subarch = X86_SUBARCH_MRST;
+
 	initramfs = (u8 *)BZIMAGE_OFFSET + *(u32 *)BZIMAGE_SIZE_OFFSET;
 	if (*initramfs) {
 		bs_printk("Relocating initramfs to high memory ...\n");
@@ -96,12 +168,14 @@ static void setup_boot_params(struct boot_params *bp, struct setup_header *sh)
 	} else {
 		bs_printk("Won't relocate initramfs, are you in SLE?\n");
 	}
-	if (mid_identify_cpu() == MID_CPU_CHIP_VALLEYVIEW2) {
-		nr_entries = get_e820_by_bios(bp->e820_map);
-		bp->e820_entries = (nr_entries > 0) ? nr_entries : 0;
-	} else {
-		sfi_setup_e820(bp);
+
+	while (1){
+		if (*(u32 *)ptr == SETUP_SIGNATURE && *(u32 *)(ptr+4) == 0)
+			break;
+		ptr++;
 	}
+	ptr+=4;
+	return (((unsigned int)ptr+511)/512)*512;
 }
 
 static int get_32bit_entry(unsigned char *ptr)
@@ -190,29 +264,253 @@ static void setup_spi(void)
 	}
 }
 
+static void setup_gdt(void)
+{
+        static const u64 boot_gdt[] __attribute__((aligned(16))) = {
+                /* CS: code, read/execute, 4 GB, base 0 */
+                [GDT_ENTRY_BOOT_CS] = GDT_ENTRY(0xc09b, 0, 0xfffff),
+                /* DS: data, read/write, 4 GB, base 0 */
+                [GDT_ENTRY_BOOT_DS] = GDT_ENTRY(0xc093, 0, 0xfffff),
+        };
+        static struct gdt_ptr gdt;
+
+        gdt.len = sizeof(boot_gdt)-1;
+        gdt.ptr = (u32)&boot_gdt;
+
+        asm volatile("lgdtl %0" : : "m" (gdt));
+}
+
+static void setup_idt(void)
+{
+        static const struct gdt_ptr null_idt = {0, 0};
+        asm volatile("lidtl %0" : : "m" (null_idt));
+}
+
+static void vxe_fw_setup(void)
+{
+	u8 *vxe_fw_image;
+	u32 vxe_fw_size;
+	u32 vxe_fw_load_adrs;
+
+	vxe_fw_size = *(u32*)VXE_FW_SIZE_OFFSET;
+	/* do we have a VXE FW image? */
+	if (vxe_fw_size == 0)
+		return;
+
+	/* Do we have enough room to load the image? */
+	if (vxe_fw_size > imr6_toc.entries[IMR_TOC_ENTRY_VXE_FW].size) {
+		bs_printk("FATAL ERROR: VXE FW image size is too large for IMR\n");
+		FATAL_HANG();
+	}
+
+	vxe_fw_image = (u8 *)(
+		BZIMAGE_OFFSET
+		+ *(u32 *)BZIMAGE_SIZE_OFFSET
+		+ *(u32 *)INITRD_SIZE_OFFSET
+	);
+
+	vxe_fw_load_adrs = IMR6_START_ADDRESS + imr6_toc.entries[IMR_TOC_ENTRY_VXE_FW].start_offset;
+	memcpy((u8 *)vxe_fw_load_adrs, vxe_fw_image, vxe_fw_size);
+}
+
+static void load_imr_toc(u32 imr, u32 imrsize, imr_toc_t *toc, u32 tocsize)
+{
+	if (imr == 0 || imrsize == 0 || toc == NULL || tocsize == 0 || imrsize < tocsize )
+	{
+                bs_printk("FATAL ERROR: TOC size is too large for IMR\n");
+		FATAL_HANG();
+	}
+	memcpy((u8 *)imr, (u8 *)toc, tocsize);
+}
+
+
+static u32 xen_multiboot_setup(void)
+{
+	u32 *magic, *xen_image, i;
+	char *src, *dst;
+	u32 xen_size;
+	u32 xen_jump_adrs;
+	static module_t modules[3];
+	static multiboot_info_t mb = {
+		.flags = MBI_CMDLINE | MBI_MODULES | MBI_MEMMAP | MBI_DRIVES,
+		.mmap_addr = (u32)mb_mmap,
+		.mods_count = 3,
+		.mods_addr = (u32)modules,
+	};
+
+	xen_size =  *(u32 *)XEN_SIZE_OFFSET;
+	/* do we have a xen image? */
+	if (xen_size == 0) {
+		return 0;
+        }
+
+	/* Compute the actual offset of the Xen image */
+	xen_image = (u32*)(
+		BZIMAGE_OFFSET
+		+ *(u32 *)BZIMAGE_SIZE_OFFSET
+		+ *(u32 *)INITRD_SIZE_OFFSET
+		+ *(u32 *)VXE_FW_SIZE_OFFSET
+		+ *(u32 *)SEC_PLAT_SVCS_SIZE_OFFSET
+	);
+
+	/* the multiboot signature should be located in the first 8192 bytes */
+	for (magic = xen_image; magic < xen_image + 2048; magic++)
+		if (*magic == MULTIBOOT_HEADER_MAGIC)
+			break;
+	if (*magic != MULTIBOOT_HEADER_MAGIC) {
+		return 0;
+        }
+
+	mb.cmdline = (u32)strnchr((char *)CMDLINE_OFFSET, '$', CMDLINE_SIZE) + 1;
+	dst = mb.cmdline + strnlen(mb.cmdline, CMDLINE_SIZE) - 1;
+	*dst = ' ';
+	dst++;
+	src = (const char *)CMDLINE_OFFSET;
+	for (i = 0 ;i < strnlen((const char *)CMDLINE_OFFSET, CMDLINE_SIZE);i++) {
+		if (!strncmp(src, "capfreq=", 8)) {
+			while (*src != ' ' && *src != 0) {
+				*dst = *src;
+				dst++;
+				src++;
+			}
+			break;
+		}
+		src++;
+	}
+
+	/* fill in the multiboot module information: dom0 kernel + initrd + Platform Services Image */
+	modules[0].mod_start = BZIMAGE_OFFSET;
+	modules[0].mod_end = BZIMAGE_OFFSET + *(u32 *)BZIMAGE_SIZE_OFFSET;
+	modules[0].string = CMDLINE_OFFSET;
+
+	modules[1].mod_start = modules[0].mod_end ;
+	modules[1].mod_end = modules[1].mod_start + *(u32 *)INITRD_SIZE_OFFSET;
+	modules[1].string = 0;
+
+	modules[2].mod_start = sps_load_adrs;
+	modules[2].mod_end = modules[2].mod_start + *(u32 *)SEC_PLAT_SVCS_SIZE_OFFSET;
+	modules[2].string = 0;
+
+	mb.drives_addr = IMR6_START_ADDRESS + imr6_toc.entries[IMR_TOC_ENTRY_XEN_EXTRA].start_offset;
+	mb.drives_length = imr6_toc.entries[IMR_TOC_ENTRY_XEN_EXTRA].size;
+
+	for(i = 0; i < E820MAX; i++)
+		if (!mb_mmap[i].size)
+			break;
+	mb.mmap_length = i * sizeof(memory_map_t);
+
+	/* relocate xen to start address */
+	if (xen_size > imr7_size) {
+		bs_printk("FATAL ERROR: Xen image size is too large for IMR\n");
+		FATAL_HANG();
+	}
+	xen_jump_adrs = IMR7_START_ADDRESS;
+	memcpy((u8 *)xen_jump_adrs, xen_image, xen_size);
+
+	mb_info = (u32)&mb;
+	mb_magic = MULTIBOOT_BOOTLOADER_MAGIC;
+
+	return (u32)xen_jump_adrs;
+}
+
+static void sec_plat_svcs_setup(void)
+{
+	u8 *sps_image;
+	u32 sps_size;
+
+	sps_size = PAGE_ALIGN_FWD(*(u32*)SEC_PLAT_SVCS_SIZE_OFFSET);
+	/* do we have a SPS image? */
+	if (sps_size == 0)
+		return;
+
+	/* Do we have enough room to load the image? */
+	if (sps_size > imr7_size) {
+		bs_printk("FATAL ERROR: SPS image size is too large for IMR\n");
+		FATAL_HANG();
+	}
+
+	sps_image = (u8 *)(
+		BZIMAGE_OFFSET
+		+ *(u32 *)BZIMAGE_SIZE_OFFSET
+		+ *(u32 *)INITRD_SIZE_OFFSET
+		+ *(u32 *)VXE_FW_SIZE_OFFSET
+	);
+
+	/* load SPS image (with assumed CHAABI Mailboxes suffixed) */
+	/* at bottom of IMR7 */
+	/* Must be page-aligned or Xen will panic */
+	sps_load_adrs = PAGE_ALIGN_BACK(IMR7_START_ADDRESS + imr7_size - sps_size);
+	memcpy((u8 *)sps_load_adrs, sps_image, sps_size);
+
+	/* reduce remaining size for Xen image size check */
+	imr7_size -= sps_size;
+}
+
 int bootstub(void)
 {
+	u32 jmp;
+	struct boot_params *bp = (struct boot_params *)BOOT_PARAMS_OFFSET;
+	struct setup_header *sh = (struct setup_header *)SETUP_HEADER_OFFSET;
+	u32 imr_size;
+	int nr_entries;
+
+        setup_idt();
+	setup_gdt();
 	setup_spi();
 	bs_printk("Bootstub Version: 1.3 ...\n");
-	setup_boot_params((struct boot_params *)BOOT_PARAMS_OFFSET, 
-		(struct setup_header *)SETUP_HEADER_OFFSET);
-	bs_printk("Jump to kernel 32bit entry ...\n");
-	return get_32bit_entry((unsigned char *)BZIMAGE_OFFSET);
+
+	memset(bp, 0, sizeof (struct boot_params));
+
+	if (mid_identify_cpu() == MID_CPU_CHIP_VALLEYVIEW2) {
+		nr_entries = get_e820_by_bios(bp->e820_map);
+		bp->e820_entries = (nr_entries > 0) ? nr_entries : 0;
+	} else {
+	        sfi_setup_mmap(bp, mb_mmap);
+	}
+
+	if (mid_identify_cpu() != MID_CPU_CHIP_TANGIER) {
+		if ((IMR6_END_ADDRESS > IMR6_START_ADDRESS) && (IMR7_END_ADDRESS > IMR7_START_ADDRESS)) {
+			imr_size  = PAGE_ALIGN_FWD(IMR6_END_ADDRESS - IMR6_START_ADDRESS);
+			load_imr_toc(IMR6_START_ADDRESS, imr_size, &imr6_toc, sizeof(imr6_toc));
+			vxe_fw_setup();
+			sfi_add_e820_entry(bp, mb_mmap, IMR6_START_ADDRESS, imr_size, E820_RESERVED);
+
+			imr7_size  = PAGE_ALIGN_FWD(IMR7_END_ADDRESS - IMR7_START_ADDRESS);
+			sec_plat_svcs_setup();
+			sfi_add_e820_entry(bp, mb_mmap, IMR7_START_ADDRESS, imr7_size, E820_RESERVED);
+		} else {
+			*(u32 *)XEN_SIZE_OFFSET = 0;	/* Don't allow Xen to boot */
+		}
+	} else {
+		*(u32 *)XEN_SIZE_OFFSET = 0;	/* Don't allow Xen to boot */
+	}
+
+	setup_boot_params(bp, sh);
+
+	jmp = xen_multiboot_setup();
+	if (!jmp) {
+		bs_printk("Using bzImage to boot\n");
+		jmp = bzImage_setup(bp, sh);
+	} else
+		bs_printk("Using multiboot image to boot\n");
+
+	bs_printk("Jump to kernel 32bit entry\n");
+	return jmp;
 }
 
 void bs_printk(const char *str)
 {
-	if (*(int *)SPI_UART_SUPPRESSION)
-		return;
+        if (*(int *)SPI_UART_SUPPRESSION)
+                return;
 
-	switch (*(int *)SPI_TYPE) {
+        switch (*(int *)SPI_TYPE) {
 
-	case SPI_1:
+        case SPI_1:
                 bs_spi_printk(str);
                 break;
 
         case SPI_2:
                 bs_ssp_printk(str);
                 break;
-	}
+        }
 }
diff --git a/bootstub.h b/bootstub.h
index 21b08bb..0d4845a 100644
--- a/bootstub.h
+++ b/bootstub.h
@@ -28,12 +28,21 @@
 #define SPI_1		1
 #define SPI_2		2
 
-#define STACK_OFFSET		0x1101000
-#define BZIMAGE_OFFSET		0x1102000
+
+#define FLAGS_RESERVED_0	(SPI_TYPE + 4)
+#define FLAGS_RESERVED_1	(FLAGS_RESERVED_0 + 4)
+#define VXE_FW_SIZE_OFFSET		(FLAGS_RESERVED_1 + 4)
+#define SEC_PLAT_SVCS_SIZE_OFFSET	(VXE_FW_SIZE_OFFSET + 4)
+#define XEN_SIZE_OFFSET		(SEC_PLAT_SVCS_SIZE_OFFSET + 4)
+
+#define BOOTSTUB_OFFSET		0x1101000
+#define STACK_OFFSET		BOOTSTUB_OFFSET
+#define BZIMAGE_OFFSET		0x1103000
 
 #define SETUP_HEADER_OFFSET (BZIMAGE_OFFSET + 0x1F1)
 #define SETUP_HEADER_SIZE (0x0202 + *(unsigned char*)(0x0201+BZIMAGE_OFFSET))
 #define BOOT_PARAMS_OFFSET 0x8000
+#define BOOT_CMDLINE_OFFSET 0x10000
 #define SETUP_SIGNATURE 0x5a5aaa55
 
 #define GDT_ENTRY_BOOT_CS       2
diff --git a/head.S b/head.S
index a949e7a..afd84d0 100644
--- a/head.S
+++ b/head.S
@@ -62,53 +62,26 @@
 _start:
 	cld
 	cli
-
-	/* Set our own GDT and IDT, don't derive from IAFW */
-	lgdtl	%cs:gdtr
-	lidtl	%cs:idtr
-
-	/* Load segment registers per protected mode kernel entry requirement:
-	 * CS=0x10,
-	 * DS=ES=SS=FS=GS=0x18
-	 */
-	ljmp	$__BOOT_CS, $1f
-1:
+	/* setup stack, because we are heading off to "C" */
+	movl $STACK_OFFSET, %esp
+	calll bootstub
+	/* after call bootstub, GDT is set, IDT is clear
+	* eax contains 32-bit entry of bzImage
+	*/
 	movl $__BOOT_DS, %ebx	
 	movl %ebx, %ds
 	movl %ebx, %es
 	movl %ebx, %fs
 	movl %ebx, %gs
 	movl %ebx, %ss
-
-	/* setup stack, because we are heading off to "C" */
-	movl $STACK_OFFSET, %esp
-
-	/* bootstub() returns 32bit entry address of bzImage, stored in eax */
-	calll bootstub
-
+	ljmp $__BOOT_CS,$1f
+1:
 	/* tell kernel where is boot_param */
 	movl $(BOOT_PARAMS_OFFSET), %esi
 	xor %ebp, %ebp
 	xor %edi, %edi
-	xor %ebx, %ebx
-
-	/* Jump to the 32-bit entrypoint */
-	jmpl *%eax
-
-	.balign 8
-gdt:
-        .quad   0
-        .quad   0
-        .quad   GDT_ENTRY(0xc09b, 0, 0xfffff)
-        .quad   GDT_ENTRY(0xc093, 0, 0xfffff)
-gdtr:
-        .word   4*8-1
-        .long   gdt
+	mov %eax, %ecx
+	mov mb_magic, %eax
+	mov mb_info, %ebx
 
-	.balign 8
-idt:
-	.quad	0
-	.quad	0
-idtr:
-	.word	2*8-1
-	.long	idt
+	jmpl *%ecx    # Jump to the 32-bit entrypoint
diff --git a/imr_toc.c b/imr_toc.c
new file mode 100644
index 0000000..758fdaf
--- /dev/null
+++ b/imr_toc.c
@@ -0,0 +1,72 @@
+#include <stdint.h>
+#include "imr_toc.h"
+
+imr_toc_t imr6_toc = { {
+    /* Table of Contents */
+    { 0x00000000,
+      0x00001000,
+      IMR6_TOC_MAGIC,
+      MAKE_TOC_VERSION(IMR6_TOC_VERSION_MAJ, IMR6_TOC_VERSION_MIN)
+    },
+    /* MTX writeback buffer */
+    { 0x00002000,
+      0x00001000,
+      0, 0
+    },
+    /* VXE FW */
+    { 0x00003000,
+      0x00080000,
+      0, 0
+    },
+    /* VXE context buffer */
+    { 0x00083000,
+      0x0000C000,
+      0, 0
+    },
+    /* VXE secure page tables */
+    { 0x0008F000,
+      0x00020000,
+      0, 0
+    },
+    /* protected content bufs */
+    { 0x000AF000,
+      0x01551000,
+      IMR6_PC_BUFS_START_VADDR,
+      0
+    },
+    /* shadow page table */
+    { 0x00060000,
+      0x00020000,
+      0,
+      0
+    },
+    /* memory for Xen */
+    { 0x01600000,
+      0x00C00000,
+      0, 0
+    }}
+};
+
+imr_toc_t imr7_toc = { {
+    /* Table of Contents */
+    { 0x00000000,
+      0x00001000,
+      IMR7_TOC_MAGIC,
+      MAKE_TOC_VERSION(IMR7_TOC_VERSION_MAJ, IMR7_TOC_VERSION_MIN)
+    },
+    /* platform svcs/Chaabi mailboxes */
+    { 0x00001000,
+      0x00001000,
+      0, 0
+    },
+    /* IA runtime FW */
+    { 0x00002000,
+      0x00020000,
+      0, 0
+    },
+    /* Xen */
+    { 0x00022000,
+      0x00300000,
+      0 ,0
+    } }
+};
diff --git a/imr_toc.h b/imr_toc.h
new file mode 100644
index 0000000..e17e987
--- /dev/null
+++ b/imr_toc.h
@@ -0,0 +1,87 @@
+#ifndef _IMR_TOC_H_
+#define _IMR_TOC_H_
+
+/*
+ * IMR Table of Contents format
+ */
+typedef struct {
+    uint32_t start_offset;
+    uint32_t size;
+    uint32_t reserved1;
+    uint32_t reserved2;
+} imr_toc_entry_t;
+
+typedef struct {
+    imr_toc_entry_t entries[8];    /* pick reasonable size to make gcc happy */
+} imr_toc_t;
+
+#define MAKE_TOC_VERSION(maj, min)  ((min) << 16 | (maj))
+typedef struct {
+    uint16_t toc_maj_ver;
+    uint16_t toc_min_ver;
+} imr_toc_entry_version_t;
+
+/*
+ * IMR6 values
+ */
+
+#define IMR6_TOC_MAGIC       0x6CD96EDB
+
+#define IMR6_TOC_VERSION_MAJ 0x0001
+#define IMR6_TOC_VERSION_MIN 0x0000
+
+/* ToC entry order for IMR6 */
+enum imr6_entries {
+    IMR_TOC_ENTRY_TOC = 0,
+    IMR_TOC_ENTRY_MTX_WB_BUF,
+    IMR_TOC_ENTRY_VXE_FW,
+    IMR_TOC_ENTRY_VXE_CTX_BUF,
+    IMR_TOC_ENTRY_VXE_SEC_PGTBLS,
+    IMR_TOC_ENTRY_PC_BUFS,
+    IMR_TOC_ENTRY_VXE_SHADOW_PGTBLS,
+    IMR_TOC_ENTRY_XEN_EXTRA,
+};
+
+
+/*
+ * IMR7 values
+ */
+
+#define IMR7_TOC_MAGIC       0x6ED96CDB
+
+#define IMR7_TOC_VERSION_MAJ 0x0001
+#define IMR7_TOC_VERSION_MIN 0x0000
+
+/* ToC entry order for IMR7 */
+enum imr7_entries {
+    /* IMR_TOC_ENTRY_TOC = 0, */
+    IMR_TOC_ENTRY_MAILBOXES = 1,     /* contents per imr_ia_chaabi_mailbox_t */
+    IMR_TOC_ENTRY_IA_RUNTIME_FW,
+    IMR_TOC_ENTRY_XEN
+};
+
+/* entry-specific data structures */
+
+#define IMR6_PC_BUFS_START_VADDR  0x11223344
+
+typedef struct {
+    uint32_t hdcp_sess_status;
+    union {
+        struct {
+	    uint32_t hdcp_sess_key_00_31;
+	    uint32_t hdcp_sess_key_32_63;
+	    uint32_t hdcp_sess_key_64_95;
+	    uint32_t hdcp_sess_key_96_127;
+	};
+        uint8_t hdcp_sess_key[16];
+    };
+    union {
+        struct {
+	    uint32_t hdcp_iv_00_31;
+	    uint32_t hdcp_iv_32_63;
+	};
+        uint64_t hdcp_iv;
+    };
+} imr_ia_chaabi_mailbox_t;
+
+#endif
diff --git a/mb.h b/mb.h
new file mode 100644
index 0000000..d988f6a
--- /dev/null
+++ b/mb.h
@@ -0,0 +1,127 @@
+/* Copyright (C) 1999, 2001  Free Software Foundation, Inc.
+
+   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 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  */
+
+#ifndef __MULTIBOOT_H__
+#define __MULTIBOOT_H__
+
+/*
+ * Multiboot header structure.
+ */
+#define MULTIBOOT_HEADER_MAGIC         0x1BADB002
+#define MULTIBOOT_HEADER_MODS_ALIGNED  0x00000001
+#define MULTIBOOT_HEADER_WANT_MEMORY   0x00000002
+#define MULTIBOOT_HEADER_HAS_VBE       0x00000004
+#define MULTIBOOT_HEADER_HAS_ADDR      0x00010000
+
+/* The magic number passed by a Multiboot-compliant boot loader. */
+#define MULTIBOOT_BOOTLOADER_MAGIC     0x2BADB002
+
+#define MBI_MEMLIMITS  (1u<< 0)
+#define MBI_BOOTDEV    (1u<< 1)
+#define MBI_CMDLINE    (1u<< 2)
+#define MBI_MODULES    (1u<< 3)
+#define MBI_AOUT_SYMS  (1u<< 4)
+#define MBI_ELF_SYMS   (1u<< 5)
+#define MBI_MEMMAP     (1u<< 6)
+#define MBI_DRIVES     (1u<< 7)
+#define MBI_BIOSCONFIG (1u<< 8)
+#define MBI_LOADERNAME (1u<< 9)
+#define MBI_APM        (1u<<10)
+
+#ifndef __ASSEMBLY__
+
+/* The symbol table for a.out.  */
+typedef struct {
+	u32 tabsize;
+	u32 strsize;
+	u32 addr;
+	u32 reserved;
+} aout_symbol_table_t;
+
+/* The section header table for ELF.  */
+typedef struct {
+	u32 num;
+	u32 size;
+	u32 addr;
+	u32 shndx;
+} elf_section_header_table_t;
+
+/* The Multiboot information.  */
+typedef struct {
+	u32 flags;
+
+	/* Valid if flags sets MBI_MEMLIMITS */
+	u32 mem_lower;
+	u32 mem_upper;
+
+	/* Valid if flags sets MBI_BOOTDEV */
+	u32 boot_device;
+
+	/* Valid if flags sets MBI_CMDLINE */
+	u32 cmdline;
+
+	/* Valid if flags sets MBI_MODULES */
+	u32 mods_count;
+	u32 mods_addr;
+
+	/* Valid if flags sets ... */
+	union {
+		aout_symbol_table_t aout_sym;        /* ... MBI_AOUT_SYMS */
+		elf_section_header_table_t elf_sec;  /* ... MBI_ELF_SYMS */
+	} u;
+
+	/* Valid if flags sets MBI_MEMMAP */
+	u32 mmap_length;
+	u32 mmap_addr;
+
+	/* Valid if flags sets MBI_DRIVES */
+	u32 drives_length;
+	u32 drives_addr;
+
+	/* Valid if flags sets MBI_BIOSCONFIG */
+	u32 config_table;
+
+	/* Valid if flags sets MBI_LOADERNAME */
+	u32 boot_loader_name;
+
+	/* Valid if flags sets MBI_APM */
+	u32 apm_table;
+} multiboot_info_t;
+
+/* The module structure.  */
+typedef struct {
+	u32 mod_start;
+	u32 mod_end;
+	u32 string;
+	u32 reserved;
+} module_t;
+
+/* The memory map. Be careful that the offset 0 is base_addr_low
+   but no size.  */
+typedef struct {
+	u32 size;
+	u32 base_addr_low;
+	u32 base_addr_high;
+	u32 length_low;
+	u32 length_high;
+	u32 type;
+} memory_map_t;
+
+
+#endif /* __ASSEMBLY__ */
+
+
+#endif /* _BOOTSTUB_MB_H */
diff --git a/sfi.c b/sfi.c
index 1f88d71..6636493 100644
--- a/sfi.c
+++ b/sfi.c
@@ -20,92 +20,90 @@
 #include "types.h"
 #include "bootparam.h"
 #include "bootstub.h"
-#include "spi-uart.h"
+#include "mb.h"
 #include "sfi.h"
 
 #define SFI_BASE_ADDR		0x000E0000
 #define SFI_LENGTH		0x00020000
-#define SFI_TABLE_LENGTH	16
 
-static int sfi_table_check(struct sfi_table_header *sbh)
+static unsigned long sfi_search_mmap(unsigned long start, int len)
 {
-	char chksum = 0;
-	char *pos = (char *)sbh;
-	int i;
-
-	if (sbh->length < SFI_TABLE_LENGTH)
-		return -1;
-
-	if (sbh->length > SFI_LENGTH)
-		return -1;
-
-	for (i = 0; i < sbh->length; i++)
-		chksum += *pos++;
-
-	if (chksum)
-		bs_printk("sfi: Invalid checksum\n");
-
-	/* checksum is ok if zero */
-	return chksum;
+	unsigned long i = 0;
+	char *pchar = (char *)start;
+
+	for (i = 0; i < len; i++, pchar++) {
+		if (pchar[0] == 'M'
+			&& pchar[1] == 'M'
+			&& pchar[2] == 'A'
+			&& pchar[3] == 'P')
+			return start + i;
+	}
+	return 0;
 }
 
-static unsigned long sfi_search_mmap(void)
+int sfi_add_e820_entry(struct boot_params *bp, memory_map_t *mb_mmap, u64 start, u64 size, int type)
 {
-	u32 i = 0;
-	u32 *pos = (u32 *)SFI_BASE_ADDR;
-	u32 *end = (u32 *)(SFI_BASE_ADDR + SFI_LENGTH);
-	struct sfi_table_header *sbh;
-	struct sfi_table *sb;
-	u32 sys_entry_cnt = 0;
-
-	/* Find SYST table */
-	for (; pos < end; pos += 4) {
-		if (*pos == SFI_SYST_MAGIC) {
-			if (!sfi_table_check((struct sfi_table_header *)pos))
-				break;
+        struct e820entry * e820_entry;
+	memory_map_t	*mb_mmap_entry;
+	int	i;
+
+	/* ASSERT: bp != NULL */
+	/* ASSERT: mb_mmap != NULL */
+
+	for (i=0; i < bp->e820_entries; i++) {
+		e820_entry = &(bp->e820_map[i]);
+		mb_mmap_entry = &(mb_mmap[i]);
+		if (e820_entry->addr == start) {
+			/* Override size and type */
+			e820_entry->size = size;
+			e820_entry->type = type;
+			mb_mmap_entry->length_low = size;
+			mb_mmap_entry->length_high = 0;
+			mb_mmap_entry->type = (type == E820_RAM)?1:0;
+			return 0;
 		}
 	}
 
-	if (pos >= end) {
-		bs_printk("Bootstub: failed to locate SFI SYST table\n");
-		return 0;
-	}
+	/* ASSERT: no duplicate start address found */
+	if (bp->e820_entries == E820MAX)
+		return -1;
 
-	/* map table pointers */
-	sb = (struct sfi_table *)pos;
-	sbh = (struct sfi_table_header *)sb;
+	e820_entry = &(bp->e820_map[bp->e820_entries]);
+	mb_mmap_entry = &(mb_mmap[bp->e820_entries]);
 
-	sys_entry_cnt = (sbh->length - sizeof(struct sfi_table_header)) >> 3;
+	e820_entry->addr = start;
+	e820_entry->size = size;
+	e820_entry->type = type;
 
-	/* Search through each SYST entry for MMAP table */
-	for (i = 0; i < sys_entry_cnt; i++) {
-		sbh = (struct sfi_table_header *)sb->entry[i].low;
-		if (*(u32 *)sbh->signature == SFI_MMAP_MAGIC) {
-			if (!sfi_table_check((struct sfi_table_header *)sbh))
-				return (unsigned long) sbh;
-		}
-	}
+	mb_mmap_entry->size = 20;
+	mb_mmap_entry->base_addr_low = start;
+	mb_mmap_entry->base_addr_high = 0;
+	mb_mmap_entry->length_low = size;
+	mb_mmap_entry->length_high = 0;
+	mb_mmap_entry->type = (type == E820_RAM)?1:0;
+
+	bp->e820_entries++;
 
 	return 0;
 }
 
-void sfi_setup_e820(struct boot_params *bp)
+void sfi_setup_mmap(struct boot_params *bp, memory_map_t *mb_mmap)
 {
 	struct sfi_table *sb;
 	struct sfi_mem_entry *mentry;
 	unsigned long long start, end, size;
-	int i, num, type, total;
+	int i, num, type;
 
-	bp->e820_entries = 0;
-	total = 0;
+	if (bp)
+		bp->e820_entries = 0;
 
 	/* search for sfi mmap table */
-	sb = (struct sfi_table *)sfi_search_mmap();
+	sb = (struct sfi_table *)sfi_search_mmap(SFI_BASE_ADDR, SFI_LENGTH);
 	if (!sb) {
-		bs_printk("Bootstub: failed to locate SFI MMAP table\n");
+		bs_printk("Bootstub: SFI MMAP table not found\n");
 		return;
 	}
-	bs_printk("Bootstub: will use sfi mmap table for e820 table\n");
+	bs_printk("Bootstub: map SFI MMAP to e820 table\n");
 	num = SFI_GET_ENTRY_NUM(sb, sfi_mem_entry);
 	mentry = (struct sfi_mem_entry *)sb->pentry;
 
@@ -130,14 +128,10 @@ void sfi_setup_e820(struct boot_params *bp)
 			type = E820_RESERVED;
 		}
 
-		if (total == E820MAX)
+		if (sfi_add_e820_entry(bp, mb_mmap, start, size, type) != 0)
 			break;
-		bp->e820_map[total].addr = start;
-		bp->e820_map[total].size = size;
-		bp->e820_map[total++].type = type;
 
 		mentry++;
 	}
 
-	bp->e820_entries = total;
 }
diff --git a/sfi.h b/sfi.h
index 21e5d1a..2a182cd 100644
--- a/sfi.h
+++ b/sfi.h
@@ -37,17 +37,9 @@ struct sfi_table_header {
 	char oem_table_id[8];
 }__attribute__((packed));
 
-struct __sfi_quad_word {
-	u32 low;
-	u32 high;
-};
-
 struct sfi_table {
 	struct sfi_table_header header;
-	union {
-		u64 pentry[1];
-		struct __sfi_quad_word entry[1];
-	};
+	u64 pentry[1];
 }__attribute__((packed));
 
 #define SFI_TBL_HEADER_LEN      24
@@ -62,8 +54,7 @@ struct sfi_table {
 #define E820_NVS	4
 #define E820_UNUSABLE	5
 
-#define SFI_SYST_MAGIC	0x54535953 /* SYST */
-#define SFI_MMAP_MAGIC	0x50414d4d /* MMAP */
+extern void sfi_setup_mmap(struct boot_params *bp, memory_map_t *mb_mmap);
+extern int sfi_add_e820_entry(struct boot_params *bp, memory_map_t *mb_mmap, u64 start, u64 size, int type);
 
-extern void sfi_setup_e820(struct boot_params *bp);
 #endif /* _SFI_H */