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(tVAddr 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(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(int EmptyUser)
 595 {
 596         Uint32  *tmp_map;
 597
 598 //      MM_DumpTables(0, KERNEL_BASE);
 599
 600         tPAddr  ret = MM_AllocateRootTable();
 601
 602         Uint32  *new_lvl1_1 = MM_MapTemp(ret);
 603         Uint32  *new_lvl1_2 = MM_MapTemp(ret+0x1000);
 604
 605         if( !EmptyUser )
 606         {
 607                 Uint32 *cur = (void*)MM_TABLE0USER;
 608                 tmp_map = new_lvl1_1;
 609                 for( int 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
 634         // Allocate Fractal table
 635         {
 636                 tPAddr  tmp = MM_AllocPhys();
 637                 Uint32  *table = MM_MapTemp(tmp);
 638
 639                 // Map table to last 4MiB of user space
 640                 new_lvl1_2[0x3FC] = tmp + 0*0x400 + 1;
 641                 new_lvl1_2[0x3FD] = tmp + 1*0x400 + 1;
 642                 new_lvl1_2[0x3FE] = tmp + 2*0x400 + 1;
 643                 new_lvl1_2[0x3FF] = tmp + 3*0x400 + 1;
 644
 645                 tmp_map = new_lvl1_1;
 646                  int    j = 0;
 647                 for( ; j < 512; j ++ )
 648                 {
 649                         if( j == 256 )
 650                                 tmp_map = &new_lvl1_2[-0x400];
 651                         if( (tmp_map[j*4] & 3) == 1 )
 652                         {
 653                                 table[j] = tmp_map[j*4] & PADDR_MASK_LVL1;// 0xFFFFFC00;
 654                                 table[j] |= 0x813;      // nG, Kernel Only, Small page, XN
 655                         }
 656                         else
 657                                 table[j] = 0;
 658                 }
 659                 // Fractal
 660                 table[j++] = (ret + 0x0000) | 0x813;
 661                 table[j++] = (ret + 0x1000) | 0x813;
 662                 // Clear the rest of the table
 663                 for( ; j < 1024; j ++ )
 664                         table[j] = 0;
 665
 666                 // Get kernel stack bottom
 667                 register Uint32 __SP asm("sp");
 668                 Uint32  sp = __SP & ~(MM_KSTACK_SIZE-1);
 669                 j = (sp / 0x1000) % 1024;
 670
 671                 // Copy stack pages
 672                 for(int num = MM_KSTACK_SIZE/PAGE_SIZE; num--; j ++, sp += PAGE_SIZE)
 673                 {
 674                         tVAddr  page;
 675                         void    *tmp_page;
 676
 677                         page = MM_AllocPhys();
 678 //                      Log("page = %P", page);
 679                         table[j] = page | 0x813;
 680
 681                         tmp_page = MM_MapTemp(page);
 682                         memcpy(tmp_page, (void*)sp, 0x1000);
 683                         MM_FreeTemp( tmp_page );
 684                 }
 685
 686                 MM_FreeTemp( table );
 687         }
 688
 689         MM_FreeTemp( new_lvl1_1 );
 690         MM_FreeTemp( new_lvl1_2 );
 691
 692 //      Log("MM_Clone: ret = %P", ret);
 693
 694         return ret;
 695 }
 696
 697 void MM_ClearUser(void)
 698 {
 699          int    i, j;
 700         const int       user_table_count = USER_STACK_TOP / (256*0x1000);
 701         Uint32  *cur = (void*)MM_TABLE0USER;
 702         Uint32  *tab;
 703
 704 //      MM_DumpTables(0, 0x80000000);
 705
 706 //      Log("user_table_count = %i (as opposed to %i)", user_table_count, 0x800-4);
 707
 708         for( i = 0; i < user_table_count; i ++ )
 709         {
 710                 switch( cur[i] & 3 )
 711                 {
 712                 case 0: break;  // Already unmapped
 713                 case 1: // Sub pages
 714                         tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 715                         for( j = 0; j < 1024; j ++ )
 716                         {
 717                                 switch( tab[j] & 3 )
 718                                 {
 719                                 case 0: break;  // Unmapped
 720                                 case 1:
 721                                         Log_Error("MMVirt", "TODO: Support large pages in MM_ClearUser");
 722                                         break;
 723                                 case 2:
 724                                 case 3:
 725                                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE-1) );
 726                                         break;
 727                                 }
 728                         }
 729                         MM_DerefPhys( cur[i] & ~(PAGE_SIZE-1) );
 730                         cur[i+0] = 0;
 731                         cur[i+1] = 0;
 732                         cur[i+2] = 0;
 733                         i += 3;
 734                         break;
 735                 case 2:
 736                 case 3:
 737                         Log_Error("MMVirt", "TODO: Implement sections/supersections in MM_ClearUser");
 738                         break;
 739                 }
 740                 cur[i] = 0;
 741         }
 742
 743         // Final block of 4 tables are KStack
 744         i = 0x800 - 4;
 745
 746         // Clear out unused stacks
 747         {
 748                 register Uint32 __SP asm("sp");
 749                  int    cur_stack_base = ((__SP & ~(MM_KSTACK_SIZE-1)) / PAGE_SIZE) % 1024;
 750
 751                 tab = (void*)(MM_TABLE1USER + i*256*sizeof(Uint32));
 752
 753                 // First 512 is the Table1 mapping + 2 for Table0 mapping
 754                 for( j = 512+2; j < 1024; j ++ )
 755                 {
 756                         // Skip current stack
 757                         if( j == cur_stack_base ) {
 758                                 j += (MM_KSTACK_SIZE / PAGE_SIZE) - 1;
 759                                 continue ;
 760                         }
 761                         if( !(tab[j] & 3) )     continue;
 762                         ASSERT( (tab[j] & 3) == 2 );
 763                         MM_DerefPhys( tab[j] & ~(PAGE_SIZE) );
 764                         tab[j] = 0;
 765                 }
 766         }
 767
 768
 769 //      MM_DumpTables(0, 0x80000000);
 770 }
 771
 772 void *MM_MapTemp(tPAddr PAddr)
 773 {
 774         tVAddr  ret;
 775         tMM_PageInfo    pi;
 776
 777         for( ret = MM_TMPMAP_BASE; ret < MM_TMPMAP_END - PAGE_SIZE; ret += PAGE_SIZE )
 778         {
 779                 if( MM_int_GetPageInfo(ret, &pi) == 0 )
 780                         continue;
 781
 782 //              Log("MapTemp %P at %p by %p", PAddr, ret, __builtin_return_address(0));
 783                 MM_RefPhys(PAddr);      // Counter the MM_Deallocate in FreeTemp
 784                 MM_Map(ret, PAddr);
 785
 786                 return (void*)ret;
 787         }
 788         Log_Warning("MMVirt", "MM_MapTemp: All slots taken");
 789         return 0;
 790 }
 791
 792 void MM_FreeTemp(void *Ptr)
 793 {
 794         tVAddr  VAddr = (tVAddr)Ptr;
 795         if( VAddr < MM_TMPMAP_BASE || VAddr >= MM_TMPMAP_END ) {
 796                 Log_Warning("MMVirt", "MM_FreeTemp: Passed an addr not from MM_MapTemp (%p)", VAddr);
 797                 return ;
 798         }
 799
 800         MM_Deallocate(VAddr);
 801 }
 802
 803 void *MM_MapHWPages(tPAddr PAddr, Uint NPages)
 804 {
 805         tVAddr  ret;
 806          int    i;
 807         tMM_PageInfo    pi;
 808
 809         ENTER("xPAddr iNPages", PAddr, NPages);
 810
 811         // Scan for a location
 812         for( ret = MM_HWMAP_BASE; ret < MM_HWMAP_END - NPages * PAGE_SIZE; ret += PAGE_SIZE )
 813         {
 814 //              LOG("checking %p", ret);
 815                 // Check if there is `NPages` free pages
 816                 for( i = 0; i < NPages; i ++ )
 817                 {
 818                         if( MM_int_GetPageInfo(ret + i*PAGE_SIZE, &pi) == 0 )
 819                                 break;
 820                 }
 821                 // Nope, jump to after the used page found and try again
 822 //              LOG("i = %i, ==? %i", i, NPages);
 823                 if( i != NPages ) {
 824                         ret += i * PAGE_SIZE;
 825                         continue ;
 826                 }
 827
 828                 // Map the pages
 829                 for( i = 0; i < NPages; i ++ )
 830                         MM_Map(ret+i*PAGE_SIZE, PAddr+i*PAGE_SIZE);
 831                 // and return
 832                 LEAVE('p', ret);
 833                 return (void*)ret;
 834         }
 835         Log_Warning("MMVirt", "MM_MapHWPages: No space for a %i page block", NPages);
 836         LEAVE('p', 0);
 837         return 0;
 838 }
 839
 840 void *MM_AllocDMA(int Pages, int MaxBits, tPAddr *PAddr)
 841 {
 842         tPAddr  phys;
 843         void    *ret;
 844
 845         phys = MM_AllocPhysRange(Pages, MaxBits);
 846         if(!phys) {
 847                 Log_Warning("MMVirt", "No space left for a %i page block (MM_AllocDMA)", Pages);
 848                 return 0;
 849         }
 850
 851         ret = MM_MapHWPages(phys, Pages);
 852         if( !ret ) {
 853                 MM_DerefPhys(phys);
 854                 return NULL;
 855         }
 856         if( PAddr )
 857                 *PAddr = phys;
 858
 859         return ret;
 860 }
 861
 862 void MM_UnmapHWPages(tVAddr Vaddr, Uint Number)
 863 {
 864         Log_Error("MMVirt", "TODO: Implement MM_UnmapHWPages");
 865 }
 866
 867 tVAddr MM_NewKStack(int bShared)
 868 {
 869         tVAddr  min_addr, max_addr;
 870         tVAddr  addr, ofs;
 871
 872         if( bShared ) {
 873                 min_addr = MM_GLOBALSTACKS;
 874                 max_addr = MM_GLOBALSTACKS_END;
 875         }
 876         else {
 877                 min_addr = MM_KSTACK_BASE;
 878                 max_addr = MM_KSTACK_END;
 879         }
 880
 881         // Locate a free slot
 882         for( addr = min_addr; addr < max_addr; addr += MM_KSTACK_SIZE )
 883         {
 884                 tMM_PageInfo    pi;
 885                 if( MM_int_GetPageInfo(addr+MM_KSTACK_SIZE-PAGE_SIZE, &pi) )    break;
 886         }
 887
 888         // Check for an error
 889         if(addr >= max_addr) {
 890                 return 0;
 891         }
 892
 893         // 1 guard page
 894         for( ofs = PAGE_SIZE; ofs < MM_KSTACK_SIZE; ofs += PAGE_SIZE )
 895         {
 896                 if( MM_Allocate(addr + ofs) == 0 )
 897                 {
 898                         while(ofs)
 899                         {
 900                                 ofs -= PAGE_SIZE;
 901                                 MM_Deallocate(addr + ofs);
 902                         }
 903                         Log_Warning("MMVirt", "MM_NewKStack: Unable to allocate");
 904                         return 0;
 905                 }
 906         }
 907         return addr + ofs;
 908 }
 909
 910 tVAddr MM_NewUserStack(void)
 911 {
 912         tVAddr  addr, ofs;
 913
 914         addr = USER_STACK_TOP - USER_STACK_SIZE;
 915         if( MM_GetPhysAddr( (void*)(addr + PAGE_SIZE) ) ) {
 916                 Log_Error("MMVirt", "Unable to create initial user stack, addr %p taken",
 917                         addr + PAGE_SIZE
 918                         );
 919                 MM_DumpTables(0,KERNEL_BASE);
 920                 return 0;
 921         }
 922
 923         // 1 guard page
 924         for( ofs = PAGE_SIZE; ofs < USER_STACK_SIZE; ofs += PAGE_SIZE )
 925         {
 926                 tPAddr  rv;
 927                 if(ofs >= USER_STACK_SIZE - USER_STACK_COMM)
 928                         rv = MM_Allocate(addr + ofs);
 929                 else
 930                         rv = MM_AllocateZero(addr + ofs);
 931                 if(rv == 0)
 932                 {
 933                         while(ofs)
 934                         {
 935                                 ofs -= PAGE_SIZE;
 936                                 MM_Deallocate(addr + ofs);
 937                         }
 938                         Log_Warning("MMVirt", "MM_NewUserStack: Unable to allocate");
 939                         return 0;
 940                 }
 941                 MM_SetFlags(addr+ofs, 0, MM_PFLAG_KERNEL);
 942         }
 943         Log("Return %p", addr + ofs);
 944 //      MM_DumpTables(0, 0x80000000);
 945         return addr + ofs;
 946 }
 947
 948 void MM_int_DumpTableEnt(tVAddr Start, size_t Len, tMM_PageInfo *Info)
 949 {
 950         if( giMM_ZeroPage && Info->PhysAddr == giMM_ZeroPage )
 951         {
 952                 Debug("0x%08x => %8s - 0x%7x D%i %x %s %s",
 953                         Start, "ZERO", Len,
 954                         Info->Domain, Info->AP,
 955                         Info->bExecutable ? " X" : "nX",
 956                         Info->bGlobal ? " G" : "nG"
 957                         );
 958         }
 959         else
 960         {
 961                 void    *node;
 962                 MM_GetPageNode(Info->PhysAddr, &node);
 963                 Debug("0x%08x => %8x - 0x%7x D%i %x %s %s %p",
 964                         Start, Info->PhysAddr-Len, Len,
 965                         Info->Domain, Info->AP,
 966                         Info->bExecutable ? " X" : "nX",
 967                         Info->bGlobal ? " G" : "nG",
 968                         node
 969                         );
 970         }
 971 }
 972
 973 void MM_DumpTables(tVAddr Start, tVAddr End)
 974 {
 975         tVAddr  range_start = 0, addr;
 976         tMM_PageInfo    pi, pi_old;
 977          int    i = 0, inRange=0;
 978
 979         memset(&pi_old, 0, sizeof(pi_old));
 980
 981         Debug("Page Table Dump (%p to %p):", Start, End);
 982         range_start = Start;
 983         for( addr = Start; i == 0 || (addr && addr < End); i = 1 )
 984         {
 985                  int    rv;
 986 //              Log("addr = %p", addr);
 987                 rv = MM_int_GetPageInfo(addr, &pi);
 988                 if( rv
 989                  || pi.Size != pi_old.Size
 990                  || pi.Domain != pi_old.Domain
 991                  || pi.AP != pi_old.AP
 992                  || pi.bGlobal != pi_old.bGlobal
 993                  || pi_old.PhysAddr != pi.PhysAddr )
 994                 {
 995                         if(inRange) {
 996                                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi_old);
 997                         }
 998                         addr &= ~((1 << pi.Size)-1);
 999                         range_start = addr;
1000                 }
1001
1002                 pi_old = pi;
1003                 // Handle the zero page
1004                 if( !giMM_ZeroPage || pi_old.Size != 12 || pi_old.PhysAddr != giMM_ZeroPage )
1005                         pi_old.PhysAddr += 1 << pi_old.Size;
1006                 addr += 1 << pi_old.Size;
1007                 inRange = (rv == 0);
1008         }
1009         if(inRange)
1010                 MM_int_DumpTableEnt(range_start, addr - range_start, &pi);
1011         Debug("Done");
1012 }
1013
1014 // NOTE: Runs in abort context, not much difference, just a smaller stack
1015 void MM_PageFault(Uint32 PC, Uint32 Addr, Uint32 DFSR, int bPrefetch, Uint32 SystemLR, Uint32 UserLR)
1016 {
1017          int    rv;
1018         tMM_PageInfo    pi;
1019
1020         rv = MM_int_GetPageInfo(Addr, &pi);
1021
1022         // Check for COW
1023         if( rv == 0 &&  pi.AP == AP_RO_BOTH )
1024         {
1025                 pi.AP = AP_RW_BOTH;
1026                 if( giMM_ZeroPage && pi.PhysAddr == giMM_ZeroPage )
1027                 {
1028                         tPAddr  newpage;
1029                         newpage = MM_AllocPhys();
1030                         if( !newpage ) {
1031                                 Log_Error("MMVirt", "Unable to allocate new page for COW of ZERO");
1032                                 for(;;);
1033                         }
1034
1035                         #if TRACE_COW
1036                         Log_Notice("MMVirt", "COW %p caused by %p, ZERO duped to %P (RefCnt(%i)--)", Addr, PC,
1037                                 newpage, MM_GetRefCount(pi.PhysAddr));
1038                         #endif
1039
1040                         MM_DerefPhys(pi.PhysAddr);
1041                         pi.PhysAddr = newpage;
1042                         pi.AP = AP_RW_BOTH;
1043                         MM_int_SetPageInfo(Addr, &pi);
1044
1045                         memset( (void*)(Addr & ~(PAGE_SIZE-1)), 0, PAGE_SIZE );
1046
1047                         return ;
1048                 }
1049                 else if( MM_GetRefCount(pi.PhysAddr) > 1 )
1050                 {
1051                         // Duplicate the page
1052                         tPAddr  newpage;
1053                         void    *dst, *src;
1054
1055                         newpage = MM_AllocPhys();
1056                         if(!newpage) {
1057                                 Log_Error("MMVirt", "Unable to allocate new page for COW");
1058                                 for(;;);
1059                         }
1060                         dst = MM_MapTemp(newpage);
1061                         src = (void*)(Addr & ~(PAGE_SIZE-1));
1062                         memcpy( dst, src, PAGE_SIZE );
1063                         MM_FreeTemp( dst );
1064
1065                         #if TRACE_COW
1066                         Log_Notice("MMVirt", "COW %p caused by %p, %P duped to %P (RefCnt(%i)--)", Addr, PC,
1067                                 pi.PhysAddr, newpage, MM_GetRefCount(pi.PhysAddr));
1068                         #endif
1069
1070                         MM_DerefPhys(pi.PhysAddr);
1071                         pi.PhysAddr = newpage;
1072                 }
1073                 #if TRACE_COW
1074                 else {
1075                         Log_Notice("MMVirt", "COW %p caused by %p, took last reference to %P",
1076                                 Addr, PC, pi.PhysAddr);
1077                 }
1078                 #endif
1079                 // Unset COW
1080                 pi.AP = AP_RW_BOTH;
1081                 MM_int_SetPageInfo(Addr, &pi);
1082                 return ;
1083         }
1084
1085
1086         Log_Error("MMVirt", "Code at %p accessed %p (DFSR = 0x%x)%s", PC, Addr, DFSR,
1087                 (bPrefetch ? " - Prefetch" : "")
1088                 );
1089         Log_Error("MMVirt", "- User LR = 0x%x, System LR = 0x%x", UserLR, SystemLR);
1090         const char * const dfsr_errors[] = {
1091                 /* 00000 */ "-", "Alignment Fault",
1092                 /* 00010 */ "Debug event", "Access Flag (Section)",
1093                 /* 00100 */ "Instr Cache Maint", "Translation (Section)",
1094                 /* 00110 */ "Access Flag (Page)", "Translation (Page)",
1095                 /* 01000 */ "Sync. External abort", "Domain (Section)",
1096                 /* 01010 */ "-", "Domain (Page)",
1097                 /* 01100 */ "Table Walk sync ext (lvl 1)", "Permission (Section)",
1098                 /* 01110 */ "Table Walk sync ext (lvl 2)", "Permission (Page)",
1099                 // 0b10000
1100                 /* 10000 */ "-", "-",
1101                 /* 10010 */ "-", "-",
1102                 /* 10100 */ "IMPL (Lockdown)", "-",
1103                 /* 10110 */ "Async. Extern. Abort", "-",
1104                 /* 11000 */ "Mem. access async pairity error", "Mem. access async pairity error",
1105                 /* 11010 */ "IMPL (Coprocessor abort)", "-",
1106                 /* 11100 */ "Table Walk Sync parity (lvl 1)", "-",
1107                 /* 11110 */ "Table Walk Sync parity (lvl 2)", "-"
1108                 };
1109          int    errcode = (DFSR & 0xF) | (((DFSR >> 10) & 1) << 4);
1110         Log_Error("MMVirt", "- Errcode 0b%05b", errcode);
1111         Log_Error("MMVirt", "- Dom %i %s %s",
1112                 (DFSR >> 4) & 0xF, (DFSR & 0x800 ? "Write": "Read"),
1113                 dfsr_errors[errcode]
1114                 );
1115         Log_Error("MMVirt", "- AP=%i(%s) %s", pi.AP, caAPValueNames[pi.AP], pi.bExecutable ? " Executable":"");
1116         if( Addr < 0x80000000 )
1117                 MM_DumpTables(0, 0x80000000);
1118         else
1119                 MM_DumpTables(0x80000000, -1);
1120         for(;;);
1121 }
1122