aseg-paging/pagingstub.c

   1 #include <io.h>
   2 #include <smram.h>
   3 #include <video_defines.h>
   4 #include <minilib.h>
   5 #include <smi.h>
   6 #include <pci-bother.h>
   7 #include <serial.h>
   8 #include <output.h>
   9 #include "traps.h"
  10 #include "../net/net.h"
  11 #include "vga-overlay.h"
  12
  13 extern void smi_init();
  14 #include "vm_flags.h"
  15
  16 extern void smi_entry();
  17 void set_cr0(unsigned int);
  18 void ps_switch_stack (void (*call)(), int stack);
  19
  20 #define get_cr0() \
  21     ({ \
  22         register unsigned int _temp__; \
  23         asm volatile("mov %%cr0, %0" : "=r" (_temp__)); \
  24         _temp__; \
  25     })
  26
  27
  28 #define set_cr3(value) \
  29     { \
  30         register unsigned int _temp__ = (value); \
  31         asm volatile("mov %0, %%cr3" : : "r" (_temp__)); \
  32      }
  33 #define CR0_PG  0x80000000
  34
  35 #define MAP_FLAGS       (PTE_PRESENT | PTE_READ_WRITE)
  36
  37 static int initialized = 0;
  38 static int paging_enb = 0;
  39 static unsigned long *pd;
  40
  41 extern int _bss, _bssend, _end;
  42
  43 static unsigned long curmapped = 0xFFFFFFFF;
  44
  45 unsigned long v2p(void *virt)
  46 {
  47         unsigned long _virt = (unsigned long)virt;
  48
  49         if (!paging_enb)
  50                 return _virt;
  51
  52         if (_virt >= 0xA0000 && _virt < 0xC0000)
  53                 return _virt;
  54         if (_virt >= 0x200000 && _virt < 0x300000)
  55                 return _virt - 0x200000 + /* XXX */ 0x1FF82000;
  56         if (_virt >= 0x1F0000 && _virt < 0x1F2000)
  57                 return _virt - 0x1F0000 + 0x1FF80000;
  58         if ((_virt & ~0xFFF) == 0x4000)
  59                 return _virt - 0x4000 + curmapped;
  60
  61         outputf("WARNING: v2p(%08x)", _virt);
  62         return 0xFFFFFFFF;
  63 }
  64
  65 void *p2v(unsigned long phys)
  66 {
  67         if (!paging_enb)
  68                 return (void*)phys;
  69
  70         if (phys >= 0xA0000 && phys < 0xC0000)
  71                 return (void*)phys;
  72         if (phys >= 0x1FF82000 && phys < 0x20000000)
  73                 return (void*)(phys - 0x1FF82000 + 0x200000);
  74         if (phys >= 0x1FF80000 && phys < 0x1FF82000)
  75                 return (void*)(phys - 0x1FF80000 + 0x1F0000);
  76
  77         if ((phys & ~0xFFF) != curmapped)       /* If it's not mapped, map it in. */
  78         {
  79                 curmapped = phys & ~0xFFF;
  80                 addmap(0x4000, curmapped);
  81                 asm volatile("invlpg 0x4000");
  82         }
  83         return (void*)(0x4000 + (phys & 0xFFF));
  84 }
  85
  86 int addmap(unsigned long vaddr, unsigned long paddr)
  87 {
  88         unsigned long pde = ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(vaddr)];
  89         unsigned long *pt;
  90
  91         if (!(pde & PTE_PRESENT))
  92                 return -1;
  93
  94         pt = (unsigned long *)p2v(ADDR_12_MASK(pde));
  95         pt[PTE_FOR(vaddr)] = paddr | PTE_PRESENT | PTE_READ_WRITE;
  96
  97         return 0;
  98 }
  99
 100 void *demap(unsigned long client_pd, unsigned long vaddr)
 101 {
 102         unsigned long pde = ((unsigned long *)p2v(client_pd))[PDE_FOR(vaddr)];
 103         unsigned long pte;
 104
 105         if (!(pde & PTE_PRESENT))
 106                 return (void*)0x0;
 107         pte = ((unsigned long *)p2v(ADDR_12_MASK(pde)))[PTE_FOR(vaddr)];
 108         if (!(pte & PTE_PRESENT))
 109                 return (void*)0x0;
 110         return p2v((pte & ~0xFFF) + (vaddr & 0xFFF));
 111 }
 112
 113 static void pt_setup(int tseg_start, int tseg_size) {
 114         int i;
 115
 116         /* The page directory and page table live at TSEG and TSEG + 0x1000,
 117          * respectively. */
 118         unsigned long *pagedirectory = (unsigned long *) tseg_start;
 119         unsigned long *pagetable = (unsigned long *) (tseg_start + 0x1000);
 120
 121         pd = pagedirectory;
 122
 123         /* Clear out the page directory except for one entry pointing to the
 124          * page table, and clear the page table entirely. */
 125         pagedirectory[0] = (tseg_start + 0x1000) | PTE_PRESENT | PTE_READ_WRITE;
 126         for (i = 1; i < 1024; i++)
 127                 pagedirectory[i] = 0;
 128
 129         for (i = 0; i < 1024; i++)
 130                 pagetable[i] = 0;
 131
 132         /* Map 0x0A0000:0x0BFFFF to itself. */
 133         for (i = 0; i < 32; i++)
 134                 addmap(0xA0000 + i * 0x1000, 0xA0000 + i * 0x1000);
 135
 136         /* Map 0x200000:0x300000 to TSEG data */
 137         for (i = 0; i < 256; i++)
 138                 addmap(0x200000 + i * 0x1000, tseg_start + (i + 2) * 0x1000);
 139
 140         /* Map 0x300000:0x400000 to 0x200000, so we can copy our code out of
 141          * RAM the first time around */
 142         for (i = 0; i < 256; i++)
 143                 addmap(0x300000 + i * 0x1000, 0x200000 + i * 0x1000);
 144
 145         /* Map 0x1F0000:0x1F2000 to TSEG paging info */
 146         for (i = 0; i < 2; i++)
 147                 addmap(0x1F0000 + i * 0x1000, tseg_start + i * 0x1000);
 148 }
 149
 150 void c_entry(void)
 151 {
 152         paging_enb = 0;
 153
 154         outb(0x80, 0x01);
 155         if (!initialized)
 156                 pt_setup(0x1FF80000, 0x80000);
 157         outb(0x80, 0x02);
 158
 159         /* Enable paging. */
 160         set_cr3((unsigned long)pd);
 161         set_cr0(get_cr0() | CR0_PG);
 162         outb(0x80, 0x03);
 163         paging_enb = 1;
 164
 165         /* If this is the first goround, copy in data. */
 166         if (!initialized)
 167         {
 168                 unsigned char *p;
 169
 170                 outb(0x80, 0x04);
 171                 for (p = (void *)0x200000; (void *)p < (void *)&_bss; p++)
 172                         *p = *(p + 0x100000);
 173                 for (p = (void *)&_bss; (void *)p < (void *)&_bssend; p++)
 174                         *p = 0;
 175                 outb(0x80, 0x05);
 176
 177                 /* Only now is it safe to call other functions. */
 178                 serial_init();
 179                 dolog("Evacuation to TSEG complete.");
 180         }
 181
 182         outb(0x80, 0x06);
 183
 184         traps_install();
 185
 186         outb(0x80, 0x07);
 187
 188         if (!initialized)
 189         {
 190                 smi_init();     /* Run the firstrun. */
 191                 outb(0x80, 0x08);
 192
 193                 initialized = 1;
 194         }
 195
 196         outb(0x80, 0x09);
 197         ps_switch_stack(smi_entry, 0x270000);
 198         outb(0x80, 0xFA);
 199 }