--- a/x86_64/elilo_kernel.c	Sat Feb 16 16:06:09 2013 +0900
+++ b/x86_64/elilo_kernel.c	Sun Feb 17 15:31:38 2013 +0900
@@ -4,6 +4,10 @@
 #include "elilo.h"
 #include "pgtable_flags.h"
 #include "sysdeps.h"
+#include "registers.h"
+
+
+#define ALIGN_4K 12 /* use 4KB aligned */
 
 #define MEMCPY(to, from, cnt) { \
         UINT8 *t = (UINT8 *)(to); \
@@ -52,42 +56,38 @@
 extern pml4_t *pml4;
 extern pdpte_t *pdpte;
 
-#define X86_CR4_PAE 0x00000020
-#define X86_CR4_PSE 0x00000010
-
 INTN
-enable_cr4_pae()
+enable_cr4_pae(cr4_t cr4)
 {
-    UINT64 __force_order;
-    UINT64 val;
-    asm volatile("mov %%cr4,%0\n\t" : "=r" (val), "=m" (__force_order));
-    val |= X86_CR4_PAE;
-    asm volatile("mov %0,%%cr4": : "r" (val), "m" (__force_order));
+    asm volatile("mov %%cr4,%0\n\t" : "=r" (cr4));
+    cr4.pae = ENABLE;
+    asm volatile("mov %0,%%cr4": : "r" (cr4));
     //asm volatile("movq %%rax, %%cr4"::"a"(cr4_flag));
     return 0;
 }
 
-
 UINTN
-insert_addr_to_cr3(UINT64 val)
+insert_addr_to_cr3(cr3_t cr3, UINT64 addr)
 {
-    UINT64 __force_order;
     // asm volatile ("movq %0, %%rax \n\tmovq %%rax, %%cr3" :: "m"(addr) );
     /* write cr3 */
-    asm volatile("mov %0,%%cr3": : "r" (val), "m" (__force_order));
+    Print(L"Read cr3.\n");
+    asm volatile("mov %%cr3,%0\n\t" : "=r" (cr3));
+    Print(L"Getting cr3 is pwt:%d, pcd:%d pdb 0x%lx\n addr:%lx \n", cr3.pwt, cr3.pcd, cr3.pdb, addr);
+    addr = addr >> ALIGN_4K;
+    cr3.pdb = addr;
+    Print(L"Write addr:%lx to cr3 / cr3.pdb: %lx.\n", addr, cr3.pdb);
+    asm volatile("mov %0,%%cr3": : "r" (cr3));
+    Print(L"Written cr3.\n");
     return 0;
 }
 
-#define X86_CR0_PG 0x80000000
-
 INTN
-enable_paging_cr0()
+enable_paging_cr0(cr0_t cr0)
 {   
-    UINT64 __force_order;
-    UINT64 val;
-    asm volatile("mov %%cr0,%0\n\t" : "=r" (val), "=m" (__force_order));
-    val |= X86_CR0_PG;
-    asm volatile("mov %0,%%cr0": : "r" (val), "m" (__force_order));
+    asm volatile("mov %%cr0,%0\n\t" : "=r" (cr0));
+    cr0.pg = ENABLE;
+    asm volatile("mov %0,%%cr0": : "r" (cr0));
     // asm volatile("movl %0, %%eax \n\t movq %%rax, %%cr0"::"m"(cr0_flag));
     return 0;
 }
@@ -108,10 +108,13 @@
 }
 
 #define PML4_START 0x00270000
-#define PDPTE_START PML4_START + (PML4_SIZE + 1) * PDPTE_SIZE
+#define PDPTE_START PML4_START + PML4_SIZE
 
 /* alloc pages use how many pages for 4KiB */
-#define PGCNT_BYTE 1024
+#define PGCNT_BYTE 4096
+
+/* PDPTE indicate original page is 1GB */
+#define ORIG_PAGE_SIZE 1048576 
 
 
 /* init_pgtable()
@@ -145,18 +148,22 @@
     init_pgtable_value((VOID *)pml4, PML4_SIZE * sizeof(pml4_t), 0);
     init_pgtable_value((VOID *)pdpte, PDPTE_SIZE * PML4_SIZE * sizeof(pdpte_t), 0);
 
+    UINT64 orig_addr_start = PDPTE_START + (PDPTE_SIZE * PML4_SIZE * sizeof(UINT64));
     UINTN i = 0;
     for (; i<PML4_SIZE ;i++) {
-        pml4[i].paddr = (UINT64)&pdpte[(PDPTE_SIZE * i) + PDPTE_SIZE];
+        UINT64 tmp_pdpte_addr = (UINT64)&pdpte[PDPTE_SIZE * i];
+        tmp_pdpte_addr = tmp_pdpte_addr >> ALIGN_4K;
+        pml4[i].paddr = tmp_pdpte_addr;
         pml4[i].p = ENABLE;
         UINTN j = 0;
         for (;j < PDPTE_SIZE; j++) {
             pdpte[(PDPTE_SIZE * i) + j].p = ENABLE;
             pdpte[(PDPTE_SIZE * i) + j].ps = ENABLE;
+            pdpte[(PDPTE_SIZE * i) + j].paddr = orig_addr_start + (PDPTE_SIZE * i + j * ORIG_PAGE_SIZE) * sizeof(UINT64);
         }
     }
 
-    return (UINT64)pml4 + PML4_SIZE;
+    return (UINT64)pml4;
 }
 
 VOID 
@@ -167,28 +174,38 @@
     }
 }
 
-
 EFI_STATUS
 start_elilo_kernel()
 {
+    cr0_t cr0;
+    cr3_t cr3;
+    cr4_t cr4;
+
+    MEMSET(&cr0, sizeof(UINT64), 0);
+    MEMSET(&cr3, sizeof(UINT64), 0);
+    MEMSET(&cr4, sizeof(UINT64), 0);
+
+    asm volatile ("cli"::);
+
     MEMSET(gdt_addr.base, gdt_addr.limit, 0);
     MEMCPY(gdt_addr.base, init_gdt, sizeof_init_gdt);
 
-    asm volatile ( "lidt %0" : : "m" (idt_addr) );
-    asm volatile ( "lgdt %0" : : "m" (gdt_addr) );
-
-    Print(L"enable cr4 pae...\n");
-    enable_cr4_pae(); 
+    // asm volatile ( "lidt %0" : : "m" (idt_addr) );
+    // asm volatile ( "lgdt %0" : : "m" (gdt_addr) );
 
     Print(L"init pagetable...\n");
     UINT64 addr = init_pgtable();
 
-    Print(L"insert addr %lx to cr3...\n", addr);
-    while(1) { }
-    insert_addr_to_cr3(addr);
+    Print(L"enable cr4 pae...\n");
+    enable_cr4_pae(cr4); 
 
     Print(L"enable paging cr0...\n");
-    enable_paging_cr0();
+    enable_paging_cr0(cr0);
+
+    Print(L"insert addr %lx to cr3...\n", addr);
+    insert_addr_to_cr3(cr3, addr);
+
+    while(1) { }
 
     Print(L"finish to initialize...\n");