From d541c141106099ac631f6ca17d5eb3329688e5e4 Mon Sep 17 00:00:00 2001
From: Frantisek Burian <BuFran@seznam.cz>
Date: Wed, 11 Sep 2013 06:05:48 -0700
Subject: [PATCH] [CM3] Systick Add helper function to easily set the desired
 frequency

---
 include/libopencm3/cm3/systick.h |  8 ++++++--
 lib/cm3/systick.c                | 31 +++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/include/libopencm3/cm3/systick.h b/include/libopencm3/cm3/systick.h
index bbd2ff27..e33b8aa0 100644
--- a/include/libopencm3/cm3/systick.h
+++ b/include/libopencm3/cm3/systick.h
@@ -85,8 +85,11 @@
 #define STK_CSR_CLKSOURCE_EXT		(0 << STK_CSR_CLKSOURCE_LSB)
 #define STK_CSR_CLKSOURCE_AHB		(1 << STK_CSR_CLKSOURCE_LSB)
 #else
-#define STK_CTRL_CLKSOURCE_AHB_DIV8	(0 << STK_CTRL_CLKSOURCE_LSB)
-#define STK_CTRL_CLKSOURCE_AHB		(1 << STK_CTRL_CLKSOURCE_LSB)
+#define STK_CTRL_CLKSOURCE_AHB_DIV8	(0 << STK_CSR_CLKSOURCE_LSB)
+#define STK_CTRL_CLKSOURCE_AHB		(1 << STK_CSR_CLKSOURCE_LSB)
+
+#define STK_CSR_CLKSOURCE_AHB_DIV8	(0 << STK_CSR_CLKSOURCE_LSB)
+#define STK_CSR_CLKSOURCE_AHB		(1 << STK_CSR_CLKSOURCE_LSB)
 #endif
 /**@}*/
 
@@ -123,6 +126,7 @@
 BEGIN_DECLS
 
 void systick_set_reload(uint32_t value);
+bool systick_set_frequency(uint32_t freq, uint32_t ahb);
 uint32_t systick_get_reload(void);
 uint32_t systick_get_value(void);
 void systick_set_clocksource(uint8_t clocksource);
diff --git a/lib/cm3/systick.c b/lib/cm3/systick.c
index 9d37e874..219104af 100644
--- a/lib/cm3/systick.c
+++ b/lib/cm3/systick.c
@@ -65,6 +65,37 @@ uint32_t systick_get_reload(void)
 	return STK_RVR & STK_RVR_RELOAD;
 }
 
+/** @brief SysTick Set clock and frequency of overflow
+ *
+ * This function sets the systick to AHB clock source, and the prescaler to
+ * generate interrupts with the desired frequency. The function fails, if
+ * the frequency is too low.
+ *
+ * @param[in] freq uint32_t The desired frequency in Hz
+ * @param[in] ahb uint32_t The current AHB frequency in Hz
+ * @returns true, if success, false if the desired frequency cannot be set.
+ */
+bool systick_set_frequency(uint32_t freq, uint32_t ahb)
+{
+	uint32_t ratio = ahb / freq;
+
+#if defined(__ARM_ARCH_6M__)
+	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
+#else
+	if (ratio >= (STK_RVR_RELOAD * 8)) {
+		/* This frequency is too slow */
+		return false;
+	} else if (ratio >= STK_RVR_RELOAD) {
+		ratio /= 8;
+		systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
+	} else {
+		systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
+	}
+#endif
+	systick_set_reload(ratio - 1);
+	return true;
+}
+
 /*---------------------------------------------------------------------------*/
 /** @brief Get the current SysTick counter value.
  *