cortex-a8 abort handling

#include <stdint.h>
#include <stddef.h>

// which processor state (and associated stack) is used for exception handling.
// this should be supervisor (0b10011) or system (0b11111) and must match
// whatever the assembly wrappers use (see further down).
#define MODE_HND 0b10011

// state saved on handler stack by assembly code (see below)
struct ExcState {
    // saved r0-r12 shared between modes.
    uint32_t r[13];

    // saved link register of handler mode.
    uint32_t lr;

    // saved state of interrupted processor mode.
    uint32_t pc;
    uint32_t psr;
};

static void dump_state( ExcState *state )
{
    unsigned mode = ( state->psr & 0b11111 );
    printf( "psr: 0x%08x\n", state->psr );
    printf( "pc:  0x%08x\n", state->pc );
    if( mode == MODE_HND || ( MODE_HND == 0b11111 && mode == 0b10000 ) ) {
        // the interrupted mode uses same SP and LR as handler mode
        printf( "lr:  0x%08x\n", state->lr );
        printf( "sp:  0x%08x\n", (uint32_t)( state + 1 ) );
    } else {
        // FIXME obtain banked SP and LR for other modes
    }
    for( int i = 0; i <= 9; ++i )
        printf( "r%u:  0x%08x\n", i, state->r[i] );
    for( int i = 10; i <= 12; ++i )
        printf( "r%u: 0x%08x\n", i, state->r[i] );
}

static void abort_common( ExcState *state, char const *msg )
{
    if( msg )
        printf( "\e[1;31m### %s\e[m\n", msg );
    if( state )
        dump_state( state );

    // XXX if you want to trigger a system reset, this is the place for it.
    // otherwise just hang in infinite loop:
    for(;;)
        asm( "wfe" );
}


//-------------- Debug monitor events ----------------------------------------//

static void debug_watchpoint( ExcState *state )
{
    abort_common( state, "unexpected watchpoint" );
}

static void debug_breakpoint( ExcState *state )
{
    abort_common( state, "unexpected breakpoint" );
}


//-------------- Asynchronous aborts -----------------------------------------//
//
// In these cases the CPU gives you no info whatsoever about the access that
// caused the abort, and it may be taken long after the instruction that caused
// it.  In general, the instruction at state->pc is not related to the error.
//
// Fortunately on the Cortex-A8 most aborts are synchronous.  (In contrast, on
// the Cortex-A15 nearly all aborts are asynchronous.)

static void async_bus_error()
{
    abort_common( NULL, "async bus error" );
}

static void async_parity_error()
{
    abort_common( NULL, "async parity error" );
}


//-------------- Synchronous aborts ------------------------------------------//
//
static void sync_abort_common( ExcState *state, uint32_t fsr, uint32_t addr )
{
    // create order in the chaos of fault types
    static uint8_t const typemap[2][16] = {
           0, 0x01,    0, 0x21,    0, 0x20, 0x41, 0x40,
        0x71, 0x22,    0, 0x42, 0x11, 0x23, 0x31, 0x43,
        0x02,    0,    0,    0, 0x03,    0,    0,    0,
           0, 0x70, 0x04,    0, 0x10,    0, 0x30,    0,
    };
    int type = typemap[ fsr >> 10 & 1 ][ fsr & 0xf ];

    if( ( type & 0x1f ) == 0x11 )  // bus error
        type += fsr >> 12 & 1;  // bus error subtype

    int phase = type >> 4;
    int subtype = type & 0xf;
    int access = ( fsr >> 11 & 1 ) | ( fsr >> 13 & 1 ) << 1;

    char const *fault_name = NULL;

    if( phase == 0 ) {
        // subtype:
        static char const *const names[] = {
            "unknown abort",
            "alignment fault",
            "TLB conflict",     // since v7L/v7VE
            "lockdown abort",   // implementation-defined
            "coprocessor abort",    // implementation-defined
        };
        fault_name = names[ subtype ];

    } else if( phase & 1 ) {
        // phase:
        //  1 section table walk
        //  3 page table walk
        //  7 access
        //
        // subtype:
        static char const *const names[] = {
            "cache parity/ECC error",
            "bus error (DECERR)",
            "bus error (SLVERR)",
        };
        fault_name = names[ subtype ];

    } else {
        // phase:
        //  2 section translation
        //  4 page translation
        //
        // subtype:
        static char const *const names[] = {
            "translation fault",
            "access flag fault",
            "domain fault",
            "permission fault",
        };
        fault_name = names[ subtype ];
    }

    static char const *const phase_names[8] = {
        NULL,
        "section table walk",
        "section translation",
        "page table walk",
        "page translation",
        NULL,
        NULL,
        NULL,
    };

    static char const *const access_names[] = {
        "data read",
        "data write",
        "instruction fetch",
        "i-side maintenance",
    };

    printf( "\e[1;31m### %s on ", fault_name );
    if( phase_names[ phase ] )
        printf( "%s for ", phase_names[ phase ] );
    printf( "%s at 0x%08x\e[m\n", access_names[ access ], addr );

    if( type == 0 )
        printf( "fsr: 0x%08x\n", fsr );
    abort_common( state, NULL );
}


//-------------- Exception handlers ------------------------------------------//
//
// These can't be called directly from the vector table but require small
// assembly wrappers:  (Note: MODE_HND is defined at the top of this file)
//
// undef_exception:
//  srsfd   sp!, MODE_HND   // push { lr, spsr } to handler stack
//  cpsid   ia, MODE_HND    // change to handler mode, mask async aborts
//  push    { r0-r12, lr }
//  mov r0, sp
//  blx undef_handler
//  pop { r0-r12, lr }
//  rfefd   sp!     // pop { pc, cpsr }
//
// iabort_exception:
//  sub lr, 4       // fix return address
//  srsfd   sp!, MODE_HND   // push { lr, spsr } to handler stack
//  cpsid   ia, MODE_HND    // change to handler mode, mask async aborts
//  push    { r0-r12, lr }
//  mov r0, sp
//  blx iabort_handler
//  pop { r0-r12, lr }
//  rfefd   sp!     // pop { pc, cpsr }
//
// dabort_exception:
//  sub lr, 8       // fix return address
//  srsfd   sp!, MODE_HND   // push { lr, spsr } to handler stack
//  cpsid   ia, MODE_HND    // change to handler mode, mask async aborts
//  push    { r0-r12, lr }
//  mov r0, sp
//  blx dabort_handler
//  pop { r0-r12, lr }
//  rfefd   sp!     // pop { pc, cpsr }

extern "C"
void undef_handler( ExcState *state )
{
    if( state->psr & (1 << 5) ) {
        // thumb (or thumbEE if psr bit 24 also set)
        state->pc -= 2;

    } else if( state->psr & (1 << 24) ) {
        // Can only happen on attempted exception return to jazelle
        // state when not implemented.
        //
        // state->pc is undefined.

    } else {
        // legacy ARM mode
        state->pc -= 4;
    }

    // Returning with the pc as corrected above will retry the instruction,
    // which could be ok if for example you'd implement task switching with
    // lazy fpu/neon register save/restore.
    //
    // If you emulate an instruction in thumb mode, don't forget to check
    // the IT state whether it passes its condition and advance IT state.

    abort_common( state, "undefined instruction or invalid cpu state" );
}

// Abort resulting from instruction fetch.
extern "C"
void iabort_handler( ExcState *state )
{
    uint32_t fsr;
    asm( "mrc  p15, 0, %0, c5, c0, 1" : "=r"(fsr) );  // read IFSR
    fsr |= 0x2000;

    switch( fsr & 0x040f ) {
    case 0x0002:
        return debug_breakpoint( state );
    }

    uint32_t addr;
    asm( "mrc  p15, 0, %0, c6, c0, 2" : "=r"(addr) );  // read IFAR

    return sync_abort_common( state, fsr, addr );
}

// Abort resulting from instruction execution.
extern "C"
void dabort_handler( ExcState *state )
{
    uint32_t fsr;
    asm( "mrc  p15, 0, %0, c5, c0, 0" : "=r"(fsr) );  // read DFSR

    switch( fsr & 0x040f ) {
    case 0x0406:
        return async_bus_error();
    case 0x0408:
        return async_parity_error();
    case 0x0002:
        return debug_watchpoint( state );
    case 0x0004:
        // Instruction-side maintenance fault.
        asm( "mrc  p15, 0, %0, c5, c0, 1" : "=r"(fsr) );  // read IFSR
        fsr |= 0x2800;
        break;
    }

    uint32_t addr;
    asm( "mrc  p15, 0, %0, c6, c0, 0" : "=r"(addr) );  // read DFAR

    return sync_abort_common( state, fsr, addr );
}