lm3s1968: introduce a distinct inline function to evaluate SYSDIV divisor.

[bertos.git] / bertos / cpu / cortex-m3 / drv / clock_lm3s.c

diff --git a/bertos/cpu/cortex-m3/drv/clock_lm3s.c b/bertos/cpu/cortex-m3/drv/clock_lm3s.c
index b91b09bd17625b959e8c214a3d51f33cb5d12cf7..8790b0e6ed69319fcdbf3f644a2417f0d199811a 100644 (file)
--- a/bertos/cpu/cortex-m3/drv/clock_lm3s.c
+++ b/bertos/cpu/cortex-m3/drv/clock_lm3s.c
@@ -64,7 +64,25 @@ unsigned long clock_get_rate(void)
        reg32_t rcc = HWREG(SYSCTL_RCC);
 
        return rcc & SYSCTL_RCC_USESYSDIV ?
-                       PLL_VCO / RCC_TO_DIV(rcc) : PLL_VCO;
+                       PLL_VCO / 2 / RCC_TO_DIV(rcc) : PLL_VCO;
+}
+
+/*
+ * Try to evaluate the correct SYSDIV value depending on the desired CPU
+ * frequency.
+ */
+INLINE int evaluate_sysdiv(unsigned long freq)
+{
+       int i;
+
+        /*
+         * NOTE: with BYPASS=0, SYSDIV < 3 are reserved values (see LM3S1968
+         * Microcontroller DATASHEET, p.78).
+         */
+       for (i = 3; i < 16; i++)
+               if (freq >= (PLL_VCO / 2 / (i + 1)))
+                       break;
+       return i;
 }
 
 void clock_set_rate(void)
@@ -125,19 +143,12 @@ void clock_set_rate(void)
         */
        rcc &= ~(SYSCTL_RCC_SYSDIV_M | SYSCTL_RCC_USESYSDIV);
 
-       /*
-        * Try to evaluate the correct SYSDIV value depending on the desired
-        * CPU frequency.
-        *
-        * NOTE: with BYPASS=0, SYSDIV < 3 are reserved values (see LM3S1968
-        * Microcontroller DATASHEET, p.78).
-        */
        clk = PLL_VCO / 2;
        for (i = 3; i < 16; i++)
                if (CPU_FREQ >= (clk / (i + 1)))
                        break;
-       if (i)
-               rcc |= SYSCTL_RCC_USESYSDIV | (i << SYSCTL_RCC_SYSDIV_SHIFT);
+       rcc |= SYSCTL_RCC_USESYSDIV |
+                       (evaluate_sysdiv(CPU_FREQ) << SYSCTL_RCC_SYSDIV_SHIFT);
 
        /*
         * Step #4: wait for the PLL to lock by polling the PLLLRIS bit in the