// Clock Source Selection
static void program_mckr_css(unsigned long cssvalue)
{
PMC->PMC_MCKR = (PMC->PMC_MCKR & ~ PMC_MCKR_CSS_Msk) | (cssvalue & PMC_MCKR_CSS_Msk);
// Wiat MCLK ready
while ((PMC->PMC_SR & PMC_SR_MCKRDY) == 0)
;
}
// CPU prescaler
static void program_mckr_pres(unsigned long presvalue)
{
PMC->PMC_MCKR = (PMC->PMC_MCKR & ~ PMC_MCKR_PRES_Msk) | (presvalue & PMC_MCKR_PRES_Msk);
// Wiat MCLK ready
while ((PMC->PMC_SR & PMC_SR_MCKRDY) == 0)
;
}

// If a new value for CSS field corresponds to PLL Clock,
static void program_mckr_switchtopll_a(void)
{
program_mckr_pres(PMC_MCKR_PRES_CLK_2); // with /2 divider
program_mckr_css(PMC_MCKR_CSS_PLLA_CLK);
}

// If a new value for CSS field corresponds to Main Clock
static void program_mckr_switchtomain(void)
{
program_mckr_css(PMC_MCKR_CSS_MAIN_CLK);
#ifdef PMC_MCKR_PRES_CLK_1
program_mckr_pres(PMC_MCKR_PRES_CLK_1); // w/o divider
#else
program_mckr_pres(PMC_MCKR_PRES_CLK); // w/o divider
#endif
}

static void program_use_xtal(
int useXtalFlag /* 0 - использование RC генератора, не-0 - использование кварцевого генератора */
)
{
// бит CKGR_MOR_MOSCSEL - источник MAINCK это кварцевый генератор
const unsigned long mor = PMC->CKGR_MOR & ~ CKGR_MOR_KEY_Msk;
if (((mor & CKGR_MOR_MOSCSEL) != 0) == (useXtalFlag != 0))
return; // переключение не требуется

if (useXtalFlag != 0)
PMC->CKGR_MOR = CKGR_MOR_KEY(0x37) | (mor | CKGR_MOR_MOSCSEL);
else
PMC->CKGR_MOR = CKGR_MOR_KEY(0x37) | (mor & ~ CKGR_MOR_MOSCSEL);

// ожидание переключения кварцевого генератора
while ((PMC->PMC_SR & PMC_SR_MOSCSELS) == 0)
;
}

// Enable on-chip RC oscillator
static void program_enable_RC_12MHz(void)
{
#ifdef CKGR_MOR_MOSCRCF_12_MHz
PMC->CKGR_MOR = (PMC->CKGR_MOR & ~ (CKGR_MOR_MOSCRCF_Msk | CKGR_MOR_KEY_Msk)) | // остальные биты не трогаем
CKGR_MOR_KEY(0x37) | CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCRCF_12_MHz;
#else
PMC->CKGR_MOR = (PMC->CKGR_MOR & ~ (CKGR_MOR_MOSCRCF_Msk | CKGR_MOR_KEY_Msk)) | // остальные биты не трогаем
CKGR_MOR_KEY(0x37) | CKGR_MOR_MOSCRCEN | CKGR_MOR_MOSCRCF_12MHZ;
#endif
// ожидание запуска RC генератора
while ((PMC->PMC_SR & PMC_SR_MOSCRCS) == 0)
;
}

static void program_disable_rc(void)
{
const unsigned long mor = PMC->CKGR_MOR & ~ (CKGR_MOR_KEY_Msk | CKGR_MOR_MOSCRCEN);
PMC->CKGR_MOR = mor | CKGR_MOR_KEY(0x37);
}

static void program_disable_xtal(void)
{
const unsigned long mor = PMC->CKGR_MOR & ~ (CKGR_MOR_KEY_Msk | CKGR_MOR_MOSCXTEN);
PMC->CKGR_MOR = mor | CKGR_MOR_KEY(0x37);
}

// Enable high-frequency XTAL oscillator
static void program_enable_xtal(void)
{
const unsigned long mor = PMC->CKGR_MOR & ~ (CKGR_MOR_KEY_Msk | CKGR_MOR_MOSCXTST_Msk);
if ((mor & CKGR_MOR_MOSCXTEN) != 0)
return; // кварцевый генератор уже запущен

PMC->CKGR_MOR =
mor | // стврые значения битов
CKGR_MOR_KEY(0x37) |
CKGR_MOR_MOSCXTST(128) |
CKGR_MOR_MOSCXTEN;
// ожидание запуска кварцевого генератора
while ((PMC->PMC_SR & PMC_SR_MOSCXTS) == 0)
;
}

static void program_enable_plla(unsigned pllmul, unsigned plldiv)
{
#ifdef CKGR_PLLAR_ONE
/* Initialize PLLA */
PMC->CKGR_PLLAR =
(CKGR_PLLAR_ONE | // всегда должен быть установлен
((pllmul - 1) << CKGR_PLLAR_MULA_Pos) |
(0x4 << CKGR_PLLAR_PLLACOUNT_Pos) |
(plldiv << CKGR_PLLAR_DIVA_Pos));
#else
/* Initialize PLLA */
PMC->CKGR_PLLAR =
(CKGR_PLLAR_STUCKTO1 | // всегда должен быть установлен
((pllmul - 1) << CKGR_PLLAR_MULA_Pos) |
(0x4 << CKGR_PLLAR_PLLACOUNT_Pos) |
(plldiv << CKGR_PLLAR_DIVA_Pos));
#endif
// Ожидание запуска PLL A
while (!(PMC->PMC_SR & PMC_SR_LOCKA))
;
}

#if 0
// unused now.
static void program_enable_pllb(void)
{
//unsigned timer = 0xffffff;
//enum { osc_mul = 32, osc_div = 6 }; // 12 MHz / 6 * 32 = 64 MHz
enum { osc_mul = 8, osc_div = 1 }; // 12 MHz / 1 * 8 = 96 MHz
//enum { osc_mul = 32, osc_div = 3 }; // 12 MHz / 3 * 32 = 128 MHz

/* Initialize PLLA */
PMC->CKGR_PLLBR =
//CKGR_PLLBR_STUCKTO1 | // всегда должен быть установлен
((osc_mul - 1) << CKGR_PLLBR_MULB_Pos) |
(0x1 << CKGR_PLLBR_PLLBCOUNT_Pos) |
(osc_div << CKGR_PLLBR_DIVB_Pos);
//timeout = 0;
//while (!(PMC->PMC_SR & PMC_SR_LOCKA) && (timeout++ < CLOCK_TIMEOUT))
while ((PMC->PMC_SR & PMC_SR_LOCKB) == 0)
;
}


// If a new value for CSS field corresponds to Slow Clock,
static void program_mckr_switchtoslow(void)
{
program_mckr_css(PMC_MCKR_CSS_SLOW_CLK);
#ifdef PMC_MCKR_PRES_CLK_1
program_mckr_pres(PMC_MCKR_PRES_CLK_1); // w/o divider
#else
program_mckr_pres(PMC_MCKR_PRES_CLK); // w/o divider
#endif
}


#endif

/*
инициализация внутреннего умножителя частоты.
Вход - 12 МГц, внутренний RC генератор (12 МГц).
внутренняя тактовая - 48 МГц,
//частота генератора - 96 МГц
Частота сраавнения PLL = 12 МГц
*/
static void lowlevel_sam3s_init_pll_clock_12_RC12(void)
{
// Embedded Flash Wait State VDDCORE set at 1.80V
// FWS field = 0: up to 22 MHz
// FWS field = 1: up to 38 MHz
// FWS field = 2: up to 64 MHz
//EFC->EEFC_FMR = EEFC_FMR_FWS_Msk & (0U << EEFC_FMR_FWS_Pos);
EFC->EEFC_FMR = EEFC_FMR_FWS(3); // Flash Wait State

//program_mckr_switchtoslow(); // переключаем на внутренний генератор 32 кГц
program_mckr_switchtomain(); // выключить ФАПЧ, если была включена (проблема, если ранее был не ресет)
program_enable_RC_12MHz();
program_use_xtal(0);
program_disable_xtal();

EFC->EEFC_FMR = EEFC_FMR_FWS(0); // Flash Wait State
}


/*
инициализация внутреннего умножителя частоты.
Вход - 12 МГц, внутренний RC генератор (12 МГц).
внутренняя тактовая - 48 МГц,
//частота генератора - 96 МГц
Частота сраавнения PLL = 12 МГц
*/
static void lowlevel_sam3s_init_pll_clock_48_RC12(unsigned pllmul, unsigned plldiv, unsigned fws)
{
// Embedded Flash Wait State VDDCORE set at 1.80V
// FWS field = 0: up to 22 MHz
// FWS field = 1: up to 38 MHz
// FWS field = 2: up to 64 MHz
EFC->EEFC_FMR = EEFC_FMR_FWS(3); // Flash Wait State

//program_mckr_switchtoslow(); // переключаем на внутренний генератор 32 кГц
program_mckr_switchtomain(); // выключить ФАПЧ, если была включена
program_enable_RC_12MHz();
program_use_xtal(0);
program_disable_xtal();
program_enable_plla(pllmul, plldiv);
//program_enable_pllb();
program_mckr_switchtopll_a();

EFC->EEFC_FMR = EEFC_FMR_FWS(fws); // Flash Wait State
}

/*
инициализация внутреннего умножителя частоты.
Вход - 12 МГц, кварцевый резонатор
внутренняя тактовая - 64 МГц,
частота генератора - 12 МГц
Частота сраавнения PLL = 4 МГц
*/
static void
lowlevel_sam3s_init_pll_clock_48_xtal12(unsigned pllmul, unsigned plldiv, unsigned ws)
{
// Embedded Flash Wait State VDDCORE set at 1.80V
// FWS field = 0: up to 22 MHz
// FWS field = 1: up to 38 MHz
// FWS field = 2: up to 64 MHz
EFC->EEFC_FMR = EEFC_FMR_FWS(3); // Flash Wait State

//program_mckr_switchtoslow(); // переключаем на внутренний генератор 32 кГц
program_mckr_switchtomain(); // выключить ФАПЧ, если была включена
program_enable_xtal();
program_use_xtal(1);
program_disable_rc();
program_enable_plla(pllmul, plldiv);
//program_enable_pllb();
program_mckr_switchtopll_a();

EFC->EEFC_FMR = EEFC_FMR_FWS(ws); // Flash Wait State
}

/* функция вызывается из start-up до копирования всех "быстрых" функций и до инициализации переменных
*/
void
arm_cpu_initialize(void)
{
// Disable Watchdog
WDT->WDT_MR = WDT_MR_WDDIS;

// Embedded Flash Wait State VDDCORE set at 1.65V
// 17 MHz - 1 cycle = FWS = 0
// 30 MHz - 2 cycle = FWS = 1
// 54 MHz - 3 cycle = FWS = 2
// 64 MHz - 4 cycle = FWS = 3

// Embedded Flash Wait State VDDCORE set at 1.80V
// 32 MHz - 1 cycle = FWS = 0
// 38 MHz - 2 cicle = FWS = 1
// 64 MHz - 3 cycls = FWS = 2

#if CPU_FREQ == 64000000UL
enum { OSC_MUL = 32, OSC_DIV = 3, FWS = 3 }; // 12 MHz / 3 * 32 = 128 MHz
#elif CPU_FREQ == 48000000UL
enum { OSC_MUL = 8, OSC_DIV = 1, FWS = 2 }; // 12 MHz / 1 * 8 = 96 MHz
#elif CPU_FREQ == 32000000UL
enum { OSC_MUL = 16, OSC_DIV = 3, FWS = 1 }; // 12 MHz / 3 * 16 = 96 MHz
#else
#error Unsupported CPU_FREQ value
#endif

if (CPU_FREQ == 12000000UL)
{
// 12 МГц от внутреннего RC генератора
lowlevel_sam3s_init_pll_clock_12_RC12();
}
else if (0)
{
// умножение кварцевого генератора
lowlevel_sam3s_init_pll_clock_48_xtal12(OSC_MUL, OSC_DIV, FWS);
}
else if (1)
{
// умножение от внутреннего RC генератора
lowlevel_sam3s_init_pll_clock_48_RC12(OSC_MUL, OSC_DIV, FWS);
}
}

    EFC->EEFC_FMR = EEFC_FMR_FWS(2);    // Flash Wait State