#include <libk/std.h>
#include <sys/spin_lock.h>
#include <sys/stall.h>

static uint64_t stall_read_tsc(void) {
  uint32_t lo, hi;
  __asm__ volatile("rdtsc" : "=a"(lo), "=d"(hi));
  return ((uint64_t)hi << 32) | lo;
}

static uint64_t stall_get_tsc_freq_hz(void) {
  uint32_t eax, ebx, ecx, edx;
  __asm__ volatile("cpuid" : "=a"(eax), "=b"(ebx), "=c"(ecx), "=d"(edx) : "a"(0x15));

  if (eax == 0 || ebx == 0 || ecx == 0)
    return 2500000000ULL;

  return (uint64_t)ecx * ebx / eax;
}

void stall_ms(uint64_t ms) {
  uint64_t freq_hz = stall_get_tsc_freq_hz();

  uint64_t cycles = freq_hz / 1000;
  uint64_t wait_cycles = ms * cycles;

  uint64_t now = stall_read_tsc();

  while ((stall_read_tsc() - now) < wait_cycles)
    spin_lock_relax();
}