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