1 files changed, 36 insertions, 0 deletions
diff --git a/firmware/targets/f7/furi_hal/furi_hal_delay.c b/firmware/targets/f7/furi_hal/furi_hal_delay.c
new file mode 100644
index 00000000..60c9b1c4
--- /dev/null
+++ b/firmware/targets/f7/furi_hal/furi_hal_delay.c
@@ -0,0 +1,36 @@
+#include "furi_hal_delay.h"
+
+#include <furi.h>
+#include <cmsis_os2.h>
+
+#define TAG "FuriHalDelay"
+
+static uint32_t clk_per_microsecond;
+
+void furi_hal_delay_init(void) {
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+    DWT->CYCCNT = 0U;
+    clk_per_microsecond = SystemCoreClock / 1000000.0f;
+    FURI_LOG_I(TAG, "Init OK");
+}
+
+void delay_us(float microseconds) {
+    uint32_t start = DWT->CYCCNT;
+    uint32_t time_ticks = microseconds * clk_per_microsecond;
+    while((DWT->CYCCNT - start) < time_ticks) {
+    };
+}
+
+// cannot be used in ISR
+// TODO add delay_ISR variant
+void delay(float milliseconds) {
+    uint32_t ticks = milliseconds / (1000.0f / osKernelGetTickFreq());
+    osStatus_t result = osDelay(ticks);
+    (void)result;
+    furi_assert(result == osOK);
+}
+
+uint32_t millis(void) {
+    return HAL_GetTick();
+}