X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fcpu%2Farm%2Fhw%2Fcrtat91sam7_rom.S;h=c5317847769262965a9e47f833792a5c78937080;hb=ad78a25e6bfb87a82ea5ff441c054e64f99df17c;hp=0a2fa4a791be63a6c3fbe0b246cb1bb6c756654a;hpb=345f93de1963f49bdb194d2b06c8c5d7ba0a3e5f;p=bertos.git

diff --git a/bertos/cpu/arm/hw/crtat91sam7_rom.S b/bertos/cpu/arm/hw/crtat91sam7_rom.S
index 0a2fa4a7..c5317847 100644
--- a/bertos/cpu/arm/hw/crtat91sam7_rom.S
+++ b/bertos/cpu/arm/hw/crtat91sam7_rom.S
@@ -70,16 +70,19 @@
  *
  */
 
-#include "hw_cpu.h"
 #include <cpu/detect.h>
+#include "cfg/cfg_arch.h"
 
 
-#if CLOCK_FREQ != 48023000L
-#error Clock registers set for 48MHz operation, revise following code if you want a different clock.
+#if CPU_FREQ != 48023000L
+	/* Avoid errors on nightly test */
+	#if !defined(ARCH_NIGHTTEST) || !(ARCH & ARCH_NIGHTTEST)
+		#warning Clock registers set for 48.023MHz operation, revise following code if you want a different clock.
+	#endif
 #endif
 
 
-#if CPU_ARM_AT91SAM7S256 || CPU_ARM_AT91SAM7X256
+#if CPU_ARM_SAM7S_LARGE || CPU_ARM_SAM7X
 	/**
 	* With a 18.420MHz cristal, master clock is:
 	* (((18.420 * PLL_MUL_VAL + 1) / PLL_DIV_VAL) / AT91MCK_PRES) = 48.023MHz
@@ -110,7 +113,9 @@
 	#define PMC_MOSCS             (1 << 0)
 	#define PMC_LOCK              (1 << 2)
 	#define PMC_MCKRDY            (1 << 3)
+	#define PMC_CSS_MASK        0x00000003
 	#define PMC_CSS_PLL_CLK     0x00000003
+	#define PMC_PRES_MASK       0x0000001C
 	#define PMC_PRES_CLK_2      0x00000004
 
 	#define CKGR_MOR_OFF        0x00000020
@@ -228,6 +233,28 @@ wait_moscs:
         tst     r0, #PMC_MOSCS
         beq     wait_moscs
 
+        /*
+         * Switch to Slow oscillator clock.
+         */
+        ldr     r0, [r1, #PMC_MCKR_OFF]
+        and     r0, r0, #~PMC_CSS_MASK
+        str     r0, [r1, #PMC_MCKR_OFF]
+wait_slowosc:
+        ldr     r0, [r1, #PMC_SR_OFF]
+        tst     r0, #PMC_MCKRDY
+        beq     wait_slowosc
+
+        /*
+         * Switch to prescaler div 1 factor.
+         */
+        ldr     r0, [r1, #PMC_MCKR_OFF]
+        and     r0, r0, #~PMC_PRES_MASK
+        str     r0, [r1, #PMC_MCKR_OFF]
+wait_presc:
+        ldr     r0, [r1, #PMC_SR_OFF]
+        tst     r0, #PMC_MCKRDY
+        beq     wait_presc
+
         /*
          * Set PLL:
          * PLLfreq = crystal / divider * (multiplier + 1)