4 files changed, 41 insertions, 5 deletions
diff --git a/main.cpp b/main.cpp
index ea6490d..040610c 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,6 +1,7 @@
 #include "stm32.h"
 #include "interrupt.h"
 #include "thread.h"
+#include "time.h"
 
 #include "ppmsum.h"
 #include "i2c.h"
@@ -69,6 +70,9 @@ uint32_t thstack[1024];
 Thread thread(thstack, sizeof(thstack), threadmain);
 
 int main() {
+	// Initialize system timer.
+	Time::init();
+	
 	RCC.enable(RCC.AFIO);
 	RCC.enable(RCC.IOPA);
 	RCC.enable(RCC.IOPB);
diff --git a/thread.cpp b/thread.cpp
index 7839a19..a08bd61 100644
--- a/thread.cpp
+++ b/thread.cpp
@@ -26,11 +26,6 @@ inline void __attribute__((naked)) switch_context() {
 }
 
 template<>
-void interrupt<Interrupt::SysTick>() {
-	switch_context();
-}
-
-template<>
 void interrupt<Interrupt::SVCall>() {
 	switch_context();
 }
diff --git a/time.cpp b/time.cpp
new file mode 100644
index 0000000..a5d1f0b
--- /dev/null
+++ b/time.cpp
@@ -0,0 +1,9 @@
+#include "time.h"
+#include "interrupt.h"
+
+volatile uint32_t Time::systime;
+
+template<>
+void interrupt<Interrupt::SysTick>() {
+	Time::systime++;
+}
diff --git a/time.h b/time.h
new file mode 100644
index 0000000..66d8e63
--- /dev/null
+++ b/time.h
@@ -0,0 +1,28 @@
+#ifndef TIME_H
+#define TIME_H
+
+#include "stm32.h"
+#include "interrupt.h"
+#include "thread.h"
+
+class Time {
+	friend void interrupt<Interrupt::SysTick>();
+	
+	private:
+		static volatile uint32_t systime;
+	
+	public:
+		static void init() {
+			STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz.
+			STK.CTRL = 0x03;
+		}
+		
+		static void sleep(uint32_t ms) {
+			ms += systime;
+			while(systime < ms) {
+				Thread::yield();
+			}
+		}
+};
+
+#endif