/* Test MPU
 */
#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 offlimits[1024] __attribute__((aligned(1024)));

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

static
void try(volatile char *p)
{
    uint32_t tval = 0;
    testDiag("Try %x", (uint32_t)p);
    __asm__ ("mov %r0, #'X'; ldr %r0, [%r1]" : "+r"(tval) : "r"(p) :);
    testDiag("Got %x", 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;

static
void checkfault(unsigned v)
{
    testEqI(v, expect_fault, "fault state");
}

void hard(uint32_t *sp)
{
    testDiag("In HardFault handler");
    if(expect_fault==EXPECT_HARDFAULT) {
        sp = get_src_stack(sp);
        inst_skip(sp);
        testDiag("Trying access offlimits inside HardFault handler");
        try(offlimits);
        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 addr;

    sp = get_src_stack(sp);
    inst_skip(sp);
    addr = in32((void*)0xe000ed34);
    testDiag("In MemFault, Addr 0x%x, from 0x%x", addr, sp[6]);

    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)
{
    run_table.hard = &hard_entry;
    run_table.svc = &svc_entry;
    run_table.mem = &mem_entry;

    testInit(6);

    asm("cpsid if");
    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);

    /* disable all access to offlimits[] */
    set_mpu(4, (uint32_t)offlimits, sizeof(offlimits),
            MPU_XN|MPU_NORMAL|MPU_NANA);
    /* disable unpriv access to privonly[] */
    set_mpu(5, (uint32_t)privonly, sizeof(privonly),
            MPU_XN|MPU_NORMAL|MPU_RONA);

    testDiag("Access offlimits with MPU disabled (should not fault)");
    try(offlimits);
    checkfault(EXPECT_NO_FAULT);

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

    asm("cpsie if");

    expect_fault = EXPECT_MEMFAULT;
    testDiag("Access offlimits with MPU enabled (should fault)");
    try(offlimits);
    checkfault(EXPECT_TAKEN_MEMFAULT);

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

    testDiag("Access privonly (should not fault)");
    try(privonly);
    checkfault(EXPECT_NO_FAULT);

    testDiag("drop_priv");
    expect_fault = EXPECT_MEMFAULT;
    drop_priv();
    testDiag("Access privonly as user (should fault)");
    try(privonly);
    checkfault(EXPECT_TAKEN_MEMFAULT);

    testDiag("Back in Main, regaining privilege");
    asm("svc 1");
    expect_fault = EXPECT_NO_FAULT;

    testDiag("Access privonly as priv (should not fault)");
    try(privonly);
    checkfault(EXPECT_NO_FAULT);


    asm("cpsid i");


    testDiag("Check escalation of MemManage to HardFault");
    expect_fault = EXPECT_HARDFAULT;
    try(offlimits);
    checkfault(EXPECT_TAKEN_HARDFAULT);

#if 0
    /* don't try to test unrecoverable errors */
    testDiag("Enable MPU w/ HFNMIENA");
    enable_mpu(1,1,1);
    testDiag("fault to hardfault (will escalate to unrecoverable)");
    expect_fault = EXPECT_HARDFAULT;
    try(offlimits);
    testDiag("Shouldn't be here!");
#endif

    testDiag("Done.");
}