#include "io.h"
#include "psr.h"
#include "memory.h"
#include "translation_table_descriptors.h"
#include "ramfs.h"

void demo_paging_support(void)
{
  uint32_t ID_MMFR0;
  // get contents of coprocessor register to check for paging support
  asm("mrc p15, 0, %0, c0, c1, 4" : "=r" (ID_MMFR0));
  
  char *paging;
  
  switch(ID_MMFR0 & 0xf) /* lowest 4 bits indicate VMSA support */
    {
    case 0 : paging = "no paging"; break;
    case 1 : paging = "implementation defined paging"; break;
    case 2 : paging = "VMSAv6, with cache and TLB type registers"; break;
    case 3 : paging = "VMSAv7, with support for remapping and access flag"; break;
    case 4 : paging = "VMSAv7 with PXN bit supported"; break;
    case 5 : paging = "VMSAv7, PXN and long format descriptors. EPAE is supported."; break;
    default : paging = "?_? unknown paging ?_?";
    }
  
  puts(paging);
}

void demo_current_mode(void)
{

  // get content of current program status register to check the current
  // processor mode (should be system, as we set it in boot.S)
  PSR_t CPSR = read_CPSR();

  char *mode_name;
  
  switch(CPSR.fields.PSR_MODE_4_0)
    {
    case MODE_USER       : mode_name = "User (PL0)"; break;
    case MODE_FIQ        : mode_name = "FIQ (PL1)"; break;
    case MODE_IRQ        : mode_name = "IRQ (PL1)"; break;
    case MODE_SUPERVISOR : mode_name = "Supervisor (PL1)"; break;
    case MODE_MONITOR    : mode_name = "Monitor (PL1)"; break;
    case MODE_ABORT      : mode_name = "Abort (PL1)"; break;
    case MODE_HYPERVISOR : mode_name = "Hyp (PL2)"; break;
    case MODE_UNDEFINED  : mode_name = "Undefined (PL1)"; break;
    case MODE_SYSTEM     : mode_name = "System (PL1)"; break;
    default : mode_name = "Unknown mode"; break;
    }

  prints("current mode: ");
  puts(mode_name);
}

#define TRANSLATION_TABLE					     \
  ((short_section_descriptor_t volatile*) TRANSLATION_TABLE_BASE)

extern char
  _binary_ramfs_img_start,
  _binary_ramfs_img_end,
  _binary_ramfs_img_size;

void demo_setup_PL0(void)
{
  // find PL_0_test.img im ramfs
  struct ramfile PL_0_test_img;

  if (find_file(&_binary_ramfs_img_start, "PL_0_test.img",
		&PL_0_test_img))
    {
      puts("PL_0_test.img not found :(");
      asm volatile ("wfi");
    }

  short_section_descriptor_t volatile *PL0_section_entry =
    &TRANSLATION_TABLE[PL0_SECTION_NUMBER];

  short_section_descriptor_t PL0_section = *PL0_section_entry;

  // set up address of PL0 section
  PL0_section.SECTION_BASE_ADDRESS_31_20 =
    UNPRIVILEGED_MEMORY_START >> 20;

  // make the selected section available for PL0
  PL0_section.ACCESS_PERMISSIONS_2 =
    AP_2_0_MODEL_RW_ALL >> 2;
  PL0_section.ACCESS_PERMISSIONS_1_0 =
    AP_2_0_MODEL_RW_ALL & 0b011;

  *PL0_section_entry = PL0_section;

  // invalidate main Translation Lookup Buffer (just in case)
  asm("mcr p15, 0, %0, c8, c7, 0\n\r"
      "isb" :: "r" (0) : "memory");
  
  // check that translation works... by copying a string using one
  // mapping and reading it using other :D
  char message[] = "mapped sections for PL0 code";
  
  unsigned int i;
  for (i = 0; i < sizeof(message); i++)
    ((volatile char*) UNPRIVILEGED_MEMORY_START)[i] = message[i];
  
  puts((char*) VIRTUAL_PL0_MEMORY_START);

  // now paste a userspace program to that section
  for (uint32_t i = 0; i < PL_0_test_img.file_size; i++)
    ((volatile char*) VIRTUAL_PL0_MEMORY_START)[i] =
      PL_0_test_img.file_contents[i];

  puts("copied PL0 code to it's section");
}

// needed for array initialization in demo_go_unprivileged()
void *memset(void *s, int c, size_t n)
{
  char *mem = s;
  
  for (size_t i = 0; i < n; i++)
    mem[i] = c;
  
  return s;
}

void demo_go_unprivileged(void)
{
  uint32_t PL0_regs[14] = {0};
  PL0_regs[13] = VIRTUAL_PL0_MEMORY_START; // the new pc

  PSR_t new_SPSR = read_CPSR();
  new_SPSR.fields.PSR_MODE_4_0 = MODE_USER;
  write_SPSR(new_SPSR);

  puts("All ready, jumping to PL0 code");

  asm volatile("cps %[sysmode]\n\r"
	       "mov sp, %[stackaddr]\n\r"
	       "cps %[supmode]\n\r"
	       "ldm %[contextaddr], {r0 - r12, pc} ^" ::
	       [sysmode]"I" (MODE_SYSTEM),
	       [supmode]"I" (MODE_SUPERVISOR),
	       [stackaddr]"r" ((PL0_SECTION_NUMBER + 1) << 20),
	       [contextaddr]"r" (PL0_regs) : "memory");

  __builtin_unreachable();
}