#define STACK_LEN			10240	// This MUST be 10240
#define DISP_MSGS			false

#include "SupervisorMode.h"
#include <iostream.h>

void TestDecryption(void);

typedef struct SuperModeInfo
{
	unsigned long*			mySuperPatch;
	char*				myStack;
	unsigned long			patchAddr;
	unsigned long			patchRTOC;
	volatile unsigned long	returnValue;
	unsigned long			paramValue;
}SuperModeInfo;

static inline asm unsigned long _getRTOC(void) {mr r3,RTOC; blr;}

static unsigned long	gary = 'Gary';
static unsigned long	garyBirth = 0x05051956;
static unsigned long	msrANDComp = 0x0000C000;
static unsigned long	msrOR = 0x00000000;
static unsigned long	decryptionKey[] =	{	(0x7C7A02A6 ^ 0x80680004),	// lwz		r3,4(r8)
									(0x7D244B78 ^ 0x38632800),		// addi		r3,r3,10240
									(0x3863FFFC ^ 0x9423FFC0),		// stwu		sp,-64(r3)
									(0x3884FFFC ^ 0x7C611B78),		// mr		sp,r3
									(0x80A30004 ^ 0x90010018),		// stw		r0,24(sp)
									(0x84C40004 ^ 0x9101001C),		// stw		r8,28(sp)
									(0x7CA63278 ^ 0x80680008),		// lwz		r3,8(r8)
									(0x94C30004 ^ 0x90410014),		// stw		RTOC,20(sp)
									(0x80A30004 ^ 0x7C6903A6),	// mtctr		r3
									(0x84C40004 ^ 0x80680014),		// lwz		r3,20(r8)
									(0x7CA63278 ^ 0x80440004),		// lwz		RTOC,r(r4)
									(0x94C30004 ^ 0x4E800421),		// bctrl
									(0x80A30004 ^ 0x80410014),		// lwz		RTOC,20(sp)
									(0x84C40004 ^ 0x8101001C),		// lwz		r8,28(sp)
									(0x7CA63278 ^ 0x90680010),		// stw		r3,16(r8)
									(0x94C30004 ^ 0x80010018),		// lwz		r0,24(sp)
									(0x80A30004 ^ 0x80210000),		// lwz		sp,0(sp)
									(0x84C40004 ^ 0x7C8000A6),		// mfmsr		r4
									(0x7CA63278 ^ 0x6084C000),		// ori		r4,r4,0xC000
									(0x94C30004 ^ 0x7C9B03A6),		// mtsrr1	r4
									(0x80A30004 ^ 0x63C46084),		// ori		r4,r30,0x6084
									(0x84C40004 ^ 0x7C7A02A6),	// mfsrr0	r3
									(0x7CA63278 ^ 0x7C9A03A6),	// mtsrr0	r4
									(0x94C30004 ^ 0x48000180)		// b			@out
								};
unsigned short code68K[] = {0xFC1E,0x4E75};	// DC.L 0xFC1E; RTS;

static SuperModeInfo		mySuperModeInfo;
static unsigned long		patchVectorCodeLen;
static unsigned long		patchLen = 0;
static char			vmPresent = false;

static volatile unsigned short	machineType	:	0x68FFF71A;
static volatile unsigned long	jumpVector	:	0x68FFF994;

static void RTOCPatch(void);
static void end(void);
static void supervisorPatch(void);
static void _blockMovePhys(register BlockMoveInfo*);

char InstallSupervisorPatch(unsigned long* patchV,unsigned long codeLen)
{
	// Install the supervisor mode patch through the PPC trap instruction
#if DISP_MSGS
	cout << "Installing Supervisor Patch\n";
#endif

	// Check to see if this is the right kind of machine
	if( (machineType & 0xF000) != 0x2000)
	{
#if DISP_MSGS
		cout << "This code to enter supervisor mode cannot function on this class of computer\n";
#endif
		return !noErr;
	}
	
	// Find out about virtual memory
	long gestResp;
#if DISP_MSGS
	cout << "Calling Gestalt for VM info\n";
#endif
	if(Gestalt(gestaltVMAttr,&gestResp))
	{
#if DISP_MSGS
		cout << "Error calling Gestalt!\n";
#endif
		return !noErr;
	}
	vmPresent = (gestResp & (1 << gestaltVMPresent));
	
	// Get a PowerPC transition vector to the code we want to execute in supervisor mode
	mySuperModeInfo.patchAddr = patchV[0];
	mySuperModeInfo.patchRTOC = patchV[1];
	patchVectorCodeLen = codeLen;
	
	// Build the patch
#if DISP_MSGS
	cout << "Building Patch\n";
#endif
	
	// Find the start and end of the real code
	void*		patchStart = (void*)((long *)supervisorPatch)[0];
	void*		patchEnd = (void*)((long *)end)[0];
	
	// Calculate the length of the real code
	patchLen = (unsigned long)patchEnd - (unsigned long)patchStart;
	
	// Allocate memory for the patch
#if DISP_MSGS
	cout << "\tAllocating patch memory\n";
#endif
	mySuperModeInfo.mySuperPatch = (unsigned long*)NewPtr(patchLen);
	if(!mySuperModeInfo.mySuperPatch)
	{
#if DISP_MSGS
		cout << "Failed to allocate memory for the patch!\n";
#endif
		return !noErr;
	}
	
	// Allocate memory for a stack to use during supervisor time
#if DISP_MSGS
	cout << "\tAllocating patch stack\n";
#endif
	mySuperModeInfo.myStack = NewPtr(STACK_LEN);
	if(!mySuperModeInfo.myStack)
	{
#if DISP_MSGS
		cout << "Failed to allocate memory for the patch stack!\n";
#endif
		DisposePtr((char*)mySuperModeInfo.mySuperPatch);
		return !noErr;
	}
	
	// Make a copy of the real code.  This copy will become the patch
#if DISP_MSGS
	cout << "\tCopying patch\n";
#endif
	BlockMove((char*)patchStart,(char*)mySuperModeInfo.mySuperPatch,patchLen);
	
	// Perform relocations on the new patch, these relocations tell the patch what its RTOC is.
	unsigned long	rtocInstructionOffset1 = ((unsigned long*)RTOCPatch)[0] - (unsigned long)patchStart;
#if DISP_MSGS
	cout << "\tRelocating patch\n";
#endif
	mySuperModeInfo.mySuperPatch[rtocInstructionOffset1/4] |= ((_getRTOC() >> 16) & 0x0000FFFF);
	mySuperModeInfo.mySuperPatch[rtocInstructionOffset1/4 + 1] |= (_getRTOC() & 0x0000FFFF);
	
	// If virtual memory is present, lock the patch in physical memory, as well as the patch's stack.
	if(vmPresent)
	{
#if DISP_MSGS
		cout << "Locking VM zones\n";
#endif
		LockMemoryContiguous((void*)mySuperModeInfo.mySuperPatch,patchLen);
		LockMemoryContiguous((void*)mySuperModeInfo.myStack,STACK_LEN);
		LockMemoryContiguous((void*)&mySuperModeInfo,sizeof(mySuperModeInfo));
		LockMemoryContiguous((void*)mySuperModeInfo.patchAddr,patchVectorCodeLen);
	}
	
	// Flush the patch from caches so it becomes executable
#if DISP_MSGS
	cout << "Making patch executable\n";
#endif
	MakeDataExecutable((char*)mySuperModeInfo.mySuperPatch,patchLen);
	
	// We're done!
#if DISP_MSGS
	cout << "Finished installing patch\n\n";
#endif
	return noErr;
}

unsigned long CallSupervisorPatch(unsigned long param)
{
	// Call our supervisor mode patch
#if DISP_MSGS
	cout << "Firing patch\n";
#endif
	
	// Save the old trap vector
	unsigned long oldVector = jumpVector;
	
	// Install our patch as the new trap vector
	jumpVector = (unsigned long)mySuperModeInfo.mySuperPatch;
#if DISP_MSGS
	cout << "Jump vector = " << (unsigned long)mySuperModeInfo.mySuperPatch << "\n";
#endif
	
	// Call our patch.  We pass the parameter through the global "paramValue"
	mySuperModeInfo.paramValue = param;
	CallUniversalProc((UniversalProcPtr)code68K,kPascalStackBased);
	
	// Restore the previous trap vector
	jumpVector = oldVector;
	
	// We're done!
#if DISP_MSGS
	cout << "Patch fired succesfully!\n\n";
#endif
	
	return mySuperModeInfo.returnValue;
}

void RemoveSupervisorPatch(void)
{
	// Remove the supervisor mode patch
#if DISP_MSGS
	cout << "Removing patch\n";
#endif
	
	// Unlock any memory if VM is present and we locked it before.
	if(vmPresent)
	{
#if DISP_MSGS
		cout << "Unlocking VM zones\n";
#endif
		UnlockMemory((void*)mySuperModeInfo.mySuperPatch,patchLen);
		UnlockMemory((void*)mySuperModeInfo.myStack,STACK_LEN);
		UnlockMemory((void*)&mySuperModeInfo,sizeof(mySuperModeInfo));
		UnlockMemory((void*)mySuperModeInfo.patchAddr,patchVectorCodeLen);
	}
	
	// Dispose of the code part of the patch
#if DISP_MSGS
	cout << "Disposing patch memory\n";
#endif
	DisposePtr((char*)mySuperModeInfo.mySuperPatch);
	
	// Dispose of the patch's stack
#if DISP_MSGS
	cout << "Disposing stack memory\n";
#endif
	DisposePtr(mySuperModeInfo.myStack);
	
	// We're done!
#if DISP_MSGS
	cout << "Finished removing patch\n\n";
#endif
}

void BlockMovePhys2Logical(char* srcPhys,char* dstLog,unsigned long len)
{
	BlockMoveInfo	info = {srcPhys,dstLog,len};
	
	if(!InstallSupervisorPatch((unsigned long*)_blockMovePhys,100*4))
	{
		unsigned long retVal = CallSupervisorPatch((unsigned long)&info);
		RemoveSupervisorPatch();
	}
}

asm void _blockMovePhys(register BlockMoveInfo*)
{
	lwz		r5,BlockMoveInfo.len(r3);
	lwz		r4,BlockMoveInfo.dstLog(r3);
	lwz		r3,BlockMoveInfo.srcPhys(r3);
	mtctr	r5;
	subi		r4,r4,1;
	subi		r3,r3,1;
	
	// r7 contains the write msr
	mfmsr	r7;
	
	// r6 contains the read msr
	mr		r6,r7;
	rlwinm	r6,r6,0,28,26;	// Turn off data relocation.
@copyLoop:
		// Read from physical memory
		sync;
		mtmsr	r6;	// Disable data relocation
		sync;
		lbzu		r0,1(r3);
		
		// Write to logical memory
		sync;
		mtmsr	r7;	// Enable data relocation
		sync;
		stbu		r0,1(r4);
		
		// Loop
		bdnz		@copyLoop;
	
	blr;
}

asm void supervisorPatch(void)
{
	// Tell the OS to switch us into supervisor mode
	
entry static RTOCPatch
	// Get the RTOC for this code fragment
	lis		RTOC,0;
	ori		RTOC,RTOC,0;
	
	lwz		r3,gary(RTOC);
	lwz		r4,garyBirth(RTOC);
	lwz		r5,msrANDComp(RTOC);
	lwz		r7,msrOR(RTOC);
	lwz		r8,mySuperModeInfo(RTOC);
	lwz		r9,decryptionKey(RTOC);
	
	// Execute the bloody trap instruction.
	twi		31,r31,2;
@inSupervisorMode:
	mfspr	r3,26;
	mr		r4,r9;
	subi		r3,r3,4;
	subi		r4,r4,4;
	
	// lwz	r3,SuperModInfo.myStack(r8)
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// addi	r3,r3,STACK_LEN
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// stwu	sp,-64(r3);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// mr	sp,r3;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// stw	r0,24(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);	// -->b		@out
	
	// stw	r8,28(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	r3,SuperModeInfo.patchAddr(r8);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// stw	RTOC,20(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// mtctr	r3;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	r3,SuperModeInfo.paramValue(r8);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	RTOC,4(r4);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// bctrl;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	RTOC,20(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	r8,28(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// stw	r3,SuperModeInfo.returnValue(r8);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	r0,24(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// lwz	sp,0(sp);
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// mfmsr	r3;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// ori	r3,r3,0xC000;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// mtmsr	r3;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// isync;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// ori	r3,r30,0x6084;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// mtctr	r3;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
	
	// bctr;
	lwz		r5,4(r3);
	lwzu		r6,4(r4);
	xor		r6,r5,r6;
	stwu		r6,4(r3);
@flushDecryptedCodeCache:
	mfspr	r3,26;
	dcbf		r0,r3;
	addi		r3,r3,32;
	dcbf		r0,r3;
	addi		r3,r3,32;
	dcbf		r0,r3;
	addi		r3,r3,32;
	dcbf		r0,r3;
	sync;
	mfspr	r3,26;
	icbi		r0,r3;
	addi		r3,r3,32;
	icbi		r0,r3;
	addi		r3,r3,32;
	icbi		r0,r3;
	addi		r3,r3,32;
	icbi		r0,r3;
	sync;
@executeDecryption:
	rfi;
@out:
	lwz		r4,decryptionKey(RTOC);
	lis		r5,( ((sizeof(decryptionKey)/4) >> 16) & 0x0000FFFF);
	ori		r5,r5,((sizeof(decryptionKey)/4) & 0x0000FFFF);
	mtctr	r5;
	subi		r3,r3,4;
	subi		r4,r4,4;
@xorLoop:
		lwz		r5,4(r3);
		lwzu		r6,4(r4);
		xor		r6,r5,r6;
		stwu		r6,4(r3);
		dcbf		r0,r3;
		sync;
		icbi		r0,r3;
		sync;
		bdnz		@xorLoop;
	rfi;
	
	/*
	This is the code that is generated when the above is decrypted.  It is generated inline with the decryption algorithm,
	meaning that this code overwrites the algorithm as it is decrypted.  @inSupervisorMode looks like this after the decryption
	completes.
@inSupervisorMode:
	// We are now in supervisor mode.  Set up a stack for us to run on.
	lwz		r3,SuperModeInfo.myStack(r8);
	addi		r3,r3,STACK_LEN;
	
	// Make the OS's fake stack frame
	stwu		sp,-64(r3);
	mr		sp,r3;
	
	// Registers r0 and sp are non-volatile:	we MUST restore them before returning!
	stw		r0,24(sp);
	stw		r8,28(sp);
	
	// Load the RTOC and address of the function we are asked to call in supervisor mode
	lwz		r3,SuperModeInfo.patchAddr(r8);
	stw		RTOC,20(sp);
	mtctr	r3;
	lwz		r3,SuperModeInfo.paramValue(r8);
	lwz		RTOC,4(r4);
	bctrl;	// Call the function
	lwz		RTOC,20(sp);
	
	lwz		r8,28(sp);
	
	// Save the return value
	stw		r3,SuperModeInfo.returnValue(r8);
	
	// Restore register r0
	lwz		r0,24(sp);
	
	// Remove the fake caller's stack frame and restore the OS's SP at the same time
	lwz		sp,0(sp);
	
	// Re-enable interrupts
	mfmsr	r4;
	ori		r4,r4,0xC000;
	mtspr	27,r4;	// To srr1
	
	// Jump back to the OS
	ori		r4,r30,0x6084;
	mfspr	r3,26;
	mtspr	26,r4;	// To srr0
	rfi;
	*/
entry static end
}