#include "memory.h"
#include "config.h"

#include "driver_log.h"

#include <tee_internal_api.h>

static const PhysicalAddress kPaConfigSymOffsetTextOffset = 0x28;
static const PhysicalAddress kTextSymPhysOffset = 0x0000000000080000ULL;

PaTzResult PlatformMapRegion(PhysicalAddress phys, size_t size,
                             MemoryAccessType type, void **virt) {
  uintptr_t real_virt = 0;
  uint32_t offset = PageOffset(phys);

  // Calculating size aligned on page size
  size_t size_aligned = AlignToPageUp(size + offset);

  PhysicalAddress phys_aligned = AlignToPageDown(phys);

  if (PlatformSysMap(phys_aligned, size_aligned,
                     type, (void **)&real_virt) != PA_TZ_SUCCESS) {
    LOG_E("Failed mapping a region\n");
    return PA_TZ_GENERAL_ERROR;
  }

  // Finally to set start address of region pages
  *virt = (void *)(real_virt + offset);

  return PA_TZ_SUCCESS;
}

PaTzResult PlatformUnmapRegion(void *virt, size_t size) {
  uint32_t offset = PageOffset((uintptr_t)virt);

  // Calculating size aligned on page size
  uint32_t size_aligned = AlignToPageUp(size + offset);

  if (PlatformSysUnmap(virt, size_aligned) != PA_TZ_SUCCESS) {
    LOG_E("Failed unmapping a region\n");
    return PA_TZ_GENERAL_ERROR;
  }

  return PA_TZ_SUCCESS;
}

PaTzResult PhysicalGetBytes(PhysicalAddress phys, size_t size, void *out) {
  if (!out) {
    LOG_E("Invalid arguments.\n");
    return PA_TZ_GENERAL_ERROR;
  }

  void *addr = NULL;
  PaTzResult result = PlatformSysMap(phys, size, kMemoryAccessRead, &addr);
  if (result != PA_TZ_SUCCESS) {
    LOG_E("Can not map paddr to read.\n");
    return PA_TZ_GENERAL_ERROR;
  }

  if (addr) {
    TEE_MemMove(out, addr, size);
  } else {
    LOG_E("Mapping return incorrect address.\n");
  }

  result = PlatformSysUnmap(addr, size);
  if (result != PA_TZ_SUCCESS) {
    LOG_E("Can not unmap\n");
  }

  return result;
}

#define BIT(nr)         (1ULL << (nr))

PhysicalAddress KernelVirtToPhys(KernelAddress kernel_virt) {
  if (!kernel_virt) {
    return kernel_virt;
  }

  return (
    kernel_virt & BIT(GetConfig()->memory_conf.va_bits - 1) ?
      (kernel_virt & ~GetConfig()->memory_conf.page_offset) +
                                  KernelGetConfigPhysOffset() :
      (kernel_virt - GetConfig()->memory_conf.kimage_voffset));
}

//kernel virtual addresses start at va_start and end at MAX_UINT64
uint32_t IsKernelVirtualAddress(KernelAddress kernel_virt) {
  return kernel_virt >= GetConfig()->memory_conf.va_start ? 1 : 0;
}

uint32_t IsPhysicalAddress(PhysicalAddress phys) {
  for (size_t i = 0; i < GetConfig()->memory_conf.sys_ram_ranges_num; ++i) {
    if (phys >= GetConfig()->memory_conf.sys_ram_ranges[i].start &&
        phys < GetConfig()->memory_conf.sys_ram_ranges[i].end) {
          return 1;
    }
  }
  return 0;
}

PhysicalAddress KernelGetPaConfigOffsetSymAddress(void) {
  return KernelGetTextSymAddress() + kPaConfigSymOffsetTextOffset;
}

PhysicalAddress KernelGetTextSymAddress(void) {
  return KernelGetStartPhysAddr() + kTextSymPhysOffset;
}