#include "util_delay.h"

static uint8_t p_us = 0;
static uint16_t p_ms = 0;

void delay_init(void)
{
    p_us = SystemCoreClock / 8000000;
    p_ms = (uint16_t)p_us * 1000;
}

void delay_us(uint32_t n_us)
{
    SysTick->CNTL0 = 0;
    SysTick->CNTL1 = 0;
    SysTick->CNTL2 = 0;
    SysTick->CNTL3 = 0;
    SysTick->CTLR = 1;

    uint32_t i = (uint32_t)n_us * p_us;
    while((*(volatile uint32_t *)&SysTick->CNTL0) <= i);

    SysTick->CTLR = 0;
}

void delay_ms(uint32_t n_ms)
{
    SysTick->CNTL0 = 0;
    SysTick->CNTL1 = 0;
    SysTick->CNTL2 = 0;
    SysTick->CNTL3 = 0;
    SysTick->CTLR = 1;

    uint32_t i = (uint32_t)n_ms * p_ms;
    while((*(volatile uint32_t *)&SysTick->CNTL0) <= i);

    SysTick->CTLR = 0;
}