#include	"u.h"
#include	"../port/lib.h"
#include	"mem.h"
#include	"dat.h"
#include	"fns.h"
#include	"probe.h"

extern void template(void);

void dcbst(void *);
void icbi(void *);

void
meminvalidate(void *vp, int n)
{
	int i;
	u32int *fp;
	fp = vp;
	for(i = 0; i < n / sizeof fp[0]; i++){
		dcbst(fp+i);
		icbi(fp+i);
	}
}

Probe *
mkprobe(void *func, void (*pentry)(Probe *), void (*pexit)(Probe *))
{
	Probe *p;
	u32int *fp;
	u32int rjmp;
	int i;
	fp = (u32int*)func;
	p = malloc(sizeof p[0]);
print("entrycode = %p\n", p->entrycode);
//print("exitcode = %p\n", p->exitcode);
	p->entry = pentry;
	p->exit = pexit;
	p->func = fp;
	memmove(p->orig, fp, sizeof p->orig);
	/* 5 is size of rcall */
	rjmp = (ulong)p->entrycode;
/*
 00001028: 3c80dead 6084beef(804)	MOVW	$-559038737,R4
 00001030: 7c8803a6 4e800021(804)	BL	,0(R4)
*/
	i = 0;
	p->probe[i++] = 0x7cc802a6;
	p->probe[i++] = 0x3c800000 | (rjmp >> 16);
	p->probe[i++] = 0x60840000 | (rjmp & 0xffff);
	p->probe[i++] = 0x7c8803a6;
	p->probe[i] = 0x4e800020;
	memmove(p->entrycode, template, sizeof p->entrycode);
	meminvalidate(p, sizeof p[0]);
	//memmove(p->exitcode, pexittmpl, sizeof p->exitcode);
	return p;
}

void
probeinstall(Probe *p)
{
	u32int *fp;
	int x;
	x = splhi();
	fp = (u32int*)p->func;
	memmove(fp, p->probe, sizeof p->probe);
	meminvalidate(fp, sizeof p->orig);
	p->enabled = 1;
	splx(x);
}

void
probeuninstall(Probe *p)
{
	u32int *fp;
	int x;
	x = splhi();
	fp = (u32int*)p->func;
	memmove(fp, p->orig, sizeof p->orig);
	meminvalidate(fp, sizeof p->orig);
	p->enabled = 0;
	splx(x);
}

void
freeprobe(Probe *p)
{
	free(p);
}