diff --git a/lib/cm3/systick.c b/lib/cm3/systick.c
index d2bcb7a5..9d37e874 100644
--- a/lib/cm3/systick.c
+++ b/lib/cm3/systick.c
@@ -86,9 +86,8 @@ uint32_t systick_get_value(void)
 
 void systick_set_clocksource(uint8_t clocksource)
 {
-	if (clocksource < 2) {
-		STK_CSR |= clocksource;
-	}
+	STK_CSR = (STK_CSR & ~STK_CSR_CLKSOURCE) |
+		  (clocksource & STK_CSR_CLKSOURCE);
 }
 
 /*---------------------------------------------------------------------------*/