Merge branch "preempt" in "trunk".

[bertos.git] / bertos / cpu / arm / hw / crtat91sam7_rom.S

diff --git a/bertos/cpu/arm/hw/crtat91sam7_rom.S b/bertos/cpu/arm/hw/crtat91sam7_rom.S
index 0a2fa4a791be63a6c3fbe0b246cb1bb6c756654a..9bd9d46c48941d56660442a1fad22e3d451e1000 100644 (file)
--- a/bertos/cpu/arm/hw/crtat91sam7_rom.S
+++ b/bertos/cpu/arm/hw/crtat91sam7_rom.S
@@ -70,28 +70,30 @@
  *
  */
 
-#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
-       /**
-       * With a 18.420MHz cristal, master clock is:
-       * (((18.420 * PLL_MUL_VAL + 1) / PLL_DIV_VAL) / AT91MCK_PRES) = 48.023MHz
-       */
+#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
+        */
        #define PLL_MUL_VAL  72  /**< Real multiplier value is PLL_MUL_VAL + 1! */
        #define PLL_DIV_VAL  14
        #define AT91MCK_PRES PMC_PRES_CLK_2
 
-       /**
-       * Register I/O adresses.
-       * \{
-       */
+       /*
+        * Register I/O adresses.
+        */
        #define MC_BASE             0xFFFFFF00
        #define MC_FMR_OFF          0x00000060
        #define MC_FWS_2R3W         0x00000100
@@ -105,14 +107,25 @@
        #define WDT_WDDIS            (1 << 15)
 
        #define PMC_BASE            0xFFFFFC00
+       #define PMC_PCER_OFF        0x00000010
        #define PMC_SR_OFF          0x00000068
        #define PMC_MCKR_OFF        0x00000030
        #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
 
+       #if CPU_ARM_SAM7S_LARGE
+               #define PMC_PIO_CLK_EN (1 << 2)
+       #elif CPU_ARM_SAM7X
+               #define PMC_PIO_CLK_EN ((1 << 2) | (1 << 3))
+       #else
+               #error CPU not supported
+       #endif
+
        #define CKGR_MOR_OFF        0x00000020
        #define CKGR_PLLR_OFF       0x0000002C
        #define CKGR_MOSCEN           (1 << 0)
@@ -123,11 +136,13 @@
        #define RSTC_KEY            0xA5000000
        #define RSTC_URSTEN           (1 << 0)
 
+       #define ARM_MODE_USR              0x10
        #define ARM_MODE_FIQ              0x11
        #define ARM_MODE_IRQ              0x12
        #define ARM_MODE_SVC              0x13
        #define ARM_MODE_ABORT            0x17
        #define ARM_MODE_UNDEF            0x1B
+       #define ARM_MODE_SYS              0x1F
 
 #else
        #error No register I/O definition for selected ARM CPU
@@ -228,6 +243,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)
@@ -317,7 +354,17 @@ _41:
         /*
          * Initialize user stack pointer.
          */
-        ldr     r13, =__stack_end
+       /* msr     CPSR_c, #ARM_MODE_SYS | 0xC0 */
+       ldr     r13, =__stack_end
+
+
+       /*
+        * Enable clock for PIO(s)
+        */
+        ldr     r1, =PMC_BASE
+        mov     r0, #PMC_PIO_CLK_EN
+        str     r0, [r1, #PMC_PCER_OFF]
+
 
         /*
          * Jump to main