Momentum-Apps/video_game_module_tool/flasher/target.c

#include "target.h"

#include <furi.h>

#include "swd.h"

/* Cortex-M registers (taken from Blackmagic) */
#define CORTEXM_PPB_BASE 0xe0000000U

#define CORTEXM_SCS_BASE (CORTEXM_PPB_BASE + 0xe000U)

#define CORTEXM_CPUID (CORTEXM_SCS_BASE + 0xd00U)
#define CORTEXM_AIRCR (CORTEXM_SCS_BASE + 0xd0cU)
#define CORTEXM_CFSR  (CORTEXM_SCS_BASE + 0xd28U)
#define CORTEXM_HFSR  (CORTEXM_SCS_BASE + 0xd2cU)
#define CORTEXM_DFSR  (CORTEXM_SCS_BASE + 0xd30U)
#define CORTEXM_CPACR (CORTEXM_SCS_BASE + 0xd88U)
#define CORTEXM_DHCSR (CORTEXM_SCS_BASE + 0xdf0U)
#define CORTEXM_DCRSR (CORTEXM_SCS_BASE + 0xdf4U)
#define CORTEXM_DCRDR (CORTEXM_SCS_BASE + 0xdf8U)
#define CORTEXM_DEMCR (CORTEXM_SCS_BASE + 0xdfcU)

/* Debug Halting Control and Status Register (DHCSR) */
/* This key must be written to bits 31:16 for write to take effect */
#define CORTEXM_DHCSR_DBGKEY      0xa05f0000U
/* Bits 31:26 - Reserved */
#define CORTEXM_DHCSR_S_RESET_ST  (1U << 25U)
#define CORTEXM_DHCSR_S_RETIRE_ST (1U << 24U)
/* Bits 23:20 - Reserved */
#define CORTEXM_DHCSR_S_LOCKUP    (1U << 19U)
#define CORTEXM_DHCSR_S_SLEEP     (1U << 18U)
#define CORTEXM_DHCSR_S_HALT      (1U << 17U)
#define CORTEXM_DHCSR_S_REGRDY    (1U << 16U)
/* Bits 15:6 - Reserved */
#define CORTEXM_DHCSR_C_SNAPSTALL (1U << 5U) /* v7m only */
/* Bit 4 - Reserved */
#define CORTEXM_DHCSR_C_MASKINTS  (1U << 3U)
#define CORTEXM_DHCSR_C_STEP      (1U << 2U)
#define CORTEXM_DHCSR_C_HALT      (1U << 1U)
#define CORTEXM_DHCSR_C_DEBUGEN   (1U << 0U)

/* Debug Exception and Monitor Control Register (DEMCR) */
/* Bits 31:25 - Reserved */
#define CORTEXM_DEMCR_TRCENA       (1U << 24U)
/* Bits 23:20 - Reserved */
#define CORTEXM_DEMCR_MON_REQ      (1U << 19U) /* v7m only */
#define CORTEXM_DEMCR_MON_STEP     (1U << 18U) /* v7m only */
#define CORTEXM_DEMCR_VC_MON_PEND  (1U << 17U) /* v7m only */
#define CORTEXM_DEMCR_VC_MON_EN    (1U << 16U) /* v7m only */
/* Bits 15:11 - Reserved */
#define CORTEXM_DEMCR_VC_HARDERR   (1U << 10U)
#define CORTEXM_DEMCR_VC_INTERR    (1U << 9U) /* v7m only */
#define CORTEXM_DEMCR_VC_BUSERR    (1U << 8U) /* v7m only */
#define CORTEXM_DEMCR_VC_STATERR   (1U << 7U) /* v7m only */
#define CORTEXM_DEMCR_VC_CHKERR    (1U << 6U) /* v7m only */
#define CORTEXM_DEMCR_VC_NOCPERR   (1U << 5U) /* v7m only */
#define CORTEXM_DEMCR_VC_MMERR     (1U << 4U) /* v7m only */
/* Bits 3:1 - Reserved */
#define CORTEXM_DEMCR_VC_CORERESET (1U << 0U)

#define CORTEXM_DHCSR_DEBUG_HALT (CORTEXM_DHCSR_C_DEBUGEN | CORTEXM_DHCSR_C_HALT)

#define TAG "VgmTarget"

static uint32_t prev_address;

static bool target_memory_access_setup(uint32_t address) {
    bool success = false;
    do {
        // If the address was previously set up, do not waste time on it
        if(address != prev_address) {
            // Word access, no auto increment
            if(!swd_ap_write(
                   SWD_AP_REG_RW_CSW,
                   SWD_AP_REG_CSW_HPROT_DATA | SWD_AP_REG_CSW_HPROT_PRIVILIGED |
                       SWD_AP_REG_CSW_HNONSEC | SWD_AP_REG_CSW_SIZE_WORD))
                break;
            if(!swd_ap_write(SWD_AP_REG_RW_TAR, address)) break;
            prev_address = address;
        }
        success = true;
    } while(false);

    return success;
}

static bool target_dbg_power_up(void) {
    if(!swd_dp_write(SWD_DP_REG_RW_CTRL_STAT, 0)) return false;

    uint32_t status;

    do {
        if(!swd_dp_read(SWD_DP_REG_RW_CTRL_STAT, &status)) return false;
    } while(status & (SWD_DP_REG_CTRL_STAT_CDBGPWRUPACK | SWD_DP_REG_CTRL_STAT_CSYSPWRUPACK));

    if(!swd_dp_write(
           SWD_DP_REG_RW_CTRL_STAT,
           (SWD_DP_REG_CTRL_STAT_CDBGPWRUPREQ | SWD_DP_REG_CTRL_STAT_CSYSPWRUPREQ)))
        return false;

    do {
        furi_delay_us(10000);
        if(!swd_dp_read(SWD_DP_REG_RW_CTRL_STAT, &status)) return false;
    } while((status & (SWD_DP_REG_CTRL_STAT_CDBGPWRUPACK | SWD_DP_REG_CTRL_STAT_CSYSPWRUPACK)) !=
            (SWD_DP_REG_CTRL_STAT_CDBGPWRUPACK | SWD_DP_REG_CTRL_STAT_CSYSPWRUPACK));

    return true;
}

static bool target_halt(void) {
    bool success = false;

    do {
        if(!target_write_memory_32(CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_DEBUG_HALT))
            break;

        bool target_halted = false;
        for(bool target_reset = false; !target_halted;) {
            uint32_t dhcsr;
            if(!target_read_memory_32(CORTEXM_DHCSR, &dhcsr)) break;
            if((dhcsr & CORTEXM_DHCSR_S_RESET_ST) && !target_reset) {
                target_reset = true;
                continue;
            }
            if((dhcsr & CORTEXM_DHCSR_DEBUG_HALT) == CORTEXM_DHCSR_DEBUG_HALT) {
                target_halted = true;
            }
        }

        if(!target_halted) break;

        if(!target_write_memory_32(
               CORTEXM_DEMCR,
               CORTEXM_DEMCR_TRCENA | CORTEXM_DEMCR_VC_HARDERR | CORTEXM_DEMCR_VC_CORERESET))
            break;

        bool target_local_reset = false;
        for(; !target_local_reset;) {
            uint32_t dhcsr;
            if(!target_read_memory_32(CORTEXM_DHCSR, &dhcsr)) break;
            if((dhcsr & CORTEXM_DHCSR_S_RESET_ST) == 0) {
                target_local_reset = true;
            }
        }

        if(!target_local_reset) break;

        success = true;
    } while(false);

    return success;
}

bool target_attach(uint32_t id) {
    bool success = false;

    do {
        // Reset previous memory address
        prev_address = UINT32_MAX;

        swd_select_target(id);

        uint32_t dpidr;
        if(!swd_dp_read(SWD_DP_REG_RO_DPIDR, &dpidr)) {
            FURI_LOG_E(TAG, "Failed to read DPIDR");
            break;
        }

        if(dpidr == 0) {
            FURI_LOG_E(TAG, "Zero DPIDR value");
            break;
        }

        if(!target_dbg_power_up()) {
            FURI_LOG_E(TAG, "Failed to enable debug power");
            break;
        }

        if(!target_halt()) {
            FURI_LOG_E(TAG, "Failed to halt target");
            break;
        }

        success = true;
    } while(false);

    return success;
}

bool target_detach(void) {
    bool success = false;

    do {
        if(!target_write_memory_32(CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_DEBUG_HALT))
            break;
        if(!target_write_memory_32(CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_C_DEBUGEN))
            break;
        if(!target_write_memory_32(CORTEXM_DHCSR, CORTEXM_DHCSR_DBGKEY)) break;
        success = true;
    } while(false);

    return success;
}

bool target_read_memory_32(uint32_t address, uint32_t* data) {
    furi_assert((address & 3U) == 0);

    bool success = false;

    do {
        if(!target_memory_access_setup(address)) break;
        if(!swd_ap_read(SWD_AP_REG_RW_DRW, data)) break;
        success = true;
    } while(false);

    return success;
}

bool target_write_memory_32(uint32_t address, uint32_t data) {
    furi_assert((address & 3U) == 0);

    bool success = false;

    do {
        if(!target_memory_access_setup(address)) break;
        if(!swd_ap_write(SWD_AP_REG_RW_DRW, data)) break;
        success = true;
    } while(false);

    return success;
}