#include "al/cpu.h"

void init_cpu()
{

}

inline void cpu_mem_write32(uintptr_t addr, uint32_t data)
{
  volatile uint32_t *ptr = (uint32_t*)addr;
  *ptr = data;
}

inline uint32_t cpu_mem_read32(uintptr_t addr)
{
  volatile uint32_t *ptr = (uint32_t*)addr;

  return *ptr;
}

static inline void cpu_dcache_invld(const void *ptr)
{
  asm(
        "mcr p15, 0, %[ptr], c7, c6, 1\n\t"
        :: [ptr] "r"(ptr)
    );
}

static inline size_t cpu_dcache_line_size()
{
  uint32_t ret;
  asm(
        "mrc p15, 0, %[ret], c0, c0, 1\n\t"
        : [ret] "=r"(ret));

  return (((ret >> 12) & 0x3) + 3) << 1;
}

void cpu_dcache_invalidate(const void *buf, size_t size)
{
  size_t i;
  size_t dcache_line_size = cpu_dcache_line_size();

  // iterate on lines to invalidate each one of them
  for ( i=0; i<size; i+=dcache_line_size )
    cpu_dcache_invld(buf+i);
}
