amt630h-sdk-beta/STEPLDR/Src/clock.c

#include "amt630h.h"
#include "clock.h"

#define PLL_DIV_MASK			0xFF
#define PLL_DIV_OFFSET			0
#define PLL_NO_MASK				0x3
#define PLL_NO_OFFSET			12
#define PLL_ENA					(1 << 14)

typedef struct {
	uint32_t clkid;
	uint32_t clktype; 
	uint32_t source;
	int clksource[MAX_CLK_SOURCE_NUM];
	int source_index;
	int div;
	union  {
		uint32_t fixed_freq;
		struct {
			int div;
			int mult;
		} fixed_fator_property;
		struct {
			uint32_t cfgreg;
			uint32_t refclkreg;
			uint32_t offset;
			uint32_t mask;
		} pll_property;
		struct {
			uint32_t cfgreg;
			uint32_t analogreg;
		} dds_property;
		struct {
			uint32_t cfgreg;
			int index_offset;
			int index_value;
			uint32_t index_mask;
			int div_offset;
			int div_value;
			uint32_t div_mask;
			int div_mode;
		} sys_property;
	} u;
} xClockProperty;

xClockProperty xClocks[] = {
	{.clkid = CLK_XTAL24M, .clktype = FIXED_CLOCK, .u.fixed_freq = 24000000},
	{.clkid = CLK_12MHZ, .clktype = FIXED_FACTOR_CLOCK, .clksource = {CLK_XTAL24M},
	 .u.fixed_fator_property.div = 2, .u.fixed_fator_property.mult = 1},
	{.clkid = CLK_6MHZ, .clktype = FIXED_FACTOR_CLOCK, .clksource = {CLK_XTAL24M},
	 .u.fixed_fator_property.div = 4, .u.fixed_fator_property.mult = 1},
	{.clkid = CLK_CPUPLL, .clktype = PLL_CLOCK, .clksource = {CLK_6MHZ, CLK_12MHZ},
	 .u.pll_property.cfgreg = 0x60000088, .u.pll_property.refclkreg = 0x60000040,
	 .u.pll_property.offset = 24, .u.pll_property.mask = 1},
	{.clkid = CLK_SYSPLL, .clktype = PLL_CLOCK, .clksource = {CLK_6MHZ, CLK_12MHZ},
	 .u.pll_property.cfgreg = 0x6000008c, .u.pll_property.refclkreg = 0x60000040,
	 .u.pll_property.offset = 25, .u.pll_property.mask = 1},
	{.clkid = CLK_LCD, .clktype = SYS_CLOCK, .clksource = {CLK_SYSPLL, CLK_CPUPLL, CLK_XTAL24M, CLK_XTAL24M},
	 .u.sys_property.cfgreg = 0x6000004c, .u.sys_property.index_offset = 0,
	 .u.sys_property.index_mask = 0x3, .u.sys_property.index_value = 1,
	 .u.sys_property.div_offset = 3, .u.sys_property.div_mask = 0x1f,
	 .u.sys_property.div_value = 11, .u.sys_property.div_mode = DIVMODE_PLUSONE,}, 
};

#define CLOCK_NUM	(sizeof(xClocks) / sizeof(xClocks[0]))

static xClockProperty *clk_get(uint32_t clkid)
{
	int i;
	
	for (i = 0; i < CLOCK_NUM; i++) {
		if (xClocks[i].clkid == clkid) {
			return &xClocks[i];
		}
	}

	return NULL;
}

static uint32_t clk_fixed_get_rate(xClockProperty *clk)
{
	return clk->u.fixed_freq;
}

static uint32_t clk_fixed_factor_get_rate(xClockProperty *clk)
{
	xClockProperty *parentclk = clk_get(clk->clksource[0]);
	
	return clk_fixed_get_rate(parentclk) * clk->u.fixed_fator_property.mult
				/ clk->u.fixed_fator_property.div;
}

static uint32_t clk_pll_get_rate(xClockProperty *clk)
{
	uint32_t parent_rate;
	uint32_t div, no, reg;

	parent_rate = ulClkGetRate(clk->clksource[clk->source_index]);

	reg = readl(clk->u.pll_property.cfgreg);
	no = (reg >> PLL_NO_OFFSET) & PLL_NO_MASK;
	div = (reg >> PLL_DIV_OFFSET) & PLL_DIV_MASK;

	return (parent_rate * div) / (1 << no);
}

static uint32_t clk_sys_get_rate(xClockProperty *clk)
{
	uint32_t parent_rate;
	
	parent_rate = ulClkGetRate(clk->clksource[clk->source_index]);

	return parent_rate / clk->div;  
}

static void clk_pll_init(xClockProperty *clk)
{
	clk->source_index = (readl(clk->u.pll_property.refclkreg) >> clk->u.pll_property.offset)
						& clk->u.pll_property.mask;
}

static int clk_get_div(int div, int divmode)
{
	switch(divmode) {
	case DIVMODE_NOZERO:
		div = div ? div : 1;
		break;
	case DIVMODE_PLUSONE:
		div = div + 1;
		break;
	case DIVMODE_DOUBLE:
		div *= 2;
		break;
	case DIVMODE_EXPONENT:
		div = 1 << div;
		break;
	case DIVMODE_PONEDOUBLE:
		div = (div + 1) * 2;
		break;
	}
	return div;
}

static int clk_set_div(int div, int divmode)
{
	switch(divmode) {
	case DIVMODE_PLUSONE:
		div = div - 1;
		break;
	case DIVMODE_DOUBLE:
		div /= 2;
		break;
	case DIVMODE_EXPONENT:
		div = fls(div) - 1;
		break;
	case DIVMODE_PONEDOUBLE:
		div = div / 2 - 1;
		break;
	}
	return div;
}

static void clk_sys_set_rate(xClockProperty *clk, uint32_t freq)
{
	int div;
	uint32_t reg;
	uint32_t parent_rate;

	parent_rate = ulClkGetRate(clk->clksource[clk->source_index]);
	div = DIV_ROUND_UP(parent_rate, freq);
	clk->div = div;
	div = clk_set_div(div, clk->u.sys_property.div_mode);
	reg = readl(clk->u.sys_property.cfgreg);
	reg &= ~(clk->u.sys_property.div_mask << clk->u.sys_property.div_offset);
	reg |= (div & clk->u.sys_property.div_mask) << clk->u.sys_property.div_offset;
	writel(reg, clk->u.sys_property.cfgreg);
}

static void clk_sys_init(xClockProperty *clk)
{
	uint32_t reg;
	uint32_t val;

	if (clk->u.sys_property.index_value >= 0 && clk->u.sys_property.index_mask) {
		clk->source_index = clk->u.sys_property.index_value;
		val = clk->u.sys_property.index_value == 3 ? 4 : clk->u.sys_property.index_value;
		reg = readl(clk->u.sys_property.cfgreg);
		reg &= ~(clk->u.sys_property.index_mask << clk->u.sys_property.index_offset);
		reg |= (val & clk->u.sys_property.index_mask) << clk->u.sys_property.index_offset;
		writel(reg, clk->u.sys_property.cfgreg);
	} else if (clk->u.sys_property.index_mask) {
		reg = readl(clk->u.sys_property.cfgreg);
		val = (reg >> clk->u.sys_property.index_offset) & clk->u.sys_property.index_mask;
		clk->source_index = val == 4 ? 3 : val;
	}

	if (clk->u.sys_property.div_value >= 0 && clk->u.sys_property.div_mask) {
		val = clk_set_div(clk->u.sys_property.div_value, clk->u.sys_property.div_mode);
		clk->div = clk_get_div(val, clk->u.sys_property.div_mode);
		reg = readl(clk->u.sys_property.cfgreg);
		reg &= ~(clk->u.sys_property.div_mask << clk->u.sys_property.div_offset);
		reg |= (val & clk->u.sys_property.div_mask) << clk->u.sys_property.div_offset;
		writel(reg, clk->u.sys_property.cfgreg);		
	} else if (clk->u.sys_property.div_mask) {
		reg = readl(clk->u.sys_property.cfgreg);
		val = (reg >> clk->u.sys_property.div_offset) & clk->u.sys_property.div_mask;
		clk->div = clk_get_div(val, clk->u.sys_property.div_mode);
	} else if (clk->u.sys_property.div_value > 0) {
		clk->div = clk->u.sys_property.div_value;
	} else {
		clk->div = 1;
	}
}

void vClkInit(void)
{
	int i;
	xClockProperty *clk;

	for (i = 0; i < CLOCK_NUM; i++) {
		clk = &xClocks[i];
		if (clk->clktype == PLL_CLOCK) {
			clk_pll_init(clk);
		} else if (clk->clktype == SYS_CLOCK) {
			clk_sys_init(clk);
		}
	}
}

uint32_t ulClkGetRate(uint32_t clkid)
{
	xClockProperty *clk = clk_get(clkid);

	if (clk == NULL)
		return 0;

	switch (clk->clktype) {
	case FIXED_CLOCK:
		return clk_fixed_get_rate(clk);
	case FIXED_FACTOR_CLOCK:
		return clk_fixed_factor_get_rate(clk);
	case PLL_CLOCK:
		return clk_pll_get_rate(clk);
	case SYS_CLOCK:
		return clk_sys_get_rate(clk);
	}

	return 0;
}

void vClkSetRate(uint32_t clkid, uint32_t freq)
{
	xClockProperty *clk = clk_get(clkid);

	if (clk == NULL)
		return;

	if (clk->clktype == SYS_CLOCK)
		clk_sys_set_rate(clk, freq);
}