path: root/src/target/xtensa/xtensa.c

// 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)

#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),
};

/**
 * Types of memory used at xtensa target
 */
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
};

/* 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_s 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;

/**
 * Gets a config for the specific mem type
 */
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;
	}
}

/**
 * Extracts an exact xtensa_local_mem_region_config from xtensa_local_mem_config
 * for a given address
 * Returns NULL if nothing found
 */
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;
}

/**
 * Returns a corresponding xtensa_local_mem_region_config from the xtensa target
 * for a given address
 * Returns NULL if nothing found
 */
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("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]));
	}
}

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));
				}
			}
		}
	}

	/* 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);
	target_set_examined(target);
	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=%i", set, target->state);
	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, int *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,
	int current,
	target_addr_t address,
	int handle_breakpoints,
	int 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,
	int current,
	target_addr_t address,
	int handle_breakpoints,
	int 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, int current, target_addr_t address, int 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_lowered = 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->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 {
		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_lowered = 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, 0, 0);
		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_lowered) {
		LOG_DEBUG("Restoring %s after stepping: 0x%08" PRIx32,
			xtensa->core_cache->reg_list[xtensa->eps_dbglevel_idx].name,
			oldps);
		xtensa_reg_set(target, xtensa->eps_dbglevel_idx, 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, int current, target_addr_t address, int 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;
}

/**
 * Returns true if two ranges are overlapping
 */
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;
}

/**
 * Returns a size of overlapped region of two ranges.
 */
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;
}

/**
 * Check if the address gets to memory regions, and its access mode
 */
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, albuff);
			bswap = false;
		} else {
			LOG_TARGET_WARNING(target, "Failed reading %d bytes at address "TARGET_ADDR_FMT,
				count * size, address);
		}
	}

	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 */
		uint32_t issue_ihi = xtensa_is_icacheable(xtensa, address);
		uint32_t issue_dhwb = xtensa_is_dcacheable(xtensa, address);
		if (issue_ihi || issue_dhwb) {
			uint32_t ilinesize = issue_ihi ?  xtensa->core_config->icache.line_size : UINT32_MAX;
			uint32_t dlinesize = issue_dhwb ? 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_dhwb)
					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);
			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);
	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, "DHWBI, IHI for address "TARGET_ADDR_FMT, 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;
	}

	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, 0, entry_point, 1, 1);
}

/** Waits for an algorithm in the target. */
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, "ERROR: 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, "ERROR: 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, "ERROR: 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;
}

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;
		/* 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 */
		if ((strcmp(rptr->name, "mmid") == 0) || (strcmp(rptr->name, "eraccess") == 0) ||
			(strcmp(rptr->name, "ddr") == 0) || (strcmp(rptr->name, "intset") == 0) ||
			(strcmp(rptr->name, "intclear") == 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;
	}

	if (xtensa->core_config->core_type == XT_NX) {
		command_print(CMD, "ERROR: ISR step mode only supported on Xtensa LX");
		return ERROR_FAIL;
	}

	/* 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
};