#include "scheduler.h"
#include "uart.h"
#include "strings.h"
#include "armclock.h"
#include "memory.h"
#include "io.h"

// for now we only have 1 process in "queue"
// later there is going to be an actual queue
uint32_t PL0_regs[14] = {0}; // contains r0-r12, pc
uint32_t PL0_sp;
uint32_t PL0_lr;

PSR_t PL0_PSR; // to be put into spsr when jumping to user mode

PSR_t PL1_PSR;

// when set, it means process used GETCHAR system call and once we get
// a char, we have to return it
_Bool waiting_for_input = 0;

// when set, it means process used PUTCHAR system call and once we
// manage to put the char, we can return to process
_Bool waiting_for_output = 0;
char waiting_output;

// 0 if kernel code in system mode is being run
// 1 if our process is being run
// later when we have many processes and this will hold process id
uint32_t current_process;

void setup_scheduler_structures(void)
{
  PL1_PSR = read_CPSR();
}

void scheduler_try_output(void)
{
  if (waiting_for_output)
    if (!putchar_non_blocking(waiting_output))
      {
	waiting_for_output = 0;
	uart_send_irq_disable();
      }
}

void scheduler_try_input(void)
{
  if (waiting_for_input)
    if ((PL0_regs[0] = getchar_non_blocking()) != (uint32_t) (-1))
      {
	waiting_for_input = 0;
	uart_recv_irq_disable();
      }
}

void __attribute__((noreturn))
schedule_new(uint32_t pc, uint32_t sp)
{
  PL0_regs[13] = pc;
  PL0_sp = sp;
  PL0_lr = 0;
    
  PL0_PSR = read_CPSR();
  PL0_PSR.fields.PSR_MODE_4_0 = MODE_USER;
  PL0_PSR.fields.PSR_IRQ_MASK_BIT = 0;

  schedule();
}

void __attribute__((noreturn))
schedule_wait_for_output(uint32_t regs[14], char c)
{
  if (current_process == 0)
    error("SYSTEM tried waiting for output!");

  waiting_for_output = 1;
  waiting_output = c;
  uart_send_irq_enable();
  
  schedule_save_context(regs);
}

void __attribute__((noreturn))
schedule_wait_for_input(uint32_t regs[14])
{
  if (current_process == 0)
    error("SYSTEM tried waiting for input!");

  waiting_for_input = 1;
  uart_recv_irq_enable();
	
  schedule_save_context(regs);
}

void __attribute__((noreturn))
schedule_save_context(uint32_t regs[14])
{
  memcpy(PL0_regs, regs, sizeof(PL0_regs));
  
  PL0_PSR = read_SPSR();
  
  asm volatile("cps %[sysmode]\n\r"
	       "isb\n\r"
	       "mov %[sp_transfer], sp\n\r"
	       "mov %[lr_transfer], lr\n\r"
	       "cps %[supmode]\n\r"
	       "isb\n\r" :
	       [sp_transfer]"=r" (PL0_sp),
	       [lr_transfer]"=r" (PL0_lr):
	       [sysmode]"I" (MODE_SYSTEM),
	       [supmode]"I" (MODE_SUPERVISOR) : "memory");

  schedule();
}

void __attribute__((noreturn)) schedule(void)
{
  current_process = 0;
  armclk_disable_timer_irq();
  
  if (waiting_for_input || waiting_for_output)
    {
      PSR_t new_CPSR = PL1_PSR;
      new_CPSR.fields.PSR_IRQ_MASK_BIT = 0;
  
      write_CPSR(new_CPSR);
      
      asm volatile("wfi" ::: "memory");

      __builtin_unreachable();
    }

  current_process = 1;

  asm volatile("cps %[sysmode]\n\r"
  	       "isb\n\r"
  	       "mov sp, %[stackaddr]\n\r"
  	       "mov lr, %[linkaddr]\n\r"
  	       "cps %[supmode]\n\r"
  	       "isb" ::
  	       [sysmode]"I" (MODE_SYSTEM),
  	       [supmode]"I" (MODE_SUPERVISOR),
  	       [stackaddr]"r" (PL0_sp),
  	       [linkaddr]"r" (PL0_lr) : "memory");

  armclk_irq_settimeout(0x00100000);
  armclk_enable_timer_irq();

  write_SPSR(PL0_PSR);
  
  asm volatile("ldm %0, {r0 - r12, pc} ^" ::
	       "r" (PL0_regs) : "memory");

  __builtin_unreachable();
}