diff --git a/lib/sam3x/pmc.c b/lib/sam3x/pmc.c index c37e325d..cd0ad245 100644 --- a/lib/sam3x/pmc.c +++ b/lib/sam3x/pmc.c @@ -22,8 +22,9 @@ void pmc_xtal_enable(bool en, u8 startup_time) { if (en) { - CKGR_MOR |= CKGR_MOR_KEY | - CKGR_MOR_MOSCXTEN | CKGR_MOR_MOSCXTST_MASK; + CKGR_MOR = (CKGR_MOR & ~CKGR_MOR_MOSCXTST_MASK) | + CKGR_MOR_KEY | CKGR_MOR_MOSCXTEN | + (startup_time << 8); while (!(PMC_SR & PMC_SR_MOSCXTS)); } else { CKGR_MOR = CKGR_MOR_KEY | (CKGR_MOR & ~CKGR_MOR_MOSCXTEN);