/* Function profiling code V2.13 17/2/08
   See jprof2.h for details
   Copyright 2008 Jeffrey Lee
   This file is part of WOUM.
   WOUM is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   WOUM is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with WOUM.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _JPROF2_C
#define _JPROF2_C

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "kernel.h"
#include "swis.h"

#include "jprof2.h"
#include "sasm.h"
#include "error.h"

jprof2_struct *jprof2_funcs = 0;
int jprof2_numfuncs = 0;
int jprof2_ticspersec = 1000;

static int inited = 0;
static int timeptr;

void jprof2_init()
{
	int p;
	_kernel_swi_regs regs;
	if (inited)
		return;
	if (_kernel_swi(0x4D400,&regs,&regs) != 0)
	{
		warning("Can't get time. Is the DTime module loaded?");
		return; /* Can't get time */
	}
	timeptr = regs.r[0];
	p=jprof2_numfuncs=0;
	do {
		p = jprof2_nextfunc(p,1);
		jprof2_numfuncs++;
	} while (p != 0);
	jprof2_numfuncs--;
	jprof2_funcs = malloc(sizeof(jprof2_struct)*jprof2_numfuncs);
	if (jprof2_funcs == 0)
		return;
	p=jprof2_numfuncs=0;
	do {
		p = jprof2_nextfunc(p,1);
		if (p != 0)
		{
			jprof2_funcs[jprof2_numfuncs].name = jprof2_funcname(p);
			jprof2_funcs[jprof2_numfuncs].adr = (int *) p;
			jprof2_funcs[jprof2_numfuncs].func_entered = 0;
			jprof2_funcs[jprof2_numfuncs].time_total = 0;
			jprof2_funcs[jprof2_numfuncs].count = 0;
			jprof2_funcs[jprof2_numfuncs].func_r14 = 0;
			jprof2_funcs[jprof2_numfuncs].code = 0;
			jprof2_funcs[jprof2_numfuncs++].instr = *((int *) p);
		}
	} while (p != 0);
	inited = 1;
}

static jprof2_struct *findfunc(char *name)
{
	int i;
	for (i=0;i<jprof2_numfuncs;i++)
		if (strcmp(name,jprof2_funcs[i].name) == 0)
			return &(jprof2_funcs[i]);
	return 0;
}

int jprof2_forceadd(void *func,char *name)
{
	int i;
	jprof2_struct *funcs;
	if(!inited)
		return 1;
	if(findfunc(name))
		return 1;
	/* If we've generated any code, or it's already listed, abort */
	for(i=0;i<jprof2_numfuncs;i++)
		if((jprof2_funcs[i].code) || (jprof2_funcs[i].adr == func))
			return 1;
//	fprintf(stderr,"%s = %08x = %08x\n",name,func,*((int *) func));
	/* Put it in */
	funcs = malloc(sizeof(jprof2_struct)*(jprof2_numfuncs+1));
	for(i=0;(i < jprof2_numfuncs) && ((int) jprof2_funcs[i].adr < (int) func);i++)
		memcpy(&(funcs[i]),&(jprof2_funcs[i]),sizeof(jprof2_struct));
	funcs[i].name = name;
	funcs[i].adr = (int *) func;
	funcs[i].func_entered = 0;
	funcs[i].time_total = 0;
	funcs[i].count = 0;
	funcs[i].func_r14 = 0;
	funcs[i].code = 0;
	funcs[i].instr = *((int *) func);
	for(;i<jprof2_numfuncs;i++)
		memcpy(&(funcs[i+1]),&(jprof2_funcs[i]),sizeof(jprof2_struct));
	free(jprof2_funcs);
	jprof2_funcs = funcs;
	jprof2_numfuncs++;
	return 0;
}

int jprof2_profile(char *name)
{
	jprof2_struct *s;
	if (!inited)
		return 1;
	s = findfunc(name);
	if (s == 0)
		return 1; /* Can't profile unknown func */
	if (*s->adr != s->instr)
		return 0; /* Already profiling */
	if (s->code == 0)
	{
		/* Generate profiling code */
		s->code = malloc(jprof2_codeblock_length);
		if (s->code == 0)
			return 1;
		memcpy(s->code,jprof2_codeblock,jprof2_codeblock_length);
		if((s->instr & 0xFF000000) == 0xEA000000)
		{
			/* First instruction is B */
			s->code[jprof2_codeblock_instr/4] = (s->instr & 0xFFFFFF)+(((int) s->adr)-((int) &(s->code[jprof2_codeblock_instr/4])))/4;
			s->code[jprof2_codeblock_instr/4] = 0xEA000000 | (s->code[jprof2_codeblock_instr/4] & 0xFFFFFF);
//			fprintf(stderr,"Remaped %08x @ %08x to %08x @ %08x\n",s->instr,s->adr,s->code[jprof2_codeblock_instr/4],&(s->code[jprof2_codeblock_instr/4]));
		}
		else if((s->instr & 0xFF7FF000) == 0xE51FF000)
		{
			int i;
			/* First instruction is LDR PC,&... */
			s->code[jprof2_codeblock_instr/4] = 0xE51FF004; /* Load PC from 'branch' pos */
			/* Decode instr to get PC location */
			i = s->instr & 0xFFF;
			if(!(s->instr & (1<<23)))
				i = -i;
			i += 8;
//			fprintf(stderr,"LDR offset = %08x, so PC = %08x, preceeding word = %08x\n",i,s->adr[i/4],((int *) (s->adr[i/4]))[-1]);
			s->code[jprof2_codeblock_branch/4] = s->adr[i/4];
		}
		else
		{
			s->code[jprof2_codeblock_instr/4] = s->instr;
			sasm_b(&s->code[jprof2_codeblock_branch/4],AL,0,&s->adr[1]);
		}
		s->code[jprof2_codeblock_struct/4] = (int) s;
		s->code[jprof2_codeblock_timeptr/4] = timeptr;
		sasm_sync(s->code,jprof2_codeblock_length);
	}
	sasm_b(s->adr,AL,0,&s->code[jprof2_codeblock_entry/4]); /* Enable profiling */
	sasm_sync(s->adr,4);
	return 0;
}

int jprof2_stop(char *name)
{
	jprof2_struct *s;
	if (!inited)
		return 0; /* Not running, therefore not profiling */
	s = findfunc(name);
	if (s == 0)
		return 0; /* Don't complain if unknown func */
	if (*s->adr != s->instr)
	{
		*s->adr = s->instr; /* Reset entry point */
		sasm_sync(s->adr,4);
	}
	if (s->func_entered == 0)
	{
		if (s->code)
			free(s->code); /* Dispose of profiling code */
		s->code = 0;
		return 0;
	}
	return 1; /* Profiling disabled, but code still active */
}

void jprof2_reset(char *name)
{
	jprof2_struct *s;
	int i;
	if (!inited)
		return;
	if (name == 0)
	{
		for (i=0;i<jprof2_numfuncs;i++)
			jprof2_funcs[i].time_total = jprof2_funcs[i].count = 0;
		return;
	}
	s = findfunc(name);
	if (s == 0)
		return;
	s->time_total = s->count = 0;
}

int jprof2_shutdown()
{
	int i,s;
	s = 0;
	for (i=0;i<jprof2_numfuncs;i++)
		if (jprof2_funcs[i].code)
			s |= jprof2_stop(jprof2_funcs[i].name);
	if (s)
		return 1; /* Profiling still running for something */
	free(jprof2_funcs);
	jprof2_funcs = 0;
	jprof2_numfuncs = 0;
	inited = 0;
	return 0;
}

#endif
