#include "uart.h"
#include "cpsr.h"
#include "strings.h"
#include "short_descriptor.h"

extern char __end;

void enabling_code_from_the_net(void);
  
void kernel_main(uint32_t r0, uint32_t r1, uint32_t atags)
{
  // Declare as unused
  (void) r0;
  (void) r1;
  (void) atags;

  uart_init();

  // When we attach screen session after loading kernel with socat
  // we miss kernel's greeting... So we'll make the kernel wait for
  // one char we're going to send from within screen
  uart_getc();
  
  uart_puts("Hello, kernel World!\r\n");

  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);

  uint32_t CPSR;
  // get content of current program status register to check the current
  // processor mode
  asm("mrs %0, cpsr" : "=r" (CPSR) :: "memory");
  
  char *mode_name;
  
  switch(read_processor_mode())
    {
    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...\r\n");
  uart_puts("current mode: ");
  set_system_mode();

  switch(read_processor_mode())
    {
    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(mode_name);

  char bits[33];

  // compute translation table address for TTBR0
  // translation table shall start at first 2^14-bytes aligned
  // address after the kernel image
  //  uint32_t kernel_end = (uint32_t) &__end;
  uint32_t translation_table_base = 0x4000; // for now try 0x4000
    //    ((kernel_end - 1) & ~((uint32_t) 0x3fff)) + (uint32_t) 0x4000;

  uint32_to_bits(translation_table_base, bits);
  uart_puts("\n\rbinary representation of chosen"
	    " lvl1 translation table address: ");
  uart_puts(bits);
 
  uart_puts("\n\rpreparing translation table\n\r");
  uint32_t *translation_table = (uint32_t*) translation_table_base;
  
  // flat map all memory
  translation_table[0] = sd_lvl1_make_section(0x0);
  // make all other entries in translation table invalid :)
  for (uint32_t i = 0; i < 4096; i++)
    translation_table[i] = sd_lvl1_make_section(i << 20);

  uint32_to_bits(translation_table[0], bits);
  uart_puts("translation_table_entry 0: \n\r");
  uart_puts(bits);
  
  //  uart_puts("\n\renabling the MMU\n\r");
  //  asm volatile("" ::: "memory");
  //  enabling_code_from_the_net();
  //  goto skip;
 
  
  // meddle with domain settings
  uint32_t DACR;
  asm("mrc p15, 0, %0, c3, c0, 0" : "=r" (DACR));
  uint32_to_bits(DACR, bits);

  uart_puts("initial DACR contents: ");
  uart_puts(bits);

  uart_puts("\n\rsetting domain0 to client access"
	    " and blocking other domains\n\r");

  DACR = 1;
  asm("mcr p15, 0, %0, c3, c0, 0" :: "r" (DACR));

  asm("mrc p15, 0, %0, c3, c0, 0" : "=r" (DACR));
  uint32_to_bits(DACR, bits);

  uart_puts("new DACR contents:     ");
  uart_puts(bits);


  // meddle with SCTLR, which determines how some bits in
  // table descriptors work and also controls caches
  // we don't want to use access flag, so we set AFE to 0
  // we don't want TEX remap, so we set TRE to 0
  // we also disable data and instruction caches and the MMU
  uint32_t SCTLR;
  asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (SCTLR));
  uint32_to_bits(SCTLR, bits);

  uart_puts("\n\rSCTLR contents:      ");
  uart_puts(bits);
  
  uart_puts("\n\rsetting C, I, AFE and TRE to 0 in SCTLR\n\r");

  SCTLR &= ~((((uint32_t) 1) << 29) |
	     (((uint32_t) 1) << 28) |
	     (((uint32_t) 1) << 12) |
	     (((uint32_t) 1) << 2)); // set AFE and TRE to 0
  asm("mcr p15, 0, %0, c1, c0, 0\n\r"
      "isb" :: "r" (SCTLR));

  asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (SCTLR));
  uint32_to_bits(SCTLR, bits);

  uart_puts("new SCTLR contents: ");
  uart_puts(bits);

  // invalidate instruction cache
  uart_puts("\n\rinvalidating instruction cache\n\r");
  asm("mcr p15, 0, r0, c7, c5, 0\n\r" // r0 gets ignored
      "isb" ::: "memory");

  // invalidate branch-prediction
  uart_puts("\n\rinvalidating branch-prediction\n\r");
  asm("mcr p15, 0, r0, c7, c5, 6\n\r"
      "isb" ::: "memory");

    // invalidate instruction cache
  uart_puts("\n\rinvalidating entire main TLB\n\r");
  asm("mcr p15, 0, %0, c8, c7, 0\n\r"
      "isb" :: "r" (0) : "memory");

  // now see what's in TTBCR
  // set it use TTBR0 exclusively
  uint32_t TTBCR;
  asm("mrc p15, 0, %0, c2, c0, 2" : "=r" (TTBCR));
  uint32_to_bits(TTBCR, bits);

  uart_puts("\n\rTTBCR contents:      ");
  uart_puts(bits);

  uart_puts("\n\rSetting TTBCR.N to 0, so that"
	    " TTBR0 is used everywhere\n\r");

  TTBCR &= ~((uint32_t) 0x7); // set N to 0
  asm("mcr p15, 0, %0, c2, c0, 2" :: "r" (TTBCR));

  asm("mrc p15, 0, %0, c2, c0, 2" : "=r" (TTBCR));
  uint32_to_bits(TTBCR, bits);

  uart_puts("new TTBCR contents: ");
  uart_puts(bits);

  
  // Now do stuff with TTBR0
  uint32_t TTBR0;
  asm("mrc p15, 0, %0, c2, c0, 0" : "=r" (TTBR0));
  uint32_to_bits(TTBR0, bits);

  uart_puts("\n\rTTBR0 contents:     ");
  uart_puts(bits);

  uart_puts("\n\r");

  TTBR0 = ((TTBR0 << 18) >> 18) | translation_table_base;
  TTBR0 &= ~((uint32_t) 0x1a); // set RGN and S in TTBR0 to 0
  asm("mcr p15, 0, %0, c2, c0, 0" :: "r" (TTBR0));

  asm("mrc p15, 0, %0, c2, c0, 0" : "=r" (TTBR0));
  uint32_to_bits(TTBR0, bits);

  uart_puts("new TTBR0 contents: ");
  uart_puts(bits);
  

  // enable MMU
  asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (SCTLR));
  uint32_to_bits(SCTLR, bits);

  uart_puts("\n\rSCTLR contents before MMU enabling: ");
  uart_puts(bits);

  uart_puts("\n\renabling the MMU\n\r");
  
  // set M to 0
  SCTLR |= (uint32_t) 1;

  asm("mcr p15, 0, %0, c1, c0, 0" :: "r" (SCTLR));

  asm("mrc p15, 0, %0, c1, c0, 0\r\n"
      "isb" : "=r" (SCTLR));

  uint32_to_bits(SCTLR, bits);

  uart_puts("SCTLR contents after MMU enabling:  ");
  uart_puts(bits);

 skip:
  uart_puts("skip here\n\r");
  
  while (1)
    uart_putc(uart_getc());
}