env/v/vm.c

   1 #include <stdint.h>
   2 #include <string.h>
   3 #include <stdio.h>
   4
   5 #include "riscv_test.h"
   6
   7 void trap_entry();
   8 void pop_tf(trapframe_t*);
   9
  10 static void cputchar(int x)
  11 {
  12   while (mtpcr(PCR_TOHOST, 0x0101000000000000 | (unsigned char)x));
  13 }
  14
  15 static void cputstring(const char* s)
  16 {
  17   while(*s)
  18     cputchar(*s++);
  19   cputchar('\n');
  20 }
  21
  22 static void terminate(int code)
  23 {
  24   while (mtpcr(PCR_TOHOST, code));
  25   while (1);
  26 }
  27
  28 #define stringify1(x) #x
  29 #define stringify(x) stringify1(x)
  30 #define assert(x) do { \
  31   if (x) break; \
  32   cputstring("Assertion failed: " stringify(x)); \
  33   terminate(3); \
  34 } while(0)
  35
  36 #define RELOC(x) ((typeof(x))((char*)(x) + (PGSIZE*MAX_TEST_PAGES)))
  37
  38 typedef struct { pte_t addr; void* next; } freelist_t;
  39
  40 pte_t l1pt[PTES_PER_PT] __attribute__((aligned(PGSIZE)));
  41 pte_t l2pt[PTES_PER_PT] __attribute__((aligned(PGSIZE)));
  42 pte_t l3pt[PTES_PER_PT] __attribute__((aligned(PGSIZE)));
  43 freelist_t user_mapping[MAX_TEST_PAGES];
  44 freelist_t freelist_nodes[MAX_TEST_PAGES];
  45 freelist_t *freelist_head, *freelist_tail;
  46
  47 void printhex(uint64_t x)
  48 {
  49   char str[17];
  50   for (int i = 0; i < 16; i++)
  51   {
  52     str[15-i] = (x & 0xF) + ((x & 0xF) < 10 ? '0' : 'a'-10);
  53     x >>= 4;
  54   }
  55   str[16] = 0;
  56
  57   cputstring(str);
  58 }
  59
  60 void evict(unsigned long addr)
  61 {
  62   assert(addr >= PGSIZE && addr < RELOC(0L));
  63   addr = addr/PGSIZE*PGSIZE;
  64
  65   freelist_t* node = RELOC(&user_mapping[addr/PGSIZE]);
  66   if (node->addr)
  67   {
  68     memcpy((void*)RELOC(addr), (void*)addr, PGSIZE);
  69     RELOC(&user_mapping[addr/PGSIZE])->addr = 0;
  70
  71     if (*RELOC(&freelist_tail) == 0)
  72       *RELOC(&freelist_head) = *RELOC(&freelist_tail) = node;
  73     else
  74     {
  75       (*RELOC(&freelist_tail))->next = node;
  76       *RELOC(&freelist_tail) = node;
  77     }
  78   }
  79 }
  80
  81 void handle_fault(unsigned long addr)
  82 {
  83   assert(addr >= PGSIZE && addr < RELOC(0L));
  84   addr = addr/PGSIZE*PGSIZE;
  85
  86   freelist_t* node = *RELOC(&freelist_head);
  87   assert(node);
  88   *RELOC(&freelist_head) = node->next;
  89   if (*RELOC(&freelist_head) == *RELOC(&freelist_tail))
  90     *RELOC(&freelist_tail) = 0;
  91
  92   *RELOC(&l3pt[addr/PGSIZE]) = node->addr | PTE_UW | PTE_UR | PTE_UX | PTE_SW | PTE_SR | PTE_SX | PTE_V;
  93   mtpcr(PCR_FATC, 0);
  94
  95   assert(RELOC(&user_mapping[addr/PGSIZE])->addr == 0);
  96   *RELOC(&user_mapping[addr/PGSIZE]) = *node;
  97   memcpy((void*)addr, (void*)RELOC(addr), PGSIZE);
  98
  99   __builtin___clear_cache(0,0);
 100 }
 101
 102 static void emulate_vxcptsave(trapframe_t* tf)
 103 {
 104   long* where = (long*)tf->gpr[(tf->insn >> 15) & 0x1F];
 105
 106   where[0] = vgetcfg();
 107   where[1] = vgetvl();
 108   vxcptevac(&where[2]);
 109   fence();
 110 }
 111
 112 static void do_vxcptrestore(long* where)
 113 {
 114   vsetcfg(where[0]);
 115   vsetvl(where[1]);
 116
 117   vxcpthold();
 118
 119   int idx = 2;
 120   long dword, cmd, pf;
 121   int first = 1;
 122
 123   while (1)
 124   {
 125     dword = where[idx++];
 126
 127     if (dword < 0) break;
 128
 129     if (dword_bit_cnt(dword))
 130     {
 131       venqcnt(dword, pf | (dword_bit_cmd(where[idx]) << 1));
 132     }
 133     else
 134     {
 135       if (!first)
 136       {
 137         venqcmd(cmd, pf);
 138       }
 139
 140       first = 0;
 141       cmd = dword;
 142       pf = dword_bit_pf(cmd);
 143
 144       if (dword_bit_imm1(cmd))
 145       {
 146         venqimm1(where[idx++], pf);
 147       }
 148       if (dword_bit_imm2(cmd))
 149       {
 150         venqimm2(where[idx++], pf);
 151       }
 152     }
 153   }
 154   if (!first)
 155   {
 156     venqcmd(cmd, pf);
 157   }
 158 }
 159
 160 static void emulate_vxcptrestore(trapframe_t* tf)
 161 {
 162   long* where = (long*)tf->gpr[(tf->insn >> 15) & 0x1F];
 163   vxcptkill();
 164   do_vxcptrestore(where);
 165 }
 166
 167 static void restore_vector(trapframe_t* tf)
 168 {
 169   if (mfpcr(PCR_IMPL) == IMPL_ROCKET)
 170     do_vxcptrestore(tf->evac);
 171   else
 172     vxcptrestore(tf->evac);
 173 }
 174
 175 void handle_trap(trapframe_t* tf)
 176 {
 177   if (tf->cause == CAUSE_SYSCALL)
 178   {
 179     int n = tf->gpr[18];
 180
 181     for (long i = 1; i < MAX_TEST_PAGES; i++)
 182       evict(i*PGSIZE);
 183
 184     terminate(n);
 185   }
 186   else if (tf->cause == CAUSE_FAULT_FETCH)
 187     handle_fault(tf->epc);
 188   else if (tf->cause == CAUSE_ILLEGAL_INSTRUCTION)
 189   {
 190     int fssr;
 191     asm ("la %0, 1f; lw %0, 0(%0); b 2f; 1: fssr x0; 2:" : "=r"(fssr));
 192
 193     if (tf->insn == fssr)
 194       terminate(1); // FP test on non-FP hardware.  "succeed."
 195 #if 0
 196     else if ((tf->insn & 0xF83FFFFF) == 0x37B)
 197       emulate_vxcptsave(tf);
 198     else if ((tf->insn & 0xF83FFFFF) == 0x77B)
 199       emulate_vxcptrestore(tf);
 200 #endif
 201     else
 202       assert(0);
 203     tf->epc += 4;
 204   }
 205   else if (tf->cause == CAUSE_FAULT_LOAD || tf->cause == CAUSE_FAULT_STORE)
 206     handle_fault(tf->badvaddr);
 207   else if ((tf->cause << 1) == (IRQ_COP << 1))
 208   {
 209     if (tf->hwacha_cause == HWACHA_CAUSE_VF_FAULT_FETCH ||
 210         tf->hwacha_cause == HWACHA_CAUSE_FAULT_LOAD ||
 211         tf->hwacha_cause == HWACHA_CAUSE_FAULT_STORE)
 212     {
 213       long badvaddr = vxcptaux();
 214       handle_fault(badvaddr);
 215     }
 216     else
 217       assert(0);
 218   }
 219   else
 220     assert(0);
 221
 222 out:
 223   if (!(tf->sr & SR_PS) && (tf->sr & SR_EA)) {
 224     restore_vector(tf);
 225     tf->sr |= SR_PEI;
 226   }
 227   pop_tf(tf);
 228 }
 229
 230 void vm_boot(long test_addr, long seed)
 231 {
 232   while (mfpcr(PCR_HARTID) > 0); // only core 0 proceeds
 233
 234   assert(SIZEOF_TRAPFRAME_T == sizeof(trapframe_t));
 235
 236   seed = 1 + (seed % MAX_TEST_PAGES);
 237   freelist_head = RELOC(&freelist_nodes[0]);
 238   freelist_tail = RELOC(&freelist_nodes[MAX_TEST_PAGES-1]);
 239   for (long i = 0; i < MAX_TEST_PAGES; i++)
 240   {
 241     freelist_nodes[i].addr = (MAX_TEST_PAGES+i)*PGSIZE;
 242     freelist_nodes[i].next = RELOC(&freelist_nodes[i+1]);
 243     seed = LFSR_NEXT(seed);
 244   }
 245   freelist_nodes[MAX_TEST_PAGES-1].next = 0;
 246
 247   assert(MAX_TEST_PAGES*2 < PTES_PER_PT);
 248   l1pt[0] = (pte_t)l2pt | PTE_V | PTE_T;
 249   l2pt[0] = (pte_t)l3pt | PTE_V | PTE_T;
 250   for (long i = 0; i < MAX_TEST_PAGES; i++)
 251     l3pt[i] = l3pt[i+MAX_TEST_PAGES] = (i*PGSIZE) | PTE_SW | PTE_SR | PTE_SX | PTE_V;
 252
 253   mtpcr(PCR_PTBR, l1pt);
 254   mtpcr(PCR_SR, mfpcr(PCR_SR) | SR_VM | SR_EF);
 255
 256   // relocate
 257   long adjustment = RELOC(0L), tmp;
 258   mtpcr(PCR_EVEC, (char*)&trap_entry + adjustment);
 259   asm volatile ("add sp, sp, %1\n"
 260                 "jal %0, 1f\n"
 261                 "1: add %0, %0, %1\n"
 262                 "jr %0, 8"
 263                 : "=&r"(tmp)
 264                 : "r"(adjustment));
 265
 266   memset(RELOC(&l3pt[0]), 0, MAX_TEST_PAGES*sizeof(pte_t));
 267   mtpcr(PCR_FATC, 0);
 268
 269   trapframe_t tf;
 270   memset(&tf, 0, sizeof(tf));
 271   tf.sr = SR_PEI | ((1 << IRQ_COP) << SR_IM_SHIFT) | SR_EF | SR_EA | SR_S | SR_U64 | SR_S64 | SR_VM;
 272   tf.epc = test_addr;
 273
 274   pop_tf(&tf);
 275 }