#ifndef _UTIL_HH_
#define _UTIL_HH_

#include <cstdint>
#include <cstddef>

// Functions to access hardware registers. GCC will generate memory access instructions of the correct width.
// Usage: "_reg8(base, ofs) = 0xf0;" to set
//        "uint8_t foo = _reg8(base, ofs);" to get
//        Read/modify/write like "_reg8(base, ofs) |= (1<<7);" also works as expected.
constexpr inline uint8_t volatile& _reg8(uintptr_t const& base, size_t const& ofs) noexcept __attribute__((const));
constexpr inline uint8_t volatile& _reg8(uintptr_t const& base, size_t const& ofs) noexcept {
  return *reinterpret_cast<uint8_t*>(base+ofs);
}

constexpr inline uint16_t volatile& _reg16(uintptr_t const& base, size_t const& ofs) noexcept __attribute__((const));
constexpr inline uint16_t volatile& _reg16(uintptr_t const& base, size_t const& ofs) noexcept {
  return *reinterpret_cast<uint16_t*>(base+ofs);
}

constexpr inline uint32_t volatile& _reg32(uintptr_t const& base, size_t const& ofs) noexcept __attribute__((const));
constexpr inline uint32_t volatile& _reg32(uintptr_t const& base, size_t const& ofs) noexcept {
  return *reinterpret_cast<uint32_t*>(base+ofs);
}

// log2(n)
inline unsigned _ln2(unsigned n) noexcept __attribute__((const));
inline unsigned _ln2(unsigned n) noexcept {
  uint32_t reg;
  asm ("clz %[dst], %[src]"
       : [dst] "=r"(reg) : [src] "r"(n));
  reg = 31-reg;
  if(n & ~(1<<reg))
    ++reg;
  return reg;
}

inline constexpr unsigned _pow2(unsigned n) noexcept __attribute__((const));
inline constexpr unsigned _pow2(unsigned n) noexcept {
  return (1<<n);
}

#endif