#include "uart.h"
#include "psr.h"
#include "memory.h"
#include "translation_table_descriptors.h"
#include "libkernel.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\n\r"; break;
    case 1 : paging = "implementation defined paging\n\r"; break;
    case 2 : paging = "VMSAv6, with cache and TLB type registers\n\r"; break;
    case 3 : paging = "VMSAv7, with support for remapping and access flag\n\r"; break;
    case 4 : paging = "VMSAv7 with PXN bit supported\n\r"; break;
    case 5 : paging = "VMSAv7, PXN and long format descriptors. EPAE is supported.\n\r"; break;
    default : paging = "?_? unknown paging ?_?\n\r";
    }
  
  uart_puts(paging);
}

void demo_mode_to_system(void)
{

  // get content of current program status register to check the current
  // processor mode
  PSR_t CPSR = read_CPSR();

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

  uart_puts("current mode: ");
  uart_puts(mode_name);

  uart_puts("setting mode to system (PL1)...\r\n");
  set_system_mode();
}

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

extern char
  __renamed_start,
  __renamed_end,
  __renamed_size;

void demo_setup_libkernel(void) {
  short_section_descriptor_t volatile *libkernel_section_entry =
    &TRANSLATION_TABLE[LIBKERNEL_SECTION_NUMBER];

  short_section_descriptor_t
    libkernel_section = *libkernel_section_entry;

  // make libkernel section executable and read-only for
  // unprivileged code
  libkernel_section.ACCESS_PERMISSIONS_2 =
    AP_2_0_MODEL_RW_PL1_RO_PL0 >> 2;
  libkernel_section.ACCESS_PERMISSIONS_1_0 =
    AP_2_0_MODEL_RW_PL1_RO_PL0 & 0b011;

  *libkernel_section_entry = libkernel_section;  

  // invalidate main Translation Lookup Buffer (just in case)
  asm("mcr p15, 0, %0, c8, c7, 0\n\r"
      "isb" :: "r" (0) : "memory");
  
  // copy libkernel code to libkernel section
  for (size_t i = 0; i < (size_t) &__renamed_size; i++)
    ((volatile char*) LIBKERNEL_SECTION_START)[i] =
      (&__renamed_start)[i];  
}

extern char
  _binary_PL_0_test_img_start,
  _binary_PL_0_test_img_end,
  _binary_PL_0_test_img_size;

void demo_setup_PL0(void)
{
  short_section_descriptor_t volatile *PL0_section_entry =
    &TRANSLATION_TABLE[PL0_SECTION_NUMBER];
  short_section_descriptor_t volatile *UART_memory_section_entry =
    &TRANSLATION_TABLE[((uint32_t) GPIO_BASE) >> 20];

  short_section_descriptor_t
    PL0_section = *PL0_section_entry,
    UART_memory_section = *UART_memory_section_entry;

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

  // make the selected section and uart 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;

  UART_memory_section.ACCESS_PERMISSIONS_2 =
    AP_2_0_MODEL_RW_ALL >> 2;
  UART_memory_section.ACCESS_PERMISSIONS_1_0 =
    AP_2_0_MODEL_RW_ALL & 0b011;

  *PL0_section_entry = PL0_section;
  *UART_memory_section_entry = UART_memory_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\n\r";
  
  unsigned int i;
  for (i = 0; i < sizeof(message); i++)
    ((volatile char*) UNPRIVILEGED_MEMORY_START)[i] = message[i];
  
  uart_puts((char*) VIRTUAL_PL0_MEMORY_START);

  // now paste a userspace program to that section
  for (size_t i = 0; i < (size_t) &_binary_PL_0_test_img_size; i++)
    ((volatile char*) VIRTUAL_PL0_MEMORY_START)[i] =
      (&_binary_PL_0_test_img_start)[i];

  uart_puts("copied PL0 and libkernel code to their sections");
}

void demo_go_unprivileged(void)
{
  size_t call_unprivileged_offset =
    (size_t) &call_unprivileged - (size_t) &__renamed_start;

  void *call_unprivileged_new_location =
    (void*) (LIBKERNEL_SECTION_START + call_unprivileged_offset);
  
  // call call_unprivileged from libkernel
  asm volatile("mov  r5, %0\n\r"
	       "mov  r0, %1\n\r"
	       "mov  r4, #0\n\r"
  	       "movt r4, #"PL0_SECTION_NUMBER_STR"1111\n\r"
  	       "mov  sp, r4\n\r" // setting stack is important :D
  	       "blx  r5\n\r" ::
	       "r" (call_unprivileged_new_location),
	       "r" (VIRTUAL_PL0_MEMORY_START)
	       : "memory", "r4", "r5", "r0");
}

extern char
  __interrupt_vectors_start,
  __interrupt_vectors_end,
  __interrupt_vectors_size;

extern void (*volatile system_reentry_point)(void);

void system_reentry(void)
{
  uart_puts("re-entered system");
  while(1);
}

void demo_setup_interrupts(void)
{
  system_reentry_point = system_reentry;

  for (size_t i = 0; i < (size_t) &__interrupt_vectors_size; i++)
    ((volatile char*) 0)[i] =
      (&__interrupt_vectors_start)[i];
}