diff --git a/Debug/debug.c b/Debug/debug.c
index af21d23..380fcb2 100644
--- a/Debug/debug.c
+++ b/Debug/debug.c
@@ -29,6 +29,29 @@ void Delay_Init(void)
 {
     p_us = SystemCoreClock / 8000000;
     p_ms = (uint16_t)p_us * 1000;
+
+    SysTick->CTLR &= ~0x8000001F;
+    SysTick->CTLR |= (1 << 0);
+}
+
+uint64_t SysTick_Read(void)
+{
+    uint64_t ticks;
+    SysTick->CTLR &= ~(1 << 0);
+    ticks = SysTick->CNT;
+    SysTick->CTLR |= (1 << 0);
+
+    return ticks;
+}
+
+uint64_t SysTick_Us(void)
+{
+    return SysTick_Read() / p_us;
+}
+
+uint64_t SysTick_Ms(void)
+{
+    return SysTick_Read() / p_ms;
 }
 
 /*********************************************************************
@@ -47,12 +70,18 @@ void Delay_Us(uint32_t n)
     SysTick->SR &= ~(1 << 0);
     i = (uint32_t)n * p_us;
 
+    SysTick->CMP = SysTick_Read() + i;
+    while((SysTick->SR & (1 << 0)) != (1 << 0));
+
+
+    /*
     SysTick->CMP = i;
     SysTick->CTLR |= (1 << 4);
     SysTick->CTLR |= (1 << 5) | (1 << 0);
 
     while((SysTick->SR & (1 << 0)) != (1 << 0));
     SysTick->CTLR &= ~(1 << 0);
+    */
 }
 
 /*********************************************************************
@@ -71,12 +100,17 @@ void Delay_Ms(uint32_t n)
     SysTick->SR &= ~(1 << 0);
     i = (uint32_t)n * p_ms;
 
+    SysTick->CMP = SysTick_Read() + i;
+    while((SysTick->SR & (1 << 0)) != (1 << 0));
+
+    /*
     SysTick->CMP = i;
     SysTick->CTLR |= (1 << 4);
     SysTick->CTLR |= (1 << 5) | (1 << 0);
 
     while((SysTick->SR & (1 << 0)) != (1 << 0));
     SysTick->CTLR &= ~(1 << 0);
+    */
 }
 
 /*********************************************************************
diff --git a/Debug/debug.h b/Debug/debug.h
index e2b6062..2eecb71 100644
--- a/Debug/debug.h
+++ b/Debug/debug.h
@@ -7,7 +7,7 @@
  *                      Printf , Delay functions.
 *********************************************************************************
 * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd.
-* Attention: This software (modified or not) and binary are used for 
+* Attention: This software (modified or not) and binary are used for
 * microcontroller manufactured by Nanjing Qinheng Microelectronics.
 *******************************************************************************/
 #ifndef __DEBUG_H
@@ -39,6 +39,9 @@ extern "C" {
 #endif
 
 void Delay_Init(void);
+uint64_t SysTick_Read(void);
+uint64_t SysTick_Us(void);
+uint64_t SysTick_Ms(void);
 void Delay_Us(uint32_t n);
 void Delay_Ms(uint32_t n);
 void USART_Printf_Init(uint32_t baudrate);