#pragma once

#include <rt/arch/coprocessor.h>
#include <rt/arch/cpsr.h>
#include <rt/arch/mode.h>

#include <vic.h>

#define BIG_ENDIAN (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)

struct context
{
#if RT_ARM_FP
    /* FPEXC controls whether floating-point is enabled. Give each task its own
     * copy of this register where floating-point is initially disabled, so
     * attempts to use floating-point in tasks cause undefined instruction
     * exceptions that can be handled by enabling floating-point and
     * re-executing the instruction. See section B1.11.3 of the Arm
     * Architecture Reference Manual ARMv7-A and ARMv7-R Edition. */
    uint32_t fpexc;
#endif // RT_ARM_FP

    uint32_t r4, r5, r6, r7, r8, r9, r10, r11;
    uint32_t r0, r1, r2, r3, r12, lr, pc, psr;
};

static void profile_context_init(struct context *ctx)
{
    /* Tasks start in rt_task_entry, so the initial thumb bit should be set
     * if rt_task_entry is a thumb-mode function. */
    ctx->psr = MODE_SYS | (BIG_ENDIAN ? CPSR_E : 0) |
               ((((uintptr_t)rt_task_entry & 1) != 0) ? CPSR_THUMB : 0);
#if RT_ARM_FP
    // Initialize FPEXC to floating-point disabled.
    ctx->fpexc = 0;
#endif // RT_ARM_FP
}

void rt_task_drop_privilege(void)
{
    __asm__("dsb; cps %0" : : "i"(MODE_USR) : "memory");
}

void rt_cycle_init(void)
{
    // Enable counters and reset the cycle counter.
    pmcr_oreq(PMCR_E | PMCR_C);
    // Enable the cycle counter.
    pmcntenset_oreq(PMCNTEN_C);
}

static inline uint32_t cpsr_mode(void)
{
    uint32_t cpsr;
    __asm__ __volatile__("mrs %0, cpsr" : "=r"(cpsr));
    return cpsr & MODE_MASK;
}

static inline uint32_t spsr_mode(void)
{
    uint32_t spsr;
    __asm__ __volatile__("mrs %0, spsr" : "=r"(spsr));
    return spsr & MODE_MASK;
}

static inline bool mode_is_exception(uint32_t mode)
{
    return (MODE_USR < mode) && (mode < MODE_SYS);
}

bool rt_interrupt_is_active(void)
{
    /* NOTE: this assumes that nested interrupts don't use system mode.
     * Interrupt nesting should use supervisor mode, which doesn't require each
     * task stack to accommodate interrupts. */
    return mode_is_exception(cpsr_mode());
}

/* On A/R-profile, the svc handler doesn't need to preserve volatile
 * registers, so we declare them as being clobbered. */
void rt_syscall_0(enum rt_syscall syscall)
{
    register enum rt_syscall s __asm__("r0") = syscall;
    __asm__ __volatile__("svc 0"
                         : "+r"(s)
                         :
                         : "r1", "r2", "r3", "r12", "lr", "memory");
}

void rt_syscall_1(enum rt_syscall syscall, uintptr_t arg0)
{
    register enum rt_syscall s __asm__("r0") = syscall;
    register uintptr_t a0 __asm__("r1") = arg0;
    __asm__ __volatile__("svc 0"
                         : "+r"(s), "+r"(a0)
                         :
                         : "r2", "r3", "r12", "lr", "memory");
}

void rt_syscall_2(enum rt_syscall syscall, uintptr_t arg0, uintptr_t arg1)
{
    register enum rt_syscall s __asm__("r0") = syscall;
    register uintptr_t a0 __asm__("r1") = arg0, a1 __asm__("r2") = arg1;
    __asm__ __volatile__("svc 0"
                         : "+r"(s), "+r"(a0), "+r"(a1)
                         :
                         : "r3", "r12", "lr", "memory");
}

void rt_syscall_3(enum rt_syscall syscall, uintptr_t arg0, uintptr_t arg1,
                  uintptr_t arg2)
{
    register enum rt_syscall s __asm__("r0") = syscall;
    register uintptr_t a0 __asm__("r1") = arg0, a1 __asm__("r2") = arg1,
                          a2 __asm__("r3") = arg2;
    __asm__ __volatile__("svc 0"
                         : "+r"(s), "+r"(a0), "+r"(a1), "+r"(a2)
                         :
                         : "r12", "lr", "memory");
}

void rt_syscall_pend(void)
{
    vic_syscall_pend();
}

uint32_t rt_cycle(void)
{
#if RT_CYCLE_ENABLE
    return pmccntr();
#else
    return 0;
#endif
}

void rt_tls_set(void *tls)
{
    tpidruro_set((uintptr_t)tls);
}

#if RT_ARM_FP
bool rt_lazy_enable_fp(void)
{
    uint32_t fpexc;
    __asm__ __volatile__("vmrs %0, fpexc" : "=r"(fpexc));
    /* If the FPU is already enabled or the undefined instruction was in an
     * exception handler, there's nothing to do. */
    if (((fpexc & RT_FPEXC_EN) != 0) || mode_is_exception(spsr_mode()))
    {
        return false;
    }

    // Enable the FPU and initialize the floating point state.
    __asm__("vmsr fpexc, %0" : : "r"(RT_FPEXC_EN));
    __asm__("vmsr fpscr, %0" : : "r"(0));
    __asm__("vmov d0, %0, %0" : : "r"(0));
    __asm__("vmov d1, %0, %0" : : "r"(0));
    __asm__("vmov d2, %0, %0" : : "r"(0));
    __asm__("vmov d3, %0, %0" : : "r"(0));
    __asm__("vmov d4, %0, %0" : : "r"(0));
    __asm__("vmov d5, %0, %0" : : "r"(0));
    __asm__("vmov d6, %0, %0" : : "r"(0));
    __asm__("vmov d7, %0, %0" : : "r"(0));
    __asm__("vmov d8, %0, %0" : : "r"(0));
    __asm__("vmov d9, %0, %0" : : "r"(0));
    __asm__("vmov d10, %0, %0" : : "r"(0));
    __asm__("vmov d11, %0, %0" : : "r"(0));
    __asm__("vmov d12, %0, %0" : : "r"(0));
    __asm__("vmov d13, %0, %0" : : "r"(0));
    __asm__("vmov d14, %0, %0" : : "r"(0));
    __asm__("vmov d15, %0, %0" : : "r"(0));
#if RT_ARM_FP_D32
    __asm__("vmov d16, %0, %0" : : "r"(0));
    __asm__("vmov d17, %0, %0" : : "r"(0));
    __asm__("vmov d18, %0, %0" : : "r"(0));
    __asm__("vmov d19, %0, %0" : : "r"(0));
    __asm__("vmov d20, %0, %0" : : "r"(0));
    __asm__("vmov d21, %0, %0" : : "r"(0));
    __asm__("vmov d22, %0, %0" : : "r"(0));
    __asm__("vmov d23, %0, %0" : : "r"(0));
    __asm__("vmov d24, %0, %0" : : "r"(0));
    __asm__("vmov d25, %0, %0" : : "r"(0));
    __asm__("vmov d26, %0, %0" : : "r"(0));
    __asm__("vmov d27, %0, %0" : : "r"(0));
    __asm__("vmov d28, %0, %0" : : "r"(0));
    __asm__("vmov d29, %0, %0" : : "r"(0));
    __asm__("vmov d30, %0, %0" : : "r"(0));
    __asm__("vmov d31, %0, %0" : : "r"(0));
#endif // RT_ARM_FP_D32
    return true;
}
#endif // RT_ARM_FP