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