KernelLand/Kernel/arch/armv7/mm_virt.c

   1 /*
   2  * Acess2
   3  *
   4  * ARM7 Virtual Memory Manager
   5  * - arch/arm7/mm_virt.c
   6  */
   7 #define DEBUG   0
   8 #include <acess.h>
   9 #include <mm_virt.h>
  10 #include <hal_proc.h>
  11
  12 #define TRACE_MAPS      0
  13 #define TRACE_COW       1
  14
  15 #define AP_KRW_ONLY     1       // Kernel page
  16 #define AP_KRO_ONLY     5       // Kernel RO page
  17 #define AP_RW_BOTH      3       // Standard RW
  18 #define AP_RO_BOTH      7       // COW Page
  19 #define AP_RO_USER      2       // User RO Page
  20 #define PADDR_MASK_LVL1 0xFFFFFC00
  21
  22 const char * const caAPValueNames[] = {
  23         "AP_NOACCESS", "AP_KRW_ONLY",
  24         "AP_RO_USER", "AP_RW_BOTH",
  25         "AP_???_4", "AP_KRO_ONLY",
  26         "AP_???_6", "AP_RO_BOTH"
  27 };
  28
  29 // === IMPORTS ===
  30 extern Uint32   kernel_table0[];
  31
  32 // === TYPES ===
  33 typedef struct
  34 {
  35         tPAddr  PhysAddr;
  36         Uint8   Size;
  37         Uint8   Domain;
  38         BOOL    bExecutable;
  39         BOOL    bGlobal;
  40         BOOL    bShared;
  41          int    AP;
  42 } tMM_PageInfo;
  43
  44 //#define FRACTAL(table1, addr) ((table1)[ (0xFF8/4*1024) + ((addr)>>20)])
  45 #define FRACTAL(table1, addr)   ((table1)[ (0xFF8/4*1024) + ((addr)>>22)])
  46 #define USRFRACTAL(addr)        (*((Uint32*)(0x7FDFF000) + ((addr)>>22)))
  47 #define TLBIALL()       __asm__ __volatile__ ("mcr p15, 0, %0, c8, c7, 0" : : "r" (0))
  48 #define TLBIMVA(addr)   __asm__ __volatile__ ("mcr p15, 0, %0, c8, c7, 1;dsb;isb" : : "r" (((addr)&~0xFFF)|1):"memory")
  49 #define DCCMVAC(addr)   __asm__ __volatile__ ("mcr p15, 0, %0, c7, c10, 1" : : "r" ((addr)&~0xFFF))
  50
  51 // === PROTOTYPES ===
  52 void    MM_int_GetTables(tVAddr VAddr, Uint32 **Table0, Uint32 **Table1);
  53  int    MM_int_AllocateCoarse(tVAddr VAddr, int Domain);
  54  int    MM_int_SetPageInfo(tVAddr VAddr, tMM_PageInfo *pi);
  55  int    MM_int_GetPageInfo(tVAddr VAddr, tMM_PageInfo *pi);
  56 tVAddr  MM_NewUserStack(void);
  57 tPAddr  MM_AllocateZero(tVAddr VAddr);
  58 tPAddr  MM_AllocateRootTable(void);
  59 void    MM_int_CloneTable(Uint32 *DestEnt, int Table);
  60 tPAddr  MM_Clone(void);
  61 tVAddr  MM_NewKStack(int bGlobal);
  62 void    MM_int_DumpTableEnt(tVAddr Start, size_t Len, tMM_PageInfo *Info);
  63 //void  MM_DumpTables(tVAddr Start, tVAddr End);
  64 void    MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch, Uint32 UserLR);
  65
  66 // === GLOBALS ===
  67 tPAddr  giMM_ZeroPage;
  68
  69 // === CODE ===
  70 int MM_InitialiseVirtual(void)
  71 {
  72         return 0;
  73 }
  74
  75 void MM_int_GetTables(tVAddr VAddr, Uint32 **Table0, Uint32 **Table1)
  76 {
  77         if(VAddr & 0x80000000) {
  78                 *Table0 = (void*)&kernel_table0;        // Level 0
  79                 *Table1 = (void*)MM_TABLE1KERN; // Level 1
  80         }
  81         else {
  82                 *Table0 = (void*)MM_TABLE0USER;
  83                 *Table1 = (void*)MM_TABLE1USER;
  84         }
  85 }
  86
  87 int MM_int_AllocateCoarse(tVAddr VAddr, int Domain)
  88 {
  89         Uint32  *table0, *table1;
  90         Uint32  *desc;
  91         tPAddr  paddr;
  92
  93         ENTER("xVAddr iDomain", VAddr, Domain);
  94
  95         MM_int_GetTables(VAddr, &table0, &table1);
  96
  97         VAddr &= ~(0x400000-1); // 4MiB per "block", 1 Page
  98
  99         desc = &table0[ VAddr>>20];
 100         LOG("desc = %p", desc);
 101
 102         // table0: 4 bytes = 1 MiB
 103
 104         LOG("desc[0] = %x", desc[0]);
 105         LOG("desc[1] = %x", desc[1]);
 106         LOG("desc[2] = %x", desc[2]);
 107         LOG("desc[3] = %x", desc[3]);
 108
 109         if( (desc[0] & 3) != 0 || (desc[1] & 3) != 0
 110          || (desc[2] & 3) != 0 || (desc[3] & 3) != 0 )
 111         {
 112                 // Error?
 113                 LEAVE('i', 1);
 114                 return 1;
 115         }
 116
 117         paddr = MM_AllocPhys();
 118         if( !paddr )
 119         {
 120                 // Error
 121                 LEAVE('i', 2);
 122                 return 2;
 123         }
 124
 125         *desc = paddr | (Domain << 5) | 1;
 126         desc[1] = desc[0] + 0x400;
 127         desc[2] = desc[0] + 0x800;
 128         desc[3] = desc[0] + 0xC00;
 129
 130         if( VAddr < 0x80000000 ) {
 131                 USRFRACTAL(VAddr) = paddr | 0x13;
 132         }
 133         else {
 134                 FRACTAL(table1, VAddr) = paddr | 0x13;
 135         }
 136
 137         // TLBIALL
 138         TLBIALL();
 139
 140         memset( (void*)&table1[ (VAddr >> 12) & ~(1024-1) ], 0, 0x1000 );
 141
 142         LEAVE('i', 0);
 143         return 0;
 144 }
 145
 146 int MM_int_SetPageInfo(tVAddr VAddr, tMM_PageInfo *pi)
 147 {
 148         Uint32  *table0, *table1;
 149         Uint32  *desc;
 150
 151         ENTER("pVAddr ppi", VAddr, pi);
 152
 153         MM_int_GetTables(VAddr, &table0, &table1);
 154
 155         desc = &table0[ VAddr >> 20 ];
 156         LOG("desc = %p", desc);
 157
 158         switch(pi->Size)
 159         {
 160         case 12:        // Small Page
 161         case 16:        // Large Page
 162                 LOG("Page");
 163                 if( (*desc & 3) == 0 ) {
 164                         MM_int_AllocateCoarse( VAddr, pi->Domain );
 165                 }
 166                 desc = &table1[ VAddr >> 12 ];
 167                 LOG("desc (2) = %p", desc);
 168                 if( pi->Size == 12 )
 169                 {
 170                         // Small page
 171                         // - Error if overwriting a large page
 172                         if( (*desc & 3) == 1 )  LEAVE_RET('i', 1);
 173                         if( pi->PhysAddr == 0 ) {
 174                                 *desc = 0;
 175                                 TLBIMVA( VAddr );
 176                                 DCCMVAC( (tVAddr) desc );
 177 //                              #warning "HACK: TLBIALL"
 178 //                              TLBIALL();
 179                                 LEAVE('i', 0);
 180                                 return 0;
 181                         }
 182
 183                         *desc = (pi->PhysAddr & 0xFFFFF000) | 2;
 184                         if(!pi->bExecutable)    *desc |= 1;     // XN
 185                         if(!pi->bGlobal)        *desc |= 1 << 11;       // nG
 186                         if( pi->bShared)        *desc |= 1 << 10;       // S
 187                         *desc |= (pi->AP & 3) << 4;     // AP
 188                         *desc |= ((pi->AP >> 2) & 1) << 9;      // APX
 189                         TLBIMVA( VAddr );
 190 //                      #warning "HACK: TLBIALL"
 191 //                      TLBIALL();
 192                         DCCMVAC( (tVAddr) desc );
 193                         LEAVE('i', 0);
 194                         return 0;
 195                 }
 196                 else
 197                 {
 198                         // Large page
 199                         Log_Warning("MMVirt", "TODO: Implement large pages in MM_int_SetPageInfo");
 200                 }
 201                 break;
 202         case 20:        // Section or unmapped
 203                 Log_Warning("MMVirt", "TODO: Implement sections in MM_int_SetPageInfo");
 204                 break;
 205         case 24:        // Supersection
 206                 // Error if not aligned
 207                 if( VAddr & 0xFFFFFF ) {
 208                         LEAVE('i', 1);
 209                         return 1;
 210                 }
 211                 if( (*desc & 3) == 0 || ((*desc & 3) == 2 && (*desc & (1 << 18)))  )
 212                 {
 213                         if( pi->PhysAddr == 0 ) {
 214                                 *desc = 0;
 215                         }
 216                         else {
 217                                 // Apply
 218                                 *desc = pi->PhysAddr & 0xFF000000;
 219 //                              *desc |= ((pi->PhysAddr >> 32) & 0xF) << 20;
 220 //                              *desc |= ((pi->PhysAddr >> 36) & 0x7) << 5;
 221                                 *desc |= 2 | (1 << 18);
 222                         }
 223                         // TODO: Apply to all entries
 224                         Log_Warning("MMVirt", "TODO: Apply changes to all entries of supersections");
 225                         LEAVE('i', 0);
 226                         return 0;
 227                 }
 228                 // TODO: What here?
 229                 Log_Warning("MMVirt", "TODO: 24-bit not on supersection?");
 230                 LEAVE('i', 1);
 231                 return 1;
 232         }
 233
 234         LEAVE('i', 1);
 235         return 1;
 236 }
 237
 238 int MM_int_GetPageInfo(tVAddr VAddr, tMM_PageInfo *pi)
 239 {
 240         Uint32  *table0, *table1;
 241         Uint32  desc;
 242
 243 //      LogF("MM_int_GetPageInfo: VAddr=%p, pi=%p\n", VAddr, pi);
 244
 245         MM_int_GetTables(VAddr, &table0, &table1);
 246
 247         desc = table0[ VAddr >> 20 ];
 248
 249 //      if( VAddr > 0x90000000)
 250 //              LOG("table0 desc(%p) = %x", &table0[ VAddr >> 20 ], desc);
 251
 252         pi->bExecutable = 1;
 253         pi->bGlobal = 0;
 254         pi->bShared = 0;
 255         pi->AP = 0;
 256
 257         switch( (desc & 3) )
 258         {
 259         // 0: Unmapped
 260         case 0:
 261                 pi->PhysAddr = 0;
 262                 pi->Size = 20;
 263                 pi->Domain = 0;
 264                 return 1;
 265
 266         // 1: Coarse page table
 267         case 1:
 268                 // Domain from top level table
 269                 pi->Domain = (desc >> 5) & 7;
 270                 // Get next level
 271                 desc = table1[ VAddr >> 12 ];
 272 //              LOG("table1 desc(%p) = %x", &table1[ VAddr >> 12 ], desc);
 273                 switch( desc & 3 )
 274                 {
 275                 // 0: Unmapped
 276                 case 0:
 277                         pi->Size = 12;
 278                         return 1;
 279                 // 1: Large Page (64KiB)
 280                 case 1:
 281                         pi->Size = 16;
 282                         pi->PhysAddr = desc & 0xFFFF0000;
 283                         pi->AP = ((desc >> 4) & 3) | (((desc >> 9) & 1) << 2);
 284                         pi->bExecutable = !(desc & 0x8000);
 285                         pi->bShared = (desc >> 10) & 1;
 286                         return 0;
 287                 // 2/3: Small page
 288                 case 2:
 289                 case 3:
 290                         pi->Size = 12;
 291                         pi->PhysAddr = desc & 0xFFFFF000;
 292                         pi->bExecutable = !(desc & 1);
 293                         pi->bGlobal = !(desc >> 11);
 294                         pi->bShared = (desc >> 10) & 1;
 295                         pi->AP = ((desc >> 4) & 3) | (((desc >> 9) & 1) << 2);
 296                         return 0;
 297                 }
 298                 return 1;
 299
 300         // 2: Section (or Supersection)
 301         case 2:
 302                 if( desc & (1 << 18) ) {
 303                         // Supersection
 304                         pi->PhysAddr = desc & 0xFF000000;
 305                         pi->PhysAddr |= (Uint64)((desc >> 20) & 0xF) << 32;
 306                         pi->PhysAddr |= (Uint64)((desc >> 5) & 0x7) << 36;
 307                         pi->Size = 24;
 308                         pi->Domain = 0; // Supersections default to zero
 309                         pi->AP = ((desc >> 10) & 3) | (((desc >> 15) & 1) << 2);
 310                         return 0;
 311                 }
 312
 313                 // Section
 314                 pi->PhysAddr = desc & 0xFFF80000;
 315                 pi->Size = 20;
 316                 pi->Domain = (desc >> 5) & 7;
 317                 pi->AP = ((desc >> 10) & 3) | (((desc >> 15) & 1) << 2);
 318                 return 0;
 319
 320         // 3: Reserved (invalid)
 321         case 3:
 322                 pi->PhysAddr = 0;
 323                 pi->Size = 20;
 324                 pi->Domain = 0;
 325                 return 2;
 326         }
 327         return 2;
 328 }
 329
 330 // --- Exports ---
 331 tPAddr MM_GetPhysAddr(const void *Ptr)
 332 {
 333         tMM_PageInfo    pi;
 334         if( MM_int_GetPageInfo((tVAddr)Ptr, &pi) )
 335                 return 0;
 336         return pi.PhysAddr | ((tVAddr)Ptr & ((1 << pi.Size)-1));
 337 }
 338
 339 Uint MM_GetFlags(tVAddr VAddr)
 340 {
 341         tMM_PageInfo    pi;
 342          int    ret;
 343
 344         if( MM_int_GetPageInfo(VAddr, &pi) )
 345                 return 0;
 346
 347         ret = 0;
 348
 349         switch(pi.AP)
 350         {
 351         case 0:
 352                 break;
 353         case AP_KRW_ONLY:
 354                 ret |= MM_PFLAG_KERNEL;
 355                 break;
 356         case AP_KRO_ONLY:
 357                 ret |= MM_PFLAG_KERNEL|MM_PFLAG_RO;
 358                 break;
 359         case AP_RW_BOTH:
 360                 break;
 361         case AP_RO_BOTH:
 362                 ret |= MM_PFLAG_COW;
 363                 break;
 364         case AP_RO_USER:
 365                 ret |= MM_PFLAG_RO;
 366                 break;
 367         }
 368
 369         if( pi.bExecutable )    ret |= MM_PFLAG_EXEC;
 370         return ret;
 371 }
 372
 373 void MM_SetFlags(tVAddr VAddr, Uint Flags, Uint Mask)
 374 {
 375         tMM_PageInfo    pi;
 376         Uint    curFlags;
 377
 378         if( MM_int_GetPageInfo(VAddr, &pi) )
 379                 return ;
 380
 381         curFlags = MM_GetFlags(VAddr);
 382         if( (curFlags & Mask) == Flags )
 383                 return ;
 384         curFlags &= ~Mask;
 385         curFlags |= Flags;
 386
 387         if( curFlags & MM_PFLAG_COW )
 388                 pi.AP = AP_RO_BOTH;
 389         else
 390         {
 391                 switch(curFlags & (MM_PFLAG_KERNEL|MM_PFLAG_RO) )
 392                 {
 393                 case 0:
 394                         pi.AP = AP_RW_BOTH;     break;
 395                 case MM_PFLAG_KERNEL:
 396                         pi.AP = AP_KRW_ONLY;    break;
 397                 case MM_PFLAG_RO:
 398                         pi.AP = AP_RO_USER;     break;
 399                 case MM_PFLAG_KERNEL|MM_PFLAG_RO:
 400                         pi.AP = AP_KRO_ONLY;    break;
 401                 }
 402         }
 403
 404         pi.bExecutable = !!(curFlags & MM_PFLAG_EXEC);
 405
 406         MM_int_SetPageInfo(VAddr, &pi);
 407 }
 408
 409 int MM_IsValidBuffer(tVAddr Addr, size_t Size)
 410 {
 411         tMM_PageInfo    pi;
 412          int    bUser = 0;
 413
 414         Size += Addr & (PAGE_SIZE-1);
 415         Addr &= ~(PAGE_SIZE-1);
 416
 417         if( MM_int_GetPageInfo(Addr, &pi) )     return 0;
 418         Addr += PAGE_SIZE;
 419
 420         if(pi.AP != AP_KRW_ONLY && pi.AP != AP_KRO_ONLY)
 421                 bUser = 1;
 422
 423         while( Size >= PAGE_SIZE )
 424         {
 425                 if( MM_int_GetPageInfo(Addr, &pi) )
 426                         return 0;
 427                 if(bUser && (pi.AP == AP_KRW_ONLY || pi.AP == AP_KRO_ONLY))
 428                         return 0;
 429                 Addr += PAGE_SIZE;
 430                 Size -= PAGE_SIZE;
 431         }
 432
 433         return 1;
 434 }
 435
 436 int MM_Map(tVAddr VAddr, tPAddr PAddr)
 437 {
 438         tMM_PageInfo    pi = {0};
 439         #if TRACE_MAPS
 440         Log("MM_Map %P=>%p", PAddr, VAddr);
 441         #endif
 442
 443         pi.PhysAddr = PAddr;
 444         pi.Size = 12;
 445         if(VAddr < USER_STACK_TOP)
 446                 pi.AP = AP_RW_BOTH;
 447         else
 448                 pi.AP = AP_KRW_ONLY;    // Kernel Read/Write
 449         pi.bExecutable = 1;
 450         if( MM_int_SetPageInfo(VAddr, &pi) ) {
 451 //              MM_DerefPhys(pi.PhysAddr);
 452                 return 0;
 453         }
 454         return pi.PhysAddr;
 455 }
 456
 457 tPAddr MM_Allocate(tVAddr VAddr)
 458 {
 459         tMM_PageInfo    pi = {0};
 460
 461         ENTER("pVAddr", VAddr);
 462
 463         pi.PhysAddr = MM_AllocPhys();
 464         if( pi.PhysAddr == 0 )  LEAVE_RET('i', 0);
 465         pi.Size = 12;
 466         if(VAddr < USER_STACK_TOP)
 467                 pi.AP = AP_RW_BOTH;
 468         else
 469                 pi.AP = AP_KRW_ONLY;
 470         pi.bExecutable = 0;
 471         if( MM_int_SetPageInfo(VAddr, &pi) ) {
 472                 MM_DerefPhys(pi.PhysAddr);
 473                 LEAVE('i', 0);
 474                 return 0;
 475         }
 476         LEAVE('x', pi.PhysAddr);
 477         return pi.PhysAddr;
 478 }
 479
 480 tPAddr MM_AllocateZero(tVAddr VAddr)
 481 {
 482         if( !giMM_ZeroPage ) {
 483                 giMM_ZeroPage = MM_Allocate(VAddr);
 484                 MM_RefPhys(giMM_ZeroPage);
 485                 memset((void*)VAddr, 0, PAGE_SIZE);
 486         }
 487         else {
 488                 MM_RefPhys(giMM_ZeroPage);
 489                 MM_Map(VAddr, giMM_ZeroPage);
 490         }
 491         MM_SetFlags(VAddr, MM_PFLAG_COW, MM_PFLAG_COW);
 492         return giMM_ZeroPage;
 493 }
 494
 495 void MM_Deallocate(tVAddr VAddr)
 496 {
 497         tMM_PageInfo    pi;
 498
 499         if( MM_int_GetPageInfo(VAddr, &pi) )    return ;
 500         if( pi.PhysAddr == 0 )  return;
 501         MM_DerefPhys(pi.PhysAddr);
 502
 503         pi.PhysAddr = 0;
 504         pi.AP = 0;
 505         pi.bExecutable = 0;
 506         MM_int_SetPageInfo(VAddr, &pi);
 507 }
 508
 509 tPAddr MM_AllocateRootTable(void)
 510 {
 511         tPAddr  ret;
 512
 513         ret = MM_AllocPhysRange(2, -1);
 514         if( ret & 0x1000 ) {
 515                 MM_DerefPhys(ret);
 516                 MM_DerefPhys(ret+0x1000);
 517                 ret = MM_AllocPhysRange(3, -1);
 518                 if( ret & 0x1000 ) {
 519                         MM_DerefPhys(ret);
 520                         ret += 0x1000;
 521 //                      Log("MM_AllocateRootTable: Second try not aligned, %P", ret);
 522                 }
 523                 else {
 524                         MM_DerefPhys(ret + 0x2000);
 525 //                      Log("MM_AllocateRootTable: Second try aligned, %P", ret);
 526                 }
 527         }
 528 //      else
 529 //              Log("MM_AllocateRootTable: Got it in one, %P", ret);
 530         return ret;
 531 }
 532
 533 void MM_int_CloneTable(Uint32 *DestEnt, int Table)
 534 {
 535         tPAddr  table;
 536         Uint32  *tmp_map;
 537         Uint32  *cur = (void*)MM_TABLE1USER;
 538 //      Uint32  *cur = &FRACTAL(MM_TABLE1USER,0);
 539          int    i;
 540
 541         table = MM_AllocPhys();
 542         if(!table)      return ;
 543
 544         cur += 256*Table;
 545
 546         tmp_map = MM_MapTemp(table);
 547
 548         for( i = 0; i < 1024; i ++ )
 549         {
 550 //              Log_Debug("MMVirt", "cur[%i] (%p) = %x", Table*256+i, &cur[Table*256+i], cur[Table*256+i]);
 551                 switch(cur[i] & 3)
 552                 {
 553                 case 0: tmp_map[i] = 0; break;
 554                 case 1:
 555                         tmp_map[i] = 0;
 556                         Log_Error("MMVirt", "TODO: Support large pages in MM_int_CloneTable (%p)", (Table*256+i)*0x1000);
 557                         // Large page?
 558                         break;
 559                 case 2:
 560                 case 3:
 561                         // Small page
 562                         // - If full RW
 563 //                      Debug("%p cur[%i] & 0x230 = 0x%x", Table*256*0x1000, i, cur[i] & 0x230);
 564                         if( (cur[i] & 0x230) == 0x010 )
 565                         {
 566                                 void    *dst, *src;
 567                                 tPAddr  newpage;
 568                                 newpage = MM_AllocPhys();
 569                                 src = (void*)( (Table*256+i)*0x1000 );
 570                                 dst = MM_MapTemp(newpage);
 571 //                              Debug("Taking a copy of kernel page %p (%P)", src, cur[i] & ~0xFFF);
 572                                 memcpy(dst, src, PAGE_SIZE);
 573                                 MM_FreeTemp( dst );
 574                                 tmp_map[i] = newpage | (cur[i] & 0xFFF);
 575                         }
 576                         else
 577                         {
 578                                 if( (cur[i] & 0x230) == 0x030 )
 579                                         cur[i] |= 0x200;        // Set to full RO (Full RO=COW, User RO = RO)
 580                                 tmp_map[i] = cur[i];
 581                                 MM_RefPhys( tmp_map[i] & ~0xFFF );
 582                         }
 583                         break;
 584                 }
 585         }
 586         MM_FreeTemp( tmp_map );
 587
 588         DestEnt[0] = table + 0*0x400 + 1;
 589         DestEnt[1] = table + 1*0x400 + 1;
 590         DestEnt[2] = table + 2*0x400 + 1;
 591         DestEnt[3] = table + 3*0x400 + 1;
 592 }
 593
 594 tPAddr MM_Clone(void)
 595 {
 596         tPAddr  ret;
 597         Uint32  *new_lvl1_1, *new_lvl1_2, *cur;
 598         Uint32  *tmp_map;
 599          int    i;
 600
 601 //      MM_DumpTables(0, KERNEL_BASE);
 602
 603         ret = MM_AllocateRootTable();
 604
 605         cur = (void*)MM_TABLE0USER;
 606         new_lvl1_1 = MM_MapTemp(ret);
 607         new_lvl1_2 = MM_MapTemp(ret+0x1000);
 608         tmp_map = new_lvl1_1;
 609         for( i = 0; i < 0x800-4; i ++ )
 610         {
 611                 // HACK! Ignore the original identity mapping
 612                 if( i == 0 && Threads_GetTID() == 0 ) {
 613                         tmp_map[0] = 0;
 614                         continue;
 615                 }
 616                 if( i == 0x400 )
 617                         tmp_map = &new_lvl1_2[-0x400];
 618                 switch( cur[i] & 3 )
 619                 {
 620                 case 0: tmp_map[i] = 0; break;
 621                 case 1:
 622                         MM_int_CloneTable(&tmp_map[i], i);
 623                         i += 3; // Tables are alocated in blocks of 4
 624                         break;
 625                 case 2:
 626                 case 3:
 627                         Log_Error("MMVirt", "TODO: Support Sections/Supersections in MM_Clone (i=%i)", i);
 628                         tmp_map[i] = 0;
 629                         break;
 630                 }
 631         }
 632
 633         // Allocate Fractal table
 634         {
 635                  int    j, num;
 636                 tPAddr  tmp = MM_AllocPhys();
 637                 Uint32  *table = MM_MapTemp(tmp);
 638                 Uint32  sp;
 639                 register Uint32 __SP asm("sp");
 640
 641                 // Map table to last 4MiB of user space
 642                 new_lvl1_2[0x3FC] = tmp + 0*0x400 + 1;
 643                 new_lvl1_2[0x3FD] = tmp + 1*0x400 + 1;
 644                 new_lvl1_2[0x3FE] = tmp + 2*0x400 + 1;
 645                 new_lvl1_2[0x3FF] = tmp + 3*0x400 + 1;
 646
 647                 tmp_map = new_lvl1_1;
 648                 for( j = 0; j < 512; j ++ )
 649                 {
 650                         if( j == 256 )
 651                                 tmp_map = &new_lvl1_2[-0x400];
 652                         if( (tmp_map[j*4] & 3) == 1 )
 653                         {
 654                                 table[j] = tmp_map[j*4] & PADDR_MASK_LVL1;// 0xFFFFFC00;
 655                                 table[j] |= 0x813;      // nG, Kernel Only, Small page, XN
 656                         }
 657                         else
 658                                 table[j] = 0;
 659                 }
 660                 // Fractal
 661                 table[j++] = (ret + 0x0000) | 0x813;
 662                 table[j++] = (ret + 0x1000) | 0x813;
 663                 // Nuke the rest
 664                 for(      ; j < 1024; j ++ )
 665                         table[j] = 0;
 666
 667                 // Get kernel stack bottom
 668                 sp = __SP & ~(MM_KSTACK_SIZE-1);
 669                 j = (sp / 0x1000) % 1024;
 670                 num = MM_KSTACK_SIZE/0x1000;
 671
 672 //              Log("num = %i, sp = %p, j = %i", num, sp, j);
 673
 674                 // Copy stack pages
 675                 for(; num--; j ++, sp += 0x1000)
 676                 {
 677                         tVAddr  page;
 678                         void    *tmp_page;
 679
 680                         page = MM_AllocPhys();
 681 //                      Log("page = %P", page);
 682                         table[j] = page | 0x813;
 683
 684                         tmp_page = MM_MapTemp(page);
 685                         memcpy(tmp_page, (void*)sp, 0x1000);
 686                         MM_FreeTemp( tmp_page );
 687                 }
 688
 689                 MM_FreeTemp( table );
 690         }
 691
 692         MM_FreeTemp( new_lvl1_1 );
 693         MM_FreeTemp( new_lvl1_2 );
 694
 695 //      Log("MM_Clone: ret = %P", ret);
 696
 697         return ret;
 698 }
 699
 700 void MM_ClearUser(void)
 701 {
 702          int    i, j;
 703         const int       user_table_count = USER_STACK_TOP / (256*0x1000);
 704         Uint32  *cur = (void*)MM_TABLE0USER;
 705         Uint32  *tab;
 706
 707 //      MM_DumpTables(0, 0x80000000);
 708
 709 //      Log("user_table_count = %i (as opposed to %i)", user_table_count, 0x800-4);
 710
 711         for( i = 0; i < user_table_count; i ++ )
 712         {
 713                 switch( cur[i] & 3 )
 714                 {
 715                 case 0: break;  // Already unmapped
 716                 case 1: // Sub pages
 717                         tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 718                         for( j = 0; j < 1024; j ++ )
 719                         {
 720                                 switch( tab[j] & 3 )
 721                                 {
 722                                 case 0: break;  // Unmapped
 723                                 case 1:
 724                                         Log_Error("MMVirt", "TODO: Support large pages in MM_ClearUser");
 725                                         break;
 726                                 case 2:
 727                                 case 3:
 728                                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE-1) );
 729                                         break;
 730                                 }
 731                         }
 732                         MM_DerefPhys( cur[i] & ~(PAGE_SIZE-1) );
 733                         cur[i+0] = 0;
 734                         cur[i+1] = 0;
 735                         cur[i+2] = 0;
 736                         i += 3;
 737                         break;
 738                 case 2:
 739                 case 3:
 740                         Log_Error("MMVirt", "TODO: Implement sections/supersections in MM_ClearUser");
 741                         break;
 742                 }
 743                 cur[i] = 0;
 744         }
 745
 746         // Final block of 4 tables are KStack
 747         i = 0x800 - 4;
 748
 749         // Clear out unused stacks
 750         {
 751                 register Uint32 __SP asm("sp");
 752                  int    cur_stack_base = ((__SP & ~(MM_KSTACK_SIZE-1)) / PAGE_SIZE) % 1024;
 753
 754                 tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 755
 756                 // First 512 is the Table1 mapping + 2 for Table0 mapping
 757                 for( j = 512+2; j < 1024; j ++ )
 758                 {
 759                         // Skip current stack
 760                         if( j == cur_stack_base ) {
 761                                 j += (MM_KSTACK_SIZE / PAGE_SIZE) - 1;
 762                                 continue ;
 763                         }
 764                         if( !(tab[j] & 3) )     continue;
 765                         ASSERT( (tab[j] & 3) == 2 );
 766                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE) );
 767                         tab[j] = 0;
 768                 }
 769         }
 770
 771
 772 //      MM_DumpTables(0, 0x80000000);
 773 }
 774
 775 void *MM_MapTemp(tPAddr PAddr)
 776 {
 777         tVAddr  ret;
 778         tMM_PageInfo    pi;
 779
 780         for( ret = MM_TMPMAP_BASE; ret < MM_TMPMAP_END - PAGE_SIZE; ret += PAGE_SIZE )
 781         {
 782                 if( MM_int_GetPageInfo(ret, &pi) == 0 )
 783                         continue;
 784
 785 //              Log("MapTemp %P at %p by %p", PAddr, ret, __builtin_return_address(0));
 786                 MM_RefPhys(PAddr);      // Counter the MM_Deallocate in FreeTemp
 787                 MM_Map(ret, PAddr);
 788
 789                 return (void*)ret;
 790         }
 791         Log_Warning("MMVirt", "MM_MapTemp: All slots taken");
 792         return 0;
 793 }
 794
 795 void MM_FreeTemp(void *Ptr)
 796 {
 797         tVAddr  VAddr = (tVAddr)Ptr;
 798         if( VAddr < MM_TMPMAP_BASE || VAddr >= MM_TMPMAP_END ) {
 799                 Log_Warning("MMVirt", "MM_FreeTemp: Passed an addr not from MM_MapTemp (%p)", VAddr);
 800                 return ;
 801         }
 802
 803         MM_Deallocate(VAddr);
 804 }
 805
 806 tVAddr MM_MapHWPages(tPAddr PAddr, Uint NPages)
 807 {
 808         tVAddr  ret;
 809          int    i;
 810         tMM_PageInfo    pi;
 811
 812         ENTER("xPAddr iNPages", PAddr, NPages);
 813
 814         // Scan for a location
 815         for( ret = MM_HWMAP_BASE; ret < MM_HWMAP_END - NPages * PAGE_SIZE; ret += PAGE_SIZE )
 816         {
 817 //              LOG("checking %p", ret);
 818                 // Check if there is `NPages` free pages
 819                 for( i = 0; i < NPages; i ++ )
 820                 {
 821                         if( MM_int_GetPageInfo(ret + i*PAGE_SIZE, &pi) == 0 )
 822                                 break;
 823                 }
 824                 // Nope, jump to after the used page found and try again
 825 //              LOG("i = %i, ==? %i", i, NPages);
 826                 if( i != NPages ) {
 827                         ret += i * PAGE_SIZE;
 828                         continue ;
 829                 }
 830
 831                 // Map the pages
 832                 for( i = 0; i < NPages; i ++ )
 833                         MM_Map(ret+i*PAGE_SIZE, PAddr+i*PAGE_SIZE);
 834                 // and return
 835                 LEAVE('p', ret);
 836                 return ret;
 837         }
 838         Log_Warning("MMVirt", "MM_MapHWPages: No space for a %i page block", NPages);
 839         LEAVE('p', 0);
 840         return 0;
 841 }
 842
 843 tVAddr MM_AllocDMA(int Pages, int MaxBits, tPAddr *PAddr)
 844 {
 845         tPAddr  phys;
 846         tVAddr  ret;
 847
 848         phys = MM_AllocPhysRange(Pages, MaxBits);
 849         if(!phys) {
 850                 Log_Warning("MMVirt", "No space left for a %i page block (MM_AllocDMA)", Pages);
 851                 return 0;
 852         }
 853
 854         ret = MM_MapHWPages(phys, Pages);
 855         *PAddr = phys;
 856
 857         return ret;
 858 }
 859
 860 void MM_UnmapHWPages(tVAddr Vaddr, Uint Number)
 861 {
 862         Log_Error("MMVirt", "TODO: Implement MM_UnmapHWPages");
 863 }
 864
 865 tVAddr MM_NewKStack(int bShared)
 866 {
 867         tVAddr  min_addr, max_addr;
 868         tVAddr  addr, ofs;
 869
 870         if( bShared ) {
 871                 min_addr = MM_GLOBALSTACKS;
 872                 max_addr = MM_GLOBALSTACKS_END;
 873         }
 874         else {
 875                 min_addr = MM_KSTACK_BASE;
 876                 max_addr = MM_KSTACK_END;
 877         }
 878
 879         // Locate a free slot
 880         for( addr = min_addr; addr < max_addr; addr += MM_KSTACK_SIZE )
 881         {
 882                 tMM_PageInfo    pi;
 883                 if( MM_int_GetPageInfo(addr+MM_KSTACK_SIZE-PAGE_SIZE, &pi) )    break;
 884         }
 885
 886         // Check for an error
 887         if(addr >= max_addr) {
 888                 return 0;
 889         }
 890
 891         // 1 guard page
 892         for( ofs = PAGE_SIZE; ofs < MM_KSTACK_SIZE; ofs += PAGE_SIZE )
 893         {
 894                 if( MM_Allocate(addr + ofs) == 0 )
 895                 {
 896                         while(ofs)
 897                         {
 898                                 ofs -= PAGE_SIZE;
 899                                 MM_Deallocate(addr + ofs);
 900                         }
 901                         Log_Warning("MMVirt", "MM_NewKStack: Unable to allocate");
 902                         return 0;
 903                 }
 904         }
 905         return addr + ofs;
 906 }
 907
 908 tVAddr MM_NewUserStack(void)
 909 {
 910         tVAddr  addr, ofs;
 911
 912         addr = USER_STACK_TOP - USER_STACK_SIZE;
 913         if( MM_GetPhysAddr( (void*)(addr + PAGE_SIZE) ) ) {
 914                 Log_Error("MMVirt", "Unable to create initial user stack, addr %p taken",
 915                         addr + PAGE_SIZE
 916                         );
 917                 return 0;
 918         }
 919
 920         // 1 guard page
 921         for( ofs = PAGE_SIZE; ofs < USER_STACK_SIZE; ofs += PAGE_SIZE )
 922         {
 923                 tPAddr  rv;
 924                 if(ofs >= USER_STACK_SIZE - USER_STACK_COMM)
 925                         rv = MM_Allocate(addr + ofs);
 926                 else
 927                         rv = MM_AllocateZero(addr + ofs);
 928                 if(rv == 0)
 929                 {
 930                         while(ofs)
 931                         {
 932                                 ofs -= PAGE_SIZE;
 933                                 MM_Deallocate(addr + ofs);
 934                         }
 935                         Log_Warning("MMVirt", "MM_NewUserStack: Unable to allocate");
 936                         return 0;
 937                 }
 938                 MM_SetFlags(addr+ofs, 0, MM_PFLAG_KERNEL);
 939         }
 940 //      Log("Return %p", addr + ofs);
 941 //      MM_DumpTables(0, 0x80000000);
 942         return addr + ofs;
 943 }
 944
 945 void MM_int_DumpTableEnt(tVAddr Start, size_t Len, tMM_PageInfo *Info)
 946 {
 947         if( giMM_ZeroPage && Info->PhysAddr == giMM_ZeroPage )
 948         {
 949                 Debug("%p => %8s - 0x%7x D%i %x %s %s",
 950                         Start, "ZERO", Len,
 951                         Info->Domain, Info->AP,
 952                         Info->bExecutable ? " X" : "nX",
 953                         Info->bGlobal ? " G" : "nG"
 954                         );
 955         }
 956         else
 957         {
 958                 Debug("%p => %8x - 0x%7x D%i %x %s %s",
 959                         Start, Info->PhysAddr-Len, Len,
 960                         Info->Domain, Info->AP,
 961                         Info->bExecutable ? " X" : "nX",
 962                         Info->bGlobal ? " G" : "nG"
 963                         );
 964         }
 965 }
 966
 967 void MM_DumpTables(tVAddr Start, tVAddr End)
 968 {
 969         tVAddr  range_start = 0, addr;
 970         tMM_PageInfo    pi, pi_old;
 971          int    i = 0, inRange=0;
 972
 973         memset(&pi_old, 0, sizeof(pi_old));
 974
 975         Debug("Page Table Dump (%p to %p):", Start, End);
 976         range_start = Start;
 977         for( addr = Start; i == 0 || (addr && addr < End); i = 1 )
 978         {
 979                  int    rv;
 980 //              Log("addr = %p", addr);
 981                 rv = MM_int_GetPageInfo(addr, &pi);
 982                 if( rv
 983                  || pi.Size != pi_old.Size
 984                  || pi.Domain != pi_old.Domain
 985                  || pi.AP != pi_old.AP
 986                  || pi.bGlobal != pi_old.bGlobal
 987                  || pi_old.PhysAddr != pi.PhysAddr )
 988                 {
 989                         if(inRange) {
 990                                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi_old);
 991                         }
 992                         addr &= ~((1 << pi.Size)-1);
 993                         range_start = addr;
 994                 }
 995
 996                 pi_old = pi;
 997                 // Handle the zero page
 998                 if( !giMM_ZeroPage || pi_old.Size != 12 || pi_old.PhysAddr != giMM_ZeroPage )
 999                         pi_old.PhysAddr += 1 << pi_old.Size;
1000                 addr += 1 << pi_old.Size;
1001                 inRange = (rv == 0);
1002         }
1003         if(inRange)
1004                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi);
1005         Debug("Done");
1006 }
1007
1008 // NOTE: Runs in abort context, not much difference, just a smaller stack
1009 void MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch, Uint32 UserLR)
1010 {
1011          int    rv;
1012         tMM_PageInfo    pi;
1013
1014         rv = MM_int_GetPageInfo(Addr, &pi);
1015
1016         // Check for COW
1017         if( rv == 0 &&  pi.AP == AP_RO_BOTH )
1018         {
1019                 pi.AP = AP_RW_BOTH;
1020                 if( giMM_ZeroPage && pi.PhysAddr == giMM_ZeroPage )
1021                 {
1022                         tPAddr  newpage;
1023                         newpage = MM_AllocPhys();
1024                         if( !newpage ) {
1025                                 Log_Error("MMVirt", "Unable to allocate new page for COW of ZERO");
1026                                 for(;;);
1027                         }
1028
1029                         #if TRACE_COW
1030                         Log_Notice("MMVirt", "COW %p caused by %p, ZERO duped to %P (RefCnt(%i)--)", Addr, PC,
1031                                 newpage, MM_GetRefCount(pi.PhysAddr));
1032                         #endif
1033
1034                         MM_DerefPhys(pi.PhysAddr);
1035                         pi.PhysAddr = newpage;
1036                         pi.AP = AP_RW_BOTH;
1037                         MM_int_SetPageInfo(Addr, &pi);
1038
1039                         memset( (void*)(Addr & ~(PAGE_SIZE-1)), 0, PAGE_SIZE );
1040
1041                         return ;
1042                 }
1043                 else if( MM_GetRefCount(pi.PhysAddr) > 1 )
1044                 {
1045                         // Duplicate the page
1046                         tPAddr  newpage;
1047                         void    *dst, *src;
1048
1049                         newpage = MM_AllocPhys();
1050                         if(!newpage) {
1051                                 Log_Error("MMVirt", "Unable to allocate new page for COW");
1052                                 for(;;);
1053                         }
1054                         dst = MM_MapTemp(newpage);
1055                         src = (void*)(Addr & ~(PAGE_SIZE-1));
1056                         memcpy( dst, src, PAGE_SIZE );
1057                         MM_FreeTemp( dst );
1058
1059                         #if TRACE_COW
1060                         Log_Notice("MMVirt", "COW %p caused by %p, %P duped to %P (RefCnt(%i)--)", Addr, PC,
1061                                 pi.PhysAddr, newpage, MM_GetRefCount(pi.PhysAddr));
1062                         #endif
1063
1064                         MM_DerefPhys(pi.PhysAddr);
1065                         pi.PhysAddr = newpage;
1066                 }
1067                 #if TRACE_COW
1068                 else {
1069                         Log_Notice("MMVirt", "COW %p caused by %p, took last reference to %P",
1070                                 Addr, PC, pi.PhysAddr);
1071                 }
1072                 #endif
1073                 // Unset COW
1074                 pi.AP = AP_RW_BOTH;
1075                 MM_int_SetPageInfo(Addr, &pi);
1076                 return ;
1077         }
1078
1079
1080         Log_Error("MMVirt", "Code at %p accessed %p (DFSR = 0x%x)%s", PC, Addr, DFSR,
1081                 (bPrefetch ? " - Prefetch" : "")
1082                 );
1083         Log_Error("MMVirt", "- User LR = 0x%x", UserLR);
1084         const char * const dfsr_errors[] = {
1085                 /* 00000 */ "-", "Alignment Fault",
1086                 /* 00010 */ "Debug event", "Access Flag (Section)",
1087                 /* 00100 */ "Instr Cache Maint", "Translation (Section)",
1088                 /* 00110 */ "Access Flag (Page)", "Translation (Page)",
1089                 /* 01000 */ "Sync. External abort", "Domain (Section)",
1090                 /* 01010 */ "-", "Domain (Page)",
1091                 /* 01100 */ "Table Walk sync ext (lvl 1)", "Permission (Section)",
1092                 /* 01110 */ "Table Walk sync ext (lvl 2)", "Permission (Page)",
1093                 // 0b10000
1094                 /* 10000 */ "-", "-",
1095                 /* 10010 */ "-", "-",
1096                 /* 10100 */ "IMPL (Lockdown)", "-",
1097                 /* 10110 */ "Async. Extern. Abort", "-",
1098                 /* 11000 */ "Mem. access async pairity error", "Mem. access async pairity error",
1099                 /* 11010 */ "IMPL (Coprocessor abort)", "-",
1100                 /* 11100 */ "Table Walk Sync parity (lvl 1)", "-",
1101                 /* 11110 */ "Table Walk Sync parity (lvl 2)", "-"
1102                 };
1103          int    errcode = (DFSR & 0xF) | (((DFSR >> 10) & 1) << 4);
1104         Log_Error("MMVirt", "- Errcode 0b%05b", errcode);
1105         Log_Error("MMVirt", "- Dom %i %s %s",
1106                 (DFSR >> 4) & 0xF, (DFSR & 0x800 ? "Write": "Read"),
1107                 dfsr_errors[errcode]
1108                 );
1109         Log_Error("MMVirt", "- AP=%i(%s) %s", pi.AP, caAPValueNames[pi.AP], pi.bExecutable ? " Executable":"");
1110         if( Addr < 0x80000000 )
1111                 MM_DumpTables(0, 0x80000000);
1112         else
1113                 MM_DumpTables(0x80000000, -1);
1114         for(;;);
1115 }
1116