netwatch/pagingstub.c

   1 /* pagingstub.c
   2  * Paging enable routines
   3  * NetWatch system management mode administration console
   4  *
   5  * Copyright (c) 2008 Jacob Potter and Joshua Wise.  All rights reserved.
   6  * This program is free software; you can redistribute and/or modify it under
   7  * the terms found in the file LICENSE in the root of this source tree.
   8  *
   9  */
  10
  11
  12 #include <io.h>
  13 #include <smram.h>
  14 #include <video_defines.h>
  15 #include <minilib.h>
  16 #include <smi.h>
  17 #include <pci-bother.h>
  18 #include <serial.h>
  19 #include <output.h>
  20 #include "traps.h"
  21 #include "../net/net.h"
  22 #include "vga-overlay.h"
  23
  24 extern void smi_init();
  25 #include "vm_flags.h"
  26
  27 extern void smi_entry();
  28 void set_cr0(unsigned int);
  29 void ps_switch_stack (void (*call)(), int stack);
  30
  31 #define get_cr0() \
  32     ({ \
  33         register unsigned int _temp__; \
  34         asm volatile("mov %%cr0, %0" : "=r" (_temp__)); \
  35         _temp__; \
  36     })
  37 #define set_cr3(value) \
  38     { \
  39         register unsigned int _temp__ = (value); \
  40         asm volatile("mov %0, %%cr3" : : "r" (_temp__)); \
  41      }
  42
  43 #define get_cr4() \
  44     ({ \
  45         register unsigned int _temp__; \
  46         asm volatile("mov %%cr4, %0" : "=r" (_temp__)); \
  47         _temp__; \
  48     })
  49 #define set_cr4(value) \
  50     { \
  51         register unsigned int _temp__ = (value); \
  52         asm volatile("mov %0, %%cr4" : : "r" (_temp__)); \
  53      }
  54
  55 #define CR0_PG  0x80000000
  56 #define CR4_PSE 0x00000010
  57 #define CR4_OSFXSR 0x00000200
  58
  59 #define MAP_FLAGS       (PTE_PRESENT | PTE_READ_WRITE)
  60
  61 static int initialized = 0;
  62 static int paging_enb = 0;
  63 static unsigned long *pd;
  64
  65 extern int _bss, _bssend, _end;
  66
  67 static unsigned long curmapped = 0xFFFFFFFF;
  68
  69 unsigned long v2p(void *virt)
  70 {
  71         unsigned long _virt = (unsigned long)virt;
  72
  73         if (!paging_enb)
  74                 return _virt;
  75
  76         unsigned long pde = ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(_virt)];
  77         unsigned long pte;
  78
  79         if (!(pde & PTE_PRESENT))
  80                 return 0xFFFFFFFF;
  81
  82         if (pde & PDE_PAGE_SIZE)
  83                 return ADDR_12_MASK(pde) + (_virt & 0x3FFFFF);
  84
  85         pte = ((unsigned long *)p2v(ADDR_12_MASK(pde)))[PTE_FOR(_virt)];
  86         if (!(pte & PTE_PRESENT))
  87                 return 0xFFFFFFFF;
  88         return (pte & ~0xFFF) + (_virt & 0xFFF);
  89 }
  90
  91 void *p2v(unsigned long phys)
  92 {
  93         if (!paging_enb)
  94                 return (void*)phys;
  95
  96         if (phys >= 0xA0000 && phys < 0xC0000)
  97                 return (void*)phys;
  98         if (phys >= 0x1FF82000 && phys < 0x20000000)
  99                 return (void*)(phys - 0x1FF82000 + 0x200000);
 100         if (phys >= 0x1FF80000 && phys < 0x1FF82000)
 101                 return (void*)(phys - 0x1FF80000 + 0x1F0000);
 102
 103         if ((phys & ~0xFFF) != curmapped)       /* If it's not mapped, map it in. */
 104         {
 105                 curmapped = phys & ~0xFFF;
 106                 addmap(0x4000, curmapped);
 107                 asm volatile("invlpg 0x4000");
 108         }
 109         return (void*)(0x4000 + (phys & 0xFFF));
 110 }
 111
 112 int addmap(unsigned long vaddr, unsigned long paddr)
 113 {
 114         unsigned long pde = ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(vaddr)];
 115         unsigned long *pt;
 116
 117         if (!(pde & PTE_PRESENT))
 118                 return -1;
 119
 120         pt = (unsigned long *)p2v(ADDR_12_MASK(pde));
 121         pt[PTE_FOR(vaddr)] = paddr | PTE_PRESENT | PTE_READ_WRITE;
 122
 123         return 0;
 124 }
 125
 126 int addmap_4m(unsigned long vaddr, unsigned long paddr)
 127 {
 128         /* PDE_PAGE_SIZE = (1 << 7) */
 129         ((unsigned long *)p2v((unsigned long)pd))[PDE_FOR(vaddr)] =
 130                 paddr | PDE_PRESENT | PDE_READ_WRITE | PDE_PAGE_SIZE;
 131
 132         return 0;
 133 }
 134
 135 void *demap(unsigned long client_pd, unsigned long vaddr)
 136 {
 137         unsigned long pde = ((unsigned long *)p2v(client_pd))[PDE_FOR(vaddr)];
 138         unsigned long pte;
 139
 140         if (!(pde & PTE_PRESENT))
 141                 return (void*)0x0;
 142         pte = ((unsigned long *)p2v(ADDR_12_MASK(pde)))[PTE_FOR(vaddr)];
 143         if (!(pte & PTE_PRESENT))
 144                 return (void*)0x0;
 145         return p2v((pte & ~0xFFF) + (vaddr & 0xFFF));
 146 }
 147
 148 static void pt_setup(int tseg_start, int tseg_size) {
 149         int i;
 150
 151         /* The page directory and page table live at TSEG and TSEG + 0x1000,
 152          * respectively. */
 153         unsigned long *pagedirectory = (unsigned long *) tseg_start;
 154         unsigned long *pagetable = (unsigned long *) (tseg_start + 0x1000);
 155
 156         pd = pagedirectory;
 157
 158         /* Clear out the page directory except for one entry pointing to the
 159          * page table, and clear the page table entirely. */
 160         pagedirectory[0] = (tseg_start + 0x1000) | PTE_PRESENT | PTE_READ_WRITE;
 161         for (i = 1; i < 1024; i++)
 162                 pagedirectory[i] = 0;
 163
 164         for (i = 0; i < 1024; i++)
 165                 pagetable[i] = 0;
 166
 167         /* Map 0x0A0000:0x0BFFFF to itself. */
 168         for (i = 0; i < 32; i++)
 169                 addmap(0xA0000 + i * 0x1000, 0xA0000 + i * 0x1000);
 170
 171         /* Map 0x200000:0x300000 to TSEG data */
 172         for (i = 0; i < 256; i++)
 173                 addmap(0x200000 + i * 0x1000, tseg_start + (i + 2) * 0x1000);
 174
 175         /* Map 0x300000:0x400000 to 0x200000, so we can copy our code out of
 176          * RAM the first time around */
 177         for (i = 0; i < 256; i++)
 178                 addmap(0x300000 + i * 0x1000, 0x200000 + i * 0x1000);
 179
 180         /* Map 0x1F0000:0x1F2000 to TSEG paging info */
 181         for (i = 0; i < 2; i++)
 182                 addmap(0x1F0000 + i * 0x1000, tseg_start + i * 0x1000);
 183 }
 184
 185 void init_and_run(void)
 186 {
 187         DBG(0x0A);
 188
 189         if (!initialized)
 190         {
 191                 DBG(0x0B);
 192                 smi_init();
 193                 initialized = 1;
 194         }
 195
 196         DBG(0x0C);
 197         smi_entry();
 198
 199         DBG(0xCC);
 200 }
 201
 202 void c_entry(void)
 203 {
 204         paging_enb = 0;
 205
 206         DBG(0x01);
 207
 208         if (!initialized)
 209                 pt_setup(0x1FF80000, 0x80000);
 210
 211         DBG(0x02);
 212
 213         /* Enable paging. */
 214         set_cr3((unsigned long)pd);
 215         set_cr4(get_cr4() | CR4_PSE | CR4_OSFXSR);      /* ITT, we 4MByte page. */
 216         set_cr0(get_cr0() | CR0_PG);
 217
 218         DBG(0x03);
 219         paging_enb = 1;
 220
 221         /* If this is the first goround, copy in data. */
 222         if (!initialized)
 223         {
 224                 unsigned char *p;
 225
 226                 DBG(0x04);
 227
 228                 for (p = (void *)0x200000; (void *)p < (void *)&_bss; p++)
 229                         *p = *(p + 0x100000);
 230                 for (p = (void *)&_bss; (void *)p < (void *)&_bssend; p++)
 231                         *p = 0;
 232
 233                 DBG(0x05);
 234
 235                 /* Only now is it safe to call other functions. */
 236                 serial_init();
 237                 DBG(0x06);
 238                 dolog("Evacuation to TSEG complete.");
 239                 DBG(0x07);
 240         }
 241
 242         DBG(0x08);
 243         traps_install();
 244
 245         DBG(0x09);
 246         ps_switch_stack(init_and_run, 0xa2000);
 247
 248         DBG(0xFA);
 249 }