xtensa.c

Go to the documentation of this file.

// SPDX-License-Identifier: GPL-2.0-or-later
 
/***************************************************************************
 *   Generic Xtensa target API for OpenOCD                                 *
 *   Copyright (C) 2020-2022 Cadence Design Systems, Inc.                  *
 *   Copyright (C) 2016-2019 Espressif Systems Ltd.                        *
 *   Derived from esp108.c                                                 *
 *   Author: Angus Gratton gus@projectgus.com                              *
 ***************************************************************************/
 
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
 
#include <stdlib.h>
#include <helper/time_support.h>
#include <helper/align.h>
#include <target/register.h>
#include <target/algorithm.h>
 
#include "xtensa_chip.h"
#include "xtensa.h"
 
/* Swap 4-bit Xtensa opcodes and fields */
#define XT_NIBSWAP8(V)                                  \
    ((((V) & 0x0F) << 4)                                \
        | (((V) & 0xF0) >> 4))
 
#define XT_NIBSWAP16(V)                                 \
    ((((V) & 0x000F) << 12)                             \
        | (((V) & 0x00F0) << 4)                         \
        | (((V) & 0x0F00) >> 4)                         \
        | (((V) & 0xF000) >> 12))
 
#define XT_NIBSWAP24(V)                                 \
    ((((V) & 0x00000F) << 20)                           \
        | (((V) & 0x0000F0) << 12)                      \
        | (((V) & 0x000F00) << 4)                       \
        | (((V) & 0x00F000) >> 4)                       \
        | (((V) & 0x0F0000) >> 12)                      \
        | (((V) & 0xF00000) >> 20))
 
/* _XT_INS_FORMAT_*()
 * Instruction formatting converted from little-endian inputs
 * and shifted to the MSB-side of DIR for BE systems.
 */
#define _XT_INS_FORMAT_RSR(X, OPCODE, SR, T)            \
    (XT_ISBE(X) ? (XT_NIBSWAP24(OPCODE)                 \
            | (((T) & 0x0F) << 16)                      \
            | (((SR) & 0xFF) << 8)) << 8                \
        : (OPCODE)                                      \
        | (((SR) & 0xFF) << 8)                          \
        | (((T) & 0x0F) << 4))
 
#define _XT_INS_FORMAT_RRR(X, OPCODE, ST, R)            \
    (XT_ISBE(X) ? (XT_NIBSWAP24(OPCODE)                 \
            | ((XT_NIBSWAP8((ST) & 0xFF)) << 12)        \
            | (((R) & 0x0F) << 8)) << 8                 \
        : (OPCODE)                                      \
        | (((ST) & 0xFF) << 4)                          \
        | (((R) & 0x0F) << 12))
 
#define _XT_INS_FORMAT_RRRN(X, OPCODE, S, T, IMM4)      \
    (XT_ISBE(X) ? (XT_NIBSWAP16(OPCODE)                 \
            | (((T) & 0x0F) << 8)                       \
            | (((S) & 0x0F) << 4)                       \
            | ((IMM4) & 0x0F)) << 16                    \
        : (OPCODE)                                      \
        | (((T) & 0x0F) << 4)                           \
        | (((S) & 0x0F) << 8)                           \
        | (((IMM4) & 0x0F) << 12))
 
#define _XT_INS_FORMAT_RRI8(X, OPCODE, R, S, T, IMM8)   \
    (XT_ISBE(X) ? (XT_NIBSWAP24(OPCODE)                 \
            | (((T) & 0x0F) << 16)                      \
            | (((S) & 0x0F) << 12)                      \
            | (((R) & 0x0F) << 8)                       \
            | ((IMM8) & 0xFF)) << 8                     \
        : (OPCODE)                                      \
        | (((IMM8) & 0xFF) << 16)                       \
        | (((R) & 0x0F) << 12)                          \
        | (((S) & 0x0F) << 8)                           \
        | (((T) & 0x0F) << 4))
 
#define _XT_INS_FORMAT_RRI4(X, OPCODE, IMM4, R, S, T)   \
    (XT_ISBE(X) ? (XT_NIBSWAP24(OPCODE)                 \
            | (((T) & 0x0F) << 16)                      \
            | (((S) & 0x0F) << 12)                      \
            | (((R) & 0x0F) << 8)) << 8                 \
        | ((IMM4) & 0x0F)                               \
        : (OPCODE)                                      \
        | (((IMM4) & 0x0F) << 20)                       \
        | (((R) & 0x0F) << 12)                          \
        | (((S) & 0x0F) << 8)                           \
        | (((T) & 0x0F) << 4))
 
/* Xtensa processor instruction opcodes
*/
/* "Return From Debug Operation" to Normal */
#define XT_INS_RFDO(X) (XT_ISBE(X) ? 0x000e1f << 8 : 0xf1e000)
/* "Return From Debug and Dispatch" - allow sw debugging stuff to take over */
#define XT_INS_RFDD(X) (XT_ISBE(X) ? 0x010e1f << 8 : 0xf1e010)
 
/* Load to DDR register, increase addr register */
#define XT_INS_LDDR32P(X, S) (XT_ISBE(X) ? (0x0E0700 | ((S) << 12)) << 8 : (0x0070E0 | ((S) << 8)))
/* Store from DDR register, increase addr register */
#define XT_INS_SDDR32P(X, S) (XT_ISBE(X) ? (0x0F0700 | ((S) << 12)) << 8 : (0x0070F0 | ((S) << 8)))
 
/* Load 32-bit Indirect from A(S)+4*IMM8 to A(T) */
#define XT_INS_L32I(X, S, T, IMM8)  _XT_INS_FORMAT_RRI8(X, 0x002002, 0, S, T, IMM8)
/* Load 16-bit Unsigned from A(S)+2*IMM8 to A(T) */
#define XT_INS_L16UI(X, S, T, IMM8) _XT_INS_FORMAT_RRI8(X, 0x001002, 0, S, T, IMM8)
/* Load 8-bit Unsigned from A(S)+IMM8 to A(T) */
#define XT_INS_L8UI(X, S, T, IMM8)  _XT_INS_FORMAT_RRI8(X, 0x000002, 0, S, T, IMM8)
 
/* Store 32-bit Indirect to A(S)+4*IMM8 from A(T) */
#define XT_INS_S32I(X, S, T, IMM8) _XT_INS_FORMAT_RRI8(X, 0x006002, 0, S, T, IMM8)
/* Store 16-bit to A(S)+2*IMM8 from A(T) */
#define XT_INS_S16I(X, S, T, IMM8) _XT_INS_FORMAT_RRI8(X, 0x005002, 0, S, T, IMM8)
/* Store 8-bit to A(S)+IMM8 from A(T) */
#define XT_INS_S8I(X, S, T, IMM8)  _XT_INS_FORMAT_RRI8(X, 0x004002, 0, S, T, IMM8)
 
/* Cache Instructions */
#define XT_INS_IHI(X, S, IMM8) _XT_INS_FORMAT_RRI8(X, 0x0070E2, 0, S, 0, IMM8)
#define XT_INS_DHWBI(X, S, IMM8) _XT_INS_FORMAT_RRI8(X, 0x007052, 0, S, 0, IMM8)
#define XT_INS_DHWB(X, S, IMM8) _XT_INS_FORMAT_RRI8(X, 0x007042, 0, S, 0, IMM8)
#define XT_INS_ISYNC(X) (XT_ISBE(X) ? 0x000200 << 8 : 0x002000)
 
/* Control Instructions */
#define XT_INS_JX(X, S) (XT_ISBE(X) ? (0x050000 | ((S) << 12)) : (0x0000a0 | ((S) << 8)))
#define XT_INS_CALL0(X, IMM18) (XT_ISBE(X) ? (0x500000 | ((IMM18) & 0x3ffff)) : (0x000005 | (((IMM18) & 0x3ffff) << 6)))
 
/* Read Special Register */
#define XT_INS_RSR(X, SR, T) _XT_INS_FORMAT_RSR(X, 0x030000, SR, T)
/* Write Special Register */
#define XT_INS_WSR(X, SR, T) _XT_INS_FORMAT_RSR(X, 0x130000, SR, T)
/* Swap Special Register */
#define XT_INS_XSR(X, SR, T) _XT_INS_FORMAT_RSR(X, 0x610000, SR, T)
 
/* Rotate Window by (-8..7) */
#define XT_INS_ROTW(X, N) (XT_ISBE(X) ? ((0x000804) | (((N) & 15) << 16)) << 8 : ((0x408000) | (((N) & 15) << 4)))
 
/* Read User Register */
#define XT_INS_RUR(X, UR, T) _XT_INS_FORMAT_RRR(X, 0xE30000, UR, T)
/* Write User Register */
#define XT_INS_WUR(X, UR, T) _XT_INS_FORMAT_RSR(X, 0xF30000, UR, T)
 
/* Read Floating-Point Register */
#define XT_INS_RFR(X, FR, T) _XT_INS_FORMAT_RRR(X, 0xFA0000, ((FR << 4) | 0x4), T)
/* Write Floating-Point Register */
#define XT_INS_WFR(X, FR, T) _XT_INS_FORMAT_RRR(X, 0xFA0000, ((T << 4) | 0x5), FR)
 
#define XT_INS_L32E(X, R, S, T) _XT_INS_FORMAT_RRI4(X, 0x090000, 0, R, S, T)
#define XT_INS_S32E(X, R, S, T) _XT_INS_FORMAT_RRI4(X, 0x490000, 0, R, S, T)
#define XT_INS_L32E_S32E_MASK(X)   (XT_ISBE(X) ? 0xF000FF << 8 : 0xFF000F)
 
#define XT_INS_RFWO(X) (XT_ISBE(X) ? 0x004300 << 8 : 0x003400)
#define XT_INS_RFWU(X) (XT_ISBE(X) ? 0x005300 << 8 : 0x003500)
#define XT_INS_RFWO_RFWU_MASK(X)   (XT_ISBE(X) ? 0xFFFFFF << 8 : 0xFFFFFF)
 
/* Read Protection TLB Entry Info */
#define XT_INS_PPTLB(X, S, T) _XT_INS_FORMAT_RRR(X, 0x500000, ((S) << 4) | (T), 0xD)
 
#define XT_TLB1_ACC_SHIFT       8
#define XT_TLB1_ACC_MSK         0xF
 
#define XT_WATCHPOINTS_NUM_MAX  2
 
/* Special register number macro for DDR, PS, WB, A3, A4 registers.
 * These get used a lot so making a shortcut is useful.
 */
#define XT_SR_DDR         (xtensa_regs[XT_REG_IDX_DDR].reg_num)
#define XT_SR_PS          (xtensa_regs[XT_REG_IDX_PS].reg_num)
#define XT_SR_WB          (xtensa_regs[XT_REG_IDX_WINDOWBASE].reg_num)
#define XT_REG_A0         (xtensa_regs[XT_REG_IDX_AR0].reg_num)
#define XT_REG_A3         (xtensa_regs[XT_REG_IDX_AR3].reg_num)
#define XT_REG_A4         (xtensa_regs[XT_REG_IDX_AR4].reg_num)
 
#define XT_PS_REG_NUM               (0xe6U)
#define XT_EPS_REG_NUM_BASE         (0xc0U) /* (EPS2 - 2), for adding DBGLEVEL */
#define XT_EPC_REG_NUM_BASE         (0xb0U) /* (EPC1 - 1), for adding DBGLEVEL */
#define XT_PC_REG_NUM_VIRTUAL       (0xffU) /* Marker for computing PC (EPC[DBGLEVEL) */
#define XT_PC_DBREG_NUM_BASE        (0x20U) /* External (i.e., GDB) access */
#define XT_NX_IBREAKC_BASE          (0xc0U) /* (IBREAKC0..IBREAKC1) for NX */
 
#define XT_SW_BREAKPOINTS_MAX_NUM       32
#define XT_HW_IBREAK_MAX_NUM            2
#define XT_HW_DBREAK_MAX_NUM            2
 
struct xtensa_reg_desc xtensa_regs[XT_NUM_REGS] = {
    XT_MK_REG_DESC("pc", XT_PC_REG_NUM_VIRTUAL, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("ar0", 0x00, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar1", 0x01, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar2", 0x02, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar3", 0x03, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar4", 0x04, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar5", 0x05, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar6", 0x06, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar7", 0x07, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar8", 0x08, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar9", 0x09, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar10", 0x0A, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar11", 0x0B, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar12", 0x0C, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar13", 0x0D, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar14", 0x0E, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar15", 0x0F, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar16", 0x10, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar17", 0x11, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar18", 0x12, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar19", 0x13, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar20", 0x14, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar21", 0x15, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar22", 0x16, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar23", 0x17, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar24", 0x18, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar25", 0x19, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar26", 0x1A, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar27", 0x1B, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar28", 0x1C, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar29", 0x1D, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar30", 0x1E, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar31", 0x1F, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar32", 0x20, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar33", 0x21, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar34", 0x22, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar35", 0x23, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar36", 0x24, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar37", 0x25, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar38", 0x26, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar39", 0x27, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar40", 0x28, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar41", 0x29, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar42", 0x2A, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar43", 0x2B, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar44", 0x2C, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar45", 0x2D, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar46", 0x2E, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar47", 0x2F, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar48", 0x30, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar49", 0x31, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar50", 0x32, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar51", 0x33, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar52", 0x34, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar53", 0x35, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar54", 0x36, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar55", 0x37, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar56", 0x38, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar57", 0x39, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar58", 0x3A, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar59", 0x3B, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar60", 0x3C, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar61", 0x3D, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar62", 0x3E, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("ar63", 0x3F, XT_REG_GENERAL, 0),
    XT_MK_REG_DESC("windowbase", 0x48, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("windowstart", 0x49, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("ps", XT_PS_REG_NUM, XT_REG_SPECIAL, 0), /* PS (not mapped through EPS[]) */
    XT_MK_REG_DESC("ibreakenable", 0x60, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("ddr", 0x68, XT_REG_DEBUG, XT_REGF_NOREAD),
    XT_MK_REG_DESC("ibreaka0", 0x80, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("ibreaka1", 0x81, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("dbreaka0", 0x90, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("dbreaka1", 0x91, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("dbreakc0", 0xA0, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("dbreakc1", 0xA1, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("cpenable", 0xE0, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("exccause", 0xE8, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("debugcause", 0xE9, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("icount", 0xEC, XT_REG_SPECIAL, 0),
    XT_MK_REG_DESC("icountlevel", 0xED, XT_REG_SPECIAL, 0),
 
    /* WARNING: For these registers, regnum points to the
     * index of the corresponding ARx registers, NOT to
     * the processor register number! */
    XT_MK_REG_DESC("a0", XT_REG_IDX_AR0, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a1", XT_REG_IDX_AR1, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a2", XT_REG_IDX_AR2, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a3", XT_REG_IDX_AR3, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a4", XT_REG_IDX_AR4, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a5", XT_REG_IDX_AR5, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a6", XT_REG_IDX_AR6, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a7", XT_REG_IDX_AR7, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a8", XT_REG_IDX_AR8, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a9", XT_REG_IDX_AR9, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a10", XT_REG_IDX_AR10, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a11", XT_REG_IDX_AR11, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a12", XT_REG_IDX_AR12, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a13", XT_REG_IDX_AR13, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a14", XT_REG_IDX_AR14, XT_REG_RELGEN, 0),
    XT_MK_REG_DESC("a15", XT_REG_IDX_AR15, XT_REG_RELGEN, 0),
};
 
enum xtensa_mem_region_type {
    XTENSA_MEM_REG_IROM = 0x0,
    XTENSA_MEM_REG_IRAM,
    XTENSA_MEM_REG_DROM,
    XTENSA_MEM_REG_DRAM,
    XTENSA_MEM_REG_SRAM,
    XTENSA_MEM_REG_SROM,
    XTENSA_MEM_REGS_NUM
};
 
enum xtensa_mpu_access_type {
    XTENSA_ACC_00X_000 = 0x2,
    XTENSA_ACC_000_00X,
    XTENSA_ACC_R00_000,
    XTENSA_ACC_R0X_000,
    XTENSA_ACC_RW0_000,
    XTENSA_ACC_RWX_000,
    XTENSA_ACC_0W0_0W0,
    XTENSA_ACC_RW0_RWX,
    XTENSA_ACC_RW0_R00,
    XTENSA_ACC_RWX_R0X,
    XTENSA_ACC_R00_R00,
    XTENSA_ACC_R0X_R0X,
    XTENSA_ACC_RW0_RW0,
    XTENSA_ACC_RWX_RWX
};
 
/* Register definition as union for list allocation */
union xtensa_reg_val_u {
    xtensa_reg_val_t val;
    uint8_t buf[4];
};
 
static const struct xtensa_keyval_info xt_qerr[XT_QERR_NUM] = {
    { .chrval = "E00", .intval = ERROR_FAIL },
    { .chrval = "E01", .intval = ERROR_FAIL },
    { .chrval = "E02", .intval = ERROR_COMMAND_ARGUMENT_INVALID },
    { .chrval = "E03", .intval = ERROR_FAIL },
};
 
/* Set to true for extra debug logging */
static const bool xtensa_extra_debug_log;
 
static inline const struct xtensa_local_mem_config *xtensa_get_mem_config(
    struct xtensa *xtensa,
    enum xtensa_mem_region_type type)
{
    switch (type) {
    case XTENSA_MEM_REG_IROM:
        return &xtensa->core_config->irom;
    case XTENSA_MEM_REG_IRAM:
        return &xtensa->core_config->iram;
    case XTENSA_MEM_REG_DROM:
        return &xtensa->core_config->drom;
    case XTENSA_MEM_REG_DRAM:
        return &xtensa->core_config->dram;
    case XTENSA_MEM_REG_SRAM:
        return &xtensa->core_config->sram;
    case XTENSA_MEM_REG_SROM:
        return &xtensa->core_config->srom;
    default:
        return NULL;
    }
}
 
static inline const struct xtensa_local_mem_region_config *xtensa_memory_region_find(
    const struct xtensa_local_mem_config *mem,
    target_addr_t address)
{
    for (unsigned int i = 0; i < mem->count; i++) {
        const struct xtensa_local_mem_region_config *region = &mem->regions[i];
        if (address >= region->base && address < (region->base + region->size))
            return region;
    }
    return NULL;
}
 
static inline const struct xtensa_local_mem_region_config *xtensa_target_memory_region_find(
    struct xtensa *xtensa,
    target_addr_t address)
{
    const struct xtensa_local_mem_region_config *result;
    const struct xtensa_local_mem_config *mcgf;
    for (unsigned int mtype = 0; mtype < XTENSA_MEM_REGS_NUM; mtype++) {
        mcgf = xtensa_get_mem_config(xtensa, mtype);
        result = xtensa_memory_region_find(mcgf, address);
        if (result)
            return result;
    }
    return NULL;
}
 
static inline bool xtensa_is_cacheable(const struct xtensa_cache_config *cache,
    const struct xtensa_local_mem_config *mem,
    target_addr_t address)
{
    if (!cache->size)
        return false;
    return xtensa_memory_region_find(mem, address);
}
 
static inline bool xtensa_is_icacheable(struct xtensa *xtensa, target_addr_t address)
{
    return xtensa_is_cacheable(&xtensa->core_config->icache, &xtensa->core_config->iram, address) ||
           xtensa_is_cacheable(&xtensa->core_config->icache, &xtensa->core_config->irom, address) ||
           xtensa_is_cacheable(&xtensa->core_config->icache, &xtensa->core_config->sram, address) ||
           xtensa_is_cacheable(&xtensa->core_config->icache, &xtensa->core_config->srom, address);
}
 
static inline bool xtensa_is_dcacheable(struct xtensa *xtensa, target_addr_t address)
{
    return xtensa_is_cacheable(&xtensa->core_config->dcache, &xtensa->core_config->dram, address) ||
           xtensa_is_cacheable(&xtensa->core_config->dcache, &xtensa->core_config->drom, address) ||
           xtensa_is_cacheable(&xtensa->core_config->dcache, &xtensa->core_config->sram, address) ||
           xtensa_is_cacheable(&xtensa->core_config->dcache, &xtensa->core_config->srom, address);
}
 
static int xtensa_core_reg_get(struct reg *reg)
{
    /* We don't need this because we read all registers on halt anyway. */
    struct xtensa *xtensa = (struct xtensa *)reg->arch_info;
    struct target *target = xtensa->target;
 
    if (target->state != TARGET_HALTED)
        return ERROR_TARGET_NOT_HALTED;
    if (!reg->exist) {
        if (strncmp(reg->name, "?0x", 3) == 0) {
            unsigned int regnum = strtoul(reg->name + 1, NULL, 0);
            LOG_WARNING("Read unknown register 0x%04x ignored", regnum);
            return ERROR_OK;
        }
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
    return ERROR_OK;
}
 
static int xtensa_core_reg_set(struct reg *reg, uint8_t *buf)
{
    struct xtensa *xtensa = (struct xtensa *)reg->arch_info;
    struct target *target = xtensa->target;
 
    assert(reg->size <= 64 && "up to 64-bit regs are supported only!");
    if (target->state != TARGET_HALTED)
        return ERROR_TARGET_NOT_HALTED;
 
    if (!reg->exist) {
        if (strncmp(reg->name, "?0x", 3) == 0) {
            unsigned int regnum = strtoul(reg->name + 1, NULL, 0);
            LOG_WARNING("Write unknown register 0x%04x ignored", regnum);
            return ERROR_OK;
        }
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    buf_cpy(buf, reg->value, reg->size);
 
    if (xtensa->core_config->windowed) {
        /* If the user updates a potential scratch register, track for conflicts */
        for (enum xtensa_ar_scratch_set_e s = 0; s < XT_AR_SCRATCH_NUM; s++) {
            if (strcmp(reg->name, xtensa->scratch_ars[s].chrval) == 0) {
                LOG_DEBUG("Scratch reg %s [0x%08" PRIx32 "] set from gdb", reg->name,
                    buf_get_u32(reg->value, 0, 32));
                LOG_DEBUG("scratch_ars mapping: a3/%s, a4/%s",
                    xtensa->scratch_ars[XT_AR_SCRATCH_AR3].chrval,
                    xtensa->scratch_ars[XT_AR_SCRATCH_AR4].chrval);
                xtensa->scratch_ars[s].intval = true;
                break;
            }
        }
    }
    reg->dirty = true;
    reg->valid = true;
 
    return ERROR_OK;
}
 
static const struct reg_arch_type xtensa_reg_type = {
    .get = xtensa_core_reg_get,
    .set = xtensa_core_reg_set,
};
 
/* Convert a register index that's indexed relative to windowbase, to the real address. */
static enum xtensa_reg_id xtensa_windowbase_offset_to_canonical(struct xtensa *xtensa,
    enum xtensa_reg_id reg_idx,
    int windowbase)
{
    unsigned int idx;
    if (reg_idx >= XT_REG_IDX_AR0 && reg_idx <= XT_REG_IDX_ARLAST) {
        idx = reg_idx - XT_REG_IDX_AR0;
    } else if (reg_idx >= XT_REG_IDX_A0 && reg_idx <= XT_REG_IDX_A15) {
        idx = reg_idx - XT_REG_IDX_A0;
    } else {
        LOG_ERROR("Can't convert register %d to non-windowbased register", reg_idx);
        return -1;
    }
    /* Each windowbase value represents 4 registers on LX and 8 on NX */
    int base_inc = (xtensa->core_config->core_type == XT_LX) ? 4 : 8;
    return ((idx + windowbase * base_inc) & (xtensa->core_config->aregs_num - 1)) + XT_REG_IDX_AR0;
}
 
static enum xtensa_reg_id xtensa_canonical_to_windowbase_offset(struct xtensa *xtensa,
    enum xtensa_reg_id reg_idx,
    int windowbase)
{
    return xtensa_windowbase_offset_to_canonical(xtensa, reg_idx, -windowbase);
}
 
static void xtensa_mark_register_dirty(struct xtensa *xtensa, enum xtensa_reg_id reg_idx)
{
    struct reg *reg_list = xtensa->core_cache->reg_list;
    reg_list[reg_idx].dirty = true;
}
 
static void xtensa_queue_exec_ins(struct xtensa *xtensa, uint32_t ins)
{
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DIR0EXEC, ins);
}
 
static void xtensa_queue_exec_ins_wide(struct xtensa *xtensa, uint8_t *ops, uint8_t oplen)
{
    const int max_oplen = 64;   /* 8 DIRx regs: max width 64B */
    if ((oplen > 0) && (oplen <= max_oplen)) {
        uint8_t ops_padded[max_oplen];
        memcpy(ops_padded, ops, oplen);
        memset(ops_padded + oplen, 0, max_oplen - oplen);
        unsigned int oplenw = DIV_ROUND_UP(oplen, sizeof(uint32_t));
        for (int32_t i = oplenw - 1; i > 0; i--)
            xtensa_queue_dbg_reg_write(xtensa,
                XDMREG_DIR0 + i,
                target_buffer_get_u32(xtensa->target, &ops_padded[sizeof(uint32_t) * i]));
        /* Write DIR0EXEC last */
        xtensa_queue_dbg_reg_write(xtensa,
            XDMREG_DIR0EXEC,
            target_buffer_get_u32(xtensa->target, &ops_padded[0]));
    }
}
 
/* NOTE: Assumes A3 has already been saved and marked dirty; A3 will be clobbered */
static inline bool xtensa_region_ar_exec(struct target *target, target_addr_t start, target_addr_t end)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    if (xtensa->core_config->mpu.enabled) {
        /* For cores with the MPU option, issue PPTLB on start and end addresses.
         * Parse access rights field, and confirm both have execute permissions.
         */
        for (int i = 0; i <= 1; i++) {
            uint32_t at, acc;
            uint8_t at_buf[4];
            bool exec_acc;
            target_addr_t addr = i ? end : start;
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, addr);
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
            xtensa_queue_exec_ins(xtensa, XT_INS_PPTLB(xtensa, XT_REG_A3, XT_REG_A3));
            xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
            xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, at_buf);
            int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
            if (res != ERROR_OK)
                LOG_TARGET_ERROR(target, "Error queuing PPTLB: %d", res);
            res = xtensa_core_status_check(target);
            if (res != ERROR_OK)
                LOG_TARGET_ERROR(target, "Error issuing PPTLB: %d", res);
            at = buf_get_u32(at_buf, 0, 32);
            acc = (at >> XT_TLB1_ACC_SHIFT) & XT_TLB1_ACC_MSK;
            exec_acc = ((acc == XTENSA_ACC_00X_000) || (acc == XTENSA_ACC_R0X_000) ||
                        (acc == XTENSA_ACC_RWX_000) || (acc == XTENSA_ACC_RWX_R0X) ||
                        (acc == XTENSA_ACC_R0X_R0X) || (acc == XTENSA_ACC_RWX_RWX));
            LOG_TARGET_DEBUG(target, "PPTLB(" TARGET_ADDR_FMT ") -> 0x%08" PRIx32 " exec_acc %d",
                addr, at, exec_acc);
            if (!exec_acc)
                return false;
        }
    }
    return true;
}
 
static int xtensa_queue_pwr_reg_write(struct xtensa *xtensa, unsigned int reg, uint32_t data)
{
    struct xtensa_debug_module *dm = &xtensa->dbg_mod;
    return dm->pwr_ops->queue_reg_write(dm, reg, data);
}
 
/* NOTE: Assumes A3 has already been saved */
static int xtensa_window_state_save(struct target *target, uint32_t *woe)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int woe_sr = (xtensa->core_config->core_type == XT_LX) ? XT_SR_PS : XT_SR_WB;
    uint32_t woe_dis;
    uint8_t woe_buf[4];
 
    if (xtensa->core_config->windowed) {
        /* Save PS (LX) or WB (NX) and disable window overflow exceptions prior to AR save */
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, woe_sr, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, woe_buf);
        int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        if (res != ERROR_OK) {
            LOG_TARGET_ERROR(target, "Failed to read %s (%d)!",
                (woe_sr == XT_SR_PS) ? "PS" : "WB", res);
            return res;
        }
        xtensa_core_status_check(target);
        *woe = buf_get_u32(woe_buf, 0, 32);
        woe_dis = *woe & ~((woe_sr == XT_SR_PS) ? XT_PS_WOE_MSK : XT_WB_S_MSK);
        LOG_TARGET_DEBUG(target, "Clearing %s (0x%08" PRIx32 " -> 0x%08" PRIx32 ")",
            (woe_sr == XT_SR_PS) ? "PS.WOE" : "WB.S", *woe, woe_dis);
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, woe_dis);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, woe_sr, XT_REG_A3));
    }
    return ERROR_OK;
}
 
/* NOTE: Assumes A3 has already been saved */
static void xtensa_window_state_restore(struct target *target, uint32_t woe)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int woe_sr = (xtensa->core_config->core_type == XT_LX) ? XT_SR_PS : XT_SR_WB;
    if (xtensa->core_config->windowed) {
        /* Restore window overflow exception state */
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, woe);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, woe_sr, XT_REG_A3));
        LOG_TARGET_DEBUG(target, "Restored %s (0x%08" PRIx32 ")",
            (woe_sr == XT_SR_PS) ? "PS.WOE" : "WB", woe);
    }
}
 
static bool xtensa_reg_is_readable(int flags, int cpenable)
{
    if (flags & XT_REGF_NOREAD)
        return false;
    if ((flags & XT_REGF_COPROC0) && (cpenable & BIT(0)) == 0)
        return false;
    return true;
}
 
static bool xtensa_scratch_regs_fixup(struct xtensa *xtensa, struct reg *reg_list, int i, int j, int a_idx, int ar_idx)
{
    int a_name = (a_idx == XT_AR_SCRATCH_A3) ? 3 : 4;
    if (xtensa->scratch_ars[a_idx].intval && !xtensa->scratch_ars[ar_idx].intval) {
        LOG_DEBUG("AR conflict: a%d -> ar%d", a_name, j - XT_REG_IDX_AR0);
        memcpy(reg_list[j].value, reg_list[i].value, sizeof(xtensa_reg_val_t));
    } else {
        LOG_DEBUG("AR conflict: ar%d -> a%d", j - XT_REG_IDX_AR0, a_name);
        memcpy(reg_list[i].value, reg_list[j].value, sizeof(xtensa_reg_val_t));
    }
    return xtensa->scratch_ars[a_idx].intval && xtensa->scratch_ars[ar_idx].intval;
}
 
static int xtensa_write_dirty_registers(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int res;
    xtensa_reg_val_t regval, windowbase = 0;
    bool scratch_reg_dirty = false, delay_cpenable = false;
    struct reg *reg_list = xtensa->core_cache->reg_list;
    unsigned int reg_list_size = xtensa->core_cache->num_regs;
    bool preserve_a3 = false;
    uint8_t a3_buf[4];
    xtensa_reg_val_t a3 = 0, woe;
    unsigned int ms_idx = (xtensa->core_config->core_type == XT_NX) ?
        xtensa->nx_reg_idx[XT_NX_REG_IDX_MS] : reg_list_size;
    xtensa_reg_val_t ms = 0;
    bool restore_ms = false;
 
    LOG_TARGET_DEBUG(target, "start");
 
    /* We need to write the dirty registers in the cache list back to the processor.
     * Start by writing the SFR/user registers. */
    for (unsigned int i = 0; i < reg_list_size; i++) {
        struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
        unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
        if (reg_list[i].dirty) {
            if (rlist[ridx].type == XT_REG_SPECIAL ||
                rlist[ridx].type == XT_REG_USER ||
                rlist[ridx].type == XT_REG_FR) {
                scratch_reg_dirty = true;
                if (i == XT_REG_IDX_CPENABLE) {
                    delay_cpenable = true;
                    continue;
                }
                regval = xtensa_reg_get(target, i);
                LOG_TARGET_DEBUG(target, "Writing back reg %s (%d) val %08" PRIX32,
                    reg_list[i].name,
                    rlist[ridx].reg_num,
                    regval);
                xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, regval);
                xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
                if (reg_list[i].exist) {
                    unsigned int reg_num = rlist[ridx].reg_num;
                    if (rlist[ridx].type == XT_REG_USER) {
                        xtensa_queue_exec_ins(xtensa, XT_INS_WUR(xtensa, reg_num, XT_REG_A3));
                    } else if (rlist[ridx].type == XT_REG_FR) {
                        xtensa_queue_exec_ins(xtensa, XT_INS_WFR(xtensa, reg_num, XT_REG_A3));
                    } else {/*SFR */
                        if (reg_num == XT_PC_REG_NUM_VIRTUAL) {
                            if (xtensa->core_config->core_type == XT_LX) {
                                /* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
                                reg_num = (XT_EPC_REG_NUM_BASE + xtensa->core_config->debug.irq_level);
                                xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, reg_num, XT_REG_A3));
                            } else {
                                /* NX PC set through issuing a jump instruction */
                                xtensa_queue_exec_ins(xtensa, XT_INS_JX(xtensa, XT_REG_A3));
                            }
                        } else if (i == ms_idx) {
                            /* MS must be restored after ARs.  This ensures ARs remain in correct
                             * order even for reversed register groups (overflow/underflow).
                             */
                            ms = regval;
                            restore_ms = true;
                            LOG_TARGET_DEBUG(target, "Delaying MS write: 0x%x", ms);
                        } else {
                            xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, reg_num, XT_REG_A3));
                        }
                    }
                }
                reg_list[i].dirty = false;
            }
        }
    }
    if (scratch_reg_dirty)
        xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
    if (delay_cpenable) {
        regval = xtensa_reg_get(target, XT_REG_IDX_CPENABLE);
        LOG_TARGET_DEBUG(target, "Writing back reg cpenable (224) val %08" PRIX32, regval);
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, regval);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa,
            xtensa_regs[XT_REG_IDX_CPENABLE].reg_num,
            XT_REG_A3));
        reg_list[XT_REG_IDX_CPENABLE].dirty = false;
    }
 
    preserve_a3 = (xtensa->core_config->windowed) || (xtensa->core_config->core_type == XT_NX);
    if (preserve_a3) {
        /* Save (windowed) A3 for scratch use */
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, a3_buf);
        res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        if (res != ERROR_OK)
            return res;
        xtensa_core_status_check(target);
        a3 = buf_get_u32(a3_buf, 0, 32);
    }
 
    if (xtensa->core_config->windowed) {
        res = xtensa_window_state_save(target, &woe);
        if (res != ERROR_OK)
            return res;
        /* Grab the windowbase, we need it. */
        uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
            XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
        windowbase = xtensa_reg_get(target, wb_idx);
        if (xtensa->core_config->core_type == XT_NX)
            windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
 
        /* Check if there are mismatches between the ARx and corresponding Ax registers.
         * When the user sets a register on a windowed config, xt-gdb may set the ARx
         * register directly.  Thus we take ARx as priority over Ax if both are dirty
         * and it's unclear if the user set one over the other explicitly.
         */
        for (unsigned int i = XT_REG_IDX_A0; i <= XT_REG_IDX_A15; i++) {
            unsigned int j = xtensa_windowbase_offset_to_canonical(xtensa, i, windowbase);
            if (reg_list[i].dirty && reg_list[j].dirty) {
                if (memcmp(reg_list[i].value, reg_list[j].value, sizeof(xtensa_reg_val_t)) != 0) {
                    bool show_warning = true;
                    if (i == XT_REG_IDX_A3)
                        show_warning = xtensa_scratch_regs_fixup(xtensa,
                            reg_list, i, j, XT_AR_SCRATCH_A3, XT_AR_SCRATCH_AR3);
                    else if (i == XT_REG_IDX_A4)
                        show_warning = xtensa_scratch_regs_fixup(xtensa,
                            reg_list, i, j, XT_AR_SCRATCH_A4, XT_AR_SCRATCH_AR4);
                    if (show_warning)
                        LOG_WARNING(
                            "Warning: Both A%d [0x%08" PRIx32
                            "] as well as its underlying physical register "
                            "(AR%d) [0x%08" PRIx32 "] are dirty and differ in value",
                            i - XT_REG_IDX_A0,
                            buf_get_u32(reg_list[i].value, 0, 32),
                            j - XT_REG_IDX_AR0,
                            buf_get_u32(reg_list[j].value, 0, 32));
                }
            } else if (reg_list[i].dirty) {
                memcpy(reg_list[j].value, reg_list[i].value, sizeof(xtensa_reg_val_t));
            } else if (reg_list[j].dirty) {
                memcpy(reg_list[i].value, reg_list[j].value, sizeof(xtensa_reg_val_t));
            }
        }
    }
 
    /* Write A0-A16. */
    for (unsigned int i = 0; i < 16; i++) {
        if (reg_list[XT_REG_IDX_A0 + i].dirty) {
            regval = xtensa_reg_get(target, XT_REG_IDX_A0 + i);
            LOG_TARGET_DEBUG(target, "Writing back reg %s value %08" PRIX32 ", num =%i",
                xtensa_regs[XT_REG_IDX_A0 + i].name,
                regval,
                xtensa_regs[XT_REG_IDX_A0 + i].reg_num);
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, regval);
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, i));
            reg_list[XT_REG_IDX_A0 + i].dirty = false;
            if (i == 3) {
                /* Avoid stomping A3 during restore at end of function */
                a3 = regval;
            }
        }
    }
 
    if (xtensa->core_config->windowed) {
        /* Now write AR registers */
        for (unsigned int j = 0; j < XT_REG_IDX_ARLAST; j += 16) {
            /* Write the 16 registers we can see */
            for (unsigned int i = 0; i < 16; i++) {
                if (i + j < xtensa->core_config->aregs_num) {
                    enum xtensa_reg_id realadr =
                        xtensa_windowbase_offset_to_canonical(xtensa, XT_REG_IDX_AR0 + i + j,
                        windowbase);
                    /* Write back any dirty un-windowed registers */
                    if (reg_list[realadr].dirty) {
                        regval = xtensa_reg_get(target, realadr);
                        LOG_TARGET_DEBUG(
                            target,
                            "Writing back reg %s value %08" PRIX32 ", num =%i",
                            xtensa_regs[realadr].name,
                            regval,
                            xtensa_regs[realadr].reg_num);
                        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, regval);
                        xtensa_queue_exec_ins(xtensa,
                            XT_INS_RSR(xtensa, XT_SR_DDR,
                                xtensa_regs[XT_REG_IDX_AR0 + i].reg_num));
                        reg_list[realadr].dirty = false;
                        if ((i + j) == 3)
                            /* Avoid stomping AR during A3 restore at end of function */
                            a3 = regval;
                    }
                }
            }
 
            /* Now rotate the window so we'll see the next 16 registers. The final rotate
             * will wraparound, leaving us in the state we were.
             * Each ROTW rotates 4 registers on LX and 8 on NX */
            int rotw_arg = (xtensa->core_config->core_type == XT_LX) ? 4 : 2;
            xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, rotw_arg));
        }
 
        xtensa_window_state_restore(target, woe);
 
        for (enum xtensa_ar_scratch_set_e s = 0; s < XT_AR_SCRATCH_NUM; s++)
            xtensa->scratch_ars[s].intval = false;
    }
 
    if (restore_ms) {
        uint32_t ms_regno = xtensa->optregs[ms_idx - XT_NUM_REGS].reg_num;
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, ms);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, ms_regno, XT_REG_A3));
        LOG_TARGET_DEBUG(target, "Delayed MS (0x%x) write complete: 0x%x", ms_regno, ms);
    }
 
    if (preserve_a3) {
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, a3);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
    }
 
    res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    xtensa_core_status_check(target);
 
    return res;
}
 
static inline bool xtensa_is_stopped(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    return xtensa->dbg_mod.core_status.dsr & OCDDSR_STOPPED;
}
 
int xtensa_examine(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int cmd = PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) | PWRCTL_COREWAKEUP(xtensa);
 
    LOG_TARGET_DEBUG(target, "");
 
    if (xtensa->core_config->core_type == XT_UNDEF) {
        LOG_ERROR("XTensa core not configured; is xtensa-core-openocd.cfg missing?");
        return ERROR_FAIL;
    }
 
    xtensa_queue_pwr_reg_write(xtensa, XDMREG_PWRCTL, cmd);
    xtensa_queue_pwr_reg_write(xtensa, XDMREG_PWRCTL, cmd | PWRCTL_JTAGDEBUGUSE(xtensa));
    xtensa_dm_queue_enable(&xtensa->dbg_mod);
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK)
        return res;
    if (!xtensa_dm_is_online(&xtensa->dbg_mod)) {
        LOG_ERROR("Unexpected OCD_ID = %08" PRIx32, xtensa->dbg_mod.device_id);
        return ERROR_TARGET_FAILURE;
    }
    LOG_DEBUG("OCD_ID = %08" PRIx32, xtensa->dbg_mod.device_id);
    xtensa_smpbreak_write(xtensa, xtensa->smp_break);
    return ERROR_OK;
}
 
int xtensa_wakeup(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int cmd = PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) | PWRCTL_COREWAKEUP(xtensa);
 
    if (xtensa->reset_asserted)
        cmd |= PWRCTL_CORERESET(xtensa);
    xtensa_queue_pwr_reg_write(xtensa, XDMREG_PWRCTL, cmd);
    /* TODO: can we join this with the write above? */
    xtensa_queue_pwr_reg_write(xtensa, XDMREG_PWRCTL, cmd | PWRCTL_JTAGDEBUGUSE(xtensa));
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    return xtensa_dm_queue_execute(&xtensa->dbg_mod);
}
 
int xtensa_smpbreak_write(struct xtensa *xtensa, uint32_t set)
{
    uint32_t dsr_data = 0x00110000;
    uint32_t clear = (set | OCDDCR_ENABLEOCD) ^
        (OCDDCR_BREAKINEN | OCDDCR_BREAKOUTEN | OCDDCR_RUNSTALLINEN |
        OCDDCR_DEBUGMODEOUTEN | OCDDCR_ENABLEOCD);
 
    LOG_TARGET_DEBUG(xtensa->target, "write smpbreak set=0x%" PRIx32 " clear=0x%" PRIx32, set, clear);
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRSET, set | OCDDCR_ENABLEOCD);
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRCLR, clear);
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DSR, dsr_data);
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    return xtensa_dm_queue_execute(&xtensa->dbg_mod);
}
 
int xtensa_smpbreak_set(struct target *target, uint32_t set)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int res = ERROR_OK;
 
    xtensa->smp_break = set;
    if (target_was_examined(target))
        res = xtensa_smpbreak_write(xtensa, xtensa->smp_break);
    LOG_TARGET_DEBUG(target, "set smpbreak=%" PRIx32 ", state %s", set,
                     target_state_name(target));
    return res;
}
 
int xtensa_smpbreak_read(struct xtensa *xtensa, uint32_t *val)
{
    uint8_t dcr_buf[sizeof(uint32_t)];
 
    xtensa_queue_dbg_reg_read(xtensa, XDMREG_DCRSET, dcr_buf);
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    *val = buf_get_u32(dcr_buf, 0, 32);
 
    return res;
}
 
int xtensa_smpbreak_get(struct target *target, uint32_t *val)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    *val = xtensa->smp_break;
    return ERROR_OK;
}
 
static inline xtensa_reg_val_t xtensa_reg_get_value(struct reg *reg)
{
    return buf_get_u32(reg->value, 0, 32);
}
 
static inline void xtensa_reg_set_value(struct reg *reg, xtensa_reg_val_t value)
{
    buf_set_u32(reg->value, 0, 32, value);
    reg->dirty = true;
}
 
static int xtensa_imprecise_exception_occurred(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    for (enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_IEVEC; idx <= XT_NX_REG_IDX_MESR; idx++) {
        enum xtensa_reg_id ridx = xtensa->nx_reg_idx[idx];
        if (xtensa->nx_reg_idx[idx]) {
            xtensa_reg_val_t reg = xtensa_reg_get(target, xtensa->nx_reg_idx[idx]);
            if (reg & XT_IMPR_EXC_MSK) {
                LOG_TARGET_DEBUG(target, "Imprecise exception: %s: 0x%x",
                    xtensa->core_cache->reg_list[ridx].name, reg);
                return true;
            }
        }
    }
    return false;
}
 
static void xtensa_imprecise_exception_clear(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    for (enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_IEVEC; idx <= XT_NX_REG_IDX_MESRCLR; idx++) {
        enum xtensa_reg_id ridx = xtensa->nx_reg_idx[idx];
        if (ridx && idx != XT_NX_REG_IDX_MESR) {
            xtensa_reg_val_t value = (idx == XT_NX_REG_IDX_MESRCLR) ? XT_MESRCLR_IMPR_EXC_MSK : 0;
            xtensa_reg_set(target, ridx, value);
            LOG_TARGET_DEBUG(target, "Imprecise exception: clearing %s (0x%x)",
                xtensa->core_cache->reg_list[ridx].name, value);
        }
    }
}
 
int xtensa_core_status_check(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int res, needclear = 0, needimprclear = 0;
 
    xtensa_dm_core_status_read(&xtensa->dbg_mod);
    xtensa_dsr_t dsr = xtensa_dm_core_status_get(&xtensa->dbg_mod);
    LOG_TARGET_DEBUG(target, "DSR (%08" PRIX32 ")", dsr);
    if (dsr & OCDDSR_EXECBUSY) {
        if (!xtensa->suppress_dsr_errors)
            LOG_TARGET_ERROR(target, "DSR (%08" PRIX32 ") indicates target still busy!", dsr);
        needclear = 1;
    }
    if (dsr & OCDDSR_EXECEXCEPTION) {
        if (!xtensa->suppress_dsr_errors)
            LOG_TARGET_ERROR(target,
                "DSR (%08" PRIX32 ") indicates DIR instruction generated an exception!",
                dsr);
        needclear = 1;
    }
    if (dsr & OCDDSR_EXECOVERRUN) {
        if (!xtensa->suppress_dsr_errors)
            LOG_TARGET_ERROR(target,
                "DSR (%08" PRIX32 ") indicates DIR instruction generated an overrun!",
                dsr);
        needclear = 1;
    }
    if (xtensa->core_config->core_type == XT_NX && (xtensa_imprecise_exception_occurred(target))) {
        if (!xtensa->suppress_dsr_errors)
            LOG_TARGET_ERROR(target,
                "%s: Imprecise exception occurred!", target_name(target));
        needclear = 1;
        needimprclear = 1;
    }
    if (needclear) {
        res = xtensa_dm_core_status_clear(&xtensa->dbg_mod,
            OCDDSR_EXECEXCEPTION | OCDDSR_EXECOVERRUN);
        if (res != ERROR_OK && !xtensa->suppress_dsr_errors)
            LOG_TARGET_ERROR(target, "clearing DSR failed!");
        if (xtensa->core_config->core_type == XT_NX && needimprclear)
            xtensa_imprecise_exception_clear(target);
        return ERROR_FAIL;
    }
    return ERROR_OK;
}
 
xtensa_reg_val_t xtensa_reg_get(struct target *target, enum xtensa_reg_id reg_id)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct reg *reg = &xtensa->core_cache->reg_list[reg_id];
    return xtensa_reg_get_value(reg);
}
 
void xtensa_reg_set(struct target *target, enum xtensa_reg_id reg_id, xtensa_reg_val_t value)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct reg *reg = &xtensa->core_cache->reg_list[reg_id];
    if (xtensa_reg_get_value(reg) == value)
        return;
    xtensa_reg_set_value(reg, value);
}
 
/* Set Ax (XT_REG_RELGEN) register along with its underlying ARx (XT_REG_GENERAL) */
void xtensa_reg_set_deep_relgen(struct target *target, enum xtensa_reg_id a_idx, xtensa_reg_val_t value)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
        XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
    uint32_t windowbase = (xtensa->core_config->windowed ?
        xtensa_reg_get(target, wb_idx) : 0);
    if (xtensa->core_config->core_type == XT_NX)
        windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
    int ar_idx = xtensa_windowbase_offset_to_canonical(xtensa, a_idx, windowbase);
    xtensa_reg_set(target, a_idx, value);
    xtensa_reg_set(target, ar_idx, value);
}
 
/* Read cause for entering halted state; return bitmask in DEBUGCAUSE_* format */
uint32_t xtensa_cause_get(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    if (xtensa->core_config->core_type == XT_LX) {
        /* LX cause in DEBUGCAUSE */
        return xtensa_reg_get(target, XT_REG_IDX_DEBUGCAUSE);
    }
    if (xtensa->nx_stop_cause & DEBUGCAUSE_VALID)
        return xtensa->nx_stop_cause;
 
    /* NX cause determined from DSR.StopCause */
    if (xtensa_dm_core_status_read(&xtensa->dbg_mod) != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Read DSR error");
    } else {
        uint32_t dsr = xtensa_dm_core_status_get(&xtensa->dbg_mod);
        /* NX causes are prioritized; only 1 bit can be set */
        switch ((dsr & OCDDSR_STOPCAUSE) >> OCDDSR_STOPCAUSE_SHIFT) {
        case OCDDSR_STOPCAUSE_DI:
            xtensa->nx_stop_cause = DEBUGCAUSE_DI;
            break;
        case OCDDSR_STOPCAUSE_SS:
            xtensa->nx_stop_cause = DEBUGCAUSE_IC;
            break;
        case OCDDSR_STOPCAUSE_IB:
            xtensa->nx_stop_cause = DEBUGCAUSE_IB;
            break;
        case OCDDSR_STOPCAUSE_B:
        case OCDDSR_STOPCAUSE_B1:
            xtensa->nx_stop_cause = DEBUGCAUSE_BI;
            break;
        case OCDDSR_STOPCAUSE_BN:
            xtensa->nx_stop_cause = DEBUGCAUSE_BN;
            break;
        case OCDDSR_STOPCAUSE_DB0:
        case OCDDSR_STOPCAUSE_DB1:
            xtensa->nx_stop_cause = DEBUGCAUSE_DB;
            break;
        default:
            LOG_TARGET_ERROR(target, "Unknown stop cause (DSR: 0x%08x)", dsr);
            break;
        }
        if (xtensa->nx_stop_cause)
            xtensa->nx_stop_cause |= DEBUGCAUSE_VALID;
    }
    return xtensa->nx_stop_cause;
}
 
void xtensa_cause_clear(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    if (xtensa->core_config->core_type == XT_LX) {
        xtensa_reg_set(target, XT_REG_IDX_DEBUGCAUSE, 0);
        xtensa->core_cache->reg_list[XT_REG_IDX_DEBUGCAUSE].dirty = false;
    } else {
        /* NX DSR.STOPCAUSE is not writeable; clear cached copy but leave it valid */
        xtensa->nx_stop_cause = DEBUGCAUSE_VALID;
    }
}
 
void xtensa_cause_reset(struct target *target)
{
    /* Clear DEBUGCAUSE_VALID to trigger re-read (on NX) */
    struct xtensa *xtensa = target_to_xtensa(target);
    xtensa->nx_stop_cause = 0;
}
 
int xtensa_assert_reset(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    LOG_TARGET_DEBUG(target, " begin");
    xtensa_queue_pwr_reg_write(xtensa,
        XDMREG_PWRCTL,
        PWRCTL_JTAGDEBUGUSE(xtensa) | PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) |
        PWRCTL_COREWAKEUP(xtensa) | PWRCTL_CORERESET(xtensa));
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK)
        return res;
 
    /* registers are now invalid */
    xtensa->reset_asserted = true;
    register_cache_invalidate(xtensa->core_cache);
    target->state = TARGET_RESET;
    return ERROR_OK;
}
 
int xtensa_deassert_reset(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    LOG_TARGET_DEBUG(target, "halt=%d", target->reset_halt);
    if (target->reset_halt)
        xtensa_queue_dbg_reg_write(xtensa,
            XDMREG_DCRSET,
            OCDDCR_ENABLEOCD | OCDDCR_DEBUGINTERRUPT);
    xtensa_queue_pwr_reg_write(xtensa,
        XDMREG_PWRCTL,
        PWRCTL_JTAGDEBUGUSE(xtensa) | PWRCTL_DEBUGWAKEUP(xtensa) | PWRCTL_MEMWAKEUP(xtensa) |
        PWRCTL_COREWAKEUP(xtensa));
    xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK)
        return res;
    target->state = TARGET_RUNNING;
    xtensa->reset_asserted = false;
    return res;
}
 
int xtensa_soft_reset_halt(struct target *target)
{
    LOG_TARGET_DEBUG(target, "begin");
    return xtensa_assert_reset(target);
}
 
int xtensa_fetch_all_regs(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct reg *reg_list = xtensa->core_cache->reg_list;
    unsigned int reg_list_size = xtensa->core_cache->num_regs;
    xtensa_reg_val_t cpenable = 0, windowbase = 0, a0 = 0, a3;
    unsigned int ms_idx = reg_list_size;
    uint32_t ms = 0;
    uint32_t woe;
    uint8_t a0_buf[4], a3_buf[4], ms_buf[4];
    bool debug_dsrs = !xtensa->regs_fetched || LOG_LEVEL_IS(LOG_LVL_DEBUG);
 
    union xtensa_reg_val_u *regvals = calloc(reg_list_size, sizeof(*regvals));
    if (!regvals) {
        LOG_TARGET_ERROR(target, "unable to allocate memory for regvals!");
        return ERROR_FAIL;
    }
    union xtensa_reg_val_u *dsrs = calloc(reg_list_size, sizeof(*dsrs));
    if (!dsrs) {
        LOG_TARGET_ERROR(target, "unable to allocate memory for dsrs!");
        free(regvals);
        return ERROR_FAIL;
    }
 
    LOG_TARGET_DEBUG(target, "start");
 
    /* Save (windowed) A3 so cache matches physical AR3; A3 usable as scratch */
    xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
    xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, a3_buf);
    if (xtensa->core_config->core_type == XT_NX) {
        /* Save (windowed) A0 as well--it will be required for reading PC */
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A0));
        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, a0_buf);
 
        /* Set MS.DispSt, clear MS.DE prior to accessing ARs.  This ensures ARs remain
         * in correct order even for reversed register groups (overflow/underflow).
         */
        ms_idx = xtensa->nx_reg_idx[XT_NX_REG_IDX_MS];
        uint32_t ms_regno = xtensa->optregs[ms_idx - XT_NUM_REGS].reg_num;
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, ms_regno, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, ms_buf);
        LOG_TARGET_DEBUG(target, "Overriding MS (0x%x): 0x%x", ms_regno, XT_MS_DISPST_DBG);
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, XT_MS_DISPST_DBG);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, ms_regno, XT_REG_A3));
    }
 
    int res = xtensa_window_state_save(target, &woe);
    if (res != ERROR_OK)
        goto xtensa_fetch_all_regs_done;
 
    /* Assume the CPU has just halted. We now want to fill the register cache with all the
     * register contents GDB needs. For speed, we pipeline all the read operations, execute them
     * in one go, then sort everything out from the regvals variable. */
 
    /* Start out with AREGS; we can reach those immediately. Grab them per 16 registers. */
    for (unsigned int j = 0; j < XT_AREGS_NUM_MAX; j += 16) {
        /*Grab the 16 registers we can see */
        for (unsigned int i = 0; i < 16; i++) {
            if (i + j < xtensa->core_config->aregs_num) {
                xtensa_queue_exec_ins(xtensa,
                    XT_INS_WSR(xtensa, XT_SR_DDR, xtensa_regs[XT_REG_IDX_AR0 + i].reg_num));
                xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR,
                    regvals[XT_REG_IDX_AR0 + i + j].buf);
                if (debug_dsrs)
                    xtensa_queue_dbg_reg_read(xtensa, XDMREG_DSR,
                        dsrs[XT_REG_IDX_AR0 + i + j].buf);
            }
        }
        if (xtensa->core_config->windowed) {
            /* Now rotate the window so we'll see the next 16 registers. The final rotate
             * will wraparound, leaving us in the state we were.
             * Each ROTW rotates 4 registers on LX and 8 on NX */
            int rotw_arg = (xtensa->core_config->core_type == XT_LX) ? 4 : 2;
            xtensa_queue_exec_ins(xtensa, XT_INS_ROTW(xtensa, rotw_arg));
        }
    }
    xtensa_window_state_restore(target, woe);
 
    if (xtensa->core_config->coproc) {
        /* As the very first thing after AREGS, go grab CPENABLE */
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, xtensa_regs[XT_REG_IDX_CPENABLE].reg_num, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, regvals[XT_REG_IDX_CPENABLE].buf);
    }
    res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK) {
        LOG_ERROR("Failed to read ARs (%d)!", res);
        goto xtensa_fetch_all_regs_done;
    }
    xtensa_core_status_check(target);
 
    a3 = buf_get_u32(a3_buf, 0, 32);
    if (xtensa->core_config->core_type == XT_NX) {
        a0 = buf_get_u32(a0_buf, 0, 32);
        ms = buf_get_u32(ms_buf, 0, 32);
    }
 
    if (xtensa->core_config->coproc) {
        cpenable = buf_get_u32(regvals[XT_REG_IDX_CPENABLE].buf, 0, 32);
 
        /* Enable all coprocessors (by setting all bits in CPENABLE) so we can read FP and user registers. */
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, 0xffffffff);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, xtensa_regs[XT_REG_IDX_CPENABLE].reg_num, XT_REG_A3));
 
        /* Save CPENABLE; flag dirty later (when regcache updated) so original value is always restored */
        LOG_TARGET_DEBUG(target, "CPENABLE: was 0x%" PRIx32 ", all enabled", cpenable);
        xtensa_reg_set(target, XT_REG_IDX_CPENABLE, cpenable);
    }
    /* We're now free to use any of A0-A15 as scratch registers
     * Grab the SFRs and user registers first. We use A3 as a scratch register. */
    for (unsigned int i = 0; i < reg_list_size; i++) {
        struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
        unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
        if (xtensa_reg_is_readable(rlist[ridx].flags, cpenable) && rlist[ridx].exist) {
            bool reg_fetched = true;
            unsigned int reg_num = rlist[ridx].reg_num;
            switch (rlist[ridx].type) {
            case XT_REG_USER:
                xtensa_queue_exec_ins(xtensa, XT_INS_RUR(xtensa, reg_num, XT_REG_A3));
                break;
            case XT_REG_FR:
                xtensa_queue_exec_ins(xtensa, XT_INS_RFR(xtensa, reg_num, XT_REG_A3));
                break;
            case XT_REG_SPECIAL:
                if (reg_num == XT_PC_REG_NUM_VIRTUAL) {
                    if (xtensa->core_config->core_type == XT_LX) {
                        /* reg number of PC for debug interrupt depends on NDEBUGLEVEL */
                        reg_num = XT_EPC_REG_NUM_BASE + xtensa->core_config->debug.irq_level;
                        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
                    } else {
                        /* NX PC read through CALL0(0) and reading A0 */
                        xtensa_queue_exec_ins(xtensa, XT_INS_CALL0(xtensa, 0));
                        xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A0));
                        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, regvals[i].buf);
                        xtensa_queue_dbg_reg_read(xtensa, XDMREG_DSR, dsrs[i].buf);
                        reg_fetched = false;
                    }
                } else if ((xtensa->core_config->core_type == XT_LX)
                    && (reg_num == xtensa_regs[XT_REG_IDX_PS].reg_num)) {
                    /* reg number of PS for debug interrupt depends on NDEBUGLEVEL */
                    reg_num = XT_EPS_REG_NUM_BASE + xtensa->core_config->debug.irq_level;
                    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
                } else if (reg_num == xtensa_regs[XT_REG_IDX_CPENABLE].reg_num) {
                    /* CPENABLE already read/updated; don't re-read */
                    reg_fetched = false;
                    break;
                } else {
                    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, reg_num, XT_REG_A3));
                }
                break;
            default:
                reg_fetched = false;
            }
            if (reg_fetched) {
                xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
                xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, regvals[i].buf);
                if (debug_dsrs)
                    xtensa_queue_dbg_reg_read(xtensa, XDMREG_DSR, dsrs[i].buf);
            }
        }
    }
    /* Ok, send the whole mess to the CPU. */
    res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK) {
        LOG_ERROR("Failed to fetch AR regs!");
        goto xtensa_fetch_all_regs_done;
    }
    xtensa_core_status_check(target);
 
    if (debug_dsrs) {
        /* DSR checking: follows order in which registers are requested. */
        for (unsigned int i = 0; i < reg_list_size; i++) {
            struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
            unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
            if (xtensa_reg_is_readable(rlist[ridx].flags, cpenable) && rlist[ridx].exist &&
                (rlist[ridx].type != XT_REG_DEBUG) &&
                (rlist[ridx].type != XT_REG_RELGEN) &&
                (rlist[ridx].type != XT_REG_TIE) &&
                (rlist[ridx].type != XT_REG_OTHER)) {
                if (buf_get_u32(dsrs[i].buf, 0, 32) & OCDDSR_EXECEXCEPTION) {
                    LOG_ERROR("Exception reading %s!", reg_list[i].name);
                    res = ERROR_FAIL;
                    goto xtensa_fetch_all_regs_done;
                }
            }
        }
    }
 
    if (xtensa->core_config->windowed) {
        /* We need the windowbase to decode the general addresses. */
        uint32_t wb_idx = (xtensa->core_config->core_type == XT_LX) ?
            XT_REG_IDX_WINDOWBASE : xtensa->nx_reg_idx[XT_NX_REG_IDX_WB];
        windowbase = buf_get_u32(regvals[wb_idx].buf, 0, 32);
        if (xtensa->core_config->core_type == XT_NX)
            windowbase = (windowbase & XT_WB_P_MSK) >> XT_WB_P_SHIFT;
    }
 
    /* Decode the result and update the cache. */
    for (unsigned int i = 0; i < reg_list_size; i++) {
        struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
        unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
        if (xtensa_reg_is_readable(rlist[ridx].flags, cpenable) && rlist[ridx].exist) {
            if ((xtensa->core_config->windowed) && (rlist[ridx].type == XT_REG_GENERAL)) {
                /* The 64-value general register set is read from (windowbase) on down.
                 * We need to get the real register address by subtracting windowbase and
                 * wrapping around. */
                enum xtensa_reg_id realadr = xtensa_canonical_to_windowbase_offset(xtensa, i,
                    windowbase);
                buf_cpy(regvals[realadr].buf, reg_list[i].value, reg_list[i].size);
            } else if (rlist[ridx].type == XT_REG_RELGEN) {
                buf_cpy(regvals[rlist[ridx].reg_num].buf, reg_list[i].value, reg_list[i].size);
                if (xtensa_extra_debug_log) {
                    xtensa_reg_val_t regval = buf_get_u32(regvals[rlist[ridx].reg_num].buf, 0, 32);
                    LOG_DEBUG("%s = 0x%x", rlist[ridx].name, regval);
                }
            } else {
                xtensa_reg_val_t regval = buf_get_u32(regvals[i].buf, 0, 32);
                bool is_dirty = (i == XT_REG_IDX_CPENABLE);
                if (xtensa_extra_debug_log)
                    LOG_INFO("Register %s: 0x%X", reg_list[i].name, regval);
                if (rlist[ridx].reg_num == XT_PC_REG_NUM_VIRTUAL &&
                    xtensa->core_config->core_type == XT_NX) {
                    /* A0 from prior CALL0 points to next instruction; decrement it */
                    regval -= 3;
                    is_dirty = 1;
                } else if (i == ms_idx) {
                    LOG_TARGET_DEBUG(target, "Caching MS: 0x%x", ms);
                    regval = ms;
                    is_dirty = 1;
                }
                xtensa_reg_set(target, i, regval);
                reg_list[i].dirty = is_dirty;   /*always do this _after_ xtensa_reg_set! */
            }
            reg_list[i].valid = true;
        } else {
            if ((rlist[ridx].flags & XT_REGF_MASK) == XT_REGF_NOREAD) {
                /* Report read-only registers all-zero but valid */
                reg_list[i].valid = true;
                xtensa_reg_set(target, i, 0);
            } else {
                reg_list[i].valid = false;
            }
        }
    }
 
    if (xtensa->core_config->windowed) {
        /* We have used A3 as a scratch register.
         * Windowed configs: restore A3's AR (XT_REG_GENERAL) and and flag for write-back.
         */
        enum xtensa_reg_id ar3_idx = xtensa_windowbase_offset_to_canonical(xtensa, XT_REG_IDX_A3, windowbase);
        xtensa_reg_set(target, ar3_idx, a3);
        xtensa_mark_register_dirty(xtensa, ar3_idx);
 
        /* Reset scratch_ars[] on fetch.  .chrval tracks AR mapping and changes w/ window */
        sprintf(xtensa->scratch_ars[XT_AR_SCRATCH_AR3].chrval, "ar%d", ar3_idx - XT_REG_IDX_AR0);
        enum xtensa_reg_id ar4_idx = xtensa_windowbase_offset_to_canonical(xtensa, XT_REG_IDX_A4, windowbase);
        sprintf(xtensa->scratch_ars[XT_AR_SCRATCH_AR4].chrval, "ar%d", ar4_idx - XT_REG_IDX_AR0);
        for (enum xtensa_ar_scratch_set_e s = 0; s < XT_AR_SCRATCH_NUM; s++)
            xtensa->scratch_ars[s].intval = false;
    }
 
    /* We have used A3 (XT_REG_RELGEN) as a scratch register.  Restore and flag for write-back. */
    xtensa_reg_set(target, XT_REG_IDX_A3, a3);
    xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
    if (xtensa->core_config->core_type == XT_NX) {
        xtensa_reg_set(target, XT_REG_IDX_A0, a0);
        xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A0);
    }
 
    xtensa->regs_fetched = true;
xtensa_fetch_all_regs_done:
    free(regvals);
    free(dsrs);
    return res;
}
 
int xtensa_get_gdb_reg_list(struct target *target,
    struct reg **reg_list[],
    int *reg_list_size,
    enum target_register_class reg_class)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int num_regs;
 
    if (reg_class == REG_CLASS_GENERAL) {
        if ((xtensa->genpkt_regs_num == 0) || !xtensa->contiguous_regs_list) {
            LOG_ERROR("reg_class %d unhandled; 'xtgregs' not found", reg_class);
            return ERROR_FAIL;
        }
        num_regs = xtensa->genpkt_regs_num;
    } else {
        /* Determine whether to return a contiguous or sparse register map */
        num_regs = xtensa->regmap_contiguous ? xtensa->total_regs_num : xtensa->dbregs_num;
    }
 
    LOG_DEBUG("reg_class=%i, num_regs=%d", (int)reg_class, num_regs);
 
    *reg_list = calloc(num_regs, sizeof(struct reg *));
    if (!*reg_list)
        return ERROR_FAIL;
 
    *reg_list_size = num_regs;
    if (xtensa->regmap_contiguous) {
        assert((num_regs <= xtensa->total_regs_num) && "contiguous regmap size internal error!");
        for (unsigned int i = 0; i < num_regs; i++)
            (*reg_list)[i] = xtensa->contiguous_regs_list[i];
        return ERROR_OK;
    }
 
    for (unsigned int i = 0; i < num_regs; i++)
        (*reg_list)[i] = (struct reg *)&xtensa->empty_regs[i];
    unsigned int k = 0;
    for (unsigned int i = 0; i < xtensa->core_cache->num_regs && k < num_regs; i++) {
        if (xtensa->core_cache->reg_list[i].exist) {
            struct xtensa_reg_desc *rlist = (i < XT_NUM_REGS) ? xtensa_regs : xtensa->optregs;
            unsigned int ridx = (i < XT_NUM_REGS) ? i : i - XT_NUM_REGS;
            int sparse_idx = rlist[ridx].dbreg_num;
            if (i == XT_REG_IDX_PS && xtensa->core_config->core_type == XT_LX) {
                if (xtensa->eps_dbglevel_idx == 0) {
                    LOG_ERROR("eps_dbglevel_idx not set\n");
                    return ERROR_FAIL;
                }
                (*reg_list)[sparse_idx] = &xtensa->core_cache->reg_list[xtensa->eps_dbglevel_idx];
                if (xtensa_extra_debug_log)
                    LOG_DEBUG("SPARSE GDB reg 0x%x getting EPS%d 0x%x",
                        sparse_idx, xtensa->core_config->debug.irq_level,
                        xtensa_reg_get_value((*reg_list)[sparse_idx]));
            } else if (rlist[ridx].type == XT_REG_RELGEN) {
                (*reg_list)[sparse_idx - XT_REG_IDX_ARFIRST] = &xtensa->core_cache->reg_list[i];
            } else {
                (*reg_list)[sparse_idx] = &xtensa->core_cache->reg_list[i];
            }
            if (i == XT_REG_IDX_PC)
                /* Make a duplicate copy of PC for external access */
                (*reg_list)[XT_PC_DBREG_NUM_BASE] = &xtensa->core_cache->reg_list[i];
            k++;
        }
    }
 
    if (k == num_regs)
        LOG_ERROR("SPARSE GDB reg list full (size %d)", k);
 
    return ERROR_OK;
}
 
int xtensa_mmu_is_enabled(struct target *target, bool *enabled)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    *enabled = xtensa->core_config->mmu.itlb_entries_count > 0 ||
        xtensa->core_config->mmu.dtlb_entries_count > 0;
    return ERROR_OK;
}
 
int xtensa_halt(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    LOG_TARGET_DEBUG(target, "start");
    if (target->state == TARGET_HALTED) {
        LOG_TARGET_DEBUG(target, "target was already halted");
        return ERROR_OK;
    }
    /* First we have to read dsr and check if the target stopped */
    int res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
    if (res != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to read core status!");
        return res;
    }
    LOG_TARGET_DEBUG(target, "Core status 0x%" PRIx32, xtensa_dm_core_status_get(&xtensa->dbg_mod));
    if (!xtensa_is_stopped(target)) {
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRSET, OCDDCR_ENABLEOCD | OCDDCR_DEBUGINTERRUPT);
        xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
        res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        if (res != ERROR_OK)
            LOG_TARGET_ERROR(target, "Failed to set OCDDCR_DEBUGINTERRUPT. Can't halt.");
    }
 
    return res;
}
 
int xtensa_prepare_resume(struct target *target,
    bool current,
    target_addr_t address,
    bool handle_breakpoints,
    bool debug_execution)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    uint32_t bpena = 0;
 
    LOG_TARGET_DEBUG(target,
        "current=%d address=" TARGET_ADDR_FMT ", handle_breakpoints=%i, debug_execution=%i)",
        current,
        address,
        handle_breakpoints,
        debug_execution);
 
    if (target->state != TARGET_HALTED) {
        LOG_TARGET_ERROR(target, "not halted");
        return ERROR_TARGET_NOT_HALTED;
    }
    xtensa->halt_request = false;
 
    if (address && !current) {
        xtensa_reg_set(target, XT_REG_IDX_PC, address);
    } else {
        uint32_t cause = xtensa_cause_get(target);
        LOG_TARGET_DEBUG(target, "DEBUGCAUSE 0x%x (watchpoint %lu) (break %lu)",
            cause, (cause & DEBUGCAUSE_DB), (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)));
        if (cause & DEBUGCAUSE_DB)
            /* We stopped due to a watchpoint. We can't just resume executing the
             * instruction again because */
            /* that would trigger the watchpoint again. To fix this, we single-step,
             * which ignores watchpoints. */
            xtensa_do_step(target, current, address, handle_breakpoints);
        if (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN))
            /* We stopped due to a break instruction. We can't just resume executing the
             * instruction again because */
            /* that would trigger the break again. To fix this, we single-step, which
             * ignores break. */
            xtensa_do_step(target, current, address, handle_breakpoints);
    }
 
    /* Write back hw breakpoints. Current FreeRTOS SMP code can set a hw breakpoint on an
     * exception; we need to clear that and return to the breakpoints gdb has set on resume. */
    for (unsigned int slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
        if (xtensa->hw_brps[slot]) {
            /* Write IBREAKA[slot] and set bit #slot in IBREAKENABLE */
            xtensa_reg_set(target, XT_REG_IDX_IBREAKA0 + slot, xtensa->hw_brps[slot]->address);
            if (xtensa->core_config->core_type == XT_NX)
                xtensa_reg_set(target, xtensa->nx_reg_idx[XT_NX_REG_IDX_IBREAKC0] + slot, XT_IBREAKC_FB);
            bpena |= BIT(slot);
        }
    }
    if (xtensa->core_config->core_type == XT_LX)
        xtensa_reg_set(target, XT_REG_IDX_IBREAKENABLE, bpena);
 
    /* Here we write all registers to the targets */
    int res = xtensa_write_dirty_registers(target);
    if (res != ERROR_OK)
        LOG_TARGET_ERROR(target, "Failed to write back register cache.");
    return res;
}
 
int xtensa_do_resume(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    LOG_TARGET_DEBUG(target, "start");
 
    xtensa_cause_reset(target);
    xtensa_queue_exec_ins(xtensa, XT_INS_RFDO(xtensa));
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to exec RFDO %d!", res);
        return res;
    }
    xtensa_core_status_check(target);
    return ERROR_OK;
}
 
int xtensa_resume(struct target *target,
    bool current,
    target_addr_t address,
    bool handle_breakpoints,
    bool debug_execution)
{
    LOG_TARGET_DEBUG(target, "start");
    int res = xtensa_prepare_resume(target, current, address,
        handle_breakpoints, debug_execution);
    if (res != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to prepare for resume!");
        return res;
    }
    res = xtensa_do_resume(target);
    if (res != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to resume!");
        return res;
    }
 
    target->debug_reason = DBG_REASON_NOTHALTED;
    if (!debug_execution)
        target->state = TARGET_RUNNING;
    else
        target->state = TARGET_DEBUG_RUNNING;
 
    target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 
    return ERROR_OK;
}
 
static bool xtensa_pc_in_winexc(struct target *target, target_addr_t pc)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    uint8_t insn_buf[XT_ISNS_SZ_MAX];
    int err = xtensa_read_buffer(target, pc, sizeof(insn_buf), insn_buf);
    if (err != ERROR_OK)
        return false;
 
    xtensa_insn_t insn = buf_get_u32(insn_buf, 0, 24);
    xtensa_insn_t masked = insn & XT_INS_L32E_S32E_MASK(xtensa);
    if (masked == XT_INS_L32E(xtensa, 0, 0, 0) || masked == XT_INS_S32E(xtensa, 0, 0, 0))
        return true;
 
    masked = insn & XT_INS_RFWO_RFWU_MASK(xtensa);
    if (masked == XT_INS_RFWO(xtensa) || masked == XT_INS_RFWU(xtensa))
        return true;
 
    return false;
}
 
int xtensa_do_step(struct target *target, bool current, target_addr_t address,
        bool handle_breakpoints)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int res;
    const uint32_t icount_val = -2; /* ICOUNT value to load for 1 step */
    xtensa_reg_val_t dbreakc[XT_WATCHPOINTS_NUM_MAX];
    xtensa_reg_val_t icountlvl, cause;
    xtensa_reg_val_t oldps, oldpc, cur_pc;
    bool ps_modified = false;
 
    LOG_TARGET_DEBUG(target, "current=%d, address=" TARGET_ADDR_FMT ", handle_breakpoints=%i",
        current, address, handle_breakpoints);
 
    if (target->state != TARGET_HALTED) {
        LOG_TARGET_ERROR(target, "not halted");
        return ERROR_TARGET_NOT_HALTED;
    }
 
    if (xtensa->eps_dbglevel_idx == 0 && xtensa->core_config->core_type == XT_LX) {
        LOG_TARGET_ERROR(target, "eps_dbglevel_idx not set\n");
        return ERROR_FAIL;
    }
 
    /* Save old ps (EPS[dbglvl] on LX), pc */
    oldps = xtensa_reg_get(target, (xtensa->core_config->core_type == XT_LX) ?
        xtensa->eps_dbglevel_idx : XT_REG_IDX_PS);
    oldpc = xtensa_reg_get(target, XT_REG_IDX_PC);
 
    cause = xtensa_cause_get(target);
    LOG_TARGET_DEBUG(target, "oldps=%" PRIx32 ", oldpc=%" PRIx32 " dbg_cause=%" PRIx32 " exc_cause=%" PRIx32,
        oldps,
        oldpc,
        cause,
        xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE));
    if (handle_breakpoints && (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN))) {
        /* handle hard-coded SW breakpoints (e.g. syscalls) */
        LOG_TARGET_DEBUG(target, "Increment PC to pass break instruction...");
        xtensa_cause_clear(target); /* so we don't recurse into the same routine */
        /* pretend that we have stepped */
        if (cause & DEBUGCAUSE_BI)
            xtensa_reg_set(target, XT_REG_IDX_PC, oldpc + 3);   /* PC = PC+3 */
        else
            xtensa_reg_set(target, XT_REG_IDX_PC, oldpc + 2);   /* PC = PC+2 */
        return ERROR_OK;
    }
 
    /* Xtensa LX has an ICOUNTLEVEL register which sets the maximum interrupt level
     * at which the instructions are to be counted while stepping.
     *
     * For example, if we need to step by 2 instructions, and an interrupt occurs
     * in between, the processor will trigger the interrupt and halt after the 2nd
     * instruction within the interrupt vector and/or handler.
     *
     * However, sometimes we don't want the interrupt handlers to be executed at all
     * while stepping through the code. In this case (XT_STEPPING_ISR_OFF),
     * ICOUNTLEVEL can be lowered to the executing code's (level + 1) to prevent ISR
     * code from being counted during stepping.  Note that C exception handlers must
     * run at level 0 and hence will be counted and stepped into, should one occur.
     *
     * TODO: Certain instructions should never be single-stepped and should instead
     * be emulated (per DUG): RSIL >= DBGLEVEL, RSR/WSR [ICOUNT|ICOUNTLEVEL], and
     * RFI >= DBGLEVEL.
     */
    if (xtensa->stepping_isr_mode == XT_STEPPING_ISR_OFF) {
        if (xtensa->core_config->core_type == XT_LX) {
            if (!xtensa->core_config->high_irq.enabled) {
                LOG_TARGET_WARNING(target,
                        "disabling IRQs while stepping is not implemented w/o high prio IRQs option!");
                return ERROR_FAIL;
            }
            /* Update ICOUNTLEVEL accordingly */
            icountlvl = MIN((oldps & 0xF) + 1, xtensa->core_config->debug.irq_level);
        } else {
            /* Xtensa NX does not have the ICOUNTLEVEL feature present in Xtensa LX
             * and instead disable interrupts while stepping. This could change
             * the timing of the system while under debug */
            xtensa_reg_val_t newps = oldps | XT_PS_DI_MSK;
            xtensa_reg_set(target, XT_REG_IDX_PS, newps);
            icountlvl = xtensa->core_config->debug.irq_level;
            ps_modified = true;
        }
    } else {
        icountlvl = xtensa->core_config->debug.irq_level;
    }
 
    if (cause & DEBUGCAUSE_DB) {
        /* We stopped due to a watchpoint. We can't just resume executing the instruction again because
         * that would trigger the watchpoint again. To fix this, we remove watchpoints,single-step and
         * re-enable the watchpoint. */
        LOG_TARGET_DEBUG(
            target,
            "Single-stepping to get past instruction that triggered the watchpoint...");
        xtensa_cause_clear(target); /* so we don't recurse into the same routine */
        /* Save all DBREAKCx registers and set to 0 to disable watchpoints */
        for (unsigned int slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
            dbreakc[slot] = xtensa_reg_get(target, XT_REG_IDX_DBREAKC0 + slot);
            xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, 0);
        }
    }
 
    if (!handle_breakpoints && (cause & (DEBUGCAUSE_BI | DEBUGCAUSE_BN)))
        /* handle normal SW breakpoint */
        xtensa_cause_clear(target); /* so we don't recurse into the same routine */
    if (xtensa->core_config->core_type == XT_LX && ((oldps & 0xf) >= icountlvl)) {
        /* Lower interrupt level to allow stepping, but flag eps[dbglvl] to be restored */
        ps_modified = true;
        uint32_t newps = (oldps & ~0xf) | (icountlvl - 1);
        xtensa_reg_set(target, xtensa->eps_dbglevel_idx, newps);
        LOG_TARGET_DEBUG(target,
            "Lowering PS.INTLEVEL to allow stepping: %s <- 0x%08" PRIx32 " (was 0x%08" PRIx32 ")",
            xtensa->core_cache->reg_list[xtensa->eps_dbglevel_idx].name,
            newps,
            oldps);
    }
    do {
        if (xtensa->core_config->core_type == XT_LX) {
            xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, icountlvl);
            xtensa_reg_set(target, XT_REG_IDX_ICOUNT, icount_val);
        } else {
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRSET, OCDDCR_STEPREQUEST);
        }
 
        /* Now that ICOUNT (LX) or DCR.StepRequest (NX) is set,
         * we can resume as if we were going to run
         */
        res = xtensa_prepare_resume(target, current, address, false, false);
        if (res != ERROR_OK) {
            LOG_TARGET_ERROR(target, "Failed to prepare resume for single step");
            return res;
        }
        res = xtensa_do_resume(target);
        if (res != ERROR_OK) {
            LOG_TARGET_ERROR(target, "Failed to resume after setting up single step");
            return res;
        }
 
        /* Wait for stepping to complete */
        long long start = timeval_ms();
        while (timeval_ms() < start + 500) {
            /* Do not use target_poll here, it also triggers other things... just manually read the DSR
             *until stepping is complete. */
            usleep(1000);
            res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
            if (res != ERROR_OK) {
                LOG_TARGET_ERROR(target, "Failed to read core status!");
                return res;
            }
            if (xtensa_is_stopped(target))
                break;
            usleep(1000);
        }
        LOG_TARGET_DEBUG(target, "Finish stepping. dsr=0x%08" PRIx32,
            xtensa_dm_core_status_get(&xtensa->dbg_mod));
        if (!xtensa_is_stopped(target)) {
            LOG_TARGET_WARNING(
                target,
                "Timed out waiting for target to finish stepping. dsr=0x%08" PRIx32,
                xtensa_dm_core_status_get(&xtensa->dbg_mod));
            target->debug_reason = DBG_REASON_NOTHALTED;
            target->state = TARGET_RUNNING;
            return ERROR_FAIL;
        }
 
        xtensa_fetch_all_regs(target);
        cur_pc = xtensa_reg_get(target, XT_REG_IDX_PC);
 
        LOG_TARGET_DEBUG(target,
            "cur_ps=%" PRIx32 ", cur_pc=%" PRIx32 " dbg_cause=%" PRIx32 " exc_cause=%" PRIx32,
            xtensa_reg_get(target, XT_REG_IDX_PS),
            cur_pc,
            xtensa_cause_get(target),
            xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE));
 
        /* Do not step into WindowOverflow if ISRs are masked.
           If we stop in WindowOverflow at breakpoint with masked ISRs and
           try to do a step it will get us out of that handler */
        if (xtensa->core_config->windowed &&
            xtensa->stepping_isr_mode == XT_STEPPING_ISR_OFF &&
            xtensa_pc_in_winexc(target, cur_pc)) {
            /* isrmask = on, need to step out of the window exception handler */
            LOG_DEBUG("Stepping out of window exception, PC=%" PRIX32, cur_pc);
            oldpc = cur_pc;
            address = oldpc + 3;
            continue;
        }
 
        if (oldpc == cur_pc)
            LOG_TARGET_WARNING(target, "Stepping doesn't seem to change PC! dsr=0x%08" PRIx32,
                xtensa_dm_core_status_get(&xtensa->dbg_mod));
        else
            LOG_DEBUG("Stepped from %" PRIX32 " to %" PRIX32, oldpc, cur_pc);
        break;
    } while (true);
 
    target->debug_reason = DBG_REASON_SINGLESTEP;
    target->state = TARGET_HALTED;
    LOG_DEBUG("Done stepping, PC=%" PRIX32, cur_pc);
 
    if (cause & DEBUGCAUSE_DB) {
        LOG_TARGET_DEBUG(target, "...Done, re-installing watchpoints.");
        /* Restore the DBREAKCx registers */
        for (unsigned int slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++)
            xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, dbreakc[slot]);
    }
 
    /* Restore int level */
    if (ps_modified) {
        LOG_DEBUG("Restoring %s after stepping: 0x%08" PRIx32,
            xtensa->core_cache->reg_list[(xtensa->core_config->core_type == XT_LX) ?
                xtensa->eps_dbglevel_idx : XT_REG_IDX_PS].name, oldps);
        xtensa_reg_set(target, (xtensa->core_config->core_type == XT_LX) ?
            xtensa->eps_dbglevel_idx : XT_REG_IDX_PS, oldps);
    }
 
    /* write ICOUNTLEVEL back to zero */
    xtensa_reg_set(target, XT_REG_IDX_ICOUNTLEVEL, 0);
    /* TODO: can we skip writing dirty registers and re-fetching them? */
    res = xtensa_write_dirty_registers(target);
    xtensa_fetch_all_regs(target);
    return res;
}
 
int xtensa_step(struct target *target, bool current, target_addr_t address,
        bool handle_breakpoints)
{
    int retval = xtensa_do_step(target, current, address, handle_breakpoints);
    if (retval != ERROR_OK)
        return retval;
    target_call_event_callbacks(target, TARGET_EVENT_HALTED);
 
    return ERROR_OK;
}
 
static inline bool xtensa_memory_regions_overlap(target_addr_t r1_start,
    target_addr_t r1_end,
    target_addr_t r2_start,
    target_addr_t r2_end)
{
    if ((r2_start >= r1_start) && (r2_start < r1_end))
        return true;    /* r2_start is in r1 region */
    if ((r2_end > r1_start) && (r2_end <= r1_end))
        return true;    /* r2_end is in r1 region */
    return false;
}
 
static inline target_addr_t xtensa_get_overlap_size(target_addr_t r1_start,
    target_addr_t r1_end,
    target_addr_t r2_start,
    target_addr_t r2_end)
{
    if (xtensa_memory_regions_overlap(r1_start, r1_end, r2_start, r2_end)) {
        target_addr_t ov_start = r1_start < r2_start ? r2_start : r1_start;
        target_addr_t ov_end = r1_end > r2_end ? r2_end : r1_end;
        return ov_end - ov_start;
    }
    return 0;
}
 
static bool xtensa_memory_op_validate_range(struct xtensa *xtensa, target_addr_t address, size_t size, int access)
{
    target_addr_t adr_pos = address;    /* address cursor set to the beginning start */
    target_addr_t adr_end = address + size; /* region end */
    target_addr_t overlap_size;
    const struct xtensa_local_mem_region_config *cm;    /* current mem region */
 
    while (adr_pos < adr_end) {
        cm = xtensa_target_memory_region_find(xtensa, adr_pos);
        if (!cm)    /* address is not belong to anything */
            return false;
        if ((cm->access & access) != access)    /* access check */
            return false;
        overlap_size = xtensa_get_overlap_size(cm->base, (cm->base + cm->size), adr_pos, adr_end);
        assert(overlap_size != 0);
        adr_pos += overlap_size;
    }
    return true;
}
 
int xtensa_read_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    /* We are going to read memory in 32-bit increments. This may not be what the calling
     * function expects, so we may need to allocate a temp buffer and read into that first. */
    target_addr_t addrstart_al = ALIGN_DOWN(address, 4);
    target_addr_t addrend_al = ALIGN_UP(address + size * count, 4);
    target_addr_t adr = addrstart_al;
    uint8_t *albuff;
    bool bswap = xtensa->target->endianness == TARGET_BIG_ENDIAN;
 
    if (target->state != TARGET_HALTED) {
        LOG_TARGET_ERROR(target, "not halted");
        return ERROR_TARGET_NOT_HALTED;
    }
 
    if (!xtensa->permissive_mode) {
        if (!xtensa_memory_op_validate_range(xtensa, address, (size * count),
                XT_MEM_ACCESS_READ)) {
            LOG_DEBUG("address " TARGET_ADDR_FMT " not readable", address);
            return ERROR_FAIL;
        }
    }
 
    unsigned int alloc_bytes = ALIGN_UP(addrend_al - addrstart_al, sizeof(uint32_t));
    albuff = calloc(alloc_bytes, 1);
    if (!albuff) {
        LOG_TARGET_ERROR(target, "Out of memory allocating %" PRId64 " bytes!",
            addrend_al - addrstart_al);
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
 
    /* We're going to use A3 here */
    xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
    /* Write start address to A3 */
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, addrstart_al);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
    /* Now we can safely read data from addrstart_al up to addrend_al into albuff */
    if (xtensa->probe_lsddr32p != 0) {
        xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(xtensa, XT_REG_A3));
        for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t))
            xtensa_queue_dbg_reg_read(xtensa,
                (adr + sizeof(uint32_t) == addrend_al) ? XDMREG_DDR : XDMREG_DDREXEC,
                &albuff[i]);
    } else {
        xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A4);
        for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t)) {
            xtensa_queue_exec_ins(xtensa, XT_INS_L32I(xtensa, XT_REG_A3, XT_REG_A4, 0));
            xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A4));
            xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, &albuff[i]);
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, adr + sizeof(uint32_t));
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        }
    }
    int res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res == ERROR_OK) {
        bool prev_suppress = xtensa->suppress_dsr_errors;
        xtensa->suppress_dsr_errors = true;
        res = xtensa_core_status_check(target);
        if (xtensa->probe_lsddr32p == -1)
            xtensa->probe_lsddr32p = 1;
        xtensa->suppress_dsr_errors = prev_suppress;
    }
    if (res != ERROR_OK) {
        if (xtensa->probe_lsddr32p != 0) {
            /* Disable fast memory access instructions and retry before reporting an error */
            LOG_TARGET_DEBUG(target, "Disabling LDDR32.P/SDDR32.P");
            xtensa->probe_lsddr32p = 0;
            res = xtensa_read_memory(target, address, size, count, buffer);
        } else {
            LOG_TARGET_WARNING(target, "Failed reading %d bytes at address "TARGET_ADDR_FMT,
                count * size, address);
        }
    } else {
        if (bswap)
            buf_bswap32(albuff, albuff, addrend_al - addrstart_al);
        memcpy(buffer, albuff + (address & 3), (size * count));
    }
    free(albuff);
    return res;
}
 
int xtensa_read_buffer(struct target *target, target_addr_t address, uint32_t count, uint8_t *buffer)
{
    /* xtensa_read_memory can also read unaligned stuff. Just pass through to that routine. */
    return xtensa_read_memory(target, address, 1, count, buffer);
}
 
int xtensa_write_memory(struct target *target,
    target_addr_t address,
    uint32_t size,
    uint32_t count,
    const uint8_t *buffer)
{
    /* This memory write function can get thrown nigh everything into it, from
     * aligned uint32 writes to unaligned uint8ths. The Xtensa memory doesn't always
     * accept anything but aligned uint32 writes, though. That is why we convert
     * everything into that. */
    struct xtensa *xtensa = target_to_xtensa(target);
    target_addr_t addrstart_al = ALIGN_DOWN(address, 4);
    target_addr_t addrend_al = ALIGN_UP(address + size * count, 4);
    target_addr_t adr = addrstart_al;
    int res;
    uint8_t *albuff;
    bool fill_head_tail = false;
 
    if (target->state != TARGET_HALTED) {
        LOG_TARGET_ERROR(target, "not halted");
        return ERROR_TARGET_NOT_HALTED;
    }
 
    if (!xtensa->permissive_mode) {
        if (!xtensa_memory_op_validate_range(xtensa, address, (size * count), XT_MEM_ACCESS_WRITE)) {
            LOG_WARNING("address " TARGET_ADDR_FMT " not writable", address);
            return ERROR_FAIL;
        }
    }
 
    if (size == 0 || count == 0 || !buffer)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    /* Allocate a temporary buffer to put the aligned bytes in, if needed. */
    if (addrstart_al == address && addrend_al == address + (size * count)) {
        if (xtensa->target->endianness == TARGET_BIG_ENDIAN)
            /* Need a buffer for byte-swapping */
            albuff = malloc(addrend_al - addrstart_al);
        else
            /* We discard the const here because albuff can also be non-const */
            albuff = (uint8_t *)buffer;
    } else {
        fill_head_tail = true;
        albuff = malloc(addrend_al - addrstart_al);
    }
    if (!albuff) {
        LOG_TARGET_ERROR(target, "Out of memory allocating %" PRId64 " bytes!",
            addrend_al - addrstart_al);
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
 
    /* We're going to use A3 here */
    xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
 
    /* If we're using a temp aligned buffer, we need to fill the head and/or tail bit of it. */
    if (fill_head_tail) {
        /* See if we need to read the first and/or last word. */
        if (address & 3) {
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, addrstart_al);
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
            if (xtensa->probe_lsddr32p == 1) {
                xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(xtensa, XT_REG_A3));
            } else {
                xtensa_queue_exec_ins(xtensa, XT_INS_L32I(xtensa, XT_REG_A3, XT_REG_A3, 0));
                xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
            }
            xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR, &albuff[0]);
        }
        if ((address + (size * count)) & 3) {
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, addrend_al - 4);
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
            if (xtensa->probe_lsddr32p == 1) {
                xtensa_queue_exec_ins(xtensa, XT_INS_LDDR32P(xtensa, XT_REG_A3));
            } else {
                xtensa_queue_exec_ins(xtensa, XT_INS_L32I(xtensa, XT_REG_A3, XT_REG_A3, 0));
                xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_DDR, XT_REG_A3));
            }
            xtensa_queue_dbg_reg_read(xtensa, XDMREG_DDR,
                &albuff[addrend_al - addrstart_al - 4]);
        }
        /* Grab bytes */
        res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        if (res != ERROR_OK) {
            LOG_ERROR("Error issuing unaligned memory write context instruction(s): %d", res);
            if (albuff != buffer)
                free(albuff);
            return res;
        }
        xtensa_core_status_check(target);
        if (xtensa->target->endianness == TARGET_BIG_ENDIAN) {
            bool swapped_w0 = false;
            if (address & 3) {
                buf_bswap32(&albuff[0], &albuff[0], 4);
                swapped_w0 = true;
            }
            if ((address + (size * count)) & 3) {
                if ((addrend_al - addrstart_al - 4 == 0) && swapped_w0) {
                    /* Don't double-swap if buffer start/end are within the same word */
                } else {
                    buf_bswap32(&albuff[addrend_al - addrstart_al - 4],
                        &albuff[addrend_al - addrstart_al - 4], 4);
                }
            }
        }
        /* Copy data to be written into the aligned buffer (in host-endianness) */
        memcpy(&albuff[address & 3], buffer, size * count);
        /* Now we can write albuff in aligned uint32s. */
    }
 
    if (xtensa->target->endianness == TARGET_BIG_ENDIAN)
        buf_bswap32(albuff, fill_head_tail ? albuff : buffer, addrend_al - addrstart_al);
 
    /* Write start address to A3 */
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, addrstart_al);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
    /* Write the aligned buffer */
    if (xtensa->probe_lsddr32p != 0) {
        for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t)) {
            if (i == 0) {
                xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, buf_get_u32(&albuff[i], 0, 32));
                xtensa_queue_exec_ins(xtensa, XT_INS_SDDR32P(xtensa, XT_REG_A3));
            } else {
                xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDREXEC, buf_get_u32(&albuff[i], 0, 32));
            }
        }
    } else {
        xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A4);
        for (unsigned int i = 0; adr != addrend_al; i += sizeof(uint32_t), adr += sizeof(uint32_t)) {
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, buf_get_u32(&albuff[i], 0, 32));
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A4));
            xtensa_queue_exec_ins(xtensa, XT_INS_S32I(xtensa, XT_REG_A3, XT_REG_A4, 0));
            xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, adr + sizeof(uint32_t));
            xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        }
    }
 
    res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (res == ERROR_OK) {
        bool prev_suppress = xtensa->suppress_dsr_errors;
        xtensa->suppress_dsr_errors = true;
        res = xtensa_core_status_check(target);
        if (xtensa->probe_lsddr32p == -1)
            xtensa->probe_lsddr32p = 1;
        xtensa->suppress_dsr_errors = prev_suppress;
    }
    if (res != ERROR_OK) {
        if (xtensa->probe_lsddr32p != 0) {
            /* Disable fast memory access instructions and retry before reporting an error */
            LOG_TARGET_INFO(target, "Disabling LDDR32.P/SDDR32.P");
            xtensa->probe_lsddr32p = 0;
            res = xtensa_write_memory(target, address, size, count, buffer);
        } else {
            LOG_TARGET_WARNING(target, "Failed writing %d bytes at address "TARGET_ADDR_FMT,
                count * size, address);
        }
    } else {
        /* Invalidate ICACHE, writeback DCACHE if present */
        bool issue_ihi = xtensa_is_icacheable(xtensa, address) &&
                         xtensa_region_ar_exec(target, addrstart_al, addrend_al);
        bool issue_dhwbi = xtensa_is_dcacheable(xtensa, address);
        LOG_TARGET_DEBUG(target, "Cache OPs: IHI %d, DHWBI %d", issue_ihi, issue_dhwbi);
        if (issue_ihi || issue_dhwbi) {
            uint32_t ilinesize = issue_ihi ?  xtensa->core_config->icache.line_size : UINT32_MAX;
            uint32_t dlinesize = issue_dhwbi ? xtensa->core_config->dcache.line_size : UINT32_MAX;
            uint32_t linesize = MIN(ilinesize, dlinesize);
            uint32_t off = 0;
            adr = addrstart_al;
 
            while ((adr + off) < addrend_al) {
                if (off == 0) {
                    /* Write start address to A3 */
                    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, adr);
                    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
                }
                if (issue_ihi)
                    xtensa_queue_exec_ins(xtensa, XT_INS_IHI(xtensa, XT_REG_A3, off));
                if (issue_dhwbi)
                    xtensa_queue_exec_ins(xtensa, XT_INS_DHWBI(xtensa, XT_REG_A3, off));
                off += linesize;
                if (off > 1020) {
                    /* IHI, DHWB have 8-bit immediate operands (0..1020) */
                    adr += off;
                    off = 0;
                }
            }
 
            /* Execute cache WB/INV instructions */
            res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
            if (res != ERROR_OK)
                LOG_TARGET_ERROR(target,
                    "Error queuing cache writeback/invaldate instruction(s): %d",
                    res);
            res = xtensa_core_status_check(target);
            if (res != ERROR_OK)
                LOG_TARGET_ERROR(target,
                    "Error issuing cache writeback/invaldate instruction(s): %d",
                    res);
        }
    }
    if (albuff != buffer)
        free(albuff);
 
    return res;
}
 
int xtensa_write_buffer(struct target *target, target_addr_t address, uint32_t count, const uint8_t *buffer)
{
    /* xtensa_write_memory can handle everything. Just pass on to that. */
    return xtensa_write_memory(target, address, 1, count, buffer);
}
 
int xtensa_checksum_memory(struct target *target, target_addr_t address, uint32_t count, uint32_t *checksum)
{
    LOG_WARNING("not implemented yet");
    return ERROR_FAIL;
}
 
int xtensa_poll(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    if (xtensa_dm_poll(&xtensa->dbg_mod) != ERROR_OK) {
        target->state = TARGET_UNKNOWN;
        return ERROR_TARGET_NOT_EXAMINED;
    }
 
    int res = xtensa_dm_power_status_read(&xtensa->dbg_mod, PWRSTAT_DEBUGWASRESET(xtensa) |
        PWRSTAT_COREWASRESET(xtensa));
    if (xtensa->dbg_mod.power_status.stat != xtensa->dbg_mod.power_status.stath)
        LOG_TARGET_DEBUG(target, "PWRSTAT: read 0x%08" PRIx32 ", clear 0x%08lx, reread 0x%08" PRIx32,
            xtensa->dbg_mod.power_status.stat,
            PWRSTAT_DEBUGWASRESET(xtensa) | PWRSTAT_COREWASRESET(xtensa),
            xtensa->dbg_mod.power_status.stath);
    if (res != ERROR_OK)
        return res;
 
    if (xtensa_dm_tap_was_reset(&xtensa->dbg_mod)) {
        LOG_TARGET_INFO(target, "Debug controller was reset.");
        res = xtensa_smpbreak_write(xtensa, xtensa->smp_break);
        if (res != ERROR_OK)
            return res;
    }
    if (xtensa_dm_core_was_reset(&xtensa->dbg_mod))
        LOG_TARGET_INFO(target, "Core was reset.");
    xtensa_dm_power_status_cache(&xtensa->dbg_mod);
    /* Enable JTAG, set reset if needed */
    res = xtensa_wakeup(target);
    if (res != ERROR_OK)
        return res;
 
    uint32_t prev_dsr = xtensa->dbg_mod.core_status.dsr;
    res = xtensa_dm_core_status_read(&xtensa->dbg_mod);
    if (res != ERROR_OK)
        return res;
    if (prev_dsr != xtensa->dbg_mod.core_status.dsr)
        LOG_TARGET_DEBUG(target,
            "DSR has changed: was 0x%08" PRIx32 " now 0x%08" PRIx32,
            prev_dsr,
            xtensa->dbg_mod.core_status.dsr);
    if (xtensa->dbg_mod.power_status.stath & PWRSTAT_COREWASRESET(xtensa)) {
        /* if RESET state is persitent  */
        target->state = TARGET_RESET;
    } else if (!xtensa_dm_is_powered(&xtensa->dbg_mod)) {
        LOG_TARGET_DEBUG(target, "not powered 0x%" PRIX32 "%ld",
            xtensa->dbg_mod.core_status.dsr,
            xtensa->dbg_mod.core_status.dsr & OCDDSR_STOPPED);
        target->state = TARGET_UNKNOWN;
        if (xtensa->come_online_probes_num == 0)
            target->examined = false;
        else
            xtensa->come_online_probes_num--;
    } else if (xtensa_is_stopped(target)) {
        if (target->state != TARGET_HALTED) {
            enum target_state oldstate = target->state;
            target->state = TARGET_HALTED;
            /* Examine why the target has been halted */
            target->debug_reason = DBG_REASON_DBGRQ;
            xtensa_fetch_all_regs(target);
            /* When setting debug reason DEBUGCAUSE events have the following
             * priorities: watchpoint == breakpoint > single step > debug interrupt. */
            /* Watchpoint and breakpoint events at the same time results in special
             * debug reason: DBG_REASON_WPTANDBKPT. */
            uint32_t halt_cause = xtensa_cause_get(target);
            /* TODO: Add handling of DBG_REASON_EXC_CATCH */
            if (halt_cause & DEBUGCAUSE_IC)
                target->debug_reason = DBG_REASON_SINGLESTEP;
            if (halt_cause & (DEBUGCAUSE_IB | DEBUGCAUSE_BN | DEBUGCAUSE_BI)) {
                if (halt_cause & DEBUGCAUSE_DB)
                    target->debug_reason = DBG_REASON_WPTANDBKPT;
                else
                    target->debug_reason = DBG_REASON_BREAKPOINT;
            } else if (halt_cause & DEBUGCAUSE_DB) {
                target->debug_reason = DBG_REASON_WATCHPOINT;
            }
            LOG_TARGET_DEBUG(target, "Target halted, pc=0x%08" PRIx32
                ", debug_reason=%08" PRIx32 ", oldstate=%08" PRIx32,
                xtensa_reg_get(target, XT_REG_IDX_PC),
                target->debug_reason,
                oldstate);
            LOG_TARGET_DEBUG(target, "Halt reason=0x%08" PRIX32 ", exc_cause=%" PRId32 ", dsr=0x%08" PRIx32,
                halt_cause,
                xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE),
                xtensa->dbg_mod.core_status.dsr);
            xtensa_dm_core_status_clear(
                &xtensa->dbg_mod,
                OCDDSR_DEBUGPENDBREAK | OCDDSR_DEBUGINTBREAK | OCDDSR_DEBUGPENDTRAX |
                OCDDSR_DEBUGINTTRAX |
                OCDDSR_DEBUGPENDHOST | OCDDSR_DEBUGINTHOST);
            if (xtensa->core_config->core_type == XT_NX) {
                /* Enable imprecise exceptions while in halted state */
                xtensa_reg_val_t ps = xtensa_reg_get(target, XT_REG_IDX_PS);
                xtensa_reg_val_t newps = ps & ~(XT_PS_DIEXC_MSK);
                xtensa_mark_register_dirty(xtensa, XT_REG_IDX_PS);
                LOG_TARGET_DEBUG(target, "Enabling PS.DIEXC: 0x%08x -> 0x%08x", ps, newps);
                xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, newps);
                xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
                xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa, XT_SR_PS, XT_REG_A3));
                res = xtensa_dm_queue_execute(&xtensa->dbg_mod);
                if (res != ERROR_OK) {
                    LOG_TARGET_ERROR(target, "Failed to write PS.DIEXC (%d)!", res);
                    return res;
                }
                xtensa_core_status_check(target);
            }
        }
    } else {
        target->debug_reason = DBG_REASON_NOTHALTED;
        if (target->state != TARGET_RUNNING && target->state != TARGET_DEBUG_RUNNING) {
            target->state = TARGET_RUNNING;
            target->debug_reason = DBG_REASON_NOTHALTED;
        }
    }
    if (xtensa->trace_active) {
        /* Detect if tracing was active but has stopped. */
        struct xtensa_trace_status trace_status;
        res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
        if (res == ERROR_OK) {
            if (!(trace_status.stat & TRAXSTAT_TRACT)) {
                LOG_INFO("Detected end of trace.");
                if (trace_status.stat & TRAXSTAT_PCMTG)
                    LOG_TARGET_INFO(target, "Trace stop triggered by PC match");
                if (trace_status.stat & TRAXSTAT_PTITG)
                    LOG_TARGET_INFO(target, "Trace stop triggered by Processor Trigger Input");
                if (trace_status.stat & TRAXSTAT_CTITG)
                    LOG_TARGET_INFO(target, "Trace stop triggered by Cross-trigger Input");
                xtensa->trace_active = false;
            }
        }
    }
    return ERROR_OK;
}
 
static int xtensa_update_instruction(struct target *target, target_addr_t address, uint32_t size, const uint8_t *buffer)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int issue_ihi = xtensa_is_icacheable(xtensa, address) &&
                             xtensa_region_ar_exec(target, address, address + size);
    unsigned int issue_dhwbi = xtensa_is_dcacheable(xtensa, address);
    uint32_t icache_line_size = issue_ihi ? xtensa->core_config->icache.line_size : UINT32_MAX;
    uint32_t dcache_line_size = issue_dhwbi ? xtensa->core_config->dcache.line_size : UINT32_MAX;
    unsigned int same_ic_line = ((address & (icache_line_size - 1)) + size) <= icache_line_size;
    unsigned int same_dc_line = ((address & (dcache_line_size - 1)) + size) <= dcache_line_size;
    int ret;
 
    if (size > icache_line_size)
        return ERROR_FAIL;
 
    if (issue_ihi || issue_dhwbi) {
        /* We're going to use A3 here */
        xtensa_mark_register_dirty(xtensa, XT_REG_IDX_A3);
 
        /* Write start address to A3 and invalidate */
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, address);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        LOG_TARGET_DEBUG(target, "IHI %d, DHWBI %d for address " TARGET_ADDR_FMT,
            issue_ihi, issue_dhwbi, address);
        if (issue_dhwbi) {
            xtensa_queue_exec_ins(xtensa, XT_INS_DHWBI(xtensa, XT_REG_A3, 0));
            if (!same_dc_line) {
                LOG_TARGET_DEBUG(target,
                    "DHWBI second dcache line for address "TARGET_ADDR_FMT,
                    address + 4);
                xtensa_queue_exec_ins(xtensa, XT_INS_DHWBI(xtensa, XT_REG_A3, 4));
            }
        }
        if (issue_ihi) {
            xtensa_queue_exec_ins(xtensa, XT_INS_IHI(xtensa, XT_REG_A3, 0));
            if (!same_ic_line) {
                LOG_TARGET_DEBUG(target,
                    "IHI second icache line for address "TARGET_ADDR_FMT,
                    address + 4);
                xtensa_queue_exec_ins(xtensa, XT_INS_IHI(xtensa, XT_REG_A3, 4));
            }
        }
 
        /* Execute invalidate instructions */
        ret = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        xtensa_core_status_check(target);
        if (ret != ERROR_OK) {
            LOG_ERROR("Error issuing cache invaldate instruction(s): %d", ret);
            return ret;
        }
    }
 
    /* Write new instructions to memory */
    ret = target_write_buffer(target, address, size, buffer);
    if (ret != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Error writing instruction to memory: %d", ret);
        return ret;
    }
 
    if (issue_dhwbi) {
        /* Flush dcache so instruction propagates.  A3 may be corrupted during memory write */
        xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, address);
        xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
        xtensa_queue_exec_ins(xtensa, XT_INS_DHWB(xtensa, XT_REG_A3, 0));
        LOG_DEBUG("DHWB dcache line for address "TARGET_ADDR_FMT, address);
        if (!same_dc_line) {
            LOG_TARGET_DEBUG(target, "DHWB second dcache line for address "TARGET_ADDR_FMT, address + 4);
            xtensa_queue_exec_ins(xtensa, XT_INS_DHWB(xtensa, XT_REG_A3, 4));
        }
 
        /* Execute invalidate instructions */
        ret = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        xtensa_core_status_check(target);
    }
 
    /* TODO: Handle L2 cache if present */
    return ret;
}
 
static int xtensa_sw_breakpoint_add(struct target *target,
    struct breakpoint *breakpoint,
    struct xtensa_sw_breakpoint *sw_bp)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int ret = target_read_buffer(target, breakpoint->address, XT_ISNS_SZ_MAX, sw_bp->insn);
    if (ret != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to read original instruction (%d)!", ret);
        return ret;
    }
 
    sw_bp->insn_sz = MIN(XT_ISNS_SZ_MAX, breakpoint->length);
    sw_bp->oocd_bp = breakpoint;
 
    uint32_t break_insn = sw_bp->insn_sz == XT_ISNS_SZ_MAX ? XT_INS_BREAK(xtensa, 0, 0) : XT_INS_BREAKN(xtensa, 0);
 
    /* Underlying memory write will convert instruction endianness, don't do that here */
    ret = xtensa_update_instruction(target, breakpoint->address, sw_bp->insn_sz, (uint8_t *)&break_insn);
    if (ret != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to write breakpoint instruction (%d)!", ret);
        return ret;
    }
 
    return ERROR_OK;
}
 
static int xtensa_sw_breakpoint_remove(struct target *target, struct xtensa_sw_breakpoint *sw_bp)
{
    int ret = xtensa_update_instruction(target, sw_bp->oocd_bp->address, sw_bp->insn_sz, sw_bp->insn);
    if (ret != ERROR_OK) {
        LOG_TARGET_ERROR(target, "Failed to write insn (%d)!", ret);
        return ret;
    }
    sw_bp->oocd_bp = NULL;
    return ERROR_OK;
}
 
int xtensa_breakpoint_add(struct target *target, struct breakpoint *breakpoint)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int slot;
 
    if (breakpoint->type == BKPT_SOFT) {
        for (slot = 0; slot < XT_SW_BREAKPOINTS_MAX_NUM; slot++) {
            if (!xtensa->sw_brps[slot].oocd_bp ||
                xtensa->sw_brps[slot].oocd_bp == breakpoint)
                break;
        }
        if (slot == XT_SW_BREAKPOINTS_MAX_NUM) {
            LOG_TARGET_WARNING(target, "No free slots to add SW breakpoint!");
            return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
        int ret = xtensa_sw_breakpoint_add(target, breakpoint, &xtensa->sw_brps[slot]);
        if (ret != ERROR_OK) {
            LOG_TARGET_ERROR(target, "Failed to add SW breakpoint!");
            return ret;
        }
        LOG_TARGET_DEBUG(target, "placed SW breakpoint %u @ " TARGET_ADDR_FMT,
            slot,
            breakpoint->address);
        return ERROR_OK;
    }
 
    for (slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
        if (!xtensa->hw_brps[slot] || xtensa->hw_brps[slot] == breakpoint)
            break;
    }
    if (slot == xtensa->core_config->debug.ibreaks_num) {
        LOG_TARGET_ERROR(target, "No free slots to add HW breakpoint!");
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
 
    xtensa->hw_brps[slot] = breakpoint;
    /* We will actually write the breakpoints when we resume the target. */
    LOG_TARGET_DEBUG(target, "placed HW breakpoint %u @ " TARGET_ADDR_FMT,
        slot,
        breakpoint->address);
 
    return ERROR_OK;
}
 
int xtensa_breakpoint_remove(struct target *target, struct breakpoint *breakpoint)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int slot;
 
    if (breakpoint->type == BKPT_SOFT) {
        for (slot = 0; slot < XT_SW_BREAKPOINTS_MAX_NUM; slot++) {
            if (xtensa->sw_brps[slot].oocd_bp && xtensa->sw_brps[slot].oocd_bp == breakpoint)
                break;
        }
        if (slot == XT_SW_BREAKPOINTS_MAX_NUM) {
            LOG_TARGET_WARNING(target, "Max SW breakpoints slot reached, slot=%u!", slot);
            return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
        int ret = xtensa_sw_breakpoint_remove(target, &xtensa->sw_brps[slot]);
        if (ret != ERROR_OK) {
            LOG_TARGET_ERROR(target, "Failed to remove SW breakpoint (%d)!", ret);
            return ret;
        }
        LOG_TARGET_DEBUG(target, "cleared SW breakpoint %u @ " TARGET_ADDR_FMT, slot, breakpoint->address);
        return ERROR_OK;
    }
 
    for (slot = 0; slot < xtensa->core_config->debug.ibreaks_num; slot++) {
        if (xtensa->hw_brps[slot] == breakpoint)
            break;
    }
    if (slot == xtensa->core_config->debug.ibreaks_num) {
        LOG_TARGET_ERROR(target, "HW breakpoint not found!");
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
    xtensa->hw_brps[slot] = NULL;
    if (xtensa->core_config->core_type == XT_NX)
        xtensa_reg_set(target, xtensa->nx_reg_idx[XT_NX_REG_IDX_IBREAKC0] + slot, 0);
    LOG_TARGET_DEBUG(target, "cleared HW breakpoint %u @ " TARGET_ADDR_FMT, slot, breakpoint->address);
    return ERROR_OK;
}
 
int xtensa_watchpoint_add(struct target *target, struct watchpoint *watchpoint)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int slot;
    xtensa_reg_val_t dbreakcval;
 
    if (target->state != TARGET_HALTED) {
        LOG_TARGET_ERROR(target, "not halted");
        return ERROR_TARGET_NOT_HALTED;
    }
 
    if (watchpoint->mask != WATCHPOINT_IGNORE_DATA_VALUE_MASK) {
        LOG_TARGET_ERROR(target, "watchpoint value masks not supported");
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
 
    for (slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
        if (!xtensa->hw_wps[slot] || xtensa->hw_wps[slot] == watchpoint)
            break;
    }
    if (slot == xtensa->core_config->debug.dbreaks_num) {
        LOG_TARGET_WARNING(target, "No free slots to add HW watchpoint!");
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
 
    /* Figure out value for dbreakc5..0
     * It's basically 0x3F with an incremental bit removed from the LSB for each extra length power of 2. */
    if (watchpoint->length < 1 || watchpoint->length > 64 ||
        !IS_PWR_OF_2(watchpoint->length) ||
        !IS_ALIGNED(watchpoint->address, watchpoint->length)) {
        LOG_TARGET_WARNING(
            target,
            "Watchpoint with length %d on address " TARGET_ADDR_FMT
            " not supported by hardware.",
            watchpoint->length,
            watchpoint->address);
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
    dbreakcval = ALIGN_DOWN(0x3F, watchpoint->length);
 
    if (watchpoint->rw == WPT_READ)
        dbreakcval |= BIT(30);
    if (watchpoint->rw == WPT_WRITE)
        dbreakcval |= BIT(31);
    if (watchpoint->rw == WPT_ACCESS)
        dbreakcval |= BIT(30) | BIT(31);
 
    /* Write DBREAKA[slot] and DBCREAKC[slot] */
    xtensa_reg_set(target, XT_REG_IDX_DBREAKA0 + slot, watchpoint->address);
    xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, dbreakcval);
    xtensa->hw_wps[slot] = watchpoint;
    LOG_TARGET_DEBUG(target, "placed HW watchpoint @ " TARGET_ADDR_FMT,
        watchpoint->address);
    return ERROR_OK;
}
 
int xtensa_watchpoint_remove(struct target *target, struct watchpoint *watchpoint)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    unsigned int slot;
 
    for (slot = 0; slot < xtensa->core_config->debug.dbreaks_num; slot++) {
        if (xtensa->hw_wps[slot] == watchpoint)
            break;
    }
    if (slot == xtensa->core_config->debug.dbreaks_num) {
        LOG_TARGET_WARNING(target, "HW watchpoint " TARGET_ADDR_FMT " not found!", watchpoint->address);
        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
    }
    xtensa_reg_set(target, XT_REG_IDX_DBREAKC0 + slot, 0);
    xtensa->hw_wps[slot] = NULL;
    LOG_TARGET_DEBUG(target, "cleared HW watchpoint @ " TARGET_ADDR_FMT,
        watchpoint->address);
    return ERROR_OK;
}
 
int xtensa_start_algorithm(struct target *target,
    int num_mem_params, struct mem_param *mem_params,
    int num_reg_params, struct reg_param *reg_params,
    target_addr_t entry_point, target_addr_t exit_point,
    void *arch_info)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct xtensa_algorithm *algorithm_info = arch_info;
    int retval = ERROR_OK;
    bool usr_ps = false;
    uint32_t newps;
 
    /* NOTE: xtensa_run_algorithm requires that each algorithm uses a software breakpoint
     * at the exit point */
 
    if (target->state != TARGET_HALTED) {
        LOG_WARNING("Target not halted!");
        return ERROR_TARGET_NOT_HALTED;
    }
 
    retval = xtensa_write_dirty_registers(target);
    if (retval != ERROR_OK) {
        LOG_ERROR("Failed to write dirty regs (%d)", retval);
        return retval;
    }
 
    for (unsigned int i = 0; i < xtensa->core_cache->num_regs; i++) {
        struct reg *reg = &xtensa->core_cache->reg_list[i];
        buf_cpy(reg->value, xtensa->algo_context_backup[i], reg->size);
    }
    /* save debug reason, it will be changed */
    if (!algorithm_info) {
        LOG_ERROR("BUG: arch_info not specified");
        return ERROR_FAIL;
    }
    algorithm_info->ctx_debug_reason = target->debug_reason;
    if (xtensa->core_config->core_type == XT_LX) {
        /* save PS and set to debug_level - 1 */
        algorithm_info->ctx_ps = xtensa_reg_get(target, xtensa->eps_dbglevel_idx);
        newps = (algorithm_info->ctx_ps & ~0xf) | (xtensa->core_config->debug.irq_level - 1);
        xtensa_reg_set(target, xtensa->eps_dbglevel_idx, newps);
    }
    /* write mem params */
    for (int i = 0; i < num_mem_params; i++) {
        if (mem_params[i].direction != PARAM_IN) {
            retval = target_write_buffer(target, mem_params[i].address,
                mem_params[i].size,
                mem_params[i].value);
            if (retval != ERROR_OK)
                return retval;
        }
    }
    /* write reg params */
    for (int i = 0; i < num_reg_params; i++) {
        if (reg_params[i].size > 32) {
            LOG_ERROR("BUG: not supported register size (%d)", reg_params[i].size);
            return ERROR_FAIL;
        }
        struct reg *reg = register_get_by_name(xtensa->core_cache, reg_params[i].reg_name, 0);
        if (!reg) {
            LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
            return ERROR_FAIL;
        }
        if (reg->size != reg_params[i].size) {
            LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
            return ERROR_FAIL;
        }
        if (memcmp(reg_params[i].reg_name, "ps", 3)) {
            usr_ps = true;
        } else if (xtensa->core_config->core_type == XT_LX) {
            unsigned int reg_id = xtensa->eps_dbglevel_idx;
            assert(reg_id < xtensa->core_cache->num_regs && "Attempt to access non-existing reg!");
            reg = &xtensa->core_cache->reg_list[reg_id];
        }
        xtensa_reg_set_value(reg, buf_get_u32(reg_params[i].value, 0, reg->size));
        reg->valid = 1;
    }
    /* ignore custom core mode if custom PS value is specified */
    if (!usr_ps && xtensa->core_config->core_type == XT_LX) {
        unsigned int eps_reg_idx = xtensa->eps_dbglevel_idx;
        xtensa_reg_val_t ps = xtensa_reg_get(target, eps_reg_idx);
        enum xtensa_mode core_mode = XT_PS_RING_GET(ps);
        if (algorithm_info->core_mode != XT_MODE_ANY && algorithm_info->core_mode != core_mode) {
            LOG_DEBUG("setting core_mode: 0x%x", algorithm_info->core_mode);
            xtensa_reg_val_t new_ps = (ps & ~XT_PS_RING_MSK) | XT_PS_RING(algorithm_info->core_mode);
            /* save previous core mode */
            /* TODO: core_mode is not restored for now. Can be added to the end of wait_algorithm */
            algorithm_info->core_mode = core_mode;
            xtensa_reg_set(target, eps_reg_idx, new_ps);
            xtensa->core_cache->reg_list[eps_reg_idx].valid = 1;
        }
    }
 
    return xtensa_resume(target, false, entry_point, true, true);
}
 
int xtensa_wait_algorithm(struct target *target,
    int num_mem_params, struct mem_param *mem_params,
    int num_reg_params, struct reg_param *reg_params,
    target_addr_t exit_point, unsigned int timeout_ms,
    void *arch_info)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct xtensa_algorithm *algorithm_info = arch_info;
    int retval = ERROR_OK;
    xtensa_reg_val_t pc;
 
    /* NOTE: xtensa_run_algorithm requires that each algorithm uses a software breakpoint
     * at the exit point */
 
    retval = target_wait_state(target, TARGET_HALTED, timeout_ms);
    /* If the target fails to halt due to the breakpoint, force a halt */
    if (retval != ERROR_OK || target->state != TARGET_HALTED) {
        retval = target_halt(target);
        if (retval != ERROR_OK)
            return retval;
        retval = target_wait_state(target, TARGET_HALTED, 500);
        if (retval != ERROR_OK)
            return retval;
        LOG_TARGET_ERROR(target, "not halted %d, pc 0x%" PRIx32 ", ps 0x%" PRIx32, retval,
            xtensa_reg_get(target, XT_REG_IDX_PC),
            xtensa_reg_get(target, (xtensa->core_config->core_type == XT_LX) ?
                xtensa->eps_dbglevel_idx : XT_REG_IDX_PS));
        return ERROR_TARGET_TIMEOUT;
    }
    pc = xtensa_reg_get(target, XT_REG_IDX_PC);
    if (exit_point && pc != exit_point) {
        LOG_ERROR("failed algorithm halted at 0x%" PRIx32 ", expected " TARGET_ADDR_FMT, pc, exit_point);
        return ERROR_TARGET_TIMEOUT;
    }
    /* Copy core register values to reg_params[] */
    for (int i = 0; i < num_reg_params; i++) {
        if (reg_params[i].direction != PARAM_OUT) {
            struct reg *reg = register_get_by_name(xtensa->core_cache, reg_params[i].reg_name, 0);
            if (!reg) {
                LOG_ERROR("BUG: register '%s' not found", reg_params[i].reg_name);
                return ERROR_FAIL;
            }
            if (reg->size != reg_params[i].size) {
                LOG_ERROR("BUG: register '%s' size doesn't match reg_params[i].size", reg_params[i].reg_name);
                return ERROR_FAIL;
            }
            buf_set_u32(reg_params[i].value, 0, 32, xtensa_reg_get_value(reg));
        }
    }
    /* Read memory values to mem_params */
    LOG_DEBUG("Read mem params");
    for (int i = 0; i < num_mem_params; i++) {
        LOG_DEBUG("Check mem param @ " TARGET_ADDR_FMT, mem_params[i].address);
        if (mem_params[i].direction != PARAM_OUT) {
            LOG_DEBUG("Read mem param @ " TARGET_ADDR_FMT, mem_params[i].address);
            retval = target_read_buffer(target, mem_params[i].address, mem_params[i].size, mem_params[i].value);
            if (retval != ERROR_OK)
                return retval;
        }
    }
 
    /* avoid gdb keep_alive warning */
    keep_alive();
 
    for (int i = xtensa->core_cache->num_regs - 1; i >= 0; i--) {
        struct reg *reg = &xtensa->core_cache->reg_list[i];
        if (i == XT_REG_IDX_PS) {
            continue;   /* restore mapped reg number of PS depends on NDEBUGLEVEL */
        } else if (i == XT_REG_IDX_DEBUGCAUSE) {
            /*FIXME: restoring DEBUGCAUSE causes exception when executing corresponding
            * instruction in DIR */
            LOG_DEBUG("Skip restoring register %s: 0x%8.8" PRIx32 " -> 0x%8.8" PRIx32,
                xtensa->core_cache->reg_list[i].name,
                buf_get_u32(reg->value, 0, 32),
                buf_get_u32(xtensa->algo_context_backup[i], 0, 32));
            buf_cpy(xtensa->algo_context_backup[i], reg->value, reg->size);
            xtensa->core_cache->reg_list[i].dirty = 0;
            xtensa->core_cache->reg_list[i].valid = 0;
        } else if (memcmp(xtensa->algo_context_backup[i], reg->value, reg->size / 8)) {
            if (reg->size <= 32) {
                LOG_DEBUG("restoring register %s: 0x%8.8" PRIx32 " -> 0x%8.8" PRIx32,
                    xtensa->core_cache->reg_list[i].name,
                    buf_get_u32(reg->value, 0, reg->size),
                    buf_get_u32(xtensa->algo_context_backup[i], 0, reg->size));
            } else if (reg->size <= 64) {
                LOG_DEBUG("restoring register %s: 0x%8.8" PRIx64 " -> 0x%8.8" PRIx64,
                    xtensa->core_cache->reg_list[i].name,
                    buf_get_u64(reg->value, 0, reg->size),
                    buf_get_u64(xtensa->algo_context_backup[i], 0, reg->size));
            } else {
                LOG_DEBUG("restoring register %s %u-bits", xtensa->core_cache->reg_list[i].name, reg->size);
            }
            buf_cpy(xtensa->algo_context_backup[i], reg->value, reg->size);
            xtensa->core_cache->reg_list[i].dirty = 1;
            xtensa->core_cache->reg_list[i].valid = 1;
        }
    }
    target->debug_reason = algorithm_info->ctx_debug_reason;
    if (xtensa->core_config->core_type == XT_LX)
        xtensa_reg_set(target, xtensa->eps_dbglevel_idx, algorithm_info->ctx_ps);
 
    retval = xtensa_write_dirty_registers(target);
    if (retval != ERROR_OK)
        LOG_ERROR("Failed to write dirty regs (%d)!", retval);
 
    return retval;
}
 
int xtensa_run_algorithm(struct target *target,
    int num_mem_params, struct mem_param *mem_params,
    int num_reg_params, struct reg_param *reg_params,
    target_addr_t entry_point, target_addr_t exit_point,
    unsigned int timeout_ms, void *arch_info)
{
    int retval = xtensa_start_algorithm(target,
        num_mem_params, mem_params,
        num_reg_params, reg_params,
        entry_point, exit_point,
        arch_info);
 
    if (retval == ERROR_OK) {
        retval = xtensa_wait_algorithm(target,
            num_mem_params, mem_params,
            num_reg_params, reg_params,
            exit_point, timeout_ms,
            arch_info);
    }
 
    return retval;
}
 
static int xtensa_build_reg_cache(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
    unsigned int last_dbreg_num = 0;
 
    if (xtensa->core_regs_num + xtensa->num_optregs != xtensa->total_regs_num)
        LOG_TARGET_WARNING(target, "Register count MISMATCH: %d core regs, %d extended regs; %d expected",
            xtensa->core_regs_num, xtensa->num_optregs, xtensa->total_regs_num);
 
    struct reg_cache *reg_cache = calloc(1, sizeof(struct reg_cache));
 
    if (!reg_cache) {
        LOG_ERROR("Failed to alloc reg cache!");
        return ERROR_FAIL;
    }
    reg_cache->name = "Xtensa registers";
    reg_cache->next = NULL;
    /* Init reglist */
    unsigned int reg_list_size = XT_NUM_REGS + xtensa->num_optregs;
    struct reg *reg_list = calloc(reg_list_size, sizeof(struct reg));
    if (!reg_list) {
        LOG_ERROR("Failed to alloc reg list!");
        goto fail;
    }
    xtensa->dbregs_num = 0;
    unsigned int didx = 0;
    for (unsigned int whichlist = 0; whichlist < 2; whichlist++) {
        struct xtensa_reg_desc *rlist = (whichlist == 0) ? xtensa_regs : xtensa->optregs;
        unsigned int listsize = (whichlist == 0) ? XT_NUM_REGS : xtensa->num_optregs;
        for (unsigned int i = 0; i < listsize; i++, didx++) {
            reg_list[didx].exist = rlist[i].exist;
            reg_list[didx].name = rlist[i].name;
            reg_list[didx].size = 32;
            reg_list[didx].value = calloc(1, 4 /*XT_REG_LEN*/); /* make Clang Static Analyzer happy */
            if (!reg_list[didx].value) {
                LOG_ERROR("Failed to alloc reg list value!");
                goto fail;
            }
            reg_list[didx].dirty = false;
            reg_list[didx].valid = false;
            reg_list[didx].type = &xtensa_reg_type;
            reg_list[didx].arch_info = xtensa;
            if (rlist[i].exist && (rlist[i].dbreg_num > last_dbreg_num))
                last_dbreg_num = rlist[i].dbreg_num;
 
            if (xtensa_extra_debug_log) {
                LOG_TARGET_DEBUG(target,
                    "POPULATE %-16s list %d exist %d, idx %d, type %d, dbreg_num 0x%04x",
                    reg_list[didx].name,
                    whichlist,
                    reg_list[didx].exist,
                    didx,
                    rlist[i].type,
                    rlist[i].dbreg_num);
            }
        }
    }
 
    xtensa->dbregs_num = last_dbreg_num + 1;
    reg_cache->reg_list = reg_list;
    reg_cache->num_regs = reg_list_size;
 
    LOG_TARGET_DEBUG(target, "xtensa->total_regs_num %d reg_list_size %d xtensa->dbregs_num %d",
        xtensa->total_regs_num, reg_list_size, xtensa->dbregs_num);
 
    /* Construct empty-register list for handling unknown register requests */
    xtensa->empty_regs = calloc(xtensa->dbregs_num, sizeof(struct reg));
    if (!xtensa->empty_regs) {
        LOG_TARGET_ERROR(target, "Out of memory");
        goto fail;
    }
    for (unsigned int i = 0; i < xtensa->dbregs_num; i++) {
        xtensa->empty_regs[i].name = calloc(8, sizeof(char));
        if (!xtensa->empty_regs[i].name) {
            LOG_TARGET_ERROR(target, "Out of memory");
            goto fail;
        }
        sprintf((char *)xtensa->empty_regs[i].name, "?0x%04x", i & 0x0000FFFF);
        xtensa->empty_regs[i].size = 32;
        xtensa->empty_regs[i].type = &xtensa_reg_type;
        xtensa->empty_regs[i].value = calloc(1, 4 /*XT_REG_LEN*/);  /* make Clang Static Analyzer happy */
        if (!xtensa->empty_regs[i].value) {
            LOG_ERROR("Failed to alloc empty reg list value!");
            goto fail;
        }
        xtensa->empty_regs[i].arch_info = xtensa;
    }
 
    /* Construct contiguous register list from contiguous descriptor list */
    if (xtensa->regmap_contiguous && xtensa->contiguous_regs_desc) {
        xtensa->contiguous_regs_list = calloc(xtensa->total_regs_num, sizeof(struct reg *));
        if (!xtensa->contiguous_regs_list) {
            LOG_TARGET_ERROR(target, "Out of memory");
            goto fail;
        }
        for (unsigned int i = 0; i < xtensa->total_regs_num; i++) {
            unsigned int j;
            for (j = 0; j < reg_cache->num_regs; j++) {
                if (!strcmp(reg_cache->reg_list[j].name, xtensa->contiguous_regs_desc[i]->name)) {
                    /*  Register number field is not filled above.
                        Here we are assigning the corresponding index from the contiguous reg list.
                        These indexes are in the same order with gdb g-packet request/response.
                        Some more changes may be required for sparse reg lists.
                    */
                    reg_cache->reg_list[j].number = i;
                    xtensa->contiguous_regs_list[i] = &(reg_cache->reg_list[j]);
                    LOG_TARGET_DEBUG(target,
                        "POPULATE contiguous regs list: %-16s, dbreg_num 0x%04x",
                        xtensa->contiguous_regs_list[i]->name,
                        xtensa->contiguous_regs_desc[i]->dbreg_num);
                    break;
                }
            }
            if (j == reg_cache->num_regs)
                LOG_TARGET_WARNING(target, "contiguous register %s not found",
                    xtensa->contiguous_regs_desc[i]->name);
        }
    }
 
    xtensa->algo_context_backup = calloc(reg_cache->num_regs, sizeof(void *));
    if (!xtensa->algo_context_backup) {
        LOG_ERROR("Failed to alloc mem for algorithm context backup!");
        goto fail;
    }
    for (unsigned int i = 0; i < reg_cache->num_regs; i++) {
        struct reg *reg = &reg_cache->reg_list[i];
        xtensa->algo_context_backup[i] = calloc(1, reg->size / 8);
        if (!xtensa->algo_context_backup[i]) {
            LOG_ERROR("Failed to alloc mem for algorithm context!");
            goto fail;
        }
    }
    xtensa->core_cache = reg_cache;
    if (cache_p)
        *cache_p = reg_cache;
    return ERROR_OK;
 
fail:
    if (reg_list) {
        for (unsigned int i = 0; i < reg_list_size; i++)
            free(reg_list[i].value);
        free(reg_list);
    }
    if (xtensa->empty_regs) {
        for (unsigned int i = 0; i < xtensa->dbregs_num; i++) {
            free((void *)xtensa->empty_regs[i].name);
            free(xtensa->empty_regs[i].value);
        }
        free(xtensa->empty_regs);
    }
    if (xtensa->algo_context_backup) {
        for (unsigned int i = 0; i < reg_cache->num_regs; i++)
            free(xtensa->algo_context_backup[i]);
        free(xtensa->algo_context_backup);
    }
    free(reg_cache);
 
    return ERROR_FAIL;
}
 
static int32_t xtensa_gdbqc_parse_exec_tie_ops(struct target *target, char *opstr)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    int32_t status = ERROR_COMMAND_ARGUMENT_INVALID;
    /* Process op[] list */
    while (opstr && (*opstr == ':')) {
        uint8_t ops[32];
        unsigned int oplen = strtoul(opstr + 1, &opstr, 16);
        if (oplen > 32) {
            LOG_TARGET_ERROR(target, "TIE access instruction too long (%d)\n", oplen);
            break;
        }
        unsigned int i = 0;
        while ((i < oplen) && opstr && (*opstr == ':'))
            ops[i++] = strtoul(opstr + 1, &opstr, 16);
        if (i != oplen) {
            LOG_TARGET_ERROR(target, "TIE access instruction malformed (%d)\n", i);
            break;
        }
 
        char insn_buf[128];
        sprintf(insn_buf, "Exec %d-byte TIE sequence: ", oplen);
        for (i = 0; i < oplen; i++)
            sprintf(insn_buf + strlen(insn_buf), "%02x:", ops[i]);
        LOG_TARGET_DEBUG(target, "%s", insn_buf);
        xtensa_queue_exec_ins_wide(xtensa, ops, oplen); /* Handles endian-swap */
        status = ERROR_OK;
    }
    return status;
}
 
static int xtensa_gdbqc_qxtreg(struct target *target, const char *packet, char **response_p)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    bool iswrite = (packet[0] == 'Q');
    enum xtensa_qerr_e error;
 
    /* Read/write TIE register.  Requires spill location.
     * qxtreg<num>:<len>:<oplen>:<op[0]>:<...>[:<oplen>:<op[0]>:<...>]
     * Qxtreg<num>:<len>:<oplen>:<op[0]>:<...>[:<oplen>:<op[0]>:<...>]=<value>
     */
    if (!(xtensa->spill_buf)) {
        LOG_ERROR("Spill location not specified. Try 'target remote <host>:3333 &spill_location0'");
        error = XT_QERR_FAIL;
        goto xtensa_gdbqc_qxtreg_fail;
    }
 
    char *delim;
    uint32_t regnum = strtoul(packet + 6, &delim, 16);
    if (*delim != ':') {
        LOG_ERROR("Malformed qxtreg packet");
        error = XT_QERR_INVAL;
        goto xtensa_gdbqc_qxtreg_fail;
    }
    uint32_t reglen = strtoul(delim + 1, &delim, 16);
    if (*delim != ':') {
        LOG_ERROR("Malformed qxtreg packet");
        error = XT_QERR_INVAL;
        goto xtensa_gdbqc_qxtreg_fail;
    }
    uint8_t regbuf[XT_QUERYPKT_RESP_MAX];
    memset(regbuf, 0, XT_QUERYPKT_RESP_MAX);
    LOG_DEBUG("TIE reg 0x%08" PRIx32 " %s (%d bytes)", regnum, iswrite ? "write" : "read", reglen);
    if (reglen * 2 + 1 > XT_QUERYPKT_RESP_MAX) {
        LOG_ERROR("TIE register too large");
        error = XT_QERR_MEM;
        goto xtensa_gdbqc_qxtreg_fail;
    }
 
    /* (1) Save spill memory, (1.5) [if write then store value to spill location],
     * (2) read old a4, (3) write spill address to a4.
     * NOTE: ensure a4 is restored properly by all error handling logic
     */
    unsigned int memop_size = (xtensa->spill_loc & 3) ? 1 : 4;
    int status = xtensa_read_memory(target, xtensa->spill_loc, memop_size,
        xtensa->spill_bytes / memop_size, xtensa->spill_buf);
    if (status != ERROR_OK) {
        LOG_ERROR("Spill memory save");
        error = XT_QERR_MEM;
        goto xtensa_gdbqc_qxtreg_fail;
    }
    if (iswrite) {
        /* Extract value and store in spill memory */
        unsigned int b = 0;
        char *valbuf = strchr(delim, '=');
        if (!(valbuf && (*valbuf == '='))) {
            LOG_ERROR("Malformed Qxtreg packet");
            error = XT_QERR_INVAL;
            goto xtensa_gdbqc_qxtreg_fail;
        }
        valbuf++;
        while (*valbuf && *(valbuf + 1)) {
            char bytestr[3] = { 0, 0, 0 };
            strncpy(bytestr, valbuf, 2);
            regbuf[b++] = strtoul(bytestr, NULL, 16);
            valbuf += 2;
        }
        if (b != reglen) {
            LOG_ERROR("Malformed Qxtreg packet");
            error = XT_QERR_INVAL;
            goto xtensa_gdbqc_qxtreg_fail;
        }
        status = xtensa_write_memory(target, xtensa->spill_loc, memop_size,
            reglen / memop_size, regbuf);
        if (status != ERROR_OK) {
            LOG_ERROR("TIE value store");
            error = XT_QERR_MEM;
            goto xtensa_gdbqc_qxtreg_fail;
        }
    }
    xtensa_reg_val_t orig_a4 = xtensa_reg_get(target, XT_REG_IDX_A4);
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, xtensa->spill_loc);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A4));
 
    int32_t tieop_status = xtensa_gdbqc_parse_exec_tie_ops(target, delim);
 
    /* Restore a4 but not yet spill memory.  Execute it all... */
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, orig_a4);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A4));
    status = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (status != ERROR_OK) {
        LOG_TARGET_ERROR(target, "TIE queue execute: %d\n", status);
        tieop_status = status;
    }
    status = xtensa_core_status_check(target);
    if (status != ERROR_OK) {
        LOG_TARGET_ERROR(target, "TIE instr execute: %d\n", status);
        tieop_status = status;
    }
 
    if (tieop_status == ERROR_OK) {
        if (iswrite) {
            /* TIE write succeeded; send OK */
            strcpy(*response_p, "OK");
        } else {
            /* TIE read succeeded; copy result from spill memory */
            status = xtensa_read_memory(target, xtensa->spill_loc, memop_size, reglen, regbuf);
            if (status != ERROR_OK) {
                LOG_TARGET_ERROR(target, "TIE result read");
                tieop_status = status;
            }
            unsigned int i;
            for (i = 0; i < reglen; i++)
                sprintf(*response_p + 2 * i, "%02x", regbuf[i]);
            *(*response_p + 2 * i) = '\0';
            LOG_TARGET_DEBUG(target, "TIE response: %s", *response_p);
        }
    }
 
    /* Restore spill memory first, then report any previous errors */
    status = xtensa_write_memory(target, xtensa->spill_loc, memop_size,
        xtensa->spill_bytes / memop_size, xtensa->spill_buf);
    if (status != ERROR_OK) {
        LOG_ERROR("Spill memory restore");
        error = XT_QERR_MEM;
        goto xtensa_gdbqc_qxtreg_fail;
    }
    if (tieop_status != ERROR_OK) {
        LOG_ERROR("TIE execution");
        error = XT_QERR_FAIL;
        goto xtensa_gdbqc_qxtreg_fail;
    }
    return ERROR_OK;
 
xtensa_gdbqc_qxtreg_fail:
    strcpy(*response_p, xt_qerr[error].chrval);
    return xt_qerr[error].intval;
}
 
int xtensa_gdb_query_custom(struct target *target, const char *packet, char **response_p)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    enum xtensa_qerr_e error;
    if (!packet || !response_p) {
        LOG_TARGET_ERROR(target, "invalid parameter: packet %p response_p %p", packet, response_p);
        return ERROR_FAIL;
    }
 
    *response_p = xtensa->qpkt_resp;
    if (strncmp(packet, "qxtn", 4) == 0) {
        strcpy(*response_p, "OpenOCD");
        return ERROR_OK;
    } else if (strncasecmp(packet, "qxtgdbversion=", 14) == 0) {
        return ERROR_OK;
    } else if ((strncmp(packet, "Qxtsis=", 7) == 0) || (strncmp(packet, "Qxtsds=", 7) == 0)) {
        /* Confirm host cache params match core .cfg file */
        struct xtensa_cache_config *cachep = (packet[4] == 'i') ?
            &xtensa->core_config->icache : &xtensa->core_config->dcache;
        unsigned int line_size = 0, size = 0, way_count = 0;
        sscanf(&packet[7], "%x,%x,%x", &line_size, &size, &way_count);
        if ((cachep->line_size != line_size) ||
            (cachep->size != size) ||
            (cachep->way_count != way_count)) {
            LOG_TARGET_WARNING(target, "%cCache mismatch; check xtensa-core-XXX.cfg file",
                cachep == &xtensa->core_config->icache ? 'I' : 'D');
        }
        strcpy(*response_p, "OK");
        return ERROR_OK;
    } else if ((strncmp(packet, "Qxtiram=", 8) == 0) || (strncmp(packet, "Qxtirom=", 8) == 0)) {
        /* Confirm host IRAM/IROM params match core .cfg file */
        struct xtensa_local_mem_config *memp = (packet[5] == 'a') ?
            &xtensa->core_config->iram : &xtensa->core_config->irom;
        unsigned int base = 0, size = 0, i;
        char *pkt = (char *)&packet[7];
        do {
            pkt++;
            size = strtoul(pkt, &pkt, 16);
            pkt++;
            base = strtoul(pkt, &pkt, 16);
            LOG_TARGET_DEBUG(target, "memcheck: %dB @ 0x%08x", size, base);
            for (i = 0; i < memp->count; i++) {
                if ((memp->regions[i].base == base) && (memp->regions[i].size == size))
                    break;
            }
            if (i == memp->count) {
                LOG_TARGET_WARNING(target, "%s mismatch; check xtensa-core-XXX.cfg file",
                    memp == &xtensa->core_config->iram ? "IRAM" : "IROM");
                break;
            }
            for (i = 0; i < 11; i++) {
                pkt++;
                strtoul(pkt, &pkt, 16);
            }
        } while (pkt && (pkt[0] == ','));
        strcpy(*response_p, "OK");
        return ERROR_OK;
    } else if (strncmp(packet, "Qxtexcmlvl=", 11) == 0) {
        /* Confirm host EXCM_LEVEL matches core .cfg file */
        unsigned int excm_level = strtoul(&packet[11], NULL, 0);
        if (!xtensa->core_config->high_irq.enabled ||
            (excm_level != xtensa->core_config->high_irq.excm_level))
            LOG_TARGET_WARNING(target, "EXCM_LEVEL mismatch; check xtensa-core-XXX.cfg file");
        strcpy(*response_p, "OK");
        return ERROR_OK;
    } else if ((strncmp(packet, "Qxtl2cs=", 8) == 0) ||
        (strncmp(packet, "Qxtl2ca=", 8) == 0) ||
        (strncmp(packet, "Qxtdensity=", 11) == 0)) {
        strcpy(*response_p, "OK");
        return ERROR_OK;
    } else if (strncmp(packet, "Qxtspill=", 9) == 0) {
        char *delim;
        uint32_t spill_loc = strtoul(packet + 9, &delim, 16);
        if (*delim != ':') {
            LOG_ERROR("Malformed Qxtspill packet");
            error = XT_QERR_INVAL;
            goto xtensa_gdb_query_custom_fail;
        }
        xtensa->spill_loc = spill_loc;
        xtensa->spill_bytes = strtoul(delim + 1, NULL, 16);
        if (xtensa->spill_buf)
            free(xtensa->spill_buf);
        xtensa->spill_buf = calloc(1, xtensa->spill_bytes);
        if (!xtensa->spill_buf) {
            LOG_ERROR("Spill buf alloc");
            error = XT_QERR_MEM;
            goto xtensa_gdb_query_custom_fail;
        }
        LOG_TARGET_DEBUG(target, "Set spill 0x%08" PRIx32 " (%d)", xtensa->spill_loc, xtensa->spill_bytes);
        strcpy(*response_p, "OK");
        return ERROR_OK;
    } else if (strncasecmp(packet, "qxtreg", 6) == 0) {
        return xtensa_gdbqc_qxtreg(target, packet, response_p);
    } else if ((strncmp(packet, "qTStatus", 8) == 0) ||
        (strncmp(packet, "qxtftie", 7) == 0) ||
        (strncmp(packet, "qxtstie", 7) == 0)) {
        /* Return empty string to indicate trace, TIE wire debug are unsupported */
        strcpy(*response_p, "");
        return ERROR_OK;
    }
 
    /* Warn for all other queries, but do not return errors */
    LOG_TARGET_WARNING(target, "Unknown target-specific query packet: %s", packet);
    strcpy(*response_p, "");
    return ERROR_OK;
 
xtensa_gdb_query_custom_fail:
    strcpy(*response_p, xt_qerr[error].chrval);
    return xt_qerr[error].intval;
}
 
int xtensa_init_arch_info(struct target *target, struct xtensa *xtensa,
    const struct xtensa_debug_module_config *dm_cfg)
{
    target->arch_info = xtensa;
    xtensa->common_magic = XTENSA_COMMON_MAGIC;
    xtensa->target = target;
    xtensa->stepping_isr_mode = XT_STEPPING_ISR_ON;
 
    xtensa->core_config = calloc(1, sizeof(struct xtensa_config));
    if (!xtensa->core_config) {
        LOG_ERROR("Xtensa configuration alloc failed\n");
        return ERROR_FAIL;
    }
 
    /* Default cache settings are disabled with 1 way */
    xtensa->core_config->icache.way_count = 1;
    xtensa->core_config->dcache.way_count = 1;
 
    /* chrval: AR3/AR4 register names will change with window mapping.
     * intval: tracks whether scratch register was set through gdb P packet.
     */
    for (enum xtensa_ar_scratch_set_e s = 0; s < XT_AR_SCRATCH_NUM; s++) {
        xtensa->scratch_ars[s].chrval = calloc(8, sizeof(char));
        if (!xtensa->scratch_ars[s].chrval) {
            for (enum xtensa_ar_scratch_set_e f = 0; f < s; f++)
                free(xtensa->scratch_ars[f].chrval);
            free(xtensa->core_config);
            LOG_ERROR("Xtensa scratch AR alloc failed\n");
            return ERROR_FAIL;
        }
        xtensa->scratch_ars[s].intval = false;
        sprintf(xtensa->scratch_ars[s].chrval, "%s%d",
            ((s == XT_AR_SCRATCH_A3) || (s == XT_AR_SCRATCH_A4)) ? "a" : "ar",
            ((s == XT_AR_SCRATCH_A3) || (s == XT_AR_SCRATCH_AR3)) ? 3 : 4);
    }
 
    return xtensa_dm_init(&xtensa->dbg_mod, dm_cfg);
}
 
void xtensa_set_permissive_mode(struct target *target, bool state)
{
    target_to_xtensa(target)->permissive_mode = state;
}
 
int xtensa_target_init(struct command_context *cmd_ctx, struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    xtensa->come_online_probes_num = 3;
    xtensa->hw_brps = calloc(XT_HW_IBREAK_MAX_NUM, sizeof(struct breakpoint *));
    if (!xtensa->hw_brps) {
        LOG_ERROR("Failed to alloc memory for HW breakpoints!");
        return ERROR_FAIL;
    }
    xtensa->hw_wps = calloc(XT_HW_DBREAK_MAX_NUM, sizeof(struct watchpoint *));
    if (!xtensa->hw_wps) {
        free(xtensa->hw_brps);
        LOG_ERROR("Failed to alloc memory for HW watchpoints!");
        return ERROR_FAIL;
    }
    xtensa->sw_brps = calloc(XT_SW_BREAKPOINTS_MAX_NUM, sizeof(struct xtensa_sw_breakpoint));
    if (!xtensa->sw_brps) {
        free(xtensa->hw_brps);
        free(xtensa->hw_wps);
        LOG_ERROR("Failed to alloc memory for SW breakpoints!");
        return ERROR_FAIL;
    }
 
    xtensa->spill_loc = 0xffffffff;
    xtensa->spill_bytes = 0;
    xtensa->spill_buf = NULL;
    xtensa->probe_lsddr32p = -1;    /* Probe for fast load/store operations */
 
    return xtensa_build_reg_cache(target);
}
 
static void xtensa_free_reg_cache(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
    struct reg_cache *cache = xtensa->core_cache;
 
    if (cache) {
        register_unlink_cache(&target->reg_cache, cache);
        for (unsigned int i = 0; i < cache->num_regs; i++) {
            free(xtensa->algo_context_backup[i]);
            free(cache->reg_list[i].value);
        }
        free(xtensa->algo_context_backup);
        free(cache->reg_list);
        free(cache);
    }
    xtensa->core_cache = NULL;
    xtensa->algo_context_backup = NULL;
 
    if (xtensa->empty_regs) {
        for (unsigned int i = 0; i < xtensa->dbregs_num; i++) {
            free((void *)xtensa->empty_regs[i].name);
            free(xtensa->empty_regs[i].value);
        }
        free(xtensa->empty_regs);
    }
    xtensa->empty_regs = NULL;
    if (xtensa->optregs) {
        for (unsigned int i = 0; i < xtensa->num_optregs; i++)
            free((void *)xtensa->optregs[i].name);
        free(xtensa->optregs);
    }
    xtensa->optregs = NULL;
    free(xtensa->contiguous_regs_desc);
    xtensa->contiguous_regs_desc = NULL;
    free(xtensa->contiguous_regs_list);
    xtensa->contiguous_regs_list = NULL;
}
 
void xtensa_target_deinit(struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    LOG_DEBUG("start");
 
    if (target_was_examined(target)) {
        int ret = xtensa_queue_dbg_reg_write(xtensa, XDMREG_DCRCLR, OCDDCR_ENABLEOCD);
        if (ret != ERROR_OK) {
            LOG_ERROR("Failed to queue OCDDCR_ENABLEOCD clear operation!");
            return;
        }
        xtensa_dm_queue_tdi_idle(&xtensa->dbg_mod);
        ret = xtensa_dm_queue_execute(&xtensa->dbg_mod);
        if (ret != ERROR_OK) {
            LOG_ERROR("Failed to clear OCDDCR_ENABLEOCD!");
            return;
        }
        xtensa_dm_deinit(&xtensa->dbg_mod);
    }
    xtensa_free_reg_cache(target);
    free(xtensa->hw_brps);
    free(xtensa->hw_wps);
    free(xtensa->sw_brps);
    if (xtensa->spill_buf) {
        free(xtensa->spill_buf);
        xtensa->spill_buf = NULL;
    }
    for (enum xtensa_ar_scratch_set_e s = 0; s < XT_AR_SCRATCH_NUM; s++)
        free(xtensa->scratch_ars[s].chrval);
    free(xtensa->core_config);
}
 
const char *xtensa_get_gdb_arch(const struct target *target)
{
    return "xtensa";
}
 
/* exe <ascii-encoded hexadecimal instruction bytes> */
static COMMAND_HELPER(xtensa_cmd_exe_do, struct target *target)
{
    struct xtensa *xtensa = target_to_xtensa(target);
 
    if (CMD_ARGC != 1)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    /* Process ascii-encoded hex byte string */
    const char *parm = CMD_ARGV[0];
    unsigned int parm_len = strlen(parm);
    if ((parm_len >= 64) || (parm_len & 1)) {
        command_print(CMD, "Invalid parameter length (%d): must be even, < 64 characters", parm_len);
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    uint8_t ops[32];
    memset(ops, 0, 32);
    unsigned int oplen = parm_len / 2;
    char encoded_byte[3] = { 0, 0, 0 };
    for (unsigned int i = 0; i < oplen; i++) {
        encoded_byte[0] = *parm++;
        encoded_byte[1] = *parm++;
        ops[i] = strtoul(encoded_byte, NULL, 16);
    }
 
    /* GDB must handle state save/restore.
     * Flush reg cache in case spill location is in an AR
     * Update CPENABLE only for this execution; later restore cached copy
     * Keep a copy of exccause in case executed code triggers an exception
     */
    int status = xtensa_write_dirty_registers(target);
    if (status != ERROR_OK) {
        command_print(CMD, "%s: Failed to write back register cache.", target_name(target));
        return ERROR_FAIL;
    }
    xtensa_reg_val_t exccause = xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE);
    xtensa_reg_val_t cpenable = xtensa_reg_get(target, XT_REG_IDX_CPENABLE);
    xtensa_reg_val_t a3 = xtensa_reg_get(target, XT_REG_IDX_A3);
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, 0xffffffff);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
    xtensa_queue_exec_ins(xtensa, XT_INS_WSR(xtensa,
            xtensa_regs[XT_REG_IDX_CPENABLE].reg_num, XT_REG_A3));
    xtensa_queue_dbg_reg_write(xtensa, XDMREG_DDR, a3);
    xtensa_queue_exec_ins(xtensa, XT_INS_RSR(xtensa, XT_SR_DDR, XT_REG_A3));
 
    /* Queue instruction list and execute everything */
    LOG_TARGET_DEBUG(target, "execute stub: %s", CMD_ARGV[0]);
    xtensa_queue_exec_ins_wide(xtensa, ops, oplen); /* Handles endian-swap */
    status = xtensa_dm_queue_execute(&xtensa->dbg_mod);
    if (status != ERROR_OK) {
        command_print(CMD, "exec: queue error %d", status);
    } else {
        status = xtensa_core_status_check(target);
        if (status != ERROR_OK)
            command_print(CMD, "exec: status error %d", status);
    }
 
    /* Reread register cache and restore saved regs after instruction execution */
    if (xtensa_fetch_all_regs(target) != ERROR_OK)
        command_print(CMD, "post-exec: register fetch error");
    if (status != ERROR_OK) {
        command_print(CMD, "post-exec: EXCCAUSE 0x%02" PRIx32,
            xtensa_reg_get(target, XT_REG_IDX_EXCCAUSE));
    }
    xtensa_reg_set(target, XT_REG_IDX_EXCCAUSE, exccause);
    xtensa_reg_set(target, XT_REG_IDX_CPENABLE, cpenable);
    return status;
}
 
COMMAND_HANDLER(xtensa_cmd_exe)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_exe_do, get_current_target(CMD_CTX));
}
 
/* xtdef <name> */
COMMAND_HELPER(xtensa_cmd_xtdef_do, struct xtensa *xtensa)
{
    if (CMD_ARGC != 1)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    const char *core_name = CMD_ARGV[0];
    if (strcasecmp(core_name, "LX") == 0) {
        xtensa->core_config->core_type = XT_LX;
    } else if (strcasecmp(core_name, "NX") == 0) {
        xtensa->core_config->core_type = XT_NX;
    } else {
        command_print(CMD, "xtdef [LX|NX]\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtdef)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtdef_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
static inline bool xtensa_cmd_xtopt_legal_val(char *opt, int val, int min, int max)
{
    if ((val < min) || (val > max)) {
        LOG_ERROR("xtopt %s (%d) out of range [%d..%d]\n", opt, val, min, max);
        return false;
    }
    return true;
}
 
/* xtopt <name> <value> */
COMMAND_HELPER(xtensa_cmd_xtopt_do, struct xtensa *xtensa)
{
    if (CMD_ARGC != 2)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    const char *opt_name = CMD_ARGV[0];
    int opt_val = strtol(CMD_ARGV[1], NULL, 0);
    if (strcasecmp(opt_name, "arnum") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("arnum", opt_val, 0, 64))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->aregs_num = opt_val;
    } else if (strcasecmp(opt_name, "windowed") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("windowed", opt_val, 0, 1))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->windowed = opt_val;
    } else if (strcasecmp(opt_name, "cpenable") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("cpenable", opt_val, 0, 1))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->coproc = opt_val;
    } else if (strcasecmp(opt_name, "exceptions") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("exceptions", opt_val, 0, 1))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->exceptions = opt_val;
    } else if (strcasecmp(opt_name, "intnum") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("intnum", opt_val, 0, 32))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->irq.enabled = (opt_val > 0);
        xtensa->core_config->irq.irq_num = opt_val;
    } else if (strcasecmp(opt_name, "hipriints") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("hipriints", opt_val, 0, 1))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->high_irq.enabled = opt_val;
    } else if (strcasecmp(opt_name, "excmlevel") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("excmlevel", opt_val, 1, 6))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        if (!xtensa->core_config->high_irq.enabled) {
            command_print(CMD, "xtopt excmlevel requires hipriints\n");
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        xtensa->core_config->high_irq.excm_level = opt_val;
    } else if (strcasecmp(opt_name, "intlevels") == 0) {
        if (xtensa->core_config->core_type == XT_LX) {
            if (!xtensa_cmd_xtopt_legal_val("intlevels", opt_val, 2, 6))
                return ERROR_COMMAND_ARGUMENT_INVALID;
        } else {
            if (!xtensa_cmd_xtopt_legal_val("intlevels", opt_val, 1, 255))
                return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        if (!xtensa->core_config->high_irq.enabled) {
            command_print(CMD, "xtopt intlevels requires hipriints\n");
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        xtensa->core_config->high_irq.level_num = opt_val;
    } else if (strcasecmp(opt_name, "debuglevel") == 0) {
        if (xtensa->core_config->core_type == XT_LX) {
            if (!xtensa_cmd_xtopt_legal_val("debuglevel", opt_val, 2, 6))
                return ERROR_COMMAND_ARGUMENT_INVALID;
        } else {
            if (!xtensa_cmd_xtopt_legal_val("debuglevel", opt_val, 0, 0))
                return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        xtensa->core_config->debug.enabled = 1;
        xtensa->core_config->debug.irq_level = opt_val;
    } else if (strcasecmp(opt_name, "ibreaknum") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("ibreaknum", opt_val, 0, 2))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->debug.ibreaks_num = opt_val;
    } else if (strcasecmp(opt_name, "dbreaknum") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("dbreaknum", opt_val, 0, 2))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->debug.dbreaks_num = opt_val;
    } else if (strcasecmp(opt_name, "tracemem") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("tracemem", opt_val, 0, 256 * 1024))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->trace.mem_sz = opt_val;
        xtensa->core_config->trace.enabled = (opt_val > 0);
    } else if (strcasecmp(opt_name, "tracememrev") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("tracememrev", opt_val, 0, 1))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->trace.reversed_mem_access = opt_val;
    } else if (strcasecmp(opt_name, "perfcount") == 0) {
        if (!xtensa_cmd_xtopt_legal_val("perfcount", opt_val, 0, 8))
            return ERROR_COMMAND_ARGUMENT_INVALID;
        xtensa->core_config->debug.perfcount_num = opt_val;
    } else {
        LOG_WARNING("Unknown xtensa command ignored: \"xtopt %s %s\"", CMD_ARGV[0], CMD_ARGV[1]);
        return ERROR_OK;
    }
 
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtopt)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtopt_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* xtmem <type> [parameters] */
COMMAND_HELPER(xtensa_cmd_xtmem_do, struct xtensa *xtensa)
{
    struct xtensa_cache_config *cachep = NULL;
    struct xtensa_local_mem_config *memp = NULL;
    int mem_access = 0;
    bool is_dcache = false;
 
    if (CMD_ARGC == 0)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    const char *mem_name = CMD_ARGV[0];
    if (strcasecmp(mem_name, "icache") == 0) {
        cachep = &xtensa->core_config->icache;
    } else if (strcasecmp(mem_name, "dcache") == 0) {
        cachep = &xtensa->core_config->dcache;
        is_dcache = true;
    } else if (strcasecmp(mem_name, "l2cache") == 0) {
        /* TODO: support L2 cache */
    } else if (strcasecmp(mem_name, "l2addr") == 0) {
        /* TODO: support L2 cache */
    } else if (strcasecmp(mem_name, "iram") == 0) {
        memp = &xtensa->core_config->iram;
        mem_access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE;
    } else if (strcasecmp(mem_name, "dram") == 0) {
        memp = &xtensa->core_config->dram;
        mem_access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE;
    } else if (strcasecmp(mem_name, "sram") == 0) {
        memp = &xtensa->core_config->sram;
        mem_access = XT_MEM_ACCESS_READ | XT_MEM_ACCESS_WRITE;
    } else if (strcasecmp(mem_name, "irom") == 0) {
        memp = &xtensa->core_config->irom;
        mem_access = XT_MEM_ACCESS_READ;
    } else if (strcasecmp(mem_name, "drom") == 0) {
        memp = &xtensa->core_config->drom;
        mem_access = XT_MEM_ACCESS_READ;
    } else if (strcasecmp(mem_name, "srom") == 0) {
        memp = &xtensa->core_config->srom;
        mem_access = XT_MEM_ACCESS_READ;
    } else {
        command_print(CMD, "xtmem types: <icache|dcache|l2cache|l2addr|iram|irom|dram|drom|sram|srom>\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    if (cachep) {
        if (CMD_ARGC != 4 && CMD_ARGC != 5)
            return ERROR_COMMAND_SYNTAX_ERROR;
        cachep->line_size = strtoul(CMD_ARGV[1], NULL, 0);
        cachep->size = strtoul(CMD_ARGV[2], NULL, 0);
        cachep->way_count = strtoul(CMD_ARGV[3], NULL, 0);
        cachep->writeback = ((CMD_ARGC == 5) && is_dcache) ?
            strtoul(CMD_ARGV[4], NULL, 0) : 0;
    } else if (memp) {
        if (CMD_ARGC != 3)
            return ERROR_COMMAND_SYNTAX_ERROR;
        struct xtensa_local_mem_region_config *memcfgp = &memp->regions[memp->count];
        memcfgp->base = strtoul(CMD_ARGV[1], NULL, 0);
        memcfgp->size = strtoul(CMD_ARGV[2], NULL, 0);
        memcfgp->access = mem_access;
        memp->count++;
    }
 
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtmem)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtmem_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* xtmpu <num FG seg> <min seg size> <lockable> <executeonly> */
COMMAND_HELPER(xtensa_cmd_xtmpu_do, struct xtensa *xtensa)
{
    if (CMD_ARGC != 4)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    unsigned int nfgseg = strtoul(CMD_ARGV[0], NULL, 0);
    unsigned int minsegsize = strtoul(CMD_ARGV[1], NULL, 0);
    unsigned int lockable = strtoul(CMD_ARGV[2], NULL, 0);
    unsigned int execonly = strtoul(CMD_ARGV[3], NULL, 0);
 
    if ((nfgseg > 32)) {
        command_print(CMD, "<nfgseg> must be within [0..32]\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    } else if (minsegsize & (minsegsize - 1)) {
        command_print(CMD, "<minsegsize> must be a power of 2 >= 32\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    } else if (lockable > 1) {
        command_print(CMD, "<lockable> must be 0 or 1\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    } else if (execonly > 1) {
        command_print(CMD, "<execonly> must be 0 or 1\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    xtensa->core_config->mpu.enabled = true;
    xtensa->core_config->mpu.nfgseg = nfgseg;
    xtensa->core_config->mpu.minsegsize = minsegsize;
    xtensa->core_config->mpu.lockable = lockable;
    xtensa->core_config->mpu.execonly = execonly;
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtmpu)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtmpu_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* xtmmu <NIREFILLENTRIES> <NDREFILLENTRIES> <IVARWAY56> <DVARWAY56> */
COMMAND_HELPER(xtensa_cmd_xtmmu_do, struct xtensa *xtensa)
{
    if (CMD_ARGC != 2)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    unsigned int nirefillentries = strtoul(CMD_ARGV[0], NULL, 0);
    unsigned int ndrefillentries = strtoul(CMD_ARGV[1], NULL, 0);
    if ((nirefillentries != 16) && (nirefillentries != 32)) {
        command_print(CMD, "<nirefillentries> must be 16 or 32\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    } else if ((ndrefillentries != 16) && (ndrefillentries != 32)) {
        command_print(CMD, "<ndrefillentries> must be 16 or 32\n");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    xtensa->core_config->mmu.enabled = true;
    xtensa->core_config->mmu.itlb_entries_count = nirefillentries;
    xtensa->core_config->mmu.dtlb_entries_count = ndrefillentries;
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtmmu)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtmmu_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* xtregs <numregs>
 * xtreg <regname> <regnum> */
COMMAND_HELPER(xtensa_cmd_xtreg_do, struct xtensa *xtensa)
{
    if (CMD_ARGC == 1) {
        int32_t numregs = strtoul(CMD_ARGV[0], NULL, 0);
        if ((numregs <= 0) || (numregs > UINT16_MAX)) {
            command_print(CMD, "xtreg <numregs>: Invalid 'numregs' (%d)", numregs);
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        if ((xtensa->genpkt_regs_num > 0) && (numregs < (int32_t)xtensa->genpkt_regs_num)) {
            command_print(CMD, "xtregs (%d) must be larger than numgenregs (%d) (if xtregfmt specified)",
                numregs, xtensa->genpkt_regs_num);
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
        xtensa->total_regs_num = numregs;
        xtensa->core_regs_num = 0;
        xtensa->num_optregs = 0;
        /* Prevent memory leak in case xtregs is called twice */
        free(xtensa->optregs);
        free(xtensa->contiguous_regs_desc);
        xtensa->contiguous_regs_desc = NULL;
        /* A little more memory than required, but saves a second initialization pass */
        xtensa->optregs = calloc(xtensa->total_regs_num, sizeof(struct xtensa_reg_desc));
        if (!xtensa->optregs) {
            LOG_ERROR("Failed to allocate xtensa->optregs!");
            return ERROR_FAIL;
        }
        return ERROR_OK;
    } else if (CMD_ARGC != 2) {
        return ERROR_COMMAND_SYNTAX_ERROR;
    }
 
    /* "xtregfmt contiguous" must be specified prior to the first "xtreg" definition
     * if general register (g-packet) requests or contiguous register maps are supported */
    if (xtensa->regmap_contiguous && !xtensa->contiguous_regs_desc) {
        xtensa->contiguous_regs_desc = calloc(xtensa->total_regs_num, sizeof(struct xtensa_reg_desc *));
        if (!xtensa->contiguous_regs_desc) {
            LOG_ERROR("Failed to allocate xtensa->contiguous_regs_desc!");
            return ERROR_FAIL;
        }
    }
 
    const char *regname = CMD_ARGV[0];
    unsigned int regnum = strtoul(CMD_ARGV[1], NULL, 0);
    if (regnum > UINT16_MAX) {
        command_print(CMD, "<regnum> must be a 16-bit number");
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    if ((xtensa->num_optregs + xtensa->core_regs_num) >= xtensa->total_regs_num) {
        if (xtensa->total_regs_num)
            command_print(CMD, "'xtreg %s 0x%04x': Too many registers (%d expected, %d core %d extended)",
                regname, regnum,
                xtensa->total_regs_num, xtensa->core_regs_num, xtensa->num_optregs);
        else
            command_print(CMD, "'xtreg %s 0x%04x': Number of registers unspecified",
                regname, regnum);
        return ERROR_FAIL;
    }
 
    /* Determine whether register belongs in xtensa_regs[] or xtensa->xtensa_spec_regs[] */
    struct xtensa_reg_desc *rptr = &xtensa->optregs[xtensa->num_optregs];
    bool is_extended_reg = true;
    unsigned int ridx;
    for (ridx = 0; ridx < XT_NUM_REGS; ridx++) {
        if (strcmp(CMD_ARGV[0], xtensa_regs[ridx].name) == 0) {
            /* Flag core register as defined */
            rptr = &xtensa_regs[ridx];
            xtensa->core_regs_num++;
            is_extended_reg = false;
            break;
        }
    }
 
    rptr->exist = true;
    if (is_extended_reg) {
        /* Register ID, debugger-visible register ID */
        rptr->name = strdup(CMD_ARGV[0]);
        rptr->dbreg_num = regnum;
        rptr->reg_num = (regnum & XT_REG_INDEX_MASK);
        xtensa->num_optregs++;
 
        /* Register type */
        if ((regnum & XT_REG_GENERAL_MASK) == XT_REG_GENERAL_VAL) {
            rptr->type = XT_REG_GENERAL;
        } else if ((regnum & XT_REG_USER_MASK) == XT_REG_USER_VAL) {
            rptr->type = XT_REG_USER;
        } else if ((regnum & XT_REG_FR_MASK) == XT_REG_FR_VAL) {
            rptr->type = XT_REG_FR;
        } else if ((regnum & XT_REG_SPECIAL_MASK) == XT_REG_SPECIAL_VAL) {
            rptr->type = XT_REG_SPECIAL;
        } else if ((regnum & XT_REG_RELGEN_MASK) == XT_REG_RELGEN_VAL) {
            /* WARNING: For these registers, regnum points to the
             * index of the corresponding ARx registers, NOT to
             * the processor register number! */
            rptr->type = XT_REG_RELGEN;
            rptr->reg_num += XT_REG_IDX_ARFIRST;
            rptr->dbreg_num += XT_REG_IDX_ARFIRST;
        } else if ((regnum & XT_REG_TIE_MASK) != 0) {
            rptr->type = XT_REG_TIE;
        } else {
            rptr->type = XT_REG_OTHER;
        }
 
        /* Register flags: includes intsetN, intclearN for LX8 */
        if ((strcmp(rptr->name, "mmid") == 0) || (strcmp(rptr->name, "eraccess") == 0) ||
            (strcmp(rptr->name, "ddr") == 0) || (strncmp(rptr->name, "intset", 6) == 0) ||
            (strncmp(rptr->name, "intclear", 8) == 0) || (strcmp(rptr->name, "mesrclr") == 0))
            rptr->flags = XT_REGF_NOREAD;
        else
            rptr->flags = 0;
 
        if (rptr->reg_num == (XT_EPS_REG_NUM_BASE + xtensa->core_config->debug.irq_level) &&
            xtensa->core_config->core_type == XT_LX && rptr->type == XT_REG_SPECIAL) {
            xtensa->eps_dbglevel_idx = XT_NUM_REGS + xtensa->num_optregs - 1;
            LOG_DEBUG("Setting PS (%s) index to %d", rptr->name, xtensa->eps_dbglevel_idx);
        }
        if (xtensa->core_config->core_type == XT_NX) {
            enum xtensa_nx_reg_idx idx = XT_NX_REG_IDX_NUM;
            if (strcmp(rptr->name, "ibreakc0") == 0)
                idx = XT_NX_REG_IDX_IBREAKC0;
            else if (strcmp(rptr->name, "wb") == 0)
                idx = XT_NX_REG_IDX_WB;
            else if (strcmp(rptr->name, "ms") == 0)
                idx = XT_NX_REG_IDX_MS;
            else if (strcmp(rptr->name, "ievec") == 0)
                idx = XT_NX_REG_IDX_IEVEC;
            else if (strcmp(rptr->name, "ieextern") == 0)
                idx = XT_NX_REG_IDX_IEEXTERN;
            else if (strcmp(rptr->name, "mesr") == 0)
                idx = XT_NX_REG_IDX_MESR;
            else if (strcmp(rptr->name, "mesrclr") == 0)
                idx = XT_NX_REG_IDX_MESRCLR;
            if (idx < XT_NX_REG_IDX_NUM) {
                if (xtensa->nx_reg_idx[idx] != 0) {
                    command_print(CMD, "nx_reg_idx[%d] previously set to %d",
                        idx, xtensa->nx_reg_idx[idx]);
                    return ERROR_FAIL;
                }
                xtensa->nx_reg_idx[idx] = XT_NUM_REGS + xtensa->num_optregs - 1;
                LOG_DEBUG("NX reg %s: index %d (%d)",
                    rptr->name, xtensa->nx_reg_idx[idx], idx);
            }
        }
    } else if (strcmp(rptr->name, "cpenable") == 0) {
        xtensa->core_config->coproc = true;
    }
 
    /* Build out list of contiguous registers in specified order */
    unsigned int running_reg_count = xtensa->num_optregs + xtensa->core_regs_num;
    if (xtensa->contiguous_regs_desc) {
        assert((running_reg_count <= xtensa->total_regs_num) && "contiguous register address internal error!");
        xtensa->contiguous_regs_desc[running_reg_count - 1] = rptr;
    }
    if (xtensa_extra_debug_log)
        LOG_DEBUG("Added %s register %-16s: 0x%04x/0x%02x t%d (%d of %d)",
            is_extended_reg ? "config-specific" : "core",
            rptr->name, rptr->dbreg_num, rptr->reg_num, rptr->type,
            is_extended_reg ? xtensa->num_optregs : ridx,
            is_extended_reg ? xtensa->total_regs_num : XT_NUM_REGS);
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_xtreg)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtreg_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* xtregfmt <contiguous|sparse> [numgregs] */
COMMAND_HELPER(xtensa_cmd_xtregfmt_do, struct xtensa *xtensa)
{
    if ((CMD_ARGC == 1) || (CMD_ARGC == 2)) {
        if (!strcasecmp(CMD_ARGV[0], "sparse")) {
            return ERROR_OK;
        } else if (!strcasecmp(CMD_ARGV[0], "contiguous")) {
            xtensa->regmap_contiguous = true;
            if (CMD_ARGC == 2) {
                unsigned int numgregs = strtoul(CMD_ARGV[1], NULL, 0);
                if ((numgregs <= 0) ||
                    ((numgregs > xtensa->total_regs_num) &&
                    (xtensa->total_regs_num > 0))) {
                    command_print(CMD, "xtregfmt: if specified, numgregs (%d) must be <= numregs (%d)",
                        numgregs, xtensa->total_regs_num);
                    return ERROR_COMMAND_ARGUMENT_INVALID;
                }
                xtensa->genpkt_regs_num = numgregs;
            }
            return ERROR_OK;
        }
    }
    return ERROR_COMMAND_SYNTAX_ERROR;
}
 
COMMAND_HANDLER(xtensa_cmd_xtregfmt)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_xtregfmt_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_permissive_mode_do, struct xtensa *xtensa)
{
    return CALL_COMMAND_HANDLER(handle_command_parse_bool,
        &xtensa->permissive_mode, "xtensa permissive mode");
}
 
COMMAND_HANDLER(xtensa_cmd_permissive_mode)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_permissive_mode_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* perfmon_enable <counter_id> <select> [mask] [kernelcnt] [tracelevel] */
COMMAND_HELPER(xtensa_cmd_perfmon_enable_do, struct xtensa *xtensa)
{
    struct xtensa_perfmon_config config = {
        .mask = 0xffff,
        .kernelcnt = 0,
        .tracelevel = -1    /* use DEBUGLEVEL by default */
    };
 
    if (CMD_ARGC < 2 || CMD_ARGC > 6)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    unsigned int counter_id = strtoul(CMD_ARGV[0], NULL, 0);
    if (counter_id >= XTENSA_MAX_PERF_COUNTERS) {
        command_print(CMD, "counter_id should be < %d", XTENSA_MAX_PERF_COUNTERS);
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    config.select = strtoul(CMD_ARGV[1], NULL, 0);
    if (config.select > XTENSA_MAX_PERF_SELECT) {
        command_print(CMD, "select should be < %d", XTENSA_MAX_PERF_SELECT);
        return ERROR_COMMAND_ARGUMENT_INVALID;
    }
 
    if (CMD_ARGC >= 3) {
        config.mask = strtoul(CMD_ARGV[2], NULL, 0);
        if (config.mask > XTENSA_MAX_PERF_MASK) {
            command_print(CMD, "mask should be < %d", XTENSA_MAX_PERF_MASK);
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
    }
 
    if (CMD_ARGC >= 4) {
        config.kernelcnt = strtoul(CMD_ARGV[3], NULL, 0);
        if (config.kernelcnt > 1) {
            command_print(CMD, "kernelcnt should be 0 or 1");
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
    }
 
    if (CMD_ARGC >= 5) {
        config.tracelevel = strtoul(CMD_ARGV[4], NULL, 0);
        if (config.tracelevel > 7) {
            command_print(CMD, "tracelevel should be <=7");
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
    }
 
    if (config.tracelevel == -1)
        config.tracelevel = xtensa->core_config->debug.irq_level;
 
    return xtensa_dm_perfmon_enable(&xtensa->dbg_mod, counter_id, &config);
}
 
COMMAND_HANDLER(xtensa_cmd_perfmon_enable)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_enable_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
/* perfmon_dump [counter_id] */
COMMAND_HELPER(xtensa_cmd_perfmon_dump_do, struct xtensa *xtensa)
{
    if (CMD_ARGC > 1)
        return ERROR_COMMAND_SYNTAX_ERROR;
 
    int counter_id = -1;
    if (CMD_ARGC == 1) {
        counter_id = strtol(CMD_ARGV[0], NULL, 0);
        if (counter_id > XTENSA_MAX_PERF_COUNTERS) {
            command_print(CMD, "counter_id should be < %d", XTENSA_MAX_PERF_COUNTERS);
            return ERROR_COMMAND_ARGUMENT_INVALID;
        }
    }
 
    unsigned int counter_start = (counter_id < 0) ? 0 : counter_id;
    unsigned int counter_end = (counter_id < 0) ? XTENSA_MAX_PERF_COUNTERS : counter_id + 1;
    for (unsigned int counter = counter_start; counter < counter_end; ++counter) {
        char result_buf[128] = { 0 };
        size_t result_pos = snprintf(result_buf, sizeof(result_buf), "Counter %d: ", counter);
        struct xtensa_perfmon_result result;
        int res = xtensa_dm_perfmon_dump(&xtensa->dbg_mod, counter, &result);
        if (res != ERROR_OK)
            return res;
        snprintf(result_buf + result_pos, sizeof(result_buf) - result_pos,
            "%-12" PRIu64 "%s",
            result.value,
            result.overflow ? " (overflow)" : "");
        command_print(CMD, "%s", result_buf);
    }
 
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_perfmon_dump)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_perfmon_dump_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_mask_interrupts_do, struct xtensa *xtensa)
{
    int state = -1;
 
    if (CMD_ARGC < 1) {
        const char *st;
        state = xtensa->stepping_isr_mode;
        if (state == XT_STEPPING_ISR_ON)
            st = "OFF";
        else if (state == XT_STEPPING_ISR_OFF)
            st = "ON";
        else
            st = "UNKNOWN";
        command_print(CMD, "Current ISR step mode: %s", st);
        return ERROR_OK;
    }
 
    /* Masking is ON -> interrupts during stepping are OFF, and vice versa */
    if (!strcasecmp(CMD_ARGV[0], "off"))
        state = XT_STEPPING_ISR_ON;
    else if (!strcasecmp(CMD_ARGV[0], "on"))
        state = XT_STEPPING_ISR_OFF;
 
    if (state == -1) {
        command_print(CMD, "Argument unknown. Please pick one of ON, OFF");
        return ERROR_FAIL;
    }
    xtensa->stepping_isr_mode = state;
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_mask_interrupts)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_mask_interrupts_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_smpbreak_do, struct target *target)
{
    int res;
    uint32_t val = 0;
 
    if (CMD_ARGC >= 1) {
        for (unsigned int i = 0; i < CMD_ARGC; i++) {
            if (!strcasecmp(CMD_ARGV[0], "none")) {
                val = 0;
            } else if (!strcasecmp(CMD_ARGV[i], "BreakIn")) {
                val |= OCDDCR_BREAKINEN;
            } else if (!strcasecmp(CMD_ARGV[i], "BreakOut")) {
                val |= OCDDCR_BREAKOUTEN;
            } else if (!strcasecmp(CMD_ARGV[i], "RunStallIn")) {
                val |= OCDDCR_RUNSTALLINEN;
            } else if (!strcasecmp(CMD_ARGV[i], "DebugModeOut")) {
                val |= OCDDCR_DEBUGMODEOUTEN;
            } else if (!strcasecmp(CMD_ARGV[i], "BreakInOut")) {
                val |= OCDDCR_BREAKINEN | OCDDCR_BREAKOUTEN;
            } else if (!strcasecmp(CMD_ARGV[i], "RunStall")) {
                val |= OCDDCR_RUNSTALLINEN | OCDDCR_DEBUGMODEOUTEN;
            } else {
                command_print(CMD, "Unknown arg %s", CMD_ARGV[i]);
                command_print(
                    CMD,
                    "use either BreakInOut, None or RunStall as arguments, or any combination of BreakIn, BreakOut, RunStallIn and DebugModeOut.");
                return ERROR_OK;
            }
        }
        res = xtensa_smpbreak_set(target, val);
        if (res != ERROR_OK)
            command_print(CMD, "Failed to set smpbreak config %d", res);
    } else {
        struct xtensa *xtensa = target_to_xtensa(target);
        res = xtensa_smpbreak_read(xtensa, &val);
        if (res == ERROR_OK)
            command_print(CMD, "Current bits set:%s%s%s%s",
                (val & OCDDCR_BREAKINEN) ? " BreakIn" : "",
                (val & OCDDCR_BREAKOUTEN) ? " BreakOut" : "",
                (val & OCDDCR_RUNSTALLINEN) ? " RunStallIn" : "",
                (val & OCDDCR_DEBUGMODEOUTEN) ? " DebugModeOut" : ""
                );
        else
            command_print(CMD, "Failed to get smpbreak config %d", res);
    }
    return res;
}
 
COMMAND_HANDLER(xtensa_cmd_smpbreak)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_smpbreak_do,
        get_current_target(CMD_CTX));
}
 
COMMAND_HELPER(xtensa_cmd_dm_rw_do, struct xtensa *xtensa)
{
    if (CMD_ARGC == 1) {
        // read: xtensa dm addr
        uint32_t addr = strtoul(CMD_ARGV[0], NULL, 0);
        uint32_t val;
        int res = xtensa_dm_read(&xtensa->dbg_mod, addr, &val);
        if (res == ERROR_OK)
            command_print(CMD, "xtensa DM(0x%08" PRIx32 ") -> 0x%08" PRIx32, addr, val);
        else
            command_print(CMD, "xtensa DM(0x%08" PRIx32 ") : read ERROR %" PRId32, addr, res);
        return res;
    } else if (CMD_ARGC == 2) {
        // write: xtensa dm addr value
        uint32_t addr = strtoul(CMD_ARGV[0], NULL, 0);
        uint32_t val = strtoul(CMD_ARGV[1], NULL, 0);
        int res = xtensa_dm_write(&xtensa->dbg_mod, addr, val);
        if (res == ERROR_OK)
            command_print(CMD, "xtensa DM(0x%08" PRIx32 ") <- 0x%08" PRIx32, addr, val);
        else
            command_print(CMD, "xtensa DM(0x%08" PRIx32 ") : write ERROR %" PRId32, addr, res);
        return res;
    }
    return ERROR_COMMAND_SYNTAX_ERROR;
}
 
COMMAND_HANDLER(xtensa_cmd_dm_rw)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_dm_rw_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_tracestart_do, struct xtensa *xtensa)
{
    struct xtensa_trace_status trace_status;
    struct xtensa_trace_start_config cfg = {
        .stoppc = 0,
        .stopmask = XTENSA_STOPMASK_DISABLED,
        .after = 0,
        .after_is_words = false
    };
 
    /* Parse arguments */
    for (unsigned int i = 0; i < CMD_ARGC; i++) {
        if ((!strcasecmp(CMD_ARGV[i], "pc")) && CMD_ARGC > i) {
            char *e;
            i++;
            cfg.stoppc = strtol(CMD_ARGV[i], &e, 0);
            cfg.stopmask = 0;
            if (*e == '/')
                cfg.stopmask = strtol(e, NULL, 0);
        } else if ((!strcasecmp(CMD_ARGV[i], "after")) && CMD_ARGC > i) {
            i++;
            cfg.after = strtol(CMD_ARGV[i], NULL, 0);
        } else if (!strcasecmp(CMD_ARGV[i], "ins")) {
            cfg.after_is_words = 0;
        } else if (!strcasecmp(CMD_ARGV[i], "words")) {
            cfg.after_is_words = 1;
        } else {
            command_print(CMD, "Did not understand %s", CMD_ARGV[i]);
            return ERROR_FAIL;
        }
    }
 
    int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
    if (res != ERROR_OK)
        return res;
    if (trace_status.stat & TRAXSTAT_TRACT) {
        LOG_WARNING("Silently stop active tracing!");
        res = xtensa_dm_trace_stop(&xtensa->dbg_mod, false);
        if (res != ERROR_OK)
            return res;
    }
 
    res = xtensa_dm_trace_start(&xtensa->dbg_mod, &cfg);
    if (res != ERROR_OK)
        return res;
 
    xtensa->trace_active = true;
    command_print(CMD, "Trace started.");
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_tracestart)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_tracestart_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_tracestop_do, struct xtensa *xtensa)
{
    struct xtensa_trace_status trace_status;
 
    int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
    if (res != ERROR_OK)
        return res;
 
    if (!(trace_status.stat & TRAXSTAT_TRACT)) {
        command_print(CMD, "No trace is currently active.");
        return ERROR_FAIL;
    }
 
    res = xtensa_dm_trace_stop(&xtensa->dbg_mod, true);
    if (res != ERROR_OK)
        return res;
 
    xtensa->trace_active = false;
    command_print(CMD, "Trace stop triggered.");
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_tracestop)
{
    return CALL_COMMAND_HANDLER(xtensa_cmd_tracestop_do,
        target_to_xtensa(get_current_target(CMD_CTX)));
}
 
COMMAND_HELPER(xtensa_cmd_tracedump_do, struct xtensa *xtensa, const char *fname)
{
    struct xtensa_trace_config trace_config;
    struct xtensa_trace_status trace_status;
    uint32_t memsz, wmem;
 
    int res = xtensa_dm_trace_status_read(&xtensa->dbg_mod, &trace_status);
    if (res != ERROR_OK)
        return res;
 
    if (trace_status.stat & TRAXSTAT_TRACT) {
        command_print(CMD, "Tracing is still active. Please stop it first.");
        return ERROR_FAIL;
    }
 
    res = xtensa_dm_trace_config_read(&xtensa->dbg_mod, &trace_config);
    if (res != ERROR_OK)
        return res;
 
    if (!(trace_config.ctrl & TRAXCTRL_TREN)) {
        command_print(CMD, "No active trace found; nothing to dump.");
        return ERROR_FAIL;
    }
 
    memsz = trace_config.memaddr_end - trace_config.memaddr_start + 1;
    command_print(CMD, "Total trace memory: %d words", memsz);
    if ((trace_config.addr &
            ((TRAXADDR_TWRAP_MASK << TRAXADDR_TWRAP_SHIFT) | TRAXADDR_TWSAT)) == 0) {
        /*Memory hasn't overwritten itself yet. */
        wmem = trace_config.addr & TRAXADDR_TADDR_MASK;
        command_print(CMD, "...but trace is only %d words", wmem);
        if (wmem < memsz)
            memsz = wmem;
    } else {
        if (trace_config.addr & TRAXADDR_TWSAT) {
            command_print(CMD, "Real trace is many times longer than that (overflow)");
        } else {
            uint32_t trc_sz = (trace_config.addr >> TRAXADDR_TWRAP_SHIFT) & TRAXADDR_TWRAP_MASK;
            trc_sz = (trc_sz * memsz) + (trace_config.addr & TRAXADDR_TADDR_MASK);
            command_print(CMD, "Real trace is %d words, but the start has been truncated.", trc_sz);
        }
    }
 
    uint8_t *tracemem = malloc(memsz * 4);
    if (!tracemem) {
        command_print(CMD, "Failed to alloc memory for trace data!");
        return ERROR_FAIL;
    }
    res = xtensa_dm_trace_data_read(&xtensa->dbg_mod, tracemem, memsz * 4);
    if (res != ERROR_OK) {
        free(tracemem);
        return res;
    }
 
    int f = open(fname, O_WRONLY | O_CREAT | O_TRUNC, 0666);
    if (f <= 0) {
        free(tracemem);
        command_print(CMD, "Unable to open file %s", fname);
        return ERROR_FAIL;
    }
    if (write(f, tracemem, memsz * 4) != (int)memsz * 4)
        command_print(CMD, "Unable to write to file %s", fname);
    else
        command_print(CMD, "Written %d bytes of trace data to %s", memsz * 4, fname);
    close(f);
 
    bool is_all_zeroes = true;
    for (unsigned int i = 0; i < memsz * 4; i++) {
        if (tracemem[i] != 0) {
            is_all_zeroes = false;
            break;
        }
    }
    free(tracemem);
    if (is_all_zeroes)
        command_print(
            CMD,
            "WARNING: File written is all zeroes. Are you sure you enabled trace memory?");
 
    return ERROR_OK;
}
 
COMMAND_HANDLER(xtensa_cmd_tracedump)
{
    if (CMD_ARGC != 1) {
        command_print(CMD, "Command takes exactly 1 parameter.Need filename to dump to as output!");
        return ERROR_FAIL;
    }
 
    return CALL_COMMAND_HANDLER(xtensa_cmd_tracedump_do,
        target_to_xtensa(get_current_target(CMD_CTX)), CMD_ARGV[0]);
}
 
static const struct command_registration xtensa_any_command_handlers[] = {
    {
        .name = "xtdef",
        .handler = xtensa_cmd_xtdef,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa core type",
        .usage = "<type>",
    },
    {
        .name = "xtopt",
        .handler = xtensa_cmd_xtopt,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa core option",
        .usage = "<name> <value>",
    },
    {
        .name = "xtmem",
        .handler = xtensa_cmd_xtmem,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa memory/cache option",
        .usage = "<type> [parameters]",
    },
    {
        .name = "xtmmu",
        .handler = xtensa_cmd_xtmmu,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa MMU option",
        .usage = "<NIREFILLENTRIES> <NDREFILLENTRIES> <IVARWAY56> <DVARWAY56>",
    },
    {
        .name = "xtmpu",
        .handler = xtensa_cmd_xtmpu,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa MPU option",
        .usage = "<num FG seg> <min seg size> <lockable> <executeonly>",
    },
    {
        .name = "xtreg",
        .handler = xtensa_cmd_xtreg,
        .mode = COMMAND_CONFIG,
        .help = "Configure Xtensa register",
        .usage = "<regname> <regnum>",
    },
    {
        .name = "xtregs",
        .handler = xtensa_cmd_xtreg,
        .mode = COMMAND_CONFIG,
        .help = "Configure number of Xtensa registers",
        .usage = "<numregs>",
    },
    {
        .name = "xtregfmt",
        .handler = xtensa_cmd_xtregfmt,
        .mode = COMMAND_CONFIG,
        .help = "Configure format of Xtensa register map",
        .usage = "<contiguous|sparse> [numgregs]",
    },
    {
        .name = "set_permissive",
        .handler = xtensa_cmd_permissive_mode,
        .mode = COMMAND_ANY,
        .help = "When set to 1, enable Xtensa permissive mode (fewer client-side checks)",
        .usage = "[0|1]",
    },
    {
        .name = "maskisr",
        .handler = xtensa_cmd_mask_interrupts,
        .mode = COMMAND_ANY,
        .help = "mask Xtensa interrupts at step",
        .usage = "['on'|'off']",
    },
    {
        .name = "smpbreak",
        .handler = xtensa_cmd_smpbreak,
        .mode = COMMAND_ANY,
        .help = "Set the way the CPU chains OCD breaks",
        .usage = "[none|breakinout|runstall] | [BreakIn] [BreakOut] [RunStallIn] [DebugModeOut]",
    },
    {
        .name = "dm",
        .handler = xtensa_cmd_dm_rw,
        .mode = COMMAND_ANY,
        .help = "Xtensa DM read/write",
        .usage = "addr [value]"
    },
    {
        .name = "perfmon_enable",
        .handler = xtensa_cmd_perfmon_enable,
        .mode = COMMAND_EXEC,
        .help = "Enable and start performance counter",
        .usage = "<counter_id> <select> [mask] [kernelcnt] [tracelevel]",
    },
    {
        .name = "perfmon_dump",
        .handler = xtensa_cmd_perfmon_dump,
        .mode = COMMAND_EXEC,
        .help = "Dump performance counter value. If no argument specified, dumps all counters.",
        .usage = "[counter_id]",
    },
    {
        .name = "tracestart",
        .handler = xtensa_cmd_tracestart,
        .mode = COMMAND_EXEC,
        .help =
            "Tracing: Set up and start a trace. Optionally set stop trigger address and amount of data captured after.",
        .usage = "[pc <pcval>/[maskbitcount]] [after <n> [ins|words]]",
    },
    {
        .name = "tracestop",
        .handler = xtensa_cmd_tracestop,
        .mode = COMMAND_EXEC,
        .help = "Tracing: Stop current trace as started by the tracestart command",
        .usage = "",
    },
    {
        .name = "tracedump",
        .handler = xtensa_cmd_tracedump,
        .mode = COMMAND_EXEC,
        .help = "Tracing: Dump trace memory to a files. One file per core.",
        .usage = "<outfile>",
    },
    {
        .name = "exe",
        .handler = xtensa_cmd_exe,
        .mode = COMMAND_ANY,
        .help = "Xtensa stub execution",
        .usage = "<ascii-encoded hexadecimal instruction bytes>",
    },
    COMMAND_REGISTRATION_DONE
};
 
const struct command_registration xtensa_command_handlers[] = {
    {
        .name = "xtensa",
        .mode = COMMAND_ANY,
        .help = "Xtensa command group",
        .usage = "",
        .chain = xtensa_any_command_handlers,
    },
    COMMAND_REGISTRATION_DONE
};