diff --git a/Makefile b/Makefile
index bbb79cc..747da6d 100644
--- a/Makefile
+++ b/Makefile
@@ -6,6 +6,7 @@ OBJS += serialno.o
 OBJS += setup.o
 OBJS += util.o
 OBJS += memory.o
+OBJS += timer.o
 OBJS += gen/bitmaps.o
 OBJS += gen/fonts.o
 
diff --git a/firmware/protect.c b/firmware/protect.c
index 715cf21..8ba3298 100644
--- a/firmware/protect.c
+++ b/firmware/protect.c
@@ -58,7 +58,7 @@ bool protectButton(ButtonRequestType type, bool confirm_only)
 
 		// button acked - check buttons
 		if (acked) {
-			usbDelay(3300);
+			usbSleep(5);
 			buttonUpdate();
 			if (button.YesUp) {
 				result = true;
@@ -165,7 +165,7 @@ bool protectPin(bool use_cached)
 		}
 		layoutDialog(&bmp_icon_info, NULL, NULL, NULL, "Wrong PIN entered", NULL, "Please wait", secstr, "to continue ...", NULL);
 		// wait one second
-		usbDelay(800000);
+		usbSleep(1000);
 		if (msg_tiny_id == MessageType_MessageType_Initialize) {
 			protectAbortedByInitialize = true;
 			msg_tiny_id = 0xFFFF;
diff --git a/firmware/storage.c b/firmware/storage.c
index 7a13f41..97f5f6a 100644
--- a/firmware/storage.c
+++ b/firmware/storage.c
@@ -338,7 +338,7 @@ void storage_setHomescreen(const uint8_t *data, uint32_t size)
 
 void get_root_node_callback(uint32_t iter, uint32_t total)
 {
-	usbDelay(10); // handle up to ten usb interrupts.
+	usbSleep(1);
 	layoutProgress("Waking up", 1000 * iter / total);
 }
 
diff --git a/firmware/trezor.c b/firmware/trezor.c
index 487a5e5..a56e3d2 100644
--- a/firmware/trezor.c
+++ b/firmware/trezor.c
@@ -27,6 +27,7 @@
 #include "layout.h"
 #include "layout2.h"
 #include "rng.h"
+#include "timer.h"
 #include "buttons.h"
 
 uint32_t __stack_chk_guard;
@@ -58,13 +59,13 @@ void check_lock_screen(void)
 		// wait until NoButton is released
 		usbTiny(1);
 		do {
-			usbDelay(3300);
+			usbSleep(5);
 			buttonUpdate();
 		} while (!button.NoUp);
 
 		// wait for confirmation/cancellation of the dialog
 		do {
-			usbDelay(3300);
+			usbSleep(5);
 			buttonUpdate();
 		} while (!button.YesUp && !button.NoUp);
 		usbTiny(0);
@@ -102,6 +103,9 @@ int main(void)
 #else
 	setupApp();
 #endif
+
+	timer_init();
+
 #if DEBUG_LINK
 	oledSetDebug(1);
 	storage_reset(); // wipe storage if debug link
diff --git a/firmware/usb.c b/firmware/usb.c
index 241d056..df50f16 100644
--- a/firmware/usb.c
+++ b/firmware/usb.c
@@ -27,6 +27,7 @@
 #include "u2f.h"
 #include "storage.h"
 #include "util.h"
+#include "timer.h"
 
 #define USB_INTERFACE_INDEX_MAIN 0
 #if DEBUG_LINK
@@ -426,9 +427,10 @@ char usbTiny(char set)
 	return old;
 }
 
-void usbDelay(int cycles)
+void usbSleep(uint32_t millis)
 {
-	while (cycles--) {
+	uint32_t end = system_millis + millis;
+	while (end > system_millis) {
 		usbd_poll(usbd_dev);
 	}
 }
diff --git a/firmware/usb.h b/firmware/usb.h
index 286e3a4..318a5a4 100644
--- a/firmware/usb.h
+++ b/firmware/usb.h
@@ -24,6 +24,6 @@ void usbInit(void);
 void usbPoll(void);
 void usbReconnect(void);
 char usbTiny(char set);
-void usbDelay(int cycles);
+void usbSleep(uint32_t millis);
 
 #endif
diff --git a/timer.c b/timer.c
new file mode 100644
index 0000000..2be93e8
--- /dev/null
+++ b/timer.c
@@ -0,0 +1,61 @@
+/*
+ * This file is part of the TREZOR project.
+ *
+ * Copyright (C) 2016 Saleem Rashid <trezor@saleemrashid.com>
+ *
+ * This library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include "timer.h"
+
+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/cm3/systick.h>
+
+/* 1 tick = 1 ms */
+volatile uint32_t system_millis;
+
+/*
+ * Initialise the Cortex-M3 SysTick timer
+ */
+void timer_init(void) {
+	system_millis = 0;
+
+	/*
+	 * MCU clock (120 MHz) as source
+	 *
+	 *     (120 MHz / 8) = 15 clock pulses
+	 *
+	 */
+	systick_set_clocksource(STK_CSR_CLKSOURCE_AHB_DIV8);
+	STK_CVR = 0;
+
+	/*
+	 * 1 tick = 1 ms @ 120 MHz
+	 *
+	 *     (15 clock pulses * 1000 ms) = 15000 clock pulses
+	 *
+	 * Send an interrupt every (N - 1) clock pulses
+	 */
+	systick_set_reload(14999);
+
+	/* SysTick as interrupt */
+	systick_interrupt_enable();
+
+	systick_counter_enable();
+}
+
+void sys_tick_handler(void) {
+	system_millis++;
+}
diff --git a/timer.h b/timer.h
new file mode 100644
index 0000000..a69879c
--- /dev/null
+++ b/timer.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of the TREZOR project.
+ *
+ * Copyright (C) 2016 Saleem Rashid <trezor@saleemrashid.com>
+ *
+ * This library is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <stdint.h>
+
+/* 1 tick = 1 ms */
+extern volatile uint32_t system_millis;
+
+void timer_init(void);
+
+void sys_tick_handler(void);