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       0
  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(volatile void *VAddr);
  58 tPAddr  MM_AllocateRootTable(void);
  59 void    MM_int_CloneTable(Uint32 *DestEnt, int Table);
  60 tPAddr  MM_Clone(int ClearUser);
  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 SystemLR, 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(volatile 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(const volatile void *VAddr)
 340 {
 341         tMM_PageInfo    pi;
 342          int    ret;
 343
 344         if( MM_int_GetPageInfo((tVAddr)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(volatile void *VAddr, Uint Flags, Uint Mask)
 374 {
 375         tMM_PageInfo    pi;
 376         Uint    curFlags;
 377
 378         if( MM_int_GetPageInfo((tVAddr)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((tVAddr)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((tVAddr)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(volatile void *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         // TODO: Double check that an address isn't being clobbered
 444
 445         pi.PhysAddr = PAddr;
 446         pi.Size = 12;
 447         pi.AP = ( (tVAddr)VAddr < USER_STACK_TOP ? AP_RW_BOTH : AP_KRW_ONLY );
 448         pi.bExecutable = 1;
 449         if( MM_int_SetPageInfo( (tVAddr)VAddr, &pi) ) {
 450 //              MM_DerefPhys(pi.PhysAddr);
 451                 return 0;
 452         }
 453         return pi.PhysAddr;
 454 }
 455
 456 tPAddr MM_Allocate(volatile void *VAddr)
 457 {
 458         tMM_PageInfo    pi = {0};
 459
 460         ENTER("pVAddr", VAddr);
 461
 462         pi.PhysAddr = MM_AllocPhys();
 463         if( pi.PhysAddr == 0 )  LEAVE_RET('i', 0);
 464         pi.Size = 12;
 465         pi.AP = ( (tVAddr)VAddr < USER_STACK_TOP ? AP_RW_BOTH : AP_KRW_ONLY );
 466         pi.bExecutable = 0;
 467         if( MM_int_SetPageInfo( (tVAddr)VAddr, &pi ) )
 468         {
 469                 MM_DerefPhys(pi.PhysAddr);
 470                 LEAVE('i', 0);
 471                 return 0;
 472         }
 473         LEAVE('x', pi.PhysAddr);
 474         return pi.PhysAddr;
 475 }
 476
 477 void MM_AllocateZero(volatile void *VAddr)
 478 {
 479         if( !giMM_ZeroPage ) {
 480                 giMM_ZeroPage = MM_Allocate(VAddr);
 481                 MM_RefPhys(giMM_ZeroPage);
 482                 memset((void*)VAddr, 0, PAGE_SIZE);
 483         }
 484         else {
 485                 MM_RefPhys(giMM_ZeroPage);
 486                 MM_Map(VAddr, giMM_ZeroPage);
 487         }
 488         MM_SetFlags(VAddr, MM_PFLAG_COW, MM_PFLAG_COW);
 489 }
 490
 491 void MM_Deallocate(volatile void *VAddr)
 492 {
 493         tMM_PageInfo    pi;
 494
 495         if( MM_int_GetPageInfo((tVAddr)VAddr, &pi) )    return ;
 496         if( pi.PhysAddr == 0 )  return;
 497         MM_DerefPhys(pi.PhysAddr);
 498
 499         pi.PhysAddr = 0;
 500         pi.AP = 0;
 501         pi.bExecutable = 0;
 502         MM_int_SetPageInfo((tVAddr)VAddr, &pi);
 503 }
 504
 505 tPAddr MM_AllocateRootTable(void)
 506 {
 507         tPAddr  ret;
 508
 509         ret = MM_AllocPhysRange(2, -1);
 510         if( ret & 0x1000 ) {
 511                 MM_DerefPhys(ret);
 512                 MM_DerefPhys(ret+0x1000);
 513                 ret = MM_AllocPhysRange(3, -1);
 514                 if( ret & 0x1000 ) {
 515                         MM_DerefPhys(ret);
 516                         ret += 0x1000;
 517 //                      Log("MM_AllocateRootTable: Second try not aligned, %P", ret);
 518                 }
 519                 else {
 520                         MM_DerefPhys(ret + 0x2000);
 521 //                      Log("MM_AllocateRootTable: Second try aligned, %P", ret);
 522                 }
 523         }
 524 //      else
 525 //              Log("MM_AllocateRootTable: Got it in one, %P", ret);
 526         return ret;
 527 }
 528
 529 void MM_int_CloneTable(Uint32 *DestEnt, int Table)
 530 {
 531         tPAddr  table;
 532         Uint32  *tmp_map;
 533         Uint32  *cur = (void*)MM_TABLE1USER;
 534 //      Uint32  *cur = &FRACTAL(MM_TABLE1USER,0);
 535          int    i;
 536
 537         table = MM_AllocPhys();
 538         if(!table)      return ;
 539
 540         cur += 256*Table;
 541
 542         tmp_map = MM_MapTemp(table);
 543
 544         for( i = 0; i < 1024; i ++ )
 545         {
 546 //              Log_Debug("MMVirt", "cur[%i] (%p) = %x", Table*256+i, &cur[Table*256+i], cur[Table*256+i]);
 547                 switch(cur[i] & 3)
 548                 {
 549                 case 0: tmp_map[i] = 0; break;
 550                 case 1:
 551                         tmp_map[i] = 0;
 552                         Log_Error("MMVirt", "TODO: Support large pages in MM_int_CloneTable (%p)", (Table*256+i)*0x1000);
 553                         // Large page?
 554                         break;
 555                 case 2:
 556                 case 3:
 557                         // Small page
 558                         // - If full RW
 559 //                      Debug("%p cur[%i] & 0x230 = 0x%x", Table*256*0x1000, i, cur[i] & 0x230);
 560                         if( (cur[i] & 0x230) == 0x010 )
 561                         {
 562                                 void    *dst, *src;
 563                                 tPAddr  newpage;
 564                                 newpage = MM_AllocPhys();
 565                                 src = (void*)( (Table*256+i)*0x1000 );
 566                                 dst = MM_MapTemp(newpage);
 567 //                              Debug("Taking a copy of kernel page %p (%P)", src, cur[i] & ~0xFFF);
 568                                 memcpy(dst, src, PAGE_SIZE);
 569                                 MM_FreeTemp( dst );
 570                                 tmp_map[i] = newpage | (cur[i] & 0xFFF);
 571                         }
 572                         else
 573                         {
 574                                 if( (cur[i] & 0x230) == 0x030 )
 575                                         cur[i] |= 0x200;        // Set to full RO (Full RO=COW, User RO = RO)
 576                                 tmp_map[i] = cur[i];
 577                                 MM_RefPhys( tmp_map[i] & ~0xFFF );
 578                         }
 579                         break;
 580                 }
 581         }
 582         MM_FreeTemp( tmp_map );
 583
 584         DestEnt[0] = table + 0*0x400 + 1;
 585         DestEnt[1] = table + 1*0x400 + 1;
 586         DestEnt[2] = table + 2*0x400 + 1;
 587         DestEnt[3] = table + 3*0x400 + 1;
 588 }
 589
 590 tPAddr MM_Clone(int EmptyUser)
 591 {
 592         Uint32  *tmp_map;
 593
 594 //      MM_DumpTables(0, KERNEL_BASE);
 595
 596         tPAddr  ret = MM_AllocateRootTable();
 597
 598         Uint32  *new_lvl1_1 = MM_MapTemp(ret);
 599         Uint32  *new_lvl1_2 = MM_MapTemp(ret+0x1000);
 600
 601         if( !EmptyUser )
 602         {
 603                 Uint32 *cur = (void*)MM_TABLE0USER;
 604                 tmp_map = new_lvl1_1;
 605                 for( int i = 0; i < 0x800-4; i ++ )
 606                 {
 607                         // HACK! Ignore the original identity mapping
 608                         if( i == 0 && Threads_GetTID() == 0 ) {
 609                                 tmp_map[0] = 0;
 610                                 continue;
 611                         }
 612                         if( i == 0x400 )
 613                                 tmp_map = &new_lvl1_2[-0x400];
 614                         switch( cur[i] & 3 )
 615                         {
 616                         case 0: tmp_map[i] = 0; break;
 617                         case 1:
 618                                 MM_int_CloneTable(&tmp_map[i], i);
 619                                 i += 3; // Tables are alocated in blocks of 4
 620                                 break;
 621                         case 2:
 622                         case 3:
 623                                 Log_Error("MMVirt", "TODO: Support Sections/Supersections in MM_Clone (i=%i)", i);
 624                                 tmp_map[i] = 0;
 625                                 break;
 626                         }
 627                 }
 628         }
 629
 630         // Allocate Fractal table
 631         {
 632                 tPAddr  tmp = MM_AllocPhys();
 633                 Uint32  *table = MM_MapTemp(tmp);
 634
 635                 // Map table to last 4MiB of user space
 636                 new_lvl1_2[0x3FC] = tmp + 0*0x400 + 1;
 637                 new_lvl1_2[0x3FD] = tmp + 1*0x400 + 1;
 638                 new_lvl1_2[0x3FE] = tmp + 2*0x400 + 1;
 639                 new_lvl1_2[0x3FF] = tmp + 3*0x400 + 1;
 640
 641                 tmp_map = new_lvl1_1;
 642                  int    j = 0;
 643                 for( ; j < 512; j ++ )
 644                 {
 645                         if( j == 256 )
 646                                 tmp_map = &new_lvl1_2[-0x400];
 647                         if( (tmp_map[j*4] & 3) == 1 )
 648                         {
 649                                 table[j] = tmp_map[j*4] & PADDR_MASK_LVL1;// 0xFFFFFC00;
 650                                 table[j] |= 0x813;      // nG, Kernel Only, Small page, XN
 651                         }
 652                         else
 653                                 table[j] = 0;
 654                 }
 655                 // Fractal
 656                 table[j++] = (ret + 0x0000) | 0x813;
 657                 table[j++] = (ret + 0x1000) | 0x813;
 658                 // Clear the rest of the table
 659                 for( ; j < 1024; j ++ )
 660                         table[j] = 0;
 661
 662                 // Get kernel stack bottom
 663                 register Uint32 __SP asm("sp");
 664                 Uint32  sp = __SP & ~(MM_KSTACK_SIZE-1);
 665                 j = (sp / 0x1000) % 1024;
 666
 667                 // Copy stack pages
 668                 for(int num = MM_KSTACK_SIZE/PAGE_SIZE; num--; j ++, sp += PAGE_SIZE)
 669                 {
 670                         tVAddr  page;
 671                         void    *tmp_page;
 672
 673                         page = MM_AllocPhys();
 674 //                      Log("page = %P", page);
 675                         table[j] = page | 0x813;
 676
 677                         tmp_page = MM_MapTemp(page);
 678                         memcpy(tmp_page, (void*)sp, 0x1000);
 679                         MM_FreeTemp( tmp_page );
 680                 }
 681
 682                 MM_FreeTemp( table );
 683         }
 684
 685         MM_FreeTemp( new_lvl1_1 );
 686         MM_FreeTemp( new_lvl1_2 );
 687
 688 //      Log("MM_Clone: ret = %P", ret);
 689
 690         return ret;
 691 }
 692
 693 void MM_ClearUser(void)
 694 {
 695          int    i, j;
 696         const int       user_table_count = USER_STACK_TOP / (256*0x1000);
 697         Uint32  *cur = (void*)MM_TABLE0USER;
 698         Uint32  *tab;
 699
 700 //      MM_DumpTables(0, 0x80000000);
 701
 702 //      Log("user_table_count = %i (as opposed to %i)", user_table_count, 0x800-4);
 703
 704         for( i = 0; i < user_table_count; i ++ )
 705         {
 706                 switch( cur[i] & 3 )
 707                 {
 708                 case 0: break;  // Already unmapped
 709                 case 1: // Sub pages
 710                         tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 711                         for( j = 0; j < 1024; j ++ )
 712                         {
 713                                 switch( tab[j] & 3 )
 714                                 {
 715                                 case 0: break;  // Unmapped
 716                                 case 1:
 717                                         Log_Error("MMVirt", "TODO: Support large pages in MM_ClearUser");
 718                                         break;
 719                                 case 2:
 720                                 case 3:
 721                                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE-1) );
 722                                         break;
 723                                 }
 724                         }
 725                         MM_DerefPhys( cur[i] & ~(PAGE_SIZE-1) );
 726                         cur[i+0] = 0;
 727                         cur[i+1] = 0;
 728                         cur[i+2] = 0;
 729                         i += 3;
 730                         break;
 731                 case 2:
 732                 case 3:
 733                         Log_Error("MMVirt", "TODO: Implement sections/supersections in MM_ClearUser");
 734                         break;
 735                 }
 736                 cur[i] = 0;
 737         }
 738
 739         // Final block of 4 tables are KStack
 740         i = 0x800 - 4;
 741
 742         // Clear out unused stacks
 743         {
 744                 register Uint32 __SP asm("sp");
 745                  int    cur_stack_base = ((__SP & ~(MM_KSTACK_SIZE-1)) / PAGE_SIZE) % 1024;
 746
 747                 tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 748
 749                 // First 512 is the Table1 mapping + 2 for Table0 mapping
 750                 for( j = 512+2; j < 1024; j ++ )
 751                 {
 752                         // Skip current stack
 753                         if( j == cur_stack_base ) {
 754                                 j += (MM_KSTACK_SIZE / PAGE_SIZE) - 1;
 755                                 continue ;
 756                         }
 757                         if( !(tab[j] & 3) )     continue;
 758                         ASSERT( (tab[j] & 3) == 2 );
 759                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE) );
 760                         tab[j] = 0;
 761                 }
 762         }
 763
 764
 765 //      MM_DumpTables(0, 0x80000000);
 766 }
 767
 768 void *MM_MapTemp(tPAddr PAddr)
 769 {
 770         for( tVAddr ret = MM_TMPMAP_BASE; ret < MM_TMPMAP_END - PAGE_SIZE; ret += PAGE_SIZE )
 771         {
 772                 tMM_PageInfo    pi;
 773
 774                 if( MM_int_GetPageInfo(ret, &pi) == 0 )
 775                         continue;
 776
 777 //              Log("MapTemp %P at %p by %p", PAddr, ret, __builtin_return_address(0));
 778                 MM_RefPhys(PAddr);      // Counter the MM_Deallocate in FreeTemp
 779                 MM_Map( (void*)ret, PAddr );
 780
 781                 return (void*)ret;
 782         }
 783         Log_Warning("MMVirt", "MM_MapTemp: All slots taken");
 784         return 0;
 785 }
 786
 787 void MM_FreeTemp(void *Ptr)
 788 {
 789         tVAddr  VAddr = (tVAddr)Ptr;
 790         if( VAddr < MM_TMPMAP_BASE || VAddr >= MM_TMPMAP_END ) {
 791                 Log_Warning("MMVirt", "MM_FreeTemp: Passed an addr not from MM_MapTemp (%p)", VAddr);
 792                 return ;
 793         }
 794
 795         MM_Deallocate(Ptr);
 796 }
 797
 798 void *MM_MapHWPages(tPAddr PAddr, Uint NPages)
 799 {
 800         tVAddr  ret;
 801          int    i;
 802         tMM_PageInfo    pi;
 803
 804         ENTER("xPAddr iNPages", PAddr, NPages);
 805
 806         // Scan for a location
 807         for( ret = MM_HWMAP_BASE; ret < MM_HWMAP_END - NPages * PAGE_SIZE; ret += PAGE_SIZE )
 808         {
 809 //              LOG("checking %p", ret);
 810                 // Check if there is `NPages` free pages
 811                 for( i = 0; i < NPages; i ++ )
 812                 {
 813                         if( MM_int_GetPageInfo(ret + i*PAGE_SIZE, &pi) == 0 )
 814                                 break;
 815                 }
 816                 // Nope, jump to after the used page found and try again
 817 //              LOG("i = %i, ==? %i", i, NPages);
 818                 if( i != NPages ) {
 819                         ret += i * PAGE_SIZE;
 820                         continue ;
 821                 }
 822
 823                 // Map the pages
 824                 for( i = 0; i < NPages; i ++ )
 825                         MM_Map( (tPage*)ret + i, PAddr+i*PAGE_SIZE);
 826                 // and return
 827                 LEAVE('p', ret);
 828                 return (void*)ret;
 829         }
 830         Log_Warning("MMVirt", "MM_MapHWPages: No space for a %i page block", NPages);
 831         LEAVE('p', 0);
 832         return 0;
 833 }
 834
 835 void *MM_AllocDMA(int Pages, int MaxBits, tPAddr *PAddr)
 836 {
 837         tPAddr  phys;
 838         void    *ret;
 839
 840         phys = MM_AllocPhysRange(Pages, MaxBits);
 841         if(!phys) {
 842                 Log_Warning("MMVirt", "No space left for a %i page block (MM_AllocDMA)", Pages);
 843                 return 0;
 844         }
 845
 846         ret = MM_MapHWPages(phys, Pages);
 847         if( !ret ) {
 848                 MM_DerefPhys(phys);
 849                 return NULL;
 850         }
 851         if( PAddr )
 852                 *PAddr = phys;
 853
 854         return ret;
 855 }
 856
 857 void MM_UnmapHWPages(volatile void *VAddr, Uint Number)
 858 {
 859         Log_Error("MMVirt", "TODO: Implement MM_UnmapHWPages");
 860 }
 861
 862 tVAddr MM_NewKStack(int bShared)
 863 {
 864         tVAddr  min_addr, max_addr;
 865         tVAddr  addr, ofs;
 866
 867         if( bShared ) {
 868                 min_addr = MM_GLOBALSTACKS;
 869                 max_addr = MM_GLOBALSTACKS_END;
 870         }
 871         else {
 872                 min_addr = MM_KSTACK_BASE;
 873                 max_addr = MM_KSTACK_END;
 874         }
 875
 876         // Locate a free slot
 877         for( addr = min_addr; addr < max_addr; addr += MM_KSTACK_SIZE )
 878         {
 879                 tMM_PageInfo    pi;
 880                 if( MM_int_GetPageInfo(addr+MM_KSTACK_SIZE-PAGE_SIZE, &pi) )    break;
 881         }
 882
 883         // Check for an error
 884         if(addr >= max_addr) {
 885                 return 0;
 886         }
 887
 888         // 1 guard page
 889         tPage   *pageptr = (void*)(addr + PAGE_SIZE);
 890         for( ofs = PAGE_SIZE; ofs < MM_KSTACK_SIZE; ofs += PAGE_SIZE )
 891         {
 892                 if( MM_Allocate( pageptr ) == 0 )
 893                 {
 894                         while(ofs)
 895                         {
 896                                 ofs -= PAGE_SIZE;
 897                                 MM_Deallocate( pageptr-- );
 898                         }
 899                         Log_Warning("MMVirt", "MM_NewKStack: Unable to allocate");
 900                         return 0;
 901                 }
 902         }
 903         return addr + ofs;
 904 }
 905
 906 tVAddr MM_NewUserStack(void)
 907 {
 908         tVAddr  addr, ofs;
 909
 910         addr = USER_STACK_TOP - USER_STACK_SIZE;
 911         if( MM_GetPhysAddr( (void*)(addr + PAGE_SIZE) ) ) {
 912                 Log_Error("MMVirt", "Unable to create initial user stack, addr %p taken",
 913                         addr + PAGE_SIZE
 914                         );
 915                 MM_DumpTables(0,KERNEL_BASE);
 916                 return 0;
 917         }
 918
 919         // 1 guard page
 920         tPage   *pageptr = (void*)addr;
 921         for( ofs = PAGE_SIZE; ofs < USER_STACK_SIZE; ofs += PAGE_SIZE, pageptr ++ )
 922         {
 923                 if(ofs >= USER_STACK_SIZE - USER_STACK_COMM) {
 924                         tPAddr  rv = MM_Allocate(pageptr);
 925                         if(rv == 0)
 926                         {
 927                                 while(ofs)
 928                                 {
 929                                         ofs -= PAGE_SIZE;
 930                                         MM_Deallocate(pageptr --);
 931                                 }
 932                                 Log_Warning("MMVirt", "MM_NewUserStack: Unable to allocate");
 933                                 return 0;
 934                         }
 935                 }
 936                 else {
 937                         MM_AllocateZero(pageptr);
 938                 }
 939                 MM_SetFlags(pageptr, 0, MM_PFLAG_KERNEL);
 940         }
 941         Log("Return %p", addr + ofs);
 942 //      MM_DumpTables(0, 0x80000000);
 943         return addr + ofs;
 944 }
 945
 946 void MM_int_DumpTableEnt(tVAddr Start, size_t Len, tMM_PageInfo *Info)
 947 {
 948         if( giMM_ZeroPage && Info->PhysAddr == giMM_ZeroPage )
 949         {
 950                 Debug("0x%08x => %8s - 0x%7x D%i %x %s %s",
 951                         Start, "ZERO", Len,
 952                         Info->Domain, Info->AP,
 953                         Info->bExecutable ? " X" : "nX",
 954                         Info->bGlobal ? " G" : "nG"
 955                         );
 956         }
 957         else
 958         {
 959                 void    *node;
 960                 MM_GetPageNode(Info->PhysAddr, &node);
 961                 Debug("0x%08x => %8x - 0x%7x D%i %x %s %s %p",
 962                         Start, Info->PhysAddr-Len, Len,
 963                         Info->Domain, Info->AP,
 964                         Info->bExecutable ? " X" : "nX",
 965                         Info->bGlobal ? " G" : "nG",
 966                         node
 967                         );
 968         }
 969 }
 970
 971 void MM_DumpTables(tVAddr Start, tVAddr End)
 972 {
 973         tVAddr  range_start = 0, addr;
 974         tMM_PageInfo    pi, pi_old;
 975          int    i = 0, inRange=0;
 976
 977         memset(&pi_old, 0, sizeof(pi_old));
 978
 979         Debug("Page Table Dump (%p to %p):", Start, End);
 980         range_start = Start;
 981         for( addr = Start; i == 0 || (addr && addr < End); i = 1 )
 982         {
 983                  int    rv;
 984 //              Log("addr = %p", addr);
 985                 rv = MM_int_GetPageInfo(addr, &pi);
 986                 if( rv
 987                  || pi.Size != pi_old.Size
 988                  || pi.Domain != pi_old.Domain
 989                  || pi.AP != pi_old.AP
 990                  || pi.bGlobal != pi_old.bGlobal
 991                  || pi_old.PhysAddr != pi.PhysAddr )
 992                 {
 993                         if(inRange) {
 994                                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi_old);
 995                         }
 996                         addr &= ~((1 << pi.Size)-1);
 997                         range_start = addr;
 998                 }
 999
1000                 pi_old = pi;
1001                 // Handle the zero page
1002                 if( !giMM_ZeroPage || pi_old.Size != 12 || pi_old.PhysAddr != giMM_ZeroPage )
1003                         pi_old.PhysAddr += 1 << pi_old.Size;
1004                 addr += 1 << pi_old.Size;
1005                 inRange = (rv == 0);
1006         }
1007         if(inRange)
1008                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi);
1009         Debug("Done");
1010 }
1011
1012 // NOTE: Runs in abort context, not much difference, just a smaller stack
1013 void MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch, Uint32 SystemLR, Uint32 UserLR)
1014 {
1015          int    rv;
1016         tMM_PageInfo    pi;
1017
1018         rv = MM_int_GetPageInfo(Addr, &pi);
1019
1020         // Check for COW
1021         if( rv == 0 &&  pi.AP == AP_RO_BOTH )
1022         {
1023                 pi.AP = AP_RW_BOTH;
1024                 if( giMM_ZeroPage && pi.PhysAddr == giMM_ZeroPage )
1025                 {
1026                         tPAddr  newpage;
1027                         newpage = MM_AllocPhys();
1028                         if( !newpage ) {
1029                                 Log_Error("MMVirt", "Unable to allocate new page for COW of ZERO");
1030                                 for(;;);
1031                         }
1032
1033                         #if TRACE_COW
1034                         Log_Notice("MMVirt", "COW %p caused by %p, ZERO duped to %P (RefCnt(%i)--)", Addr, PC,
1035                                 newpage, MM_GetRefCount(pi.PhysAddr));
1036                         #endif
1037
1038                         MM_DerefPhys(pi.PhysAddr);
1039                         pi.PhysAddr = newpage;
1040                         pi.AP = AP_RW_BOTH;
1041                         MM_int_SetPageInfo(Addr, &pi);
1042
1043                         memset( (void*)(Addr & ~(PAGE_SIZE-1)), 0, PAGE_SIZE );
1044
1045                         return ;
1046                 }
1047                 else if( MM_GetRefCount(pi.PhysAddr) > 1 )
1048                 {
1049                         // Duplicate the page
1050                         tPAddr  newpage;
1051                         void    *dst, *src;
1052
1053                         newpage = MM_AllocPhys();
1054                         if(!newpage) {
1055                                 Log_Error("MMVirt", "Unable to allocate new page for COW");
1056                                 for(;;);
1057                         }
1058                         dst = MM_MapTemp(newpage);
1059                         src = (void*)(Addr & ~(PAGE_SIZE-1));
1060                         memcpy( dst, src, PAGE_SIZE );
1061                         MM_FreeTemp( dst );
1062
1063                         #if TRACE_COW
1064                         Log_Notice("MMVirt", "COW %p caused by %p, %P duped to %P (RefCnt(%i)--)", Addr, PC,
1065                                 pi.PhysAddr, newpage, MM_GetRefCount(pi.PhysAddr));
1066                         #endif
1067
1068                         MM_DerefPhys(pi.PhysAddr);
1069                         pi.PhysAddr = newpage;
1070                 }
1071                 #if TRACE_COW
1072                 else {
1073                         Log_Notice("MMVirt", "COW %p caused by %p, took last reference to %P",
1074                                 Addr, PC, pi.PhysAddr);
1075                 }
1076                 #endif
1077                 // Unset COW
1078                 pi.AP = AP_RW_BOTH;
1079                 MM_int_SetPageInfo(Addr, &pi);
1080                 return ;
1081         }
1082
1083
1084         Log_Error("MMVirt", "Code at %p accessed %p (DFSR = 0x%x)%s", PC, Addr, DFSR,
1085                 (bPrefetch ? " - Prefetch" : "")
1086                 );
1087         Log_Error("MMVirt", "- User LR = 0x%x, System LR = 0x%x", UserLR, SystemLR);
1088         const char * const dfsr_errors[] = {
1089                 /* 00000 */ "-", "Alignment Fault",
1090                 /* 00010 */ "Debug event", "Access Flag (Section)",
1091                 /* 00100 */ "Instr Cache Maint", "Translation (Section)",
1092                 /* 00110 */ "Access Flag (Page)", "Translation (Page)",
1093                 /* 01000 */ "Sync. External abort", "Domain (Section)",
1094                 /* 01010 */ "-", "Domain (Page)",
1095                 /* 01100 */ "Table Walk sync ext (lvl 1)", "Permission (Section)",
1096                 /* 01110 */ "Table Walk sync ext (lvl 2)", "Permission (Page)",
1097                 // 0b10000
1098                 /* 10000 */ "-", "-",
1099                 /* 10010 */ "-", "-",
1100                 /* 10100 */ "IMPL (Lockdown)", "-",
1101                 /* 10110 */ "Async. Extern. Abort", "-",
1102                 /* 11000 */ "Mem. access async pairity error", "Mem. access async pairity error",
1103                 /* 11010 */ "IMPL (Coprocessor abort)", "-",
1104                 /* 11100 */ "Table Walk Sync parity (lvl 1)", "-",
1105                 /* 11110 */ "Table Walk Sync parity (lvl 2)", "-"
1106                 };
1107          int    errcode = (DFSR & 0xF) | (((DFSR >> 10) & 1) << 4);
1108         Log_Error("MMVirt", "- Errcode 0b%05b", errcode);
1109         Log_Error("MMVirt", "- Dom %i %s %s",
1110                 (DFSR >> 4) & 0xF, (DFSR & 0x800 ? "Write": "Read"),
1111                 dfsr_errors[errcode]
1112                 );
1113         Log_Error("MMVirt", "- AP=%i(%s) %s", pi.AP, caAPValueNames[pi.AP], pi.bExecutable ? " Executable":"");
1114         if( Addr < 0x80000000 )
1115                 MM_DumpTables(0, 0x80000000);
1116         else
1117                 MM_DumpTables(0x80000000, -1);
1118         for(;;);
1119 }
1120