/*
 * File      : clock.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2006, RT-Thread Development Team
 *
 *  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 2 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.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Change Logs:
 * Date           Author       Notes
 * 2011-01-13     weety       first version
 */

#include <rtthread.h>
#include "at91sam9g45.h"

static rt_list_t clocks;

struct clk {
	char name[32];
	rt_uint32_t rate_hz;
	struct clk *parent;
	rt_list_t  node;
};

static struct clk clk32k = {
	"clk32k",
	AT91C_SLOW_CLOCK,
	RT_NULL,
	{RT_NULL, RT_NULL},
};

static struct clk main_clk = {
	"main",
	0,
	RT_NULL,
	{RT_NULL, RT_NULL},
};

static struct clk plla = {
	"plla",
	0,
	&main_clk,
	{RT_NULL, RT_NULL},
};

static struct clk mck = {
	"mck",
	0,
	NULL,
	{RT_NULL, RT_NULL},
};

static struct clk upllck = {
	"upllck",
	480*1000*1000,
	&main_clk,
	{RT_NULL, RT_NULL},
};

static struct clk *const standard_pmc_clocks[] = {
	/* four primary clocks */
	&clk32k,
	&main_clk,
	&plla,

	/* MCK */
	&mck
};

/* clocks cannot be de-registered no refcounting necessary */
struct clk *clk_get(const char *id)
{
	struct clk *clk;
	rt_list_t *list;
	
	for (list = (&clocks)->next; list != &clocks; list = list->next)
	{
		clk = (struct clk *)rt_list_entry(list, struct clk, node);
		if (rt_strcmp(id, clk->name) == 0)
			return clk;
	}

	return RT_NULL;
}

rt_uint32_t clk_get_rate(struct clk *clk)
{
	rt_uint32_t	rate;

	for (;;) {
		rate = clk->rate_hz;
		if (rate || !clk->parent)
			break;
		clk = clk->parent;
	}
	return rate;
}

static void at91_upllck_init(rt_uint32_t main_clock)
{
	// EHCI USB use fixed 480MHz clock
}

static struct clk *at91_css_to_clk(unsigned long css)
{
	switch (css) {
		case AT91C_PMC_CSS_SLOW_CLK:
			return &clk32k;
		case AT91C_PMC_CSS_MAIN_CLK:
			return &main_clk;
		case AT91C_PMC_CSS_PLLA_CLK:
			return &plla;
		case AT91C_PMC_CSS_UPLL_CLK:
			return &upllck;
	}

	return RT_NULL;
}

// TODO: how to auto-set register value by OSC and MCK
/* Settings at 400/133MHz */
// In datasheet, ATMEL says 12MHz main crystal startup time less than 2ms, so we
// configure OSC startup timeout to 64*8/32768=15.6ms, should enough
#define BOARD_OSCOUNT			(AT91C_CKGR_OSCOUNT & (64 << 8))
// MAINCK => Divider(DIVA) => PLLA(MULA, OUTA) => /1/2 Divider(PLLADIV2) => PLLACK
// pls. refer to doc6438G figure 24-6 on pg294.		ICPLLA in reg PMC_PLLICPR
// 12MHz / 3 * (199 + 1) = 800MHz
// OUTA/ICPLLA can as ICPLLA:OUTA[1]:OUTA[0] = (800-PLLAOUT(MHz))/50
// PLLACOUNT field occupy bit[13:8], max value is 0x3F, then about 19.2ms
#define BOARD_CKGR_PLLA			(AT91C_CKGR_SRCA | AT91C_CKGR_OUTA_0)
#define BOARD_PLLACOUNT			(0x3F << 8)
#define BOARD_MULA				(AT91C_CKGR_MULA & (199 << 16))
#define BOARD_DIVA				(AT91C_CKGR_DIVA & 3)
// Clock Source => select(CCS) => Prescaler(PRES) => Master Clock Divider(MDIV) => MCK
//                                                => Processor Clock Divider => PCK
// Master clock can refer to doc6438G figure 25-2 on pg298
// PLLADIV2=1(div 2, 400MHz), PRES=0(no div, 400MHz),
// MDIV=3(Master Clock divided by 3, 133MHz), CSS=0(still Slow Clock)
#define BOARD_PRESCALER			(0x00001300) //400/133MHz

#define MHz(n)		((n) * 1000 * 1000)
#define OSC_FREQ	MHz(12)
#define PLLA_FREQ	MHz(800)

static void at91_plla_init(void)
{
	rt_uint32_t pllar, mckr;

	// Code refer to doc6438G, 25.10 Programming Sequence
	/* Initialize main oscillator
	 ****************************/
	// enable main OSC and wait OSC startup time timeout.
	AT91C_BASE_PMC->PMC_MOR = BOARD_OSCOUNT | AT91C_CKGR_MOSCEN;
	while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MOSCS));

	/* Initialize PLLA, Set PLL to 800MHz and wait PLL stable */
	pllar = (MHz(800) - PLLA_FREQ) / MHz(50);		// please refer to Table 46-15 of doc 6438G
	AT91C_BASE_PMC->PMC_PLLICPR = (pllar >> 2) & 1;	// ICPLLA
	pllar = (pllar & 3) << 14;	// OUTA
	pllar |= BOARD_DIVA;		// PLLA input clock as 4MHz
	pllar |= BOARD_MULA;		// PLLA output clock as 800MHz
	pllar |= BOARD_PLLACOUNT;
	pllar |= AT91C_CKGR_SRCA;	// I don't known what means, but seems must set it
	AT91C_BASE_PMC->PMC_PLLAR = pllar;

	while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_LOCKA));

	/* Wait for the master clock if it was already initialized */
	// make sure Master clock in READY status before operate it
	while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY));

	/* Switch to fast clock
	 **********************/
	/* setup main clock divisor and prescaler, 400MHz/133MHz, but don't switch */
	mckr = AT91C_BASE_PMC->PMC_MCKR;
	if ((mckr & AT91C_PMC_MDIV) != (BOARD_PRESCALER & AT91C_PMC_MDIV))
	{
		mckr = (mckr & ~(unsigned int)AT91C_PMC_MDIV) | (BOARD_PRESCALER & AT91C_PMC_MDIV);
		AT91C_BASE_PMC->PMC_MCKR = mckr;
		while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY));
	}

	/* Switch to PLL + prescaler, now Switch to PLLA as source, run on the fly */
	if ((mckr & AT91C_PMC_CSS) != AT91C_PMC_CSS_PLLA_CLK)
	{
		mckr = (mckr & ~(unsigned int)AT91C_PMC_CSS) | AT91C_PMC_CSS_PLLA_CLK;
		AT91C_BASE_PMC->PMC_MCKR = mckr;
		while (!(AT91C_BASE_PMC->PMC_SR & AT91C_PMC_MCKRDY));
	}

	plla.rate_hz = PLLA_FREQ;
}
    
#define false 0
#define true  1
int at91_clock_init(rt_uint32_t main_clock)
{
	unsigned tmp, freq, mckr, mdiv;
	int i;

	/*
	 * When the bootloader initialized the main oscillator correctly,
	 * there's no problem using the cycle counter.  But if it didn't,
	 * or when using oscillator bypass mode, we must be told the speed
	 * of the main clock.
	 */
	if (!main_clock) {
		do {
			tmp = readl(AT91C_CKGR_MCFR);
		} while (!(tmp & AT91C_CKGR_MAINRDY));
		main_clock = (tmp & AT91C_CKGR_MAINF) * (AT91C_SLOW_CLOCK / 16);
	}
	main_clk.rate_hz = main_clock;

	at91_plla_init();

	at91_upllck_init(main_clock);

	/*
	 * MCK and CPU derive from one of those primary clocks.
	 * For now, assume this parentage won't change.
	 */
	mckr = readl(AT91C_PMC_MCKR);
	mck.parent = at91_css_to_clk(mckr & AT91C_PMC_CSS);
	freq = mck.parent->rate_hz;
	freq /= (1 << ((mckr & AT91C_PMC_PRES) >> 2));	/* prescale */
	mdiv = 1 << ((mckr & AT91C_PMC_MDIV) >> 8);
	if (mdiv == 8) mdiv = 3;
	freq /= mdiv;			/* mdiv */
	if (mckr & AT91C_PMC_PLLADIV2) freq /= 2;	/* plla_div2 */
	mck.rate_hz = freq;	

	/* Register the PMC's standard clocks */
	rt_list_init(&clocks);
	for (i = 0; i < ARRAY_SIZE(standard_pmc_clocks); i++)
		rt_list_insert_after(&clocks, &standard_pmc_clocks[i]->node);

	rt_list_insert_after(&clocks, &upllck.node);

	/* MCK and CPU clock are "always on" */
	//clk_enable(&mck);

	/*rt_kprintf("Clocks: CPU %u MHz, master %u MHz, main %u.%03u MHz\n",
		freq / 1000000, (unsigned) mck.rate_hz / 1000000,
		(unsigned) main_clock / 1000000,
		((unsigned) main_clock % 1000000) / 1000);*///cause blocked

	return 0;
}

/**
 * @brief System Clock Configuration
 */
void rt_hw_clock_init(void)
{
	at91_clock_init(MHz(12));
}