reactos/boot/freeldr/freeldr/arch/powerpc/mmu.c

   1 #include <freeldr.h>
   2 #include "mmu.h"
   3
   4 inline int GetMSR() {
   5     register int res asm ("r3");
   6     __asm__("mfmsr 3");
   7     return res;
   8 }
   9
  10 inline int GetDEC() {
  11     register int res asm ("r3");
  12     __asm__("mfdec 3");
  13     return res;
  14 }
  15
  16 __asm__("\t.globl GetPhys\n"
  17         "GetPhys:\t\n"
  18         "mflr  0\n\t"
  19         "stwu  0,-16(1)\n\t"
  20         "mfmsr 5\n\t"
  21         "xori  3,3,4\n\t"     /* Undo effects of LE without swapping */
  22         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  23         "mtmsr 6\n\t"
  24         "isync\n\t"
  25         "sync\n\t"
  26         "lwz   3,0(3)\n\t"    /* Get actual value at phys addr r3 */
  27         "mtmsr 5\n\t"
  28         "isync\n\t"
  29         "sync\n\t"
  30         "lwz   0,0(1)\n\t"
  31         "addi  1,1,16\n\t"
  32         "mtlr  0\n\t"
  33         "blr"
  34     );
  35
  36 __asm__("\t.globl SetPhys\n"
  37         "SetPhys:\t\n"
  38         "mflr  0\n\t"
  39         "stwu  0,-16(1)\n\t"
  40         "mfmsr 5\n\t"
  41         "xori  3,3,4\n\t"     /* Undo effects of LE without swapping */
  42         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  43         "mtmsr 6\n\t"
  44         "sync\n\t"
  45         "eieio\n\t"
  46         "stw   4,0(3)\n\t"    /* Set actual value at phys addr r3 */
  47         "dcbst 0,3\n\t"
  48         "mtmsr 5\n\t"
  49         "sync\n\t"
  50         "eieio\n\t"
  51         "mr    3,4\n\t"
  52         "lwz   0,0(1)\n\t"
  53         "addi  1,1,16\n\t"
  54         "mtlr  0\n\t"
  55         "blr"
  56     );
  57
  58 __asm__("\t.globl SetPhysByte\n"
  59         "SetPhysByte:\t\n"
  60         "mflr  0\n\t"
  61         "stwu  0,-16(1)\n\t"
  62         "mfmsr 5\n\t"
  63         "xori  3,3,7\n\t"     /* Undo effects of LE without swapping */
  64         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  65         "mtmsr 6\n\t"
  66         "sync\n\t"
  67         "eieio\n\t"
  68         "stb   4,0(3)\n\t"    /* Set actual value at phys addr r3 */
  69         "dcbst 0,3\n\t"
  70         "mtmsr 5\n\t"
  71         "sync\n\t"
  72         "eieio\n\t"
  73         "mr    3,4\n\t"
  74         "lwz   0,0(1)\n\t"
  75         "addi  1,1,16\n\t"
  76         "mtlr  0\n\t"
  77         "blr"
  78     );
  79
  80 inline int GetSR(int n) {
  81     register int res asm ("r3");
  82     switch( n ) {
  83     case 0:
  84         __asm__("mfsr 3,0");
  85         break;
  86     case 1:
  87         __asm__("mfsr 3,1");
  88         break;
  89     case 2:
  90         __asm__("mfsr 3,2");
  91         break;
  92     case 3:
  93         __asm__("mfsr 3,3");
  94         break;
  95     case 4:
  96         __asm__("mfsr 3,4");
  97         break;
  98     case 5:
  99         __asm__("mfsr 3,5");
 100         break;
 101     case 6:
 102         __asm__("mfsr 3,6");
 103         break;
 104     case 7:
 105         __asm__("mfsr 3,7");
 106         break;
 107     case 8:
 108         __asm__("mfsr 3,8");
 109         break;
 110     case 9:
 111         __asm__("mfsr 3,9");
 112         break;
 113     case 10:
 114         __asm__("mfsr 3,10");
 115         break;
 116     case 11:
 117         __asm__("mfsr 3,11");
 118         break;
 119     case 12:
 120         __asm__("mfsr 3,12");
 121         break;
 122     case 13:
 123         __asm__("mfsr 3,13");
 124         break;
 125     case 14:
 126         __asm__("mfsr 3,14");
 127         break;
 128     case 15:
 129         __asm__("mfsr 3,15");
 130         break;
 131     }
 132     return res;
 133 }
 134
 135 inline void GetBat( int bat, int inst, int *batHi, int *batLo ) {
 136     register int bh asm("r3"), bl asm("r4");
 137     if( inst ) {
 138         switch( bat ) {
 139         case 0:
 140             __asm__("mfibatu 3,0");
 141             __asm__("mfibatl 4,0");
 142             break;
 143         case 1:
 144             __asm__("mfibatu 3,1");
 145             __asm__("mfibatl 4,1");
 146             break;
 147         case 2:
 148             __asm__("mfibatu 3,2");
 149             __asm__("mfibatl 4,2");
 150             break;
 151         case 3:
 152             __asm__("mfibatu 3,3");
 153             __asm__("mfibatl 4,3");
 154             break;
 155         }
 156     } else {
 157         switch( bat ) {
 158         case 0:
 159             __asm__("mfdbatu 3,0");
 160             __asm__("mfdbatl 4,0");
 161             break;
 162         case 1:
 163             __asm__("mfdbatu 3,1");
 164             __asm__("mfdbatl 4,1");
 165             break;
 166         case 2:
 167             __asm__("mfdbatu 3,2");
 168             __asm__("mfdbatl 4,2");
 169             break;
 170         case 3:
 171             __asm__("mfdbatu 3,3");
 172             __asm__("mfdbatl 4,3");
 173             break;
 174         }
 175     }
 176     *batHi = bh;
 177     *batLo = bl;
 178 }
 179
 180 inline void SetBat( int bat, int inst, int batHi, int batLo ) {
 181     register int bh asm("r3"), bl asm("r4");
 182     bh = batHi;
 183     bl = batLo;
 184     if( inst ) {
 185         switch( bat ) {
 186         case 0:
 187             __asm__("mtibatu 0,3");
 188             __asm__("mtibatl 0,4");
 189             break;
 190         case 1:
 191             __asm__("mtibatu 1,3");
 192             __asm__("mtibatl 1,4");
 193             break;
 194         case 2:
 195             __asm__("mtibatu 2,3");
 196             __asm__("mtibatl 2,4");
 197             break;
 198         case 3:
 199             __asm__("mtibatu 3,3");
 200             __asm__("mtibatl 3,4");
 201             break;
 202         }
 203     } else {
 204         switch( bat ) {
 205         case 0:
 206             __asm__("mtdbatu 0,3");
 207             __asm__("mtdbatl 0,4");
 208             break;
 209         case 1:
 210             __asm__("mtdbatu 1,3");
 211             __asm__("mtdbatl 1,4");
 212             break;
 213         case 2:
 214             __asm__("mtdbatu 2,3");
 215             __asm__("mtdbatl 2,4");
 216             break;
 217         case 3:
 218             __asm__("mtdbatu 3,3");
 219             __asm__("mtdbatl 3,4");
 220             break;
 221         }
 222     }
 223     __asm__("isync\n\tsync");
 224 }
 225
 226 inline int GetSDR1() {
 227     register int res asm("r3");
 228     __asm__("mfsdr1 3");
 229     return res;
 230 }
 231
 232 inline void SetSDR1( int sdr ) {
 233     __asm__("mtsdr1 3");
 234 }
 235
 236 inline int BatHit( int bath, int batl, int virt ) {
 237     int mask = 0xfffe0000 & ~((batl & 0x3f) << 17);
 238     return (batl & 0x40) && ((virt & mask) == (bath & mask));
 239 }
 240
 241 inline int BatTranslate( int bath, int batl, int virt ) {
 242     return (virt & 0x007fffff) | (batl & 0xfffe0000);
 243 }
 244
 245 /* translate address */
 246 int PpcVirt2phys( int virt, int inst ) {
 247     int msr = GetMSR();
 248     int txmask = inst ? 0x20 : 0x10;
 249     int i, bath, batl, sr, sdr1, physbase, vahi, valo;
 250     int npteg, hash, hashmask, ptehi, ptelo, ptegaddr;
 251     int vsid, pteh, ptevsid, pteapi;
 252
 253     if( msr & txmask ) {
 254         sr = GetSR( virt >> 28 );
 255         vsid = sr & 0xfffffff;
 256         vahi = vsid >> 4;
 257         valo = (vsid << 28) | (virt & 0xfffffff);
 258         if( sr & 0x80000000 ) {
 259             return valo;
 260         }
 261
 262         for( i = 0; i < 4; i++ ) {
 263             GetBat( i, inst, &bath, &batl );
 264             if( BatHit( bath, batl, virt ) ) {
 265                 return BatTranslate( bath, batl, virt );
 266             }
 267         }
 268
 269         sdr1 = GetSDR1();
 270
 271         physbase = sdr1 & ~0xffff;
 272         hashmask = ((sdr1 & 0x1ff) << 10) | 0x3ff;
 273         hash = (vsid & 0x7ffff) ^ ((valo >> 12) & 0xffff);
 274         npteg = hashmask + 1;
 275
 276         for( pteh = 0; pteh < 0x80; pteh += 64, hash ^= 0x7ffff ) {
 277             ptegaddr = ((hashmask & hash) * 64) + physbase;
 278
 279             for( i = 0; i < 8; i++ ) {
 280                 ptehi = GetPhys( ptegaddr + (i * 8) );
 281                 ptelo = GetPhys( ptegaddr + (i * 8) + 4 );
 282
 283                 ptevsid = (ptehi >> 7) & 0xffffff;
 284                 pteapi = ptehi & 0x3f;
 285
 286                 if( (ptehi & 64) != pteh ) continue;
 287                 if( ptevsid != (vsid & 0xffffff) ) continue;
 288                 if( pteapi != ((virt >> 22) & 0x3f) ) continue;
 289
 290                 return (ptelo & 0xfffff000) | (virt & 0xfff);
 291             }
 292         }
 293         return -1;
 294     } else {
 295         return virt;
 296     }
 297 }
 298
 299 /* Add a new page table entry for the indicated mapping */
 300 BOOLEAN InsertPageEntry( int virt, int phys, int slot, int _sdr1 ) {
 301     int i, ptehi, ptelo;
 302     int sdr1 = _sdr1 ? _sdr1 : GetSDR1();
 303     int sr = GetSR( (virt >> 28) & 0xf );
 304     int vsid = sr & 0xfffffff;
 305     int physbase = sdr1 & ~0xffff;
 306     int hashmask = ((sdr1 & 0x1ff) << 10) | 0x3ff;
 307     int valo = (vsid << 28) | (virt & 0xfffffff);
 308     int hash = (vsid & 0x7ffff) ^ ((valo >> 12) & 0xffff);
 309     int ptegaddr = ((hashmask & hash) * 64) + physbase;
 310
 311     for( i = 0; i < 8; i++ ) {
 312         ptehi = GetPhys( ptegaddr + (i * 8) );
 313
 314         if( (slot != i) && (ptehi & 0x80000000) ) continue;
 315
 316         ptehi = (1 << 31) | (vsid << 7) | ((virt >> 22) & 0x3f);
 317         ptelo = phys & ~0xfff;
 318
 319         SetPhys( ptegaddr + (i * 8), ptehi );
 320         SetPhys( ptegaddr + (i * 8) + 4, ptelo );
 321
 322         __asm__("ptesync");
 323         __asm__("tlbie %0,0" : : "r" (virt));
 324         __asm__("eieio");
 325         __asm__("tlbsync");
 326         __asm__("ptesync");
 327
 328         return TRUE;
 329     }
 330
 331     return FALSE;
 332 }