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