#include <cassert>
#include <errno.h>

#include "globals.hh"
#include "exceptions.hh"
#include "drv_omap35x_gpt.hh"
#include "cortexa8.hh"
#include "sleep.hh"

#define MAX_USLEEP (4294967295/USTIMER_PER_US)

// "busy" sleep for at least 'us' microseconds with microsecond granularity
void usleep(uint32_t us) {
  if(us > MAX_USLEEP)
    sleep(us/1000);

  uint32_t start = global::ustimer->getCounter();
  uint32_t end = start + us*USTIMER_PER_US;
  
  bool overflow = (end<start);
  while(true) {
    uint32_t now = global::ustimer->getCounter();
    if((now >= end) && (!overflow || (now < start)))
      return;
  }
}

// Sleep with yield, at least 10 ms, 10 ms granularity
void sleep(unsigned ms) {
  // TODO: Proper implementation
  uint32_t start = global::ticktimer->getTicks();
  uint32_t end = start + ms/TICKTIMER_MS;

  while(end > global::ticktimer->getTicks()) cortexa8::yield_svc();
}