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