reactos/lib/ppcmmu/mmuutil.c
Timo Kreuzer 6afbc8f483 Hopefully create a branch and not destroy the svn repository.
svn path=/branches/reactos-yarotows/; revision=45219
2010-01-23 23:25:04 +00:00

412 lines
8.1 KiB
C

#include "ppcmmu/mmu.h"
#include "ppcmmu/mmuutil.h"
inline int GetMSR() {
register int res asm ("r3");
__asm__("mfmsr 3");
return res;
}
inline int GetDEC() {
register int res asm ("r3");
__asm__("mfdec 3");
return res;
}
__asm__("\t.globl GetPhys\n"
"GetPhys:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"isync\n\t"
"sync\n\t"
"lwz 3,0(3)\n\t" /* Get actual value at phys addr r3 */
"mtmsr 5\n\t"
"isync\n\t"
"sync\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
__asm__("\t.globl GetPhysHalf\n"
"GetPhysHalf:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"isync\n\t"
"sync\n\t"
"lhz 3,0(3)\n\t" /* Get actual value at phys addr r3 */
"mtmsr 5\n\t"
"isync\n\t"
"sync\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
__asm__("\t.globl GetPhysByte\n"
"GetPhysByte:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"isync\n\t"
"sync\n\t"
"lbz 3,0(3)\n\t" /* Get actual value at phys addr r3 */
"mtmsr 5\n\t"
"isync\n\t"
"sync\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
__asm__("\t.globl SetPhys\n"
"SetPhys:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"sync\n\t"
"eieio\n\t"
"stw 4,0(3)\n\t" /* Set actual value at phys addr r3 */
"dcbst 0,3\n\t"
"mtmsr 5\n\t"
"sync\n\t"
"eieio\n\t"
"mr 3,4\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
__asm__("\t.globl SetPhysHalf\n"
"SetPhysHalf:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"sync\n\t"
"eieio\n\t"
"sth 4,0(3)\n\t" /* Set actual value at phys addr r3 */
"dcbst 0,3\n\t"
"mtmsr 5\n\t"
"sync\n\t"
"eieio\n\t"
"mr 3,4\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
__asm__("\t.globl SetPhysByte\n"
"SetPhysByte:\t\n"
"mflr 0\n\t"
"stwu 0,-16(1)\n\t"
"mfmsr 5\n\t"
"andi. 6,5,0xffef\n\t"/* turn off MSR[DR] */
"mtmsr 6\n\t"
"sync\n\t"
"eieio\n\t"
"stb 4,0(3)\n\t" /* Set actual value at phys addr r3 */
"dcbst 0,3\n\t"
"mtmsr 5\n\t"
"sync\n\t"
"eieio\n\t"
"mr 3,4\n\t"
"lwz 0,0(1)\n\t"
"addi 1,1,16\n\t"
"mtlr 0\n\t"
"blr"
);
inline int GetSR(int n) {
register int res = 0;
switch( n ) {
case 0:
__asm__("mfsr %0,0" : "=r" (res));
break;
case 1:
__asm__("mfsr %0,1" : "=r" (res));
break;
case 2:
__asm__("mfsr %0,2" : "=r" (res));
break;
case 3:
__asm__("mfsr %0,3" : "=r" (res));
break;
case 4:
__asm__("mfsr %0,4" : "=r" (res));
break;
case 5:
__asm__("mfsr %0,5" : "=r" (res));
break;
case 6:
__asm__("mfsr %0,6" : "=r" (res));
break;
case 7:
__asm__("mfsr %0,7" : "=r" (res));
break;
case 8:
__asm__("mfsr %0,8" : "=r" (res));
break;
case 9:
__asm__("mfsr %0,9" : "=r" (res));
break;
case 10:
__asm__("mfsr %0,10" : "=r" (res));
break;
case 11:
__asm__("mfsr %0,11" : "=r" (res));
break;
case 12:
__asm__("mfsr %0,12" : "=r" (res));
break;
case 13:
__asm__("mfsr %0,13" : "=r" (res));
break;
case 14:
__asm__("mfsr %0,14" : "=r" (res));
break;
case 15:
__asm__("mfsr %0,15" : "=r" (res));
break;
}
return res;
}
inline void SetSR(int n, int val) {
switch( n ) {
case 0:
__asm__("mtsr 0,%0" : : "r" (val));
break;
case 1:
__asm__("mtsr 1,%0" : : "r" (val));
break;
case 2:
__asm__("mtsr 2,%0" : : "r" (val));
break;
case 3:
__asm__("mtsr 3,%0" : : "r" (val));
break;
case 4:
__asm__("mtsr 4,%0" : : "r" (val));
break;
case 5:
__asm__("mtsr 5,%0" : : "r" (val));
break;
case 6:
__asm__("mtsr 6,%0" : : "r" (val));
break;
case 7:
__asm__("mtsr 7,%0" : : "r" (val));
break;
case 8:
__asm__("mtsr 8,%0" : : "r" (val));
break;
case 9:
__asm__("mtsr 9,%0" : : "r" (val));
break;
case 10:
__asm__("mtsr 10,%0" : : "r" (val));
break;
case 11:
__asm__("mtsr 11,%0" : : "r" (val));
break;
case 12:
__asm__("mtsr 12,%0" : : "r" (val));
break;
case 13:
__asm__("mtsr 13,%0" : : "r" (val));
break;
case 14:
__asm__("mtsr 14,%0" : : "r" (val));
break;
case 15:
__asm__("mtsr 15,%0" : : "r" (val));
break;
}
}
void GetBat( int bat, int inst, int *batHi, int *batLo ) {
register int bh asm("r3"), bl asm("r4");
if( inst ) {
switch( bat ) {
case 0:
__asm__("mfibatu 3,0");
__asm__("mfibatl 4,0");
break;
case 1:
__asm__("mfibatu 3,1");
__asm__("mfibatl 4,1");
break;
case 2:
__asm__("mfibatu 3,2");
__asm__("mfibatl 4,2");
break;
case 3:
__asm__("mfibatu 3,3");
__asm__("mfibatl 4,3");
break;
}
} else {
switch( bat ) {
case 0:
__asm__("mfdbatu 3,0");
__asm__("mfdbatl 4,0");
break;
case 1:
__asm__("mfdbatu 3,1");
__asm__("mfdbatl 4,1");
break;
case 2:
__asm__("mfdbatu 3,2");
__asm__("mfdbatl 4,2");
break;
case 3:
__asm__("mfdbatu 3,3");
__asm__("mfdbatl 4,3");
break;
}
}
*batHi = bh;
*batLo = bl;
}
#define BATSET(n,t) \
case n: __asm__("mt" #t "batu " #n ",%0\n\tmt" #t "batl " #n ",%1" \
: : "r" (batHi), "r" (batLo)); break;
void SetBat( int bat, int inst, int batHi, int batLo ) {
if( inst ) {
switch( bat ) {
BATSET(0,i);
BATSET(1,i);
BATSET(2,i);
BATSET(3,i);
}
} else {
switch( bat ) {
BATSET(0,d);
BATSET(1,d);
BATSET(2,d);
BATSET(3,d);
}
}
__asm__("isync\n\tsync");
}
inline int GetSDR1() {
register int res asm("r3");
__asm__("mfsdr1 3");
return res;
}
inline void SetSDR1( int sdr ) {
int i,j;
__asm__("mtsdr1 3");
__asm__("sync");
__asm__("isync");
for( i = 0; i < 256; i++ ) {
j = i << 12;
__asm__("tlbie %0,0" : : "r" (j));
}
__asm__("eieio");
__asm__("tlbsync");
__asm__("ptesync");
}
inline int BatTranslate( int batu, int batl, int virt ) {
int mask;
if(batu & 0x3fc)
{
mask = ~(0x1ffff | ((batu & 0x3fc)>>2)<<17);
if((batu & 2) && ((batu & mask) == (virt & mask)))
return (batl & mask) | (virt & ~mask);
} else {
mask = ~(0x1ffff | (batl << 17));
if(!(batl & 0x40) || ((batu & mask) != (virt & mask)))
return (batl & mask) | (virt & ~mask);
}
return -1;
}
inline int BatHit( int batu, int batl, int virt ) {
return BatTranslate( batu, batl, virt ) != -1;
}
/* translate address */
int PpcVirt2phys( vaddr_t virt, int inst ) {
int msr = GetMSR();
int txmask = inst ? 0x20 : 0x10;
int i, bath, batl, sr, sdr1, physbase, vahi, valo;
int npteg, hash, hashmask, ptehi, ptelo, ptegaddr;
int vsid, pteh, ptevsid, pteapi;
if( msr & txmask ) {
sr = GetSR( virt >> 28 );
vsid = sr & 0xfffffff;
vahi = vsid >> 4;
valo = (vsid << 28) | (virt & 0xfffffff);
if( sr & 0x80000000 ) {
return valo;
}
for( i = 0; i < 4; i++ ) {
GetBat( i, inst, &bath, &batl );
if( BatHit( bath, batl, virt ) ) {
return BatTranslate( bath, batl, virt );
}
}
sdr1 = GetSDR1();
physbase = sdr1 & ~0xffff;
hashmask = ((sdr1 & 0x1ff) << 10) | 0x3ff;
hash = (vsid & 0x7ffff) ^ ((valo >> 12) & 0xffff);
npteg = hashmask + 1;
for( pteh = 0; pteh < 0x80; pteh += 64, hash ^= 0x7ffff ) {
ptegaddr = ((hashmask & hash) * 64) + physbase;
for( i = 0; i < 8; i++ ) {
ptehi = GetPhys( ptegaddr + (i * 8) );
ptelo = GetPhys( ptegaddr + (i * 8) + 4 );
ptevsid = (ptehi >> 7) & 0xffffff;
pteapi = ptehi & 0x3f;
if( (ptehi & 64) != pteh ) continue;
if( ptevsid != (vsid & 0xffffff) ) continue;
if( pteapi != ((virt >> 22) & 0x3f) ) continue;
return (ptelo & 0xfffff000) | (virt & 0xfff);
}
}
return -1;
} else {
return virt;
}
}
int PtegNumber(vaddr_t virt, int hfun)
{
int sr = GetSR( (virt >> 28) & 0xf );
int vsid = sr & PPC_VSID_MASK;
return ((((vsid & 0x7ffff) ^ ((virt >> 12) & 0xffff)) ^ (hfun ? -1 : 0)) & ((HTABSIZ - 1) >> 3) & 0x3ff);
}