#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