#include <rt/abort.h>
#include <rt/context.h>
#include <rt/idle.h>
#include <rt/syscall.h>
#include <rt/task.h>
#include <rt/tick.h>
#include <rt/trap.h>

#include <stdbool.h>
#include <stdint.h>

#define ST1_VMAP (UINT16_C(1) << 3)
#define ST1_OBJMODE (UINT16_C(1) << 9)
#define ST1_M0M1MAP (UINT16_C(1) << 11)

struct context
{
    // Saved automatically when taking an interrupt.
    uint16_t st0, t;
    uint32_t acc;
    uint32_t p;
    uint16_t ar0, ar1;
    uint16_t st1, dp;
    uint16_t ier, dbgstat;
    uint32_t pc;

    // Volatile context saved first.
    uint16_t ar0h, ar1h;
    uint32_t xar4;
    uint32_t xar5;
    uint32_t xar6;
    uint32_t xar7;

    /* Saving tl is only possible by pushing xt, but this copy of t will be
     * clobbered by the automatic restore of t:st0. */
    uint32_t xt;
#ifdef __TMS320C28XX_FPU32__
    uint32_t rb;
    float r0h, r1h, r2h, r3h;
#ifdef __TMS320C28XX_FPU64__
    float r0l, r1l, r2l, r3l;
#endif // __TMS320C28XX_FPU64__
#endif // __TMS320C28XX_FPU32__

    // Non-volatile context saved on context switch.
    uint32_t xar2;
    uint32_t xar3;
    uint32_t rpc;
#ifdef __TMS320C28XX_FPU32__
    float r4h, r5h, r6h, r7h;
#ifdef __TMS320C28XX_FPU64__
    float r4l, r5l, r6l, r7l;
#endif // __TMS320C28XX_FPU64__
    /* stf is volatile by the ABI as it contains floating-point condition
     * flags, but it also has rounding modes and the shadow status bit. The
     * rt_syscall_run* functions should not modify these, so we keep them in
     * the non-volatile part of the context to make saving- and restoring
     * volatile context faster for the syscall handlers. For the
     * rt_user1_handler we don't save the non-automatic volatile context, so
     * including stf in this would cause problems, as you would have arbitrary
     * rounding modes and shadow status from the stack on context restore. */
    uint32_t stf;
#endif // __TMS320C28XX_FPU32__
};

#define ST0_PM(pm) ((uint16_t)((pm) + 1) << 7)

__attribute__((noreturn)) void rt_task_entry(void (*fn)(void));

void *rt_context_init(uintptr_t fn, uintptr_t arg, void *stack,
                      size_t stack_size)
{
    struct context *ctx = stack;
    ctx->st0 = ST0_PM(0);
    ctx->acc = arg;
    ctx->st1 = ST1_VMAP | ST1_M0M1MAP | ST1_OBJMODE;
    // Enable all interrupt groups in every task.
    ctx->ier = UINT16_C(0xFFFF);
    ctx->pc = (uint32_t)rt_task_entry;
    ctx->xar4 = fn;
    ctx->rpc = 0;
#ifdef __TMS320C28XX_FPU32__
    ctx->rb = 0;
    ctx->stf = 0;
#endif // __TMS320C28XX_FPU32__
    /* Return one past the end of the context because the automatic context
     * save/restore increments/decrements sp by one as the last step. It does
     * this because loads and stores to *sp ignore the least-significant bit,
     * but sp may be unaligned on interrupt entry, so adding 1 guarantees that
     * the context save will start past the end of the active stack. */
    return (char *)(ctx + 1) + 1;
}

extern __cregister volatile uint16_t IER;
extern __cregister volatile uint16_t IFR;

#define IFR_DLOGINT (UINT16_C(1) << 14)
#define IFR_TIMER2 (UINT16_C(1) << 13)

__attribute__((noreturn, weak)) void rt_idle(void)
{
    for (;;)
    {
        __asm__(" idle");
    }
}

__attribute__((noreturn, weak)) void rt_abort(void)
{
    for (;;)
    {
        __asm__(" itrap0");
    }
}

__attribute__((noreturn, weak)) void rt_trap(void)
{
    for (;;)
    {
        __asm__(" estop0");
        __asm__(" itrap0");
    }
}

bool rt_interrupt_is_active(void)
{
    /* NOTE: this will also return true if one or more interrupts is manually
     * disabled within a task. If a sem_post is called from a task in that
     * case, it will take the interrupt path and try to use the semaphore's
     * post record. If the task is then pre-empted after it claims the record
     * but before it invokes the syscall, then other interrupts will be unable
     * to make sem_post syscalls on the same semaphore until this task resumes,
     * and they will just increment the semaphore. */
    return IER != 0xFFFFU;
}

void rt_syscall_pend(void)
{
    IFR |= IFR_DLOGINT;
}

#define IPCCOUNTERL (*(volatile uint32_t *)0x5000CUL)

uint32_t rt_cycle(void)
{
    return IPCCOUNTERL;
}

void rt_task_drop_privilege(void)
{
    // The C28x architecture doesn't have a notion of privilege levels.
}