/* Test MPU small memory regions
 */
#include "armv7m.h"
#include "testme.h"

static inline void test_equal(const char *m, uint32_t expect, uint32_t actual)
{
    testEqI(expect, actual, "%s", m);
}

char __end_rom, __after_all_load;

char testpage[1024] __attribute__((aligned(1024)));

static
void try(volatile char *p)
{
    uint32_t tval = 0;
    testDiag("Try %" PRIx32, (uint32_t)p);
    __asm__ ("mov %r0, #'X'; ldr %r0, [%r1]" : "+r"(tval) : "r"(p) :);
    testDiag("Got %" PRIx32, tval);
}

#define EXPECT_NO_FAULT 1
#define EXPECT_MEMFAULT 2
#define EXPECT_TAKEN_MEMFAULT 3
#define EXPECT_HARDFAULT 10
#define EXPECT_TAKEN_HARDFAULT 11

static volatile unsigned expect_fault = EXPECT_NO_FAULT;

#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))

static const char *expect_fault_name[] = {
    [EXPECT_NO_FAULT] = "no fault",
    [EXPECT_MEMFAULT] = "MemManage fault",
    [EXPECT_TAKEN_MEMFAULT] = "MemManage fault taken",
    [EXPECT_HARDFAULT] = "HardFault",
    [EXPECT_TAKEN_HARDFAULT] = "HardFault taken",
};

const char *faultname(unsigned int f)
{
    if (f > ARRAY_SIZE(expect_fault_name) || !expect_fault_name[f]) {
        /* This would be a test case code error */
        testDiag("faultname passed bad expect_fault state 0x%x", f);
        abort();
    }
    return expect_fault_name[f];
}

static
void checkfault(unsigned v)
{
    if (v == expect_fault) {
        testPass("%s as expected", faultname(v));
    } else {
        int curstate = expect_fault;

        /* If expect_fault is still EXPECT_MEMFAULT or EXPECT_HARDFAULT
         * then we didn't actually take a fault, so report that as
         * "no fault".
         */
        if (curstate == EXPECT_MEMFAULT || curstate == EXPECT_HARDFAULT) {
            curstate = EXPECT_NO_FAULT;
        }

        testFail("expected %s but %s", faultname(v), faultname(curstate));
    }
}

typedef int testfn(int);

static int tryexec(volatile char *p)
{
    uint32_t tval = 0;
    testfn *f = (testfn *)((uintptr_t)p | 1);
    testDiag("Tryexec %" PRIx32, (uint32_t)p);
    /* The assumption is that the code at the dest address is
     *   adds r0, #1; bx lr
     * so we can call it, and it will increment our value for us.
     * For non-executable code the exception handler will
     * by-hand do the function return back to lr.
     */
    tval = f(tval);
    testDiag("Returned with %" PRIx32, tval);
    if (expect_fault == EXPECT_NO_FAULT && tval != 1) {
        testFail("expected code to return 1 but got %" PRId32 "\n", tval);
        return 0;
    }
    return 1;
}

static void write_exec_test_insns(volatile char *p)
{
    /* Write the "adds r0, #1; bx lr" sequence we'll use
     * in tryexec().
     */
    volatile uint16_t *px = (uint16_t*)p;
    px[0] = 0x3001; /* adds r0, 1 */
    px[1] = 0x4770; /* bx lr */
}

void hard(uint32_t *sp)
{
    testDiag("In HardFault handler");
    if(expect_fault==EXPECT_HARDFAULT) {
        sp = get_src_stack(sp);
        inst_skip(sp);
        expect_fault = EXPECT_TAKEN_HARDFAULT;
        return;
    }
    testDiag("Unexpected HardFault!!");
    abort();
}

static __attribute__((naked))
void hard_entry(void)
{
    asm("mov r0, sp");
    asm("b hard");
}

void mem(uint32_t *sp)
{
    uint32_t cfsr = in32(SCB(0xd28));
    uint32_t addr = in32(SCB(0xd34));

    out32(SCB(0xd28), 0xff); /* W1C the MMFSR bits */

    sp = get_src_stack(sp);

    testDiag("In MemFault, CFSR 0x%" PRIx32 ", from 0x%" PRIX32, cfsr, sp[6]);
    if (cfsr & 0x80) {
        testDiag("...MMARVALID set, MMFAR is 0x%" PRIx32, addr);
    }

    if (cfsr & 0x1) {
        /* IACCVIOL -- set PC to LR. Note that the LR will have the
         * bit-0-means-Thumb convention but an exception frame PC slot
         * has the "raw" PC.
         */
        sp[6] = sp[5] & ~1;
    } else {
        /* DACCVIOL -- just skip offending insn */
        inst_skip(sp);
    }

    switch(expect_fault) {
    case EXPECT_MEMFAULT:
        expect_fault = EXPECT_TAKEN_MEMFAULT;
        break;
    case EXPECT_NO_FAULT:
        testDiag("Unexpected MemFault!!");
        abort();
    default:
        testDiag("Unexpected state 0x%x", expect_fault);
        abort();
    }
}

static __attribute__((naked))
void mem_entry(void)
{
    asm("mov r0, sp");
    asm("b mem");
}

static inline
void drop_priv(void)
{
    uint32_t val;
    __asm__ ("mrs %0, CONTROL" : "=r"(val) ::);
    val |= 1;
    __asm__ ("msr CONTROL, %0" :: "r"(val) :);
}

void svc(void *sp)
{
    int num;
    sp = get_src_stack(sp);
    num = get_svc(sp);
    if(num<0) {
        testDiag("SVC but not looking at an SVC insn??\n");
        abort();
    } else {
        testDiag("SVC 0x%x", num);
    }
    switch(num) {
    case 1: /* restore privlage */
    {
        uint32_t val;
        __asm__ ("mrs %0, CONTROL" : "=r"(val) ::);
        val &= ~1;
        __asm__ ("msr CONTROL, %0" :: "r"(val) :);
    }
    break;
    case 2:
        abort();
    default:
        testDiag("Unknown SVC request value");
        abort();
    }
}

static __attribute__((naked))
void svc_entry(void)
{
    asm("mov r0, sp");
    asm("b svc");
}

void main(void)
{
    int has_mpu;
    uint32_t mpu_type;

    run_table.hard = &hard_entry;
    run_table.svc = &svc_entry;
    run_table.mem = &mem_entry;

    mpu_type = in32(SCB(0xD90));
    has_mpu = ((mpu_type >> 8) & 0xff) != 0;

    testInit(8);

    if (!has_mpu) {
        testDiag("No MPU present: nothing to test here");
        testDiag("Done.");
        return;
    }

    out32(SCB(0xd24), 1<<16); // enable MemFault with SHCSR

    testDiag("In Main");
    expect_fault = EXPECT_NO_FAULT;

    /* priority of entries is highest to lowest (0 checked last) */

    set_mpu(0, 0x00000000, (uint32_t)&__end_rom, MPU_NORMAL|MPU_RORO);
    set_mpu(1, 0x20000000, 0x00080000, MPU_NORMAL|MPU_RWRW|MPU_XN);
    set_mpu(2, 0x4000c000, 0x00001000, MPU_DEVICE|MPU_RWRW|MPU_XN);
    set_mpu(3, 0xe000e000, 0x00001000, MPU_DEVICE|MPU_RWRW|MPU_XN);

    /* Create several small regions inside testpage:
     * accessible in the middle, surrounded by regions which
     * are not accessible.
     */
    set_mpu(4, (uint32_t)testpage, 512,
            MPU_XN|MPU_NORMAL|MPU_NANA);
    set_mpu(5, (uint32_t)testpage + 512, 256,
            MPU_NORMAL|MPU_RWRW);
    set_mpu(6, (uint32_t)testpage + 768, 256,
            MPU_XN|MPU_NORMAL|MPU_NANA);

    write_exec_test_insns(testpage + 256);
    write_exec_test_insns(testpage + 600);
    write_exec_test_insns(testpage + 768);

    testDiag("Enable MPU");
    enable_mpu(1,1,0);

    testDiag("Access the accessible small page (should not fault)");
    expect_fault = EXPECT_NO_FAULT;
    try(testpage + 600);
    checkfault(EXPECT_NO_FAULT);

    expect_fault = EXPECT_MEMFAULT;
    testDiag("Access the preceding small page (should fault)");
    try(testpage + 256);
    checkfault(EXPECT_TAKEN_MEMFAULT);

    expect_fault = EXPECT_MEMFAULT;
    testDiag("Access following small page (should fault)");
    try(testpage + 768);
    checkfault(EXPECT_TAKEN_MEMFAULT);

    expect_fault = EXPECT_NO_FAULT;
    testDiag("Access the accessible small page again (should not fault)");
    try(testpage + 600);
    checkfault(EXPECT_NO_FAULT);

    /* Try to execute from various parts of the small page */
    expect_fault = EXPECT_NO_FAULT;
    testDiag("Execute from the accessible small page (should not fault)");
    // TODO we get a HardFault here...
    if (tryexec(testpage + 600)) {
        checkfault(EXPECT_NO_FAULT);
    }

    expect_fault = EXPECT_MEMFAULT;
    testDiag("Execute from the preceding small page (should fault)");
    if (tryexec(testpage + 256)) {
        checkfault(EXPECT_TAKEN_MEMFAULT);
    }

    expect_fault = EXPECT_MEMFAULT;
    testDiag("Execute from the following small page (should fault)");
    if (tryexec(testpage + 768)) {
        checkfault(EXPECT_TAKEN_MEMFAULT);
    }

    expect_fault = EXPECT_NO_FAULT;
    testDiag("Execute from the accessible small page again (should not fault)");
    if (tryexec(testpage + 600)) {
        checkfault(EXPECT_NO_FAULT);
    }

    testDiag("Done.");
}