#include <assert.h>
#include <stdint.h>
#include "stm32f10x.h"
#include "delay.h"

/* Sleep for nCount usec
 */
void
delay(uint32_t nCount) {
    uint32_t dly, cnt, clk_per_usec, max_dly;
    volatile uint32_t *DWT_CYCCNT = (uint32_t *)0xe0001004;

    __disable_irq();

#ifdef SYSCLK_FREQ_72MHz
    clk_per_usec = 72;
    max_dly = (1<<31) / clk_per_usec; /* Really half the maximum (still ~30 seconds at 72MHz) */
#else
#error "Unknown clock frequency"
#endif
    assert(nCount < max_dly);
    
    cnt = *DWT_CYCCNT; /* Get current cycle count */
    dly = nCount * clk_per_usec;
    dly += cnt; /* Compute cycle count to stop at */
    if (dly < cnt)
	/* Stop count wrapped, wait until the counter wraps around */
	while (*DWT_CYCCNT > cnt)
	    ;
    /* Wait until we get to the stop count */
    while (*DWT_CYCCNT < dly)
	;
    
    __enable_irq();
}