#include <environment.h>
#include <services/services.h>
#include "clockquery.h"

unsigned long g_nClockIn = 0;

#ifndef CLKIN
#error "CLKIN must be defined (in Mhz)!"
#endif

unsigned long getCoreClockFrequency (void) {
	unsigned long nClkIn = CLKIN * 1000000;
	if (g_nClockIn) {
		nClkIn = g_nClockIn;
	}
	unsigned short msel = (*pPLL_CTL >> 9) & 0x003f;
	unsigned short csel = (*pPLL_DIV >> 4) & 0x0003;
	unsigned long nVCO = msel * nClkIn;
	unsigned long nCclk = 0;
	switch (csel) {
		case 0: nCclk = nVCO;
				break;
		case 1: nCclk = nVCO / 2;
				break;
		case 2: nCclk = nVCO / 4;
				break;
		case 3: nCclk = nVCO / 8;
				break;
	}
	return nCclk;
}

unsigned long getSystemClockFrequency (void) {
	unsigned long nClkIn = CLKIN;
	if (g_nClockIn) {
		nClkIn = g_nClockIn;
	}
	unsigned short msel = (*pPLL_CTL >> 9) & 0x003f;
	unsigned short ssel = *pPLL_DIV & 0x000f;
	unsigned long nVCO = msel * nClkIn;
	return (nVCO / ssel);
}

void setClockIn (unsigned long pa_nClockIn) {
	g_nClockIn = pa_nClockIn;
}