/*------------------------------------------------------------------------------
--  This file is a part of the libuc, microcontroler library
--  Copyright (C) 2011, Alexis Jeandet
--
--  This program 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.
--
--  This program 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 this program; if not, write to the Free Software
--  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
-------------------------------------------------------------------------------
--                 Author : Alexis Jeandet
--                   Mail : alexis.jeandet@gmail.com
-------------------------------------------------------------------------------*/
#include "core.h"
extern unsigned int OSC0;
extern unsigned int INTOSC;
extern unsigned int RTCOSC;

void coresetCpuFreq(unsigned int freq)
{
	unsigned int inputFreq = 10000,PLLFREQ=300000000;
	unsigned int M0=6,N0=1;
	if(pll0getClksrc==pll0useInternal)
	{
		inputFreq=INTOSC;
	}
	if(pll0getClksrc==pll0useMainOsc)
	{
		inputFreq=OSC0;
	}
	if(pll0getClksrc==pll0useRTC)
	{
		inputFreq=RTCOSC;
	}
	CCLKCFG = (PLLFREQ/freq)-1;
	while(N0<33)
	{
		M0=(PLLFREQ*N0)/(2*inputFreq);
		if((6<M0)&&(M0<512))
		{
			PLLCFG = (M0-1)+((N0-1)<<16);
			PLLFEED = 0xAA;
			PLLFEED = 0x55;
			PLLCON = 1;
			PLLFEED = 0xAA;
			PLLFEED = 0x55;
			while (!(PLLSTAT & (1<<26)));
			PLLCON = 3;
			PLLFEED = 0xAA;
			PLLFEED = 0x55;
			while (!(PLLSTAT & ((1<<25) | (1<<24))));
			break;
		}
		N0++;
	}

}




unsigned int coregetCpuFreq()
{
	unsigned int inputFreq;
	unsigned int M0,N0,CPUDIV;
	M0 = (PLLCFG & 0x3FFF) + 1;
	N0 = ((PLLCFG>>16) & 0xFF) + 1;
	CPUDIV = CCLKCFG + 1;
	if(pll0getClksrc==pll0useInternal){ inputFreq=INTOSC;}
	if(pll0getClksrc==pll0useMainOsc) {inputFreq=OSC0;}
	if(pll0getClksrc==pll0useRTC)      {inputFreq=RTCOSC;}
	if((PLLCON & 2)==2) {return ((inputFreq*M0*2)/(N0*CPUDIV));}
	return inputFreq;
}