static int initClock(int clock, int source, int divI, int divF, int MASH)
{
int ctl[] = {CLK_GP0_CTL, CLK_GP2_CTL};
int div[] = {CLK_GP0_DIV, CLK_GP2_DIV};
int src[CLK_SRCS] =
{CLK_CTL_SRC_PLLD,
CLK_CTL_SRC_OSC,
CLK_CTL_SRC_HDMI,
CLK_CTL_SRC_PLLC};
int clkCtl, clkDiv, clkSrc;
uint32_t setting;
if ((clock < 0) || (clock > 1)) return -1;
if ((source < 0) || (source > 3 )) return -2;
if ((divI < 2) || (divI > 4095)) return -3;
if ((divF < 0) || (divF > 4095)) return -4;
if ((MASH < 0) || (MASH > 3)) return -5;
clkCtl = ctl[clock];
clkDiv = div[clock];
clkSrc = src[source];
clkReg[clkCtl] = CLK_PASSWD | CLK_CTL_KILL;
/* wait for clock to stop */
while (clkReg[clkCtl] & CLK_CTL_BUSY)
{
usleep(10);
}
clkReg[clkDiv] =
(CLK_PASSWD | CLK_DIV_DIVI(divI) | CLK_DIV_DIVF(divF));
usleep(10);
clkReg[clkCtl] =
(CLK_PASSWD | CLK_CTL_MASH(MASH) | CLK_CTL_SRC(clkSrc));
usleep(10);
clkReg[clkCtl] |= (CLK_PASSWD | CLK_CTL_ENAB);
}
int rpi_gpclk_setup(int chan, int source, int div_int, int div_frac)
{
int MASH = 0;
uint32_t clksrc;
if (!rpi_registers_mapping.mapping_initialized)
return -1;
if ((source < 0) || (source > sizeof(rpi_gpclk_src_to_reg) /
sizeof(*rpi_gpclk_src_to_reg) ))
return -2;
if ((div_int < 2) || (div_int > 4095))
return -3;
if ((div_frac < 0) || (div_frac > 4095))
return -4;
if ((MASH < 0) || (MASH > 3))
return -5;
clksrc = rpi_gpclk_src_to_reg[source];
CLK_GPx_CTL(chan) = CLK_PASSWD | CLK_CTL_KILL;
while (CLK_GPx_CTL(chan) & CLK_CTL_BUSY){
usleep(10);
}
CLK_GPx_DIV(chan) = (CLK_PASSWD | CLK_DIV_DIVI(div_int) | CLK_DIV_DIVF(div_frac));
usleep(10);
CLK_GPx_CTL(chan) = (CLK_PASSWD | CLK_CTL_MASH(MASH) | CLK_CTL_SRC(clksrc));
usleep(10);
CLK_GPx_CTL(chan) |= (CLK_PASSWD | CLK_CTL_ENAB);
return 0;
}
