#include "io.h"
#include "uart.h"
#include "svc_interface.h"
#include "armclock.h"
#include "scheduler.h"

void undefined_instruction_vector(void)
{
  error("Undefined instruction occured");
}

uint32_t supervisor_call_handler(uint32_t regs[14])
{
  switch(regs[0]) {
  case UART_PUTCHAR:
    if (putchar_non_blocking(regs[1]))
      schedule_wait_for_output(regs, regs[1]);
    break;
  case UART_GETCHAR:
    {
      int c;
      if ((c = getchar_non_blocking()) == -1)
	schedule_wait_for_input(regs);

      regs[0] = c;
      break;
    }
  case UART_WRITE:
    error("UART_WRITE not implemented!!!!!");
    break;
  default:
    // perhaps we should kill the process now?
    error("unknown supervisor call type!!!!!");
  }

  return 0; // a dummy value
}

void abort_handler(void)
{
  // TODO maybe dump registers here?
  error("re-entered system due to data/prefetch abort");
}

void generic_handler(void)
{
  error("something weird happened");
}

void irq_handler(uint32_t regs[14])
{
  if (armclk_irq_pending())
    {
      write_SPSR(PL1_PSR);
      asm volatile("mov r0, %[context]\n\r"
		   "mov lr, %[return_func]\n\r"
		   "subs pc, lr, #0" ::
		   [context]"r" (regs),
		   [return_func]"r" (schedule_save_context) :
		   "memory");
    }
  else if (uart_irq_pending())
    {
      if (uart_recv_irq_pending())
	{
	  uart_clear_recv_irq();
	  scheduler_try_input();
	}
      if (uart_send_irq_pending())
	{
	  uart_clear_send_irq();
	  scheduler_try_output();
	}

      if (read_SPSR().fields.PSR_MODE_4_0 != MODE_USER)
	{
	  // TODO set supervisor mode's stack pointer
	  write_SPSR(PL1_PSR);
	  asm volatile("mov lr, %0\n\r"
		       "subs pc, lr, #0" ::
		       "r" (schedule) : "memory");
	}
    }
  else
    error("unknown irq");

  // important - don't allow this handler to return if irq came from
  // PL1 (likely supervisor, because we don't really use system) mode
}

void fiq_handler(void)
{
  error("fiq happened");
}