#include <proto/exec.h>
#include <proto/performancemonitor.h>
#include <exec/interrupts.h>

static struct PerformanceMonitorIFace *IPM;
static struct Interrupt CounterInt;

#undef DebugPrintF
#define dprintf(format, args...)((struct ExecIFace *)((*(struct ExecBase **)4)->MainInterface))->DebugPrintF("[%s] " format, __PRETTY_FUNCTION__ , ## args)

static struct IntData
{
	struct PerformanceMonitorIFace *IPM;
	uint16 *Buffer;
	uint32 BufferSize;
	uint32 Offset;
	uint32 Scale;
	uint32 CounterStart;
} ProfileData;


uint32 
GetCounterStart(void)
{
	uint64 fsb;
	double bit0time;
	uint32 count;
	
	GetCPUInfoTags(
			GCIT_FrontsideSpeed,	&fsb,
			TAG_DONE);
	
	/* Timebase ticks at 1/4 of FSB */
	bit0time = 8.0 / (double)fsb;
	count = (uint32)(0.01 / bit0time);
	
	return 0x80000000 - count;
}
	
uint32 
CounterIntFn(struct ExceptionContext *ctx, struct ExecBase *ExecBase,
	struct IntData *ProfileData)
{
	uint32 sia = (uint32)ProfileData->IPM->GetSampledAddress();
	dprintf(".\n");
	sia = ((sia - ProfileData->Offset) * ProfileData->Scale) >> 16;
	
	if (sia <= (ProfileData->BufferSize>>1))
	{
		//if (ProfileData->Buffer[sia] != 0xffff)
			ProfileData->Buffer[sia]++;
	}
		
	IPM->CounterControl(1, ProfileData->CounterStart, PMCI_Transition);

	return 1;
}	

	
int 
profil(uint16 *buffer, uint32 bufSize, uint32 offset, uint32 scale)
{
	APTR Stack;
	
	if (buffer == 0)
	{
		Stack = SuperState();
		IPM->EventControlTags(
			PMECT_Disable, 			PMEC_MasterInterrupt,
			TAG_DONE);
	
		IPM->SetInterruptVector(1, 0);
		
		IPM->Unmark(0);
		IPM->Release();
		if (Stack) UserState(Stack);

		return 0;
	}
	
	IPM = (struct PerformanceMonitorIFace *)
			OpenResource("performancemonitor.resource");
			
	if (!IPM || IPM->Obtain() != 1)
	{
		dprintf("No performance monitor\n");
		return 0;
	}
	
	Stack = SuperState();
		
	/* Init IntData */
	ProfileData.IPM = IPM;
	ProfileData.Buffer = buffer;
	ProfileData.BufferSize = bufSize;
	ProfileData.Offset = offset;
	ProfileData.Scale = scale;
	ProfileData.CounterStart = GetCounterStart();
	
	/* Set interrupt vector */
	CounterInt.is_Code = (void (*)())CounterIntFn;
	CounterInt.is_Data = &ProfileData;
	IPM->SetInterruptVector(1, &CounterInt);
	
	/* Prepare Performance Monitor */
	IPM->MonitorControlTags(
			PMMCT_FreezeCounters,			PMMC_Unmarked,
			PMMCT_RTCBitSelect,				PMMC_BIT0,
			TAG_DONE);
	IPM->CounterControl(1, ProfileData.CounterStart, PMCI_Transition);
	
	IPM->EventControlTags(
		PMECT_Enable, 			1,
		PMECT_Enable, 			PMEC_MasterInterrupt,
		TAG_DONE);
	
	IPM->Mark(0);
	
	if (Stack) UserState(Stack);
	
	return 0;
}
