#include "uart.h"
#include "demo_functionality.h"
#include "paging.h"
#include "interrupts.h"
#include "strings.h"
#include "psr.h"

extern void enable_irq ( 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");

  // prints some info
  demo_paging_support();



  // prints some info and switches to system mode
  demo_mode_to_system();
  // prints some info
  demo_current_mode();

  // prints some info and sets upp translation table, turns on MMU
  setup_flat_map();

  demo_setup_libkernel();

  demo_setup_PL0();
    enable_timer();

    // enable interrupts
  demo_setup_interrupts();

    *(int *)(0x3000B210) = 1;
    *(int *)(0x3000B204) = 1;


    asm("LDR r0, =0x3F00B200\n\r"
        "MOV r1, #2\n\r"
        "STR r1, [r0, #0x00]\n\r");

    *(int *)(0x7E00B210) = 1;
    *(int *)(0x7E00B214) = 1;
    *(int *)(0x7E00B218) = 1;
    uart_putc(*(int*)(0x40000034) );
    *(int *)(0x40000034) = 1;

    uint32_to_bits(*(int*)(0xE000ED24),buf);
    uart_puts(buf);
    uart_puts("\r\n");
    uint32_to_bits(*(int*)(0xE002ED24),buf);
    uart_puts(buf);
    uart_puts("\r\n");

    int regVal;
    asm("mrs %0, cpsr":"=r"(regVal):);
    uint32_to_bits(regVal,buf);
    uart_puts(buf);
    uart_puts("\r\n");

//    ; Read ICC_IGRPEN1 into Rt
//    MCR p15,0,<Rt>,c12,c12,7 ; Write Rt to ICC_IGRPEN1

    uint32_t ICC_IGRPEN1;
    asm(" MRC p15,0, %0 ,c12,c12,7" : "=r"(ICC_IGRPEN1) :: "memory"); //READ FROM ICC_IGRPEN1

    uint32_to_bits(ICC_IGRPEN1,buf);
    uart_puts(buf);
    uart_puts("\r\n");


    //    uint32_to_bits(*(int*)(0x40000024),buf);
//    uart_puts(buf);
//    uart_puts("\r\n");

//    *(int *)(0x7E00B210) = 1;
//    *(int *)(0x7E00B214) = 1;
//    *(int *)(0x7E00B218) = 1;

    // prints some info and sets up a section for PL0 code, loads a blob
  // there and jumps to it... never, ever, ever returns
  demo_go_unprivileged();

  while(1);
  
  while (1)
    {
      char c = uart_getc();

      uart_putc(c);
      
      if (c == '\r')
	uart_putc('\n');
    }
}