move init to new stack

[netwatch.git] / aseg-paging / pagingstub.c

diff --git a/aseg-paging/pagingstub.c b/aseg-paging/pagingstub.c
index 7d430b92c5e200d1efb2d61d3f3b9510833972fb..941be86ef737baaaab8845cd78b41d82d78ca3e3 100644 (file)
--- a/aseg-paging/pagingstub.c
+++ b/aseg-paging/pagingstub.c
@@ -23,14 +23,26 @@ void ps_switch_stack (void (*call)(), int stack);
         asm volatile("mov %%cr0, %0" : "=r" (_temp__)); \
         _temp__; \
     })
-
-
 #define set_cr3(value) \
     { \
         register unsigned int _temp__ = (value); \
         asm volatile("mov %0, %%cr3" : : "r" (_temp__)); \
      }
+
+#define get_cr4() \
+    ({ \
+        register unsigned int _temp__; \
+        asm volatile("mov %%cr4, %0" : "=r" (_temp__)); \
+        _temp__; \
+    })
+#define set_cr4(value) \
+    { \
+        register unsigned int _temp__ = (value); \
+        asm volatile("mov %0, %%cr4" : : "r" (_temp__)); \
+     }
+
 #define        CR0_PG  0x80000000
+#define CR4_PSE        0x00000010
 
 #define MAP_FLAGS      (PTE_PRESENT | PTE_READ_WRITE)
 
@@ -49,17 +61,19 @@ unsigned long v2p(void *virt)
        if (!paging_enb)
                return _virt;
        
-       if (_virt >= 0xA0000 && _virt < 0xC0000)
-               return _virt;
-       if (_virt >= 0x200000 && _virt < 0x300000)
-               return _virt - 0x200000 + /* XXX */ 0x1FF82000;
-       if (_virt >= 0x1F0000 && _virt < 0x1F2000)
-               return _virt - 0x1F0000 + 0x1FF80000;
-       if ((_virt & ~0xFFF) == 0x4000)
-               return _virt - 0x4000 + curmapped;
+       unsigned long pde = ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(_virt)];
+       unsigned long pte;
        
-       outputf("WARNING: v2p(%08x)", _virt);
-       return 0xFFFFFFFF;
+       if (!(pde & PTE_PRESENT))
+               return 0xFFFFFFFF;
+       
+       if (pde & PDE_PAGE_SIZE)
+               return ADDR_12_MASK(pde) + (_virt & 0x3FFFFF);
+       
+       pte = ((unsigned long *)p2v(ADDR_12_MASK(pde)))[PTE_FOR(_virt)];
+       if (!(pte & PTE_PRESENT))
+               return 0xFFFFFFFF;
+       return (pte & ~0xFFF) + (_virt & 0xFFF);
 }
 
 void *p2v(unsigned long phys)
@@ -97,6 +111,15 @@ int addmap(unsigned long vaddr, unsigned long paddr)
        return 0;
 }
 
+int addmap_4m(unsigned long vaddr, unsigned long paddr)
+{
+       /* PDE_PAGE_SIZE = (1 << 7) */
+       ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(vaddr)] =
+               paddr | PDE_PRESENT | PDE_READ_WRITE | PDE_PAGE_SIZE;
+       
+       return 0;
+}
+
 void *demap(unsigned long client_pd, unsigned long vaddr)
 {
        unsigned long pde = ((unsigned long *)p2v(client_pd))[PDE_FOR(vaddr)];
@@ -147,6 +170,17 @@ static void pt_setup(int tseg_start, int tseg_size) {
                addmap(0x1F0000 + i * 0x1000, tseg_start + i * 0x1000);
 }
 
+void init_and_run(void)
+{
+       if (!initialized)
+       {
+               smi_init();
+               initialized = 1;
+       }
+       
+       smi_entry();
+}
+
 void c_entry(void)
 {
        paging_enb = 0;
@@ -158,6 +192,7 @@ void c_entry(void)
                
        /* Enable paging. */
        set_cr3((unsigned long)pd);
+       set_cr4(get_cr4() | CR4_PSE);   /* ITT, we 4MByte page. */ 
        set_cr0(get_cr0() | CR0_PG);
        outb(0x80, 0x03);
        paging_enb = 1;
@@ -185,15 +220,6 @@ void c_entry(void)
        
        outb(0x80, 0x07);
 
-       if (!initialized)
-       {
-               smi_init();     /* Run the firstrun. */
-               outb(0x80, 0x08);
-               
-               initialized = 1;
-       }
-       
-       outb(0x80, 0x09);
-       ps_switch_stack(smi_entry, 0x270000);
+       ps_switch_stack(init_and_run, 0x270000);
        outb(0x80, 0xFA);
 }