#include	"mem.h"


#define	argp		16
#define	entry	0
#define	entrycode	64
#define	exit		4
#define	func		8
#define	orig1	24
#define	orig2	28
#define	orig3	32
#define	orig4	36
#define	orig5	40
#define	probe1	44
#define	probe2	48
#define	probe3	52
#define	probe4	56
#define	probe5	60
#define	rval		20
#define	saveret	12

#define	ax	0
#define	cx	1
#define	dx	2
#define	bx	3
#define	bp	4
#define	si	5
#define	di	6
#define	sp	7
#define call0		WORD $(0x48000005)
#define DCBST(ra, rb)		WORD $((31<<26) | (54<<1) | ((rb)<<11) | ((ra)<<16))
#define ICBI(ra, rb)		WORD $((31<<26) | (982<<1) | ((rb)<<11) | ((ra)<<16))
#define INFLOOP WORD $(0x48000000)
#define	MSYNC	WORD	$((31<<26)|(598<<1))

TEXT dcbst(SB), $0
	DCBST(0, 3)
	RETURN

TEXT icbi(SB), $0
	ICBI(0, 3)
	RETURN

TEXT template(SB), $0
	/* save working register */
	/* 0(R1) return PC */
/*0*/	call0
/*1*/	MOVW	LR, R4  
/*2*/	SUB $(3*4 + entrycode), R4, R4

	/* save argp */
/*3*/	MOVW R1, R5
/*4*/	ADD	$12, R5, R5
/*5*/	MOVW R5, argp(R4)

	/* save return address of probed function */
/*6*/	MOVW R6, saveret(R4)

	/* don't touch 0(SP) so that getcallerpc will work */

	/* uninstall probe  */

/*7*/	MOVW	MSR, R7
/*8*/	RLWNM	$0, R7, $~MSR_EE, R8
/*9*/	RLWNM	$0, R8, $~MSR_CE, R8
/*10*/	MOVW	R8, MSR
/*11*/	MSYNC
/*12*/	ISYNC


/*13*/	MOVW func(R4), R5
/*14*/	MOVW orig1(R4), R6
/*15*/	MOVW	R6, (R5)
/*16*/	MOVW orig2(R4), R6
/*17*/	MOVW	R6, 4(R5)
/*18*/	MOVW orig3(R4), R6
/*19*/	MOVW	R6, 8(R5)
/*20*/	MOVW orig4(R4), R6
/*21*/	MOVW	R6, 12(R5)
/*22*/	MOVW orig5(R4), R6
/*23*/	MOVW	R6, 16(R5)

/*24*/	MOVW	MSR, R8
/*25*/	RLWMI	$0, R7, $MSR_EE, R8
/*26*/	RLWMI	$0, R7, $MSR_CE, R8
/*27*/	MOVW	R8, MSR
/*28*/	MSYNC
/*29*/	ISYNC
		

/*30*/	DCBST(0, 5);
/*31*/	ICBI(0, 5);
/*32*/	ADD	$16, R5, R5
/*33*/	DCBST(0, 5);
/*34*/	ICBI(0, 5);
/*35*/	SUB	$16, R5, R5

	/* call the hook last. charles says there's already space in the stack for first arg */
/*36*/		MOVW	R3, 4(SP)
/*37*/	SUB		$8, R1, R1
/*38*/		MOVW	R5, 0(SP)
/*39*/	MOVW	entry(R4), R5
/*40*/		MOVW	R5, LR
/*41*/		MOVW	R4, R3
/*42*/	BL		(LR)
/*43*/		MOVW	0(SP), R5
/*44*/		ADD		$8, R1, R1
/*45*/		MOVW	4(SP),R3

	/* now call the function */
/*46*/	MOVW	R5, LR

	/* undo the hidden stack update by ken's not assembler for duration of actual function exec */
/*47*/	ADD		$8, R1, R1
/*48*/	BL		(LR)

	/* redo stack update */
/*49*/	SUB	$8, R1, R1
/*50*/	call0
/*51*/	MOVW	LR, R4
	SUB $((51+2)*4 + entrycode), R4, R4
	SUB		$8, R1, R1
	MOVW	R3, 0(SP)
	MOVW	R4, 4(SP)

	/* call the hook first */
	MOVW exit(R4), R5
	MOVW	R5, LR
	MOVW	R4, R3
	BL		(LR)
	MOVW	0(SP), R3
	MOVW	4(SP), R4
	ADD		$8, R1, R1

	MOVW	MSR, R7
	RLWNM	$0, R7, $~MSR_EE, R8
	RLWNM	$0, R8, $~MSR_CE, R8
	MOVW	R8, MSR
	MSYNC
	ISYNC

	/* reinstall probe  */
	MOVW func(R4), R5
	MOVW probe1(R4), R6
	MOVW		R6, (R5)
	MOVW probe2(R4),  R6
	MOVW		R6, 4(R5)
	MOVW probe3(R4),  R6
	MOVW		R6, 8(R5)
	MOVW probe4(R4),  R6
	MOVW		R6, 12(R5)
	MOVW probe5(R4),  R6
	MOVW		R6, 16(R5)

	MOVW	MSR, R8
	RLWMI	$0, R7, $MSR_EE, R8
	RLWMI	$0, R7, $MSR_CE, R8
	MOVW	R8, MSR
	MSYNC
	ISYNC

	DCBST(0, 5);
	ICBI(0, 5);
	ADD	$16, R5, R5
	DCBST(0, 5);
	ICBI(0, 5);

	MOVW	saveret(R4), R5
	MOVW	R5,LR
	//INFLOOP;
	/* undo stack update */
	ADD	$8, R1, R1
	BR	(LR)