reactos/sdk/lib/ppcmmu/mmuutil.c

   1 #include "ppcmmu/mmu.h"
   2 #include "ppcmmu/mmuutil.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         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  22         "mtmsr 6\n\t"
  23         "isync\n\t"
  24         "sync\n\t"
  25         "lwz   3,0(3)\n\t"    /* Get actual value at phys addr r3 */
  26         "mtmsr 5\n\t"
  27         "isync\n\t"
  28         "sync\n\t"
  29         "lwz   0,0(1)\n\t"
  30         "addi  1,1,16\n\t"
  31         "mtlr  0\n\t"
  32         "blr"
  33     );
  34
  35 __asm__("\t.globl GetPhysHalf\n"
  36         "GetPhysHalf:\t\n"
  37         "mflr  0\n\t"
  38         "stwu  0,-16(1)\n\t"
  39         "mfmsr 5\n\t"
  40         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  41         "mtmsr 6\n\t"
  42         "isync\n\t"
  43         "sync\n\t"
  44         "lhz   3,0(3)\n\t"    /* Get actual value at phys addr r3 */
  45         "mtmsr 5\n\t"
  46         "isync\n\t"
  47         "sync\n\t"
  48         "lwz   0,0(1)\n\t"
  49         "addi  1,1,16\n\t"
  50         "mtlr  0\n\t"
  51         "blr"
  52     );
  53
  54 __asm__("\t.globl GetPhysByte\n"
  55         "GetPhysByte:\t\n"
  56         "mflr  0\n\t"
  57         "stwu  0,-16(1)\n\t"
  58         "mfmsr 5\n\t"
  59         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  60         "mtmsr 6\n\t"
  61         "isync\n\t"
  62         "sync\n\t"
  63         "lbz   3,0(3)\n\t"    /* Get actual value at phys addr r3 */
  64         "mtmsr 5\n\t"
  65         "isync\n\t"
  66         "sync\n\t"
  67         "lwz   0,0(1)\n\t"
  68         "addi  1,1,16\n\t"
  69         "mtlr  0\n\t"
  70         "blr"
  71     );
  72
  73 __asm__("\t.globl SetPhys\n"
  74         "SetPhys:\t\n"
  75         "mflr  0\n\t"
  76         "stwu  0,-16(1)\n\t"
  77         "mfmsr 5\n\t"
  78         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
  79         "mtmsr 6\n\t"
  80         "sync\n\t"
  81         "eieio\n\t"
  82         "stw   4,0(3)\n\t"    /* Set actual value at phys addr r3 */
  83         "dcbst 0,3\n\t"
  84         "mtmsr 5\n\t"
  85         "sync\n\t"
  86         "eieio\n\t"
  87         "mr    3,4\n\t"
  88         "lwz   0,0(1)\n\t"
  89         "addi  1,1,16\n\t"
  90         "mtlr  0\n\t"
  91         "blr"
  92     );
  93
  94 __asm__("\t.globl SetPhysHalf\n"
  95         "SetPhysHalf:\t\n"
  96         "mflr  0\n\t"
  97         "stwu  0,-16(1)\n\t"
  98         "mfmsr 5\n\t"
  99         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
 100         "mtmsr 6\n\t"
 101         "sync\n\t"
 102         "eieio\n\t"
 103         "sth   4,0(3)\n\t"    /* Set actual value at phys addr r3 */
 104         "dcbst 0,3\n\t"
 105         "mtmsr 5\n\t"
 106         "sync\n\t"
 107         "eieio\n\t"
 108         "mr    3,4\n\t"
 109         "lwz   0,0(1)\n\t"
 110         "addi  1,1,16\n\t"
 111         "mtlr  0\n\t"
 112         "blr"
 113     );
 114
 115 __asm__("\t.globl SetPhysByte\n"
 116         "SetPhysByte:\t\n"
 117         "mflr  0\n\t"
 118         "stwu  0,-16(1)\n\t"
 119         "mfmsr 5\n\t"
 120         "andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
 121         "mtmsr 6\n\t"
 122         "sync\n\t"
 123         "eieio\n\t"
 124         "stb   4,0(3)\n\t"    /* Set actual value at phys addr r3 */
 125         "dcbst 0,3\n\t"
 126         "mtmsr 5\n\t"
 127         "sync\n\t"
 128         "eieio\n\t"
 129         "mr    3,4\n\t"
 130         "lwz   0,0(1)\n\t"
 131         "addi  1,1,16\n\t"
 132         "mtlr  0\n\t"
 133         "blr"
 134     );
 135
 136 inline int GetSR(int n) {
 137     register int res = 0;
 138     switch( n ) {
 139     case 0:
 140         __asm__("mfsr %0,0" : "=r" (res));
 141         break;
 142     case 1:
 143         __asm__("mfsr %0,1" : "=r" (res));
 144         break;
 145     case 2:
 146         __asm__("mfsr %0,2" : "=r" (res));
 147         break;
 148     case 3:
 149         __asm__("mfsr %0,3" : "=r" (res));
 150         break;
 151     case 4:
 152         __asm__("mfsr %0,4" : "=r" (res));
 153         break;
 154     case 5:
 155         __asm__("mfsr %0,5" : "=r" (res));
 156         break;
 157     case 6:
 158         __asm__("mfsr %0,6" : "=r" (res));
 159         break;
 160     case 7:
 161         __asm__("mfsr %0,7" : "=r" (res));
 162         break;
 163     case 8:
 164         __asm__("mfsr %0,8" : "=r" (res));
 165         break;
 166     case 9:
 167         __asm__("mfsr %0,9" : "=r" (res));
 168         break;
 169     case 10:
 170         __asm__("mfsr %0,10" : "=r" (res));
 171         break;
 172     case 11:
 173         __asm__("mfsr %0,11" : "=r" (res));
 174         break;
 175     case 12:
 176         __asm__("mfsr %0,12" : "=r" (res));
 177         break;
 178     case 13:
 179         __asm__("mfsr %0,13" : "=r" (res));
 180         break;
 181     case 14:
 182         __asm__("mfsr %0,14" : "=r" (res));
 183         break;
 184     case 15:
 185         __asm__("mfsr %0,15" : "=r" (res));
 186         break;
 187     }
 188     return res;
 189 }
 190
 191 inline void SetSR(int n, int val) {
 192     switch( n ) {
 193     case 0:
 194         __asm__("mtsr 0,%0" : : "r" (val));
 195         break;
 196     case 1:
 197         __asm__("mtsr 1,%0" : : "r" (val));
 198         break;
 199     case 2:
 200         __asm__("mtsr 2,%0" : : "r" (val));
 201         break;
 202     case 3:
 203         __asm__("mtsr 3,%0" : : "r" (val));
 204         break;
 205     case 4:
 206         __asm__("mtsr 4,%0" : : "r" (val));
 207         break;
 208     case 5:
 209         __asm__("mtsr 5,%0" : : "r" (val));
 210         break;
 211     case 6:
 212         __asm__("mtsr 6,%0" : : "r" (val));
 213         break;
 214     case 7:
 215         __asm__("mtsr 7,%0" : : "r" (val));
 216         break;
 217     case 8:
 218         __asm__("mtsr 8,%0" : : "r" (val));
 219         break;
 220     case 9:
 221         __asm__("mtsr 9,%0" : : "r" (val));
 222         break;
 223     case 10:
 224         __asm__("mtsr 10,%0" : : "r" (val));
 225         break;
 226     case 11:
 227         __asm__("mtsr 11,%0" : : "r" (val));
 228         break;
 229     case 12:
 230         __asm__("mtsr 12,%0" : : "r" (val));
 231         break;
 232     case 13:
 233         __asm__("mtsr 13,%0" : : "r" (val));
 234         break;
 235     case 14:
 236         __asm__("mtsr 14,%0" : : "r" (val));
 237         break;
 238     case 15:
 239         __asm__("mtsr 15,%0" : : "r" (val));
 240         break;
 241     }
 242 }
 243
 244 void GetBat( int bat, int inst, int *batHi, int *batLo ) {
 245     register int bh asm("r3"), bl asm("r4");
 246     if( inst ) {
 247         switch( bat ) {
 248         case 0:
 249             __asm__("mfibatu 3,0");
 250             __asm__("mfibatl 4,0");
 251             break;
 252         case 1:
 253             __asm__("mfibatu 3,1");
 254             __asm__("mfibatl 4,1");
 255             break;
 256         case 2:
 257             __asm__("mfibatu 3,2");
 258             __asm__("mfibatl 4,2");
 259             break;
 260         case 3:
 261             __asm__("mfibatu 3,3");
 262             __asm__("mfibatl 4,3");
 263             break;
 264         }
 265     } else {
 266         switch( bat ) {
 267         case 0:
 268             __asm__("mfdbatu 3,0");
 269             __asm__("mfdbatl 4,0");
 270             break;
 271         case 1:
 272             __asm__("mfdbatu 3,1");
 273             __asm__("mfdbatl 4,1");
 274             break;
 275         case 2:
 276             __asm__("mfdbatu 3,2");
 277             __asm__("mfdbatl 4,2");
 278             break;
 279         case 3:
 280             __asm__("mfdbatu 3,3");
 281             __asm__("mfdbatl 4,3");
 282             break;
 283         }
 284     }
 285     *batHi = bh;
 286     *batLo = bl;
 287 }
 288
 289 #define BATSET(n,t) \
 290  case n: __asm__("mt" #t "batu " #n ",%0\n\tmt" #t "batl " #n ",%1" \
 291  : : "r" (batHi), "r" (batLo)); break;
 292
 293 void SetBat( int bat, int inst, int batHi, int batLo ) {
 294     if( inst ) {
 295         switch( bat ) {
 296             BATSET(0,i);
 297             BATSET(1,i);
 298             BATSET(2,i);
 299             BATSET(3,i);
 300         }
 301     } else {
 302         switch( bat ) {
 303             BATSET(0,d);
 304             BATSET(1,d);
 305             BATSET(2,d);
 306             BATSET(3,d);
 307         }
 308     }
 309     __asm__("isync\n\tsync");
 310 }
 311
 312 inline int GetSDR1() {
 313     register int res asm("r3");
 314     __asm__("mfsdr1 3");
 315     return res;
 316 }
 317
 318 inline void SetSDR1( int sdr ) {
 319     int i,j;
 320     __asm__("mtsdr1 3");
 321     __asm__("sync");
 322     __asm__("isync");
 323
 324     for( i = 0; i < 256; i++ ) {
 325         j = i << 12;
 326         __asm__("tlbie %0,0" : : "r" (j));
 327     }
 328     __asm__("eieio");
 329     __asm__("tlbsync");
 330     __asm__("ptesync");
 331 }
 332
 333 inline int BatTranslate( int batu, int batl, int virt ) {
 334     int mask;
 335     if(batu & 0x3fc)
 336     {
 337         mask = ~(0x1ffff | ((batu & 0x3fc)>>2)<<17);
 338         if((batu & 2) && ((batu & mask) == (virt & mask)))
 339             return (batl & mask) | (virt & ~mask);
 340     } else {
 341         mask = ~(0x1ffff | (batl << 17));
 342         if(!(batl & 0x40) || ((batu & mask) != (virt & mask)))
 343             return (batl & mask) | (virt & ~mask);
 344     }
 345     return -1;
 346 }
 347
 348 inline int BatHit( int batu, int batl, int virt ) {
 349     return BatTranslate( batu, batl, virt ) != -1;
 350 }
 351
 352 /* translate address */
 353 int PpcVirt2phys( vaddr_t virt, int inst ) {
 354     int msr = GetMSR();
 355     int txmask = inst ? 0x20 : 0x10;
 356     int i, bath, batl, sr, sdr1, physbase, vahi, valo;
 357     int npteg, hash, hashmask, ptehi, ptelo, ptegaddr;
 358     int vsid, pteh, ptevsid, pteapi;
 359
 360     if( msr & txmask ) {
 361         sr = GetSR( virt >> 28 );
 362         vsid = sr & 0xfffffff;
 363         vahi = vsid >> 4;
 364         valo = (vsid << 28) | (virt & 0xfffffff);
 365         if( sr & 0x80000000 ) {
 366             return valo;
 367         }
 368
 369         for( i = 0; i < 4; i++ ) {
 370             GetBat( i, inst, &bath, &batl );
 371             if( BatHit( bath, batl, virt ) ) {
 372                 return BatTranslate( bath, batl, virt );
 373             }
 374         }
 375
 376         sdr1 = GetSDR1();
 377
 378         physbase = sdr1 & ~0xffff;
 379         hashmask = ((sdr1 & 0x1ff) << 10) | 0x3ff;
 380         hash = (vsid & 0x7ffff) ^ ((valo >> 12) & 0xffff);
 381         npteg = hashmask + 1;
 382
 383         for( pteh = 0; pteh < 0x80; pteh += 64, hash ^= 0x7ffff ) {
 384             ptegaddr = ((hashmask & hash) * 64) + physbase;
 385
 386             for( i = 0; i < 8; i++ ) {
 387                 ptehi = GetPhys( ptegaddr + (i * 8) );
 388                 ptelo = GetPhys( ptegaddr + (i * 8) + 4 );
 389
 390                 ptevsid = (ptehi >> 7) & 0xffffff;
 391                 pteapi = ptehi & 0x3f;
 392
 393                 if( (ptehi & 64) != pteh ) continue;
 394                 if( ptevsid != (vsid & 0xffffff) ) continue;
 395                 if( pteapi != ((virt >> 22) & 0x3f) ) continue;
 396
 397                 return (ptelo & 0xfffff000) | (virt & 0xfff);
 398             }
 399         }
 400         return -1;
 401     } else {
 402         return virt;
 403     }
 404 }
 405
 406 int PtegNumber(vaddr_t virt, int hfun)
 407 {
 408     int sr = GetSR( (virt >> 28) & 0xf );
 409     int vsid = sr & PPC_VSID_MASK;
 410     return ((((vsid & 0x7ffff) ^ ((virt >> 12) & 0xffff)) ^ (hfun ? -1 : 0)) & ((HTABSIZ - 1) >> 3) & 0x3ff);
 411 }