#define STACK_LEN			10240
#define DISP_MSGS			false

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

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[] =	{	
								};
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);

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
}

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);
	
	// Execute the bloody trap instruction.
	twi		31,r31,2;
	
	mfsprg	r3,26;
	lwz		r4,decryptionKey(RTOC)
@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	r3;
	ori		r3,r3,0xC000;
	mtmsr	r3;
	isync;
	
	// Jump back to the OS
	ori		r3,r30,0x6084;
	mtctr	r3;
	bctr;
entry static end
}