#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);
}