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