--- a/x86_64/elilo_kernel.c	Sat Feb 16 05:47:59 2013 +0900
+++ b/x86_64/elilo_kernel.c	Sat Feb 16 16:06:09 2013 +0900
@@ -3,33 +3,37 @@
 
 #include "elilo.h"
 #include "pgtable_flags.h"
+#include "sysdeps.h"
 
-VOID
-MemCpy(UINT8 *t, UINT8 *f, UINTN n)
-{
-    if (t && f && n && (t<f)) {
-        while (n--) {
-            *t++ = *f++;
-        }
-    } else if (t && f && n && (t>f)) {
-        t +=n;
-        f +=n;
-        while (n--) {
-            *t-- = *f--;
-        }
-    }
+#define MEMCPY(to, from, cnt) { \
+        UINT8 *t = (UINT8 *)(to); \
+        UINT8 *f = (UINT8 *)(from); \
+        UINTN n = cnt; \
+        if (t && f && n && (t<f)) { \
+                    while (n--) { \
+                                    *t++ = *f++; \
+                                } \
+                } else if (t && f && n && (t>f)) { \
+                            t += n; \
+                            f += n; \
+                            while (n--) { \
+                                            *t-- = *f--; \
+                                        } \
+                        } \
 }
 
-VOID
-MemSet(UINT8 *p, UINTN *n, UINTN v)
-{
-    if (p && n) {
-        while (n--) {
-            *p++ = v;
-        }
-    }
+#define MEMSET(ptr, size, val) { \
+        UINT8 *p = (UINT8 *)(ptr); \
+        UINTN n = (UINTN)(size); \
+        UINT8 v = (UINT8)(val); \
+        if (p && n) { \
+                    while (n--) { \
+                                    *p++ = v; \
+                                } \
+                } \
 }
 
+
 #define EFER_LME 8
 #define EFER_NXE 0x00000400
 #define MSR_EFER 0xc0000080
@@ -54,43 +58,61 @@
 INTN
 enable_cr4_pae()
 {
-    UINT32 cr4_flag = X86_CR4_PAE;
-    asm volatile("movq %%rax, %%cr4"::"a"(cr4_flag));
+    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("movq %%rax, %%cr4"::"a"(cr4_flag));
     return 0;
 }
 
 
 UINTN
-insert_addr_to_cr3(UINT64 addr)
+insert_addr_to_cr3(UINT64 val)
 {
-    asm volatile ("movq %0, %%rax \n\tmovq %%rax, %%cr3" :: "m"(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));
     return 0;
 }
 
-
 #define X86_CR0_PG 0x80000000
 
 INTN
 enable_paging_cr0()
-{
-    UINT32 cr0_flag = X86_CR0_PG;
-    asm volatile("movl %0, %%eax \n\t movq %%rax, %%cr0"::"m"(cr0_flag));
+{   
+    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("movl %0, %%eax \n\t movq %%rax, %%cr0"::"m"(cr0_flag));
     return 0;
 }
 
 VOID
-init_pgtable_value(UINT64 *addr, UINT32 size, UINT64 value)
+init_pgtable_value(VOID *addr, UINT32 size, UINT64 value)
 {
     if (addr == NULL) {
         Print(L"addr is not using.\n");
         return;
     }
 
+    UINT64 *tmp = (UINT64 *)addr;
+
     while(size--) {
-        *addr++ = value;
+        *tmp++ = value;
     }
 }
 
+#define PML4_START 0x00270000
+#define PDPTE_START PML4_START + (PML4_SIZE + 1) * PDPTE_SIZE
+
+/* alloc pages use how many pages for 4KiB */
+#define PGCNT_BYTE 1024
+
 
 /* init_pgtable()
  * init pagetable. use IA-32e page table 
@@ -99,41 +121,78 @@
 UINT64
 init_pgtable()
 {
-    pml4 = (pml4_t *)alloc_pages(PML4_SIZE, EfiLoaderData, AllocateAddress, pml4);
-    pdpte = (pdpte_t *)alloc_pages(PDPTE_SIZE * PML4_SIZE * sizeof(pdpte_t), EfiLoaderData, AllocateAddress, pdpte);
+    pml4 = (pml4_t *)PML4_START;
+    UINTN pml4_size = PML4_SIZE * sizeof(pml4_t) / PGCNT_BYTE;
+    Print(L"allocate pml4 ::%lx \n", pml4);
+    pml4 = (pml4_t *)alloc_pages(pml4_size, EfiLoaderData, AllocateAddress, pml4);
+    Print(L"pml4 :%lx\n", pml4);
+    if (pml4 == NULL) {
+        Print(L"can not allocate pml4.\n");
+        return -1;
+    }
 
-    if ((pml4 != NULL)&&(pdpte != NULL)) return -1;
-    Print(L"allocate page :%lx", pdpte);
+    pdpte = (pdpte_t *)PDPTE_START;
+    Print(L"allocate pdpte ::%lx\n", pdpte);
+    UINTN pdpte_size = PDPTE_SIZE * PML4_SIZE * sizeof(pdpte_t) / PGCNT_BYTE;
+    pdpte = (pdpte_t *)alloc_pages(pdpte_size , EfiLoaderData, AllocateAddress, pdpte);
+    Print(L"pdpte :%lx\n", pdpte);
 
-    init_pgtable_value((UINT64*)pml4, PML4_SIZE, 0);
-    init_pgtable_value((UINT64*)pdpte, PDPTE_SIZE * PML4_SIZE, 0);
+    if (pdpte == NULL) {
+        Print(L"can not allocate pdpte.\n");
+        return -1;
+    }
+
+    init_pgtable_value((VOID *)pml4, PML4_SIZE * sizeof(pml4_t), 0);
+    init_pgtable_value((VOID *)pdpte, PDPTE_SIZE * PML4_SIZE * sizeof(pdpte_t), 0);
 
     UINTN i = 0;
     for (; i<PML4_SIZE ;i++) {
-        pml4[i].paddr = (UINT64 *)&pdpte[(PDPTE_SIZE * i) + PDPTE_SIZE];
+        pml4[i].paddr = (UINT64)&pdpte[(PDPTE_SIZE * i) + PDPTE_SIZE];
         pml4[i].p = ENABLE;
         UINTN j = 0;
         for (;j < PDPTE_SIZE; j++) {
-            pdpte[(PDPTE_SIZE*i) + j].p = ENABLE;
+            pdpte[(PDPTE_SIZE * i) + j].p = ENABLE;
             pdpte[(PDPTE_SIZE * i) + j].ps = ENABLE;
         }
     }
 
-    return (UINT64)pdpte + PML4_SIZE;
+    return (UINT64)pml4 + PML4_SIZE;
 }
 
+VOID 
+stop_kernel()
+{
+    Print(L"stop\n");
+    while(1) {
+    }
+}
 
 
 EFI_STATUS
 start_elilo_kernel()
 {
-    init_pgtable();
-    UINT64 addr = enable_paging_cr0();
+    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(); 
+
+    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);
-    UINT64 i=0;
-    asm volatile ("cli" : : );
-    for (;i < 100000000; i++) {
-    }
+
+    Print(L"enable paging cr0...\n");
+    enable_paging_cr0();
+
+    Print(L"finish to initialize...\n");
+
+    asm volatile ("hlt" : : );
     Print(L"finish internal kernel\n");
     return EFI_SUCCESS;
 }