From 2835db2ebfe5e91da3f332f1eef2624f3aea3f68 Mon Sep 17 00:00:00 2001
From: Stephen Simpson <megazig@gmail.com>
Date: Thu, 3 Nov 2011 00:40:37 -0500
Subject: cleaned up codehandler to allow for better changes later

---
 src/codehandler.cpp | 439 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/codehandler.s   | 368 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 807 insertions(+)
 create mode 100644 src/codehandler.cpp
 create mode 100644 src/codehandler.s

(limited to 'src')

diff --git a/src/codehandler.cpp b/src/codehandler.cpp
new file mode 100644
index 0000000..b1d67d2
--- /dev/null
+++ b/src/codehandler.cpp
@@ -0,0 +1,439 @@
+#include <common.h>
+
+u8 frozen_value = 0;
+u32 regbuffer[72] = {0};
+u32 dwordbuffer[2] = {0};
+u32 bpbuffer[4] = {0};
+u8 cmd;
+
+/*FIXME
+ * u32 register_to_match = 0;
+ * u32 register_value = 0;
+ * u32 bplist[40] = {0};
+ */
+
+#define PACKET_SIZE 0xF800
+
+extern "C" void writebranch(u32 source, u32 destination);
+extern "C" void bphandler();
+
+// return true if remainder
+bool _packetdivide(u32 memrange, u32 size, u32* full, u32* remainder) {
+	*full      = memrange / size;
+	*remainder = memrange % size;
+	return (*remainder) ? true : false;
+}
+
+u32 checkexisend(u32 EXI_command) {
+	//OSReport("checkexisend(0x%08x);\n", EXI_command);
+	*(u32*)0xCD006814 = 0xD0;
+	*(u32*)0xCD006824 = EXI_command;
+	*(u32*)0xCD006820 = 0x19;
+
+	u32 exi_chan1cr = 1;
+	while((exi_chan1cr)&1) {
+		exi_chan1cr = *(u32*)0xCD006820;
+		exi_chan1cr &= 1;
+	}
+
+	*(u32*)0xCD006814 = exi_chan1cr;
+	return *(u32*)0xCD006824;
+}
+
+struct exidata {
+	unsigned bits0to3 : 4;
+	unsigned bit4 : 1;
+	unsigned bit5 : 1;
+	unsigned bit6 : 1;
+	unsigned bit7 : 1;
+	unsigned data : 8;
+	unsigned bottom : 16;
+};
+
+bool exichecktx() {
+	*(u32*)0xCD006814 = 0xD0;
+	*(u32*)0xCD006824 = 0xC0000000;
+	*(u32*)0xCD006820 = 0x09;
+
+	u32 exi_chan1cr = 1;
+	while((exi_chan1cr)&1) {
+		exi_chan1cr = *(u32*)0xCD006820;
+		exi_chan1cr &= 1;
+	}
+
+	*(u32*)0xCD006814 = exi_chan1cr;
+	u32 ret = *(u32*)0xCD006824;
+	struct exidata* dat = (exidata*)&ret;
+	return (dat->bit5) ? true : false;
+}
+
+bool exireceivebyte(u8* byte) {
+	//OSReport("exireceivebyte()\n");
+	u32 ret = checkexisend(0xA0000000);
+	struct exidata * dat = (exidata*)&ret;
+	*byte = dat->data;
+	return (dat->bit4) ? true : false;
+}
+
+bool exicheckreceive() {
+	bool ret = false;
+	while(!ret) {
+		u32 foo = checkexisend(0xD0000000);
+		struct exidata* dat = (exidata*)&foo;
+		ret = dat->bit5;
+	}
+	return ret;
+}
+
+void exireceivebuffer(u32 counter, u8* buffer) {
+	for(u32 ii=0; ii<counter; ii++) {
+		u8 byte;
+		bool ret = false;
+		while(!ret) {
+			exicheckreceive();
+			exicheckreceive();
+			ret = exireceivebyte(&byte);
+		}
+		buffer[ii] = byte;
+	}
+}
+
+bool exisendbyte(u8 byte) {
+	u32 sendbyte = byte << 20;
+	sendbyte |= 0xB0000000;
+
+	u32 ret = checkexisend(sendbyte);
+	struct exidata* dat = (exidata*)&ret;
+	return (dat->bit5) ? true : false;
+}
+
+void exisendbuffer(u32 counter, u8* buffer) {
+	bool ret = false;
+	for(u32 ii=0; ii<counter; ii++) {
+		while(!ret) {
+			ret = exichecktx();
+		}
+		ret = false;
+		while(!ret) {
+			ret = exisendbyte( buffer[ii] );
+		}
+		//OSReport("Sent byte from buffer\n");
+	}
+}
+
+void versionnumber() {
+	// send 0x80 for wii
+	exisendbyte(0x80);
+}
+
+void pausestatus() {
+	exisendbyte(frozen_value);
+}
+
+void freezegame() {
+	frozen_value = 1;
+	//OSReport("frozen_value: %d\n", frozen_value);
+}
+
+void unfreezegame() {
+	frozen_value = 0;
+	//OSReport("frozen_value: %d\n", frozen_value);
+}
+
+u8 get_frozenvalue() {
+	return frozen_value;
+}
+
+void writedword() {
+	exireceivebuffer(8, (u8*)dwordbuffer);
+
+	u32 location = dwordbuffer[0];
+	u32 value    = dwordbuffer[1];
+	*(u32*)location = value;
+
+	asm("dcbf 0, %0; sync; icbi 0, %0; isync" : : "b"(location));
+}
+
+void writeword() {
+	exireceivebuffer(8, (u8*)dwordbuffer);
+
+	u32 location = dwordbuffer[0];
+	u32 value    = dwordbuffer[1];
+	*(u16*)location = (u16)value;
+
+	asm("dcbf 0, %0; sync; icbi 0, %0; isync" : : "b"(location));
+}
+
+void writebyte() {
+	exireceivebuffer(8, (u8*)dwordbuffer);
+
+	u32 location = dwordbuffer[0];
+	u32 value    = dwordbuffer[1];
+	*(u8*)location = (u8)value;
+
+	asm("dcbf 0, %0; sync; icbi 0, %0; isync" : : "b"(location));
+}
+
+//FIXME removed upload
+/*
+void upbpdata() {
+	exisendbyte(0xAA);
+	upload(regbuffer, 40*4);
+}
+*/
+
+void getbpdata() {
+	exisendbuffer(72*4, (u8*)regbuffer);
+}
+
+//returns false if we need to go to resume and not finish
+bool breakpoints(u8 type) {
+	//FIXME
+	u32 _bphandler = (u32)bphandler;
+	writebranch(0x80000300, _bphandler);
+	writebranch(0x80000D00, _bphandler);
+	writebranch(0x80001300, _bphandler);
+
+	bpbuffer[0] = 0;				//clears inst bp
+	bpbuffer[1] = 0;				//clears data bp
+	bpbuffer[2] = 0;				//clears alignment
+
+	u32 SRR1 = regbuffer[6];		// SRR1
+
+	if(frozen_value != 2) {			// not on breakpoint
+		bpbuffer[3] = 0;			// clear broken on address
+	}else if(type == 0x44) {		// step
+		bpbuffer[3] = frozen_value;	// set broken on address to frozen_value
+		if(frozen_value == 2) {
+			regbuffer[6] = SRR1 | 0x400;
+			unfreezegame();
+			//FIXME need to go to resume and not finish
+			return false;
+		}
+	}
+
+	//cmpwi	cr6,r29,0x10		# 0x10=instruction, 0x09/0x89=data
+	//cmpwi	cr5,r29,0x44		# 0x44=step
+
+	regbuffer[6] = SRR1 & 0xffffbfff;
+	if(type != 0x44) {
+		u32 value1, value2;
+		exireceivebuffer(4, (u8*)&value1);
+		if(type == 0x10) {
+			bpbuffer[0] = value1;		// instbp = value1;
+		}else{
+			bpbuffer[1] = value1;		// databp = value1;
+		}
+
+		if(type == 0x89) {
+			exireceivebuffer(4, (u8*)&value2);
+			bpbuffer[2] = value2;
+		}
+
+		u32 IABR = bpbuffer[0];			// IABR = instbp
+		u32 DABR = bpbuffer[1];			// DABR = databp
+		asm("mtspr 1010, %0; mtspr 1013, %1" : : "b"(IABR), "b"(DABR));
+	}
+	return true;
+}
+
+void cancelbreakpoints() {
+	u32 x = 0;
+	asm("mtspr 1013, %0; mtspr 1010, %0" : "=r"(x) : );
+
+	// clears bit 21 of SRR1
+	regbuffer[6] = regbuffer[6] & 0xfffffbff;
+}
+
+void readmem() {
+	exisendbyte(0xAA);
+
+	exireceivebuffer(8, (u8*)dwordbuffer);
+
+	u32 start_location = dwordbuffer[0];
+	u32 end_location   = dwordbuffer[1];
+	u32 memrange = end_location - start_location;
+
+	u32 full, remainder;
+	bool ret = _packetdivide(memrange, PACKET_SIZE, &full, &remainder);
+
+	u32 location = start_location;
+	while(full) {
+		exisendbuffer(PACKET_SIZE, (u8*)location);
+
+		bool ret2 = false;
+		bool ret3 = false;
+		u8 byte;
+		while(!ret3) {
+			while(!ret2) {
+				exicheckreceive();
+				exicheckreceive();
+				ret2 = exireceivebyte(&byte);
+			}
+
+			if(byte == 0xCC) {
+				goto done_readmem;
+			}else if(byte == 0xBB) {
+				ret3 = true;
+			}else if(byte == 0xAA) {
+				location += PACKET_SIZE;
+				full--;
+				ret3 = true;
+			}
+		}
+	}
+	while(remainder) {
+		exisendbuffer(remainder, (u8*)location);
+
+		//OSReport("Buffer sent\n");
+		bool ret2 = false;
+		bool ret3 = false;
+		u8 byte;
+		while(!ret3) {
+			while(!ret2) {
+				exicheckreceive();
+				exicheckreceive();
+				ret2 = exireceivebyte(&byte);
+			}
+
+			if(byte == 0xCC) {
+				goto done_readmem;
+			}else if(byte == 0xBB) {
+				ret3 = true;
+			}else if(byte == 0xAA) {
+				location += remainder;
+				remainder = 0;
+				ret3 = true;
+			}
+		}
+	}
+done_readmem:
+	asm("nop");
+}
+
+bool command_handler() {
+	cmd = 0;
+	bool ret = exireceivebyte(&cmd);
+	if(!ret)
+		return true;
+
+	switch(cmd) {
+		case 0x1:
+			writebyte();
+			break;
+		case 0x2:
+			writeword();
+			break;
+		case 0x3:
+			writedword();
+			break;
+		case 0x4:
+			readmem();
+			break;
+		case 0x6:
+			freezegame();
+			break;
+		case 0x7:
+			unfreezegame();
+			break;
+		case 0x8:		// resume
+			return false;
+			break;
+		case 0x9:
+			return breakpoints(cmd);
+			break;
+		case 0x10:
+			return breakpoints(cmd);
+			break;
+		case 0x2F:		// NOT IMPLEMENTED
+			break;
+		case 0x30:
+			getbpdata();
+			break;
+		case 0x38:
+			cancelbreakpoints();
+			break;
+		case 0x40:		// NOT IMPLEMENTED
+			break;
+		case 0x41:		// NOT IMPLEMENTED
+			break;
+		case 0x44:
+			return breakpoints(cmd);
+			break;
+		case 0x50:
+			pausestatus();
+			break;
+		case 0x60:		// NOT IMPLEMENTED
+			break;
+		case 0x89:
+			return breakpoints(cmd);
+			break;
+		case 0x99:
+			versionnumber();
+			break;
+		default:
+			break;
+	};
+	return true;
+}
+
+//FIXME
+bool handle_bps() {
+	return false;
+}
+
+#if 0	// DEFAULT IABR BPHANDLER
+		bpregs[0x0C] = r3
+		bpregs[0x10] = r4
+		bpregs[0x14] = r5
+		bpregs[0x80] = CR
+		bpregs[0x84] = LR
+		bpregs[0x88] = CTR
+		bpregs[0x8C] = XER
+		bpregs[0x198] = SRR0
+		bpregs[0x19C] = SRR1
+	80000300 7C9043A6 mtsprg 0,r4
+	80000304 808000C0 lwz r4,0xC0(r0)
+	80000308 9064000C stw r3,0xC(r4)
+	8000030c 7C7042A6 mfsprg r3,0
+	80000310 90640010 stw r3,0x10(r4)
+	80000314 90A40014 stw r5,0x14(r4)
+	80000318 A06401A2 lhz r3,0x1A2(r4)
+	8000031c 60630002 ori r3,r3,2
+	80000320 B06401A2 sth r3,0x1A2(r4)
+	80000324 7C600026 mfcr r3
+	80000328 90640080 stw r3,0x80(r4)
+	8000032c 7C6802A6 mflr r3
+	80000330 90640084 stw r3,0x84(r4)
+	80000334 7C6902A6 mfctr r3
+	80000338 90640088 stw r3,0x88(r4)
+	8000033c 7C6102A6 mfxer r3
+	80000340 9064008C stw r3,0x8C(r4)
+	80000344 7C7A02A6 mfsrr0 r3
+	80000348 90640198 stw r3,0x198(r4)
+	8000034c 7C7B02A6 mfsrr1 r3
+	80000350 9064019C stw r3,0x19C(r4)
+	80000354 7C651B78 mr r5,r3
+	80000358 60000000 nop
+	8000035c 7C6000A6 mfmsr r3
+	80000360 60630030 ori r3,r3,0x30
+	80000364 7C7B03A6 mtsrr1 r3
+	80000368 38600002 li r3,2
+	8000036c 808000D4 lwz r4,0xD4(r0)
+	80000370 54A507BD rlwinm. r5,r5,0,30,30
+	80000374 40820014 bne- 0x80000388
+	80000378 3CA0801B lis r5,0x801B
+	8000037c 38A5B0F0 subi r5,r5,20240		#0x801AB0F0
+	80000380 7CBA03A6 mtsrr0 r5
+	80000384 4C000064 rfi
+	80000388 546515BA rlwinm r5,r3,2,22,29
+	8000038c 80A53000 lwz r5,0x3000(r5)
+	80000390 7CBA03A6 mtsrr0 r5
+	80000394 4C000064 rfi
+	80000398 60A5E040 ori r5,r5,0xE040
+	8000039c 54A500BE rlwinm r5,r5,0,2,31
+	800003a0 546315BA rlwinm r3,r3,2,22,29
+	800003a4 7CA3282E lwzx r5,r3,r5
+	800003a8 7CBA03A6 mtsrr0 r5
+	800003ac 4C000064 rfi
+#endif
diff --git a/src/codehandler.s b/src/codehandler.s
new file mode 100644
index 0000000..bf9ce13
--- /dev/null
+++ b/src/codehandler.s
@@ -0,0 +1,368 @@
+.text
+
+#ifndef __MWERKS__
+.set r0,0;   .set r1,1;   .set r2,2;   .set r3,3;   .set r4,4
+.set r5,5;   .set r6,6;   .set r7,7;   .set r8,8;   .set r9,9
+.set r10,10; .set r11,11; .set r12,12; .set r13,13; .set r14,14
+.set r15,15; .set r16,16; .set r17,17; .set r18,18; .set r19,19
+.set r20,20; .set r21,21; .set r22,22; .set r23,23; .set r24,24
+.set r25,25; .set r26,26; .set r27,27; .set r28,28; .set r29,29
+.set r30,30; .set r31,31; .set f0,0;   .set f2,2;   .set f3,3
+#endif
+	 
+.extern frozen_value
+.extern regbuffer
+.extern bpbuffer
+.extern command_handler__Fv
+.extern get_frozenvalue__Fv
+.extern exisendbyte__FUc
+
+.extern OSReport
+
+.globl _start
+
+gameid:
+.long	0,0
+cheatdata:
+.long	frozenvalue
+.space 39*4
+
+_start:
+	stwu	r1,-168(r1)		# stores sp
+	stw		r0,8(r1)		# stores r0
+
+	mflr	r0
+	stw		r0,172(r1)		# stores lr
+
+	mfcr	r0
+	stw		r0,12(r1)		# stores cr
+
+	mfctr	r0
+	stw		r0,16(r1)		# stores ctr
+
+	mfxer	r0
+	stw		r0,20(r1)		# stores xer
+
+	stmw	r3,24(r1)		# saves r3-r31
+
+	mfmsr	r25
+	ori		r26,r25,0x2000	#enable floating point ?
+	andi.	r26,r26,0xF9FF
+	mtmsr	r26
+
+
+	stfd	f2,152(r1)		# stores f2
+	stfd	f3,160(r1)		# stores f3
+
+
+    lis		r20, 0xCC00
+    lhz		r28, 0x4010(r20)
+    ori		r21, r28, 0xFF
+    sth		r21, 0x4010(r20) # disable MP3 memory protection
+
+_setvalues:
+	li		r21,0
+	li		r22,0x19
+	li		r23,0xD0
+	lis		r24,0xCD00
+
+	lis		r18, frozenvalue@h
+	ori		r18, r18, frozenvalue@l	# read buffer just store in lowmem
+	lwz		r0,172(r1)				# loads lr
+	stw		r0,4(r18)				# stores lr
+    stw		r21, 0x643C(r24)		# exi speed up
+
+_check_frozen:
+	lis		r18, frozen_value@h
+	ori		r18, r18, frozen_value@l
+	lwz		r0, 0x0(r18)
+	cmpwi	r0, 2
+	bne		frozen
+#TESTING FOR EXECUTION
+	lis		r3, stringer@h
+	ori		r3, r3, stringer@l
+	bl		OSReport
+
+
+frozen:
+	bl		command_handler__Fv
+	cmpwi	r3, 0
+	beq		resumegame
+	b		finish
+
+#******************************************************************
+#                        subroutine: bphandler
+#    Data/Instruction address breakpoint handler, save context and return
+#******************************************************************
+.global bphandler
+bphandler:
+	mtsprg	0, r3
+	lis		r3, regbuffer@h
+	ori		r3, r3, regbuffer@l
+	stmw	r4, 0x2C(r3)				# Store r4 - r31
+
+	stw		r0, 0x1C(r3)				# Store r0
+	stw		r1, 0x20(r3)				# Store r1
+	stw		r2, 0x24(r3)				# Store r2
+	mr		r4, r3
+	mfsprg	r3, 0
+	stw		r3, 0x28(r4)				# Store r3
+
+	mfsrr1	r3
+	rlwinm	r3, r3,0,22,20				# clear trace
+	stw		r3, 0x18(r4)				# Store SRR1
+	rlwinm	r3, r3,0,24,15
+	ori		r3, r3,0x2000
+#	rlwinm	r3, r3,0,17,15				# clear hw interrupt
+	mtsrr1	r3							# restore srr1 with hw interrupt & trace cleared
+
+	mfsrr0	r3
+	stw		r3, 0x14(r4)				# Store SRR0
+
+	mflr	r3
+	stw		r3, 0x9C(r4)				# Store LR
+
+	mfcr	r3
+	stw		r3, 0x0(r4)					# Store CR
+
+	mfxer	r3
+	stw		r3, 0x4(r4)					# Store XER
+
+	mfctr	r3
+	stw		r3, 0x8(r4)					# Store CTR
+
+	mfdsisr	r3
+	stw		r3, 0xC(r4)					# Store DSISR
+       
+	mfdar	r3
+	stw		r3, 0x10(r4)				# Store DAR
+
+	lis		r3, break@h
+	ori		r3, r3, break@l
+	mtsrr0	r3
+	rfi
+
+break:
+	li		r3, 0
+	mtspr	1010, r3					# Clear IABR
+	mtspr	1013, r3					# Clear DABR
+
+#BACKUP FPR
+	lis		r4, regbuffer@h
+	ori		r4, r4, regbuffer@l
+	stfs	f0, 0xA0(r4)
+	stfs	f1, 0xA4(r4)
+	stfs	f2, 0xA8(r4)
+	stfs	f3, 0xAC(r4)
+	stfs	f4, 0xB0(r4)
+	stfs	f5, 0xB4(r4)
+	stfs	f6, 0xB8(r4)
+	stfs	f7, 0xBC(r4)
+	stfs	f8, 0xC0(r4)
+	stfs	f9, 0xC4(r4)
+	stfs	f10, 0xC8(r4)
+	stfs	f11, 0xCC(r4)
+	stfs	f12, 0xD0(r4)
+	stfs	f13, 0xD4(r4)
+	stfs	f14, 0xD8(r4)
+	stfs	f15, 0xDC(r4)
+	stfs	f16, 0xE0(r4)
+	stfs	f17, 0xE4(r4)
+	stfs	f18, 0xE8(r4)
+	stfs	f19, 0xEC(r4)
+	stfs	f20, 0xF0(r4)
+	stfs	f21, 0xF4(r4)
+	stfs	f22, 0xF8(r4)
+	stfs	f23, 0xFC(r4)
+	stfs	f24, 0x100(r4)
+	stfs	f25, 0x104(r4)
+	stfs	f26, 0x108(r4)
+	stfs	f27, 0x10C(r4)
+	stfs	f28, 0x110(r4)
+	stfs	f29, 0x114(r4)
+	stfs	f30, 0x118(r4)
+	stfs	f31, 0x11C(r4)
+
+#TESTING FOR EXECUTION
+#	lis		r3, stringer@h
+#	ori		r3, r3, stringer@l
+#	bl		OSReport
+
+# r4=regbuffer, r=bpbuffer
+# r16=instBp, r17=dataBp, r19=lastBp
+
+#HANDLE MORE STUFF
+	lis		r5, bpbuffer@h
+	ori		r5, r5, bpbuffer@l
+
+	lwz		r16, 0x0(r5)			# inst bp
+	lwz		r17, 0x4(r5)			# data bp
+	lwz		r19, 0xC(r5)			# last break address
+
+	cmpwi	r19, 0
+	beq		_redobp					# last break was 0. redo bp
+
+	cmpwi	r19, 2
+	bne		addr_1
+	lwz		r9, 0x14(r4)			# SRR0
+	addi	r9, r19, 3
+	stw		r9, 0x0(r5)				# inst bp
+	stw		r9, 0xC(r5)				# last broken on address
+	b		_executebp
+
+addr_1:
+	cmpw	r16, r19
+	beq		_step
+
+	cmpw	r17, r19
+	beq		_step
+
+	add		r9, r16, r17
+	stw		r9, 0xC(r5)		# counter for alignment
+
+
+_alignementcheck:
+	lwz		r16, 0x8(r5)	# bp alignment check
+	cmpwi	r16, 0
+	beq		_executebp		# no alignement = normal break
+
+	lwz		r3, 0x10(r4)	# DAR
+	cmpw	r16, r3			# we check if address = aligned address
+	bne		_step			# if no, we need to set a bp on the next instruction
+
+	li		r16, 0
+	stw		r16, 0x8(r5)	# if we are on the good address we clear the aligned bp check
+	b		_executebp		# and we break
+
+_step:
+	li		r17, 0
+	stw		r17, 0xC(r5)	# we set last broken on address to 0
+	lwz		r9, 0x18(r4)
+	ori		r9, r9, 0x400
+	stw		r9, 0x18(r4)	# SRR1 |= 0x400
+	b		_skipbp			# and we don't break right now
+
+_redobp:
+	mtspr	1010, r16		# we set back the instbp with the original value
+	mtspr	1013, r17		# we set back the databp with the original value
+	li		r9, 1
+	stw		r9, 0xC(r5)		# we set last broken on address to 1
+	b		_skipbp			# and we don't break
+
+_executebp:
+#	lis		r1, temp_stack@h
+#	ori		r1, r1, temp_stack@l	# setup temp stack
+	lis		r4, frozen_value@h
+	ori		r4, r4, frozen_value@l
+	li		r5, 2
+	stw		r5, 0x0(r4)		# Freeze once returned to let user know there is a breakpoint hit
+
+	li		r3, 0x11
+	bl		exisendbyte__FUc		# tell the PC a bp has happened (send 0x11)
+
+	bl		_start			# bl mainloop, so you can set up a new breakpoint.
+
+_skipbp:
+	mfmsr	r1
+	rlwinm	r1,r1,0,31,29
+	rlwinm	r1,r1,0,17,15
+	mtmsr	r1				# we disable the interrupt so nothing interfers with the restore
+
+	lis		r1, regbuffer@h
+	ori		r1, r1, regbuffer@l
+
+	lwz		r3,0x0(r1)
+	mtcr	r3				# restores CR
+	lwz		r3,0x14(r1)
+	mtsrr0	r3				# restores SRR0
+	lwz		r3,0x18(r1)
+	mtsrr1	r3				# restores SRR1
+	lwz		r3,0x9C(r1)
+	mtlr	r3				# restores LR
+
+	lmw		r2,0x24(r1)		# restores r2-r31
+
+	lwz		r0,0x1C(r1)		# restores r0
+	lwz		r1,0x20(r1)		# restores r1
+
+	rfi						# back to the game
+
+
+#******************************************************************
+#                Finish
+#		Check if the gecko has been paused. if no return to game
+#******************************************************************
+
+finish:
+	bl get_frozenvalue__Fv
+	cmpwi	r3, 0			# check to see if we have frozen the game
+	bne	frozen				# loop around if we have
+							# (changed to return for the bp)
+
+resumegame:
+
+    sth		r28,0x4010(r20) # restore memory protection value
+
+	lfd		f2,152(r1)		# loads f2
+	lfd		f3,160(r1)		# loads f3
+
+	mtmsr	r25
+
+	lwz		r0,172(r1)
+	mtlr	r0				# restores lr
+
+	lwz		r0,12(r1)
+	mtcr	r0				# restores cr
+
+	lwz		r0,16(r1)
+	mtctr	r0				# restores ctr
+
+	lwz		r0,20(r1)
+	mtxer	r0				# restores xer
+
+	lmw		r3,24(r1)		# restores r3-r31
+
+	lwz		r0,8(r1)		# loads r0
+
+	addi	r1,r1,168
+
+	isync
+
+	blr				# return back to game
+
+#******************************************************************
+#                        Write branch
+#    r3 - source (our mastercode location)
+#    r4 - destination (lowmem area 0x80001800 address which will branch to
+#******************************************************************
+.global writebranch
+writebranch:
+	subf	r6, r3, r4		# subtract r3 from r4 and place in r17
+	lis	r5, 0x4800		# 0x48000000
+	rlwimi	r5,r6,0,6,29
+	stw	r5, 0(r3)		# result in r3
+	
+	dcbf    r0, r3		# data cache block flush
+	sync
+	icbi    r0, r3
+	isync
+
+	blr				# return
+
+#==================================================================
+       
+frozenvalue:	#frozen value, then LR
+.long	0,0
+command:
+.byte 0,0,0,0
+
+.align 4
+.long 0
+stringer:
+.ascii	"I found a codez 1\n"
+stringer2:
+.ascii	"I found a codez 2\n"
+stringer3:
+.ascii	"I found a codez 3\n"
+.end
+
-- 
cgit v1.2.3