#include "kern/priv.h"

#include "arch.h"
#include "kern/log.h"

void handle_svc_call(uint32_t svc_number)
{
  klogf("Handle SVC: %p\n", svc_number);
  for (;;)
    ;
}

asm("	.align	2\n"
    "	.global	on_svc\n"
    "	.syntax unified\n"
    "	.fpu softvfp\n"
    "	.type	on_svc, %function\n"
    "on_svc:\n"
    "	tst   lr,#4\n"
    " ite   eq\n"
    "	moveq r12,sp\n"
    "	mrsne r12,psp\n"
    "	ldr   r12, [r12, #24]\n"
    "	ldrh  r12, [r12, #-2]\n"
    "	bics  r12, r12, #0xff00\n"
    "	push  {r4, lr}\n"
    "	mov   r0 , r12\n"
    "	bl    handle_svc_call\n"
    "	pop   {r4, lr}\n"
    "	tst   lr, #4\n"
    " ite   eq\n"
    "	moveq r12,sp\n"
    "	mrsne r12,psp\n"
    "	stm   r12,{r0-r3}\n"
    "	bx    lr\n");

// void on_svc()  // ISR handler. Override from weak symbol.
// {
// asm volatile(
//     "push {r0-r12, lr}\n\t"
//     "ldr  r0, [lr, #-4]\n\t"
//     "bic  r0, r0, #0xff000000\n\t"
//     );

// TST     LR,#4                   ; Called from Handler Mode?
// MRSNE   R12,PSP                 ; Yes, use PSP
// MOVEQ   R12,SP                  ; No, use MSP
// LDR     R12,[R12,#24]           ; Read Saved PC from Stack
// LDRH    R12,[R12,#-2]           ; Load Halfword
// BICS    R12,R12,#0xFF00         ; Extract SVC Number
// uint32_t reg;
// asm volatile(
//     "mov  r12, sp\n\t"
//     "ldr  r12, [r12,#24]\n\t"
//     // "ldrh r12,[r12, #-2]\n\t"
//     // "bics r12, r12, #0xff00\n\t"
//     "mov  %0, r12\n\t": "=r"(reg));

//   uint32_t base[0];
//   uint32_t reg;
//
//
//   asm volatile("mov  %0, sp\n\t" : "=r"(reg));
//   uint32_t* at = (uint32_t*) (reg + 44); /* Does GCC set up 5 stack frame
//   words? */
//
//   klogf("Stack pointer: %p\n", reg);
//   klogf("Stack pointer +44: %p\n", (reg + 44));
//   klogf("At_: %p: %p\n\n", 0x80009f6, *(uint32_t*)0x80009f6);
//   klogf("At: %p: %p\n\n", at, *at);
//
//
//   int i = 0;
//   for (; i < 20 && &base[i] != (void*)STACK_TOP; ++ i) {
//     kerr_logf(" (%p) %p\n", &base[i], base[i]);
//   }
//
//   // klogf("TEST %p\n", reg);
//
//   for(;;);

// register int svc_number asm ("r0");

// klogf("SVC #: %p\n", svc_number);
// }

void set_control_register(uint32_t reg)
{
  asm volatile("msr control, %0" : "=r"(reg) :);
}

uint32_t get_control_register()
{
  uint32_t control;
  asm volatile("mrs %0, control" : "=r"(control) :);
  return control;
}

void enter_user_mode()
{
  asm volatile(
      "mov r0, #1\n\t"
      "msr control, r0\n\t");
}