#include "kern/priv.h"

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

#ifdef ARCH_STM32L4
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");
}

void jump_to_user_mode()
{
  void* new_stack = kalloc(4096);
  new_stack += 4096;

  asm volatile(
      "mov  r0, %0\n\t"
      "msr  psp, r0\n\t"
      "mrs  r0, control\n\t"
      "orrs r0, r0, #3\n\t"
      "msr  control, r0\n\t"
      "isb\n\t"
      "dsb\n\t"
      "b usermode_start\n\t" : : "r"(new_stack));
}
#endif