X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fcpu%2Fcortex-m3%2Fdrv%2Fclock_lm3s.c;h=df4681ea3ef860b9c62d5cce8713c07c8722cb2c;hb=6be047844350e6988c1a82a68ff8572fb2b60b57;hp=2f2dfe6c811412db5451b497b8590e1801dd8020;hpb=41d14f76b5f57cce434048251df63eb6bbdb1df0;p=bertos.git

diff --git a/bertos/cpu/cortex-m3/drv/clock_lm3s.c b/bertos/cpu/cortex-m3/drv/clock_lm3s.c
index 2f2dfe6c..df4681ea 100644
--- a/bertos/cpu/cortex-m3/drv/clock_lm3s.c
+++ b/bertos/cpu/cortex-m3/drv/clock_lm3s.c
@@ -35,10 +35,13 @@
  * \author Andrea Righi <arighi@develer.com>
  */
 
+#include "clock_lm3s.h"
+
 #include <cfg/compiler.h>
 #include <cfg/debug.h>
-#include "io/lm3s.h"
-#include "clock_lm3s.h"
+
+#include <io/lm3s.h>
+
 
 /* The PLL VCO frequency is 400 MHz */
 #define PLL_VCO	400000000UL
@@ -51,15 +54,18 @@
 /*
  * Very small delay: each loop takes 3 cycles.
  */
-INLINE void __delay(unsigned long iterations)
+void NAKED lm3s_busyWait(unsigned long iterations)
 {
+	register uint32_t __n asm("r0") = iterations;
+
 	asm volatile (
-		"1:	subs	%0, #1\n\t"
-		"	bne 1b\n\t"
-		: "=r"(iterations) : : "memory", "cc");
+		"1: subs r0, #1\n\t"
+		"bne 1b\n\t"
+		"bx lr\n\t"
+		: : "r"(__n) : "memory", "cc");
 }
 
-unsigned long clock_get_rate(void)
+INLINE unsigned long clock_get_rate(void)
 {
 	reg32_t rcc = HWREG(SYSCTL_RCC);
 
@@ -67,7 +73,25 @@ unsigned long clock_get_rate(void)
 			PLL_VCO / 2 / RCC_TO_DIV(rcc) : PLL_VCO;
 }
 
-void clock_set_rate(void)
+/*
+ * 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_init(void)
 {
 	reg32_t rcc, rcc2;
 	unsigned long clk;
@@ -92,6 +116,8 @@ void clock_set_rate(void)
 	HWREG(SYSCTL_RCC) = rcc;
 	HWREG(SYSCTL_RCC) = rcc2;
 
+	lm3s_busyWait(16);
+
 	/*
 	 * Step #2: select the crystal value (XTAL) and oscillator source
 	 * (OSCSRC), and clear the PWRDN bit in RCC/RCC2. Setting the XTAL
@@ -116,7 +142,7 @@ void clock_set_rate(void)
         HWREG(SYSCTL_RCC) = rcc;
 	HWREG(SYSCTL_RCC) = rcc2;
 
-	__delay(16);
+	lm3s_busyWait(16);
 
 	/*
 	 * Step #3: select the desired system divider (SYSDIV) in RCC/RCC2 and
@@ -125,19 +151,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
@@ -155,5 +174,5 @@ void clock_set_rate(void)
 
 	HWREG(SYSCTL_RCC) = rcc;
 
-	__delay(16);
+	lm3s_busyWait(16);
 }