diff --git a/Makefile b/Makefile
index 0fb7d0e..1d0944a 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-OBJ=bootstub.o spi-uart.o head.o sfi.o
+OBJ=bootstub.o spi-uart.o head.o sfi.o e820_bios.o
 CMDLINE_SIZE ?= 0x400
 CFLAGS=-m32 -ffreestanding -Wall -DCMDLINE_SIZE=${CMDLINE_SIZE}
 CC ?= gcc
@@ -24,7 +24,10 @@
 	${CC} $(CFLAGS) -c sfi.c
 
 head.o:head.S bootstub.h
-	${CC} $(CFLAGS) -c head.S
+	${CC} $(CFLAGS) -D__ASSEMBLY__ -c head.S
+
+e820_bios.o:e820_bios.S bootstub.h
+	${CC} $(CFLAGS) -D__ASSEMBLY__ -c e820_bios.S
 
 clean:
 	rm -rf *.o *.bin *.elf *.bz2 *.rpm
diff --git a/bootstub.c b/bootstub.c
index f53409f..26c96d2 100644
--- a/bootstub.c
+++ b/bootstub.c
@@ -27,33 +27,6 @@
 
 #define bs_printk(x) { if (! *(int *)SPI_UART_SUPPRESSION) bs_spi_printk(x);}
 
-struct gdt_ptr {
-        u16 len;
-        u32 ptr;
-} __attribute__((packed));
-
-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 *memcpy(void *dest, const void *src, size_t count)
 {
         char *tmp = dest;
@@ -101,6 +74,7 @@
 static void setup_boot_params(struct boot_params *bp, struct setup_header *sh)
 {
 	u8 *initramfs;
+	int nr_entries;
 
 	memset(bp, 0, sizeof (struct boot_params));
 	bp->screen_info.orig_video_mode = 0;
@@ -121,7 +95,12 @@
 	} else {
 		bs_printk("Won't relocate initramfs, are you in SLE?\n");
 	}
-	sfi_setup_e820(bp);
+	if (mrst_identify_cpu() == MRST_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);
+	}
 }
 
 static int get_32bit_entry(unsigned char *ptr)
@@ -187,8 +166,6 @@
 
 int bootstub(void)
 {
-	setup_idt();
-	setup_gdt();
 	setup_spi();
 	bs_printk("Bootstub Version: 1.2 ...\n");
 	setup_boot_params((struct boot_params *)BOOT_PARAMS_OFFSET, 
diff --git a/bootstub.h b/bootstub.h
index 2e687bd..b1fc0d5 100644
--- a/bootstub.h
+++ b/bootstub.h
@@ -33,11 +33,22 @@
 #define GDT_ENTRY_BOOT_DS       (GDT_ENTRY_BOOT_CS + 1)
 #define __BOOT_DS               (GDT_ENTRY_BOOT_DS * 8)
 
+#ifdef __ASSEMBLY__
+#define GDT_ENTRY(flags, base, limit)			\
+	((((base)  & 0xff000000) << (56-24)) |	\
+	 (((flags) & 0x0000f0ff) << 40) |	\
+	 (((limit) & 0x000f0000) << (48-16)) |	\
+	 (((base)  & 0x00ffffff) << 16) |	\
+	 (((limit) & 0x0000ffff)))
+#else
 #define GDT_ENTRY(flags, base, limit)           \
         (((u64)(base & 0xff000000) << 32) |     \
          ((u64)flags << 40) |                   \
          ((u64)(limit & 0x00ff0000) << 32) |    \
          ((u64)(base & 0x00ffffff) << 16) |     \
          ((u64)(limit & 0x0000ffff)))
+int get_e820_by_bios(void *e820_buf);
+int mrst_identify_cpu(void);
+#endif
 
 #endif
diff --git a/e820_bios.S b/e820_bios.S
new file mode 100644
index 0000000..833cd31
--- /dev/null
+++ b/e820_bios.S
@@ -0,0 +1,202 @@
+/*
+ * e820_bios.S: read e820 by int 15h call.
+ *
+ * The C language function exported by this file is:
+ * int get_e820_by_bios(void *e820_buf);
+ * @e820_buf: e820 mem map buffer, allocated by caller
+ * return: number of e820 entries
+ *
+ * Copyright (C) 2013 Intel Corporation.
+ * Author: Bin Gao <bin.gao@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#include "bootstub.h"
+
+/* Real mode low memory layout */
+#define IDT_START	0x0
+#define RELOCATED_START	0xa000
+#define	STACK_START	0xb000
+#define DATA_START	0xb200
+
+#define SAVED_GDTR_ADDR	0xb100
+#define SAVED_IDTR_ADDR	0xb110
+#define	COUNT_ADDR	0xb120
+#define	TOTAL_COUNT_ADDR	0xb130
+#define MIN_BUF_LEN	20
+#define	BUF_LEN		2048
+#define MAX_NR_ENTRIES	128
+
+#define SMAP	0x534d4150
+#define E820	0xe820
+
+.text
+.section ".text.head","ax",@progbits
+
+	.code32
+	.globl get_e820_by_bios
+get_e820_by_bios:
+	jmp	start_32bit
+
+	.balign 16
+idtr:
+	.word	0xffff
+	.long	IDT_START
+
+	.balign 16
+gdt:
+	.quad	0
+	.quad	GDT_ENTRY(0x009b, 0, 0xffff)
+	.quad	GDT_ENTRY(0x0093, 0, 0xffff)
+gdtr:
+	.word	3*8-1
+	.long	gdt
+
+saved_esp:
+	.long	0
+
+start_32bit:
+	pushal
+	pushfl
+
+	/* Save ESP, GDTR and IDTR registers */
+        movl	$saved_esp, %eax
+	movl	%esp, (%eax)
+	xorl	%eax, %eax
+	sidtl	SAVED_IDTR_ADDR(%eax)
+	sgdtl	SAVED_GDTR_ADDR(%eax)
+
+	/* Relocate real mode codes to 64k segment */
+	movl    $relocated_end + 4, %ecx
+	subl	$relocated_start, %ecx
+	shrl	$2, %ecx
+        movl    $relocated_start, %esi
+        movl    $RELOCATED_START, %edi
+        rep     movsl
+
+	/* Set up real mode IDT */
+	lidtl	%cs:idtr
+
+	/* Set up real mode GDT */
+	lgdtl	%cs:gdtr
+	movl	$16, %ecx
+	movl	%ecx, %ds
+	movl	%ecx, %es
+	movl	%ecx, %fs
+	movl	%ecx, %gs
+	movl	%ecx, %ss
+
+	/* Switch to 16bit segment */
+	ljmpl	$8, $RELOCATED_START
+
+	.code16
+relocated_start:
+reloc_base = .
+
+	/* Switch to real mode */
+	andb	$0x10, %al
+	movl	%eax, %cr0
+	ljmpw	$0, $realmode_entry - relocated_start + RELOCATED_START
+
+realmode_entry = .
+	/* In real mode now, set up segment selectors */
+	movl	$0, %eax
+	movl	%eax, %ds
+	movl	%eax, %es
+	movl	%eax, %ss
+	movl	%eax, %gs
+	movl	%eax, %fs
+
+	movl	$STACK_START, %esp
+
+	/* Do int 15h call */
+	movl	$COUNT_ADDR, %eax
+	movl	$0, (%eax)
+	movl	$TOTAL_COUNT_ADDR, %eax
+	movl	$0, (%eax)
+	xorl	%ebx, %ebx
+	movw	$DATA_START, %di
+again:
+	movw	$E820, %ax
+	movw	$BUF_LEN, %cx
+	movl	$SMAP, %edx
+	int	$0x15
+	jc	error	/* EFLGAS.CF is set */
+	cmpl	$SMAP, %eax
+	jne	error	/* eax is not 'SMAP' */
+	cmpw	$MIN_BUF_LEN, %cx
+	jl	error /* returned buffer len < 20 */
+	cmpw	$BUF_LEN, %cx
+	jg	error /* returned buffer len > provided buffer len */
+	movl	$TOTAL_COUNT_ADDR, %eax
+	addw	%cx, (%eax)
+	movl	$COUNT_ADDR, %eax
+	incl	(%eax)
+	movl	(%eax), %eax
+	cmpl	$MAX_NR_ENTRIES, %eax /* max supported entries: 128 */
+	jge	done
+	testl	%ebx, %ebx /* ebx == 0: done, ebx != 0: continue */
+	je	done
+	addw	%cx, %di
+	jmp	again
+done:
+	jmp	2f
+error:
+	movl	$COUNT_ADDR, %eax
+	movl	$~0, (%eax)
+2:
+
+	/* Switch back to protected mode */
+	xorl	%ebx, %ebx
+	lidtl	SAVED_IDTR_ADDR(%ebx)
+	lgdtl	SAVED_GDTR_ADDR(%ebx)
+	movl    %cr0, %ebx
+	orb	$1, %bl
+	movl    %ebx, %cr0
+	.byte	0x66, 0xea /* opcode(JMP FAR) with operand size override */
+	.long	resumed_protected_mode	/* offset */
+	.word	__BOOT_CS /* segment selector */
+relocated_end = .
+
+	.code32
+resumed_protected_mode:
+	cli /* in case real mode codes turn on interrrupt! */
+	/* Restore segment registers */
+	movl	$__BOOT_DS, %ebx
+	movl	%ebx, %ds
+	movl	%ebx, %es
+	movl	%ebx, %gs
+	movl	%ebx, %fs
+	movl	%ebx, %ss
+
+	/* Restore stack pointer */
+	movl	$saved_esp, %eax
+	movl	(%eax), %esp
+
+	/* Copy e820 data from our buffer to caller's buffer */
+	xorl	%eax, %eax
+	movl	TOTAL_COUNT_ADDR(%eax), %ecx
+	movl    $DATA_START, %esi
+	movl    40(%esp), %edi
+	rep     movsb
+
+	popfl
+	popal
+
+	/* Return number of e820 entries */
+	movl	$COUNT_ADDR, %eax
+	movl	(%eax), %eax
+	ret
diff --git a/head.S b/head.S
index d4f21e2..a949e7a 100644
--- a/head.S
+++ b/head.S
@@ -62,26 +62,53 @@
 _start:
 	cld
 	cli
-	/* setup stack, because we are heading off to "C" */
-	movl $STACK_OFFSET, %esp
-	/* after call bootstub, GDT was set (0x10 and 0x18) IDT was clear
-	 * eax will store 32bit entry of bzImage
-         */
-	calll bootstub
-	/* DS=ES=FS=GS=10 */
+
+	/* 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:
 	movl $__BOOT_DS, %ebx	
 	movl %ebx, %ds
 	movl %ebx, %es
 	movl %ebx, %fs
 	movl %ebx, %gs
 	movl %ebx, %ss
-	ljmp $__BOOT_CS,$1f
-1:	
+
+	/* 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
+
 	/* tell kernel where is boot_param */
 	movl $(BOOT_PARAMS_OFFSET), %esi
 	xor %ebp, %ebp
 	xor %edi, %edi
 	xor %ebx, %ebx
-	
-	jmpl *%eax    # Jump to the 32-bit entrypoint
-	
+
+	/* 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
+
+	.balign 8
+idt:
+	.quad	0
+	.quad	0
+idtr:
+	.word	2*8-1
+	.long	idt