diff --git a/recipes-devtools/openocd/openocd-stm32mp.inc b/recipes-devtools/openocd/openocd-stm32mp.inc new file mode 100644 index 0000000..9437f35 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp.inc @@ -0,0 +1,53 @@ +inherit pkgconfig autotools-brokensep gettext + +SRC_URI = " \ + git://repo.or.cz/r/git2cl.git;protocol=http;destsuffix=git/tools/git2cl;name=git2cl \ + git://repo.or.cz/r/jimtcl.git;protocol=http;destsuffix=git/jimtcl;name=jimtcl \ + git://repo.or.cz/r/libjaylink.git;protocol=http;destsuffix=git/src/jtag/drivers/libjaylink;name=libjaylink \ + " + +SRCREV_git2cl = "8373c9f74993e218a08819cbcdbab3f3564bbeba" +SRCREV_jimtcl = "a9bf5975fd0f89974d689a2d9ebd0873c8d64787" +SRCREV_libjaylink = "8645845c1abebd004e991ba9a7f808f4fd0c608b" + +S = "${WORKDIR}/git" + +BBCLASSEXTEND += "native nativesdk" + +DEPENDS += "hidapi-stm32mp" +DEPENDS_class-native = "hidapi-stm32mp-native" +DEPENDS_class-nativesdk = "nativesdk-hidapi-stm32mp" + +RDEPENDS_${PN} += "libusb1 hidapi-stm32mp" + +EXTRA_OECONF = " \ + --disable-werror \ + --enable-stlink \ + --enable-cmsis-dap \ + MAKEINFO=true \ + HIDAPI_CFLAGS=-I${STAGING_INCDIR}/hidapi \ + HIDAPI_LIBS=-L${STAGING_LIBDIR}\ -lhidapi-libusb \ +" + +do_configure() { + ./bootstrap nosubmodule + oe_runconf ${EXTRA_OECONF} +} + +do_install() { + oe_runmake DESTDIR=${D} install + if [ -e "${D}${infodir}" ]; then + rm -Rf ${D}${infodir} + fi + if [ -e "${D}${mandir}" ]; then + rm -Rf ${D}${mandir} + fi + if [ -e "${D}${bindir}/.debug" ]; then + rm -Rf ${D}${bindir}/.debug + fi +} + +FILES_${PN} = " \ + ${datadir}/openocd/* \ + ${bindir}/openocd \ + " diff --git a/recipes-devtools/openocd/openocd-stm32mp/0001-Add-support-of-STLINK-for-stm32mp1.patch b/recipes-devtools/openocd/openocd-stm32mp/0001-Add-support-of-STLINK-for-stm32mp1.patch new file mode 100644 index 0000000..bff518d --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp/0001-Add-support-of-STLINK-for-stm32mp1.patch @@ -0,0 +1,5378 @@ +From 0998211996ffbc68670a07c49d3966b34c5b0a37 Mon Sep 17 00:00:00 2001 +From: Romuald JEANNE +Date: Mon, 15 Oct 2018 17:40:36 +0200 +Subject: [PATCH] Add support of STLINK for stm32mp1 + +Signed-off-by: Romuald JEANNE +--- + contrib/60-openocd.rules | 7 + + src/helper/bits.h | 88 ++ + src/helper/log.h | 1 + + src/jtag/adapter.c | 132 +++ + src/jtag/core.c | 66 +- + src/jtag/drivers/stlink_usb.c | 1413 ++++++++++++++++++++++++++---- + src/jtag/interfaces.c | 6 + + src/jtag/startup.tcl | 12 +- + src/jtag/tcl.c | 6 +- + src/server/gdb_server.c | 49 +- + src/target/Makefile.am | 4 + + src/target/adi_v5_stlink.c | 311 +++++++ + src/target/arm_adi_v5.c | 28 + + src/target/arm_adi_v5.h | 10 + + src/target/arm_dap.c | 9 + + src/target/arm_dpm.h | 29 +- + src/target/armv7a.c | 19 +- + src/target/armv7a.h | 2 + + src/target/armv7m.c | 13 + + src/target/armv7m.h | 2 +- + src/target/cortex_a.c | 313 +++++-- + src/target/cortex_m.c | 138 ++- + src/target/cortex_m.h | 1 + + src/target/startup.tcl | 250 +++++- + src/target/target.c | 189 +++- + src/target/target.h | 22 +- + src/target/target_type.h | 16 + + src/transport/transport.h | 2 + + tcl/board/stm32mp15x_dk2.cfg | 9 + + tcl/board/stm32mp15x_ev1_jlink_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev1_jlink_swd.cfg | 9 + + tcl/board/stm32mp15x_ev1_stlink_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev1_stlink_swd.cfg | 9 + + tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev1_ulink2_swd.cfg | 9 + + tcl/board/stm32mp15x_ev2_jlink_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev2_jlink_swd.cfg | 9 + + tcl/board/stm32mp15x_ev2_stlink_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev2_stlink_swd.cfg | 9 + + tcl/board/stm32mp15x_ev2_ulink2_jtag.cfg | 9 + + tcl/board/stm32mp15x_ev2_ulink2_swd.cfg | 9 + + tcl/interface/stlink-dap.cfg | 18 + + tcl/target/atsamv.cfg | 2 +- + tcl/target/imx6.cfg | 50 +- + tcl/target/kx.cfg | 7 +- + tcl/target/stm32f7x.cfg | 2 +- + tcl/target/stm32mp15x.cfg | 165 ++++ + tcl/target/stm32mp15x_stpmu1.cfg | 113 +++ + tcl/target/swj-dp.tcl | 2 + + 49 files changed, 3181 insertions(+), 433 deletions(-) + create mode 100644 src/helper/bits.h + create mode 100644 src/target/adi_v5_stlink.c + create mode 100644 tcl/board/stm32mp15x_dk2.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_jlink_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_jlink_swd.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_stlink_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_stlink_swd.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev1_ulink2_swd.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_jlink_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_jlink_swd.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_stlink_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_stlink_swd.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_ulink2_jtag.cfg + create mode 100644 tcl/board/stm32mp15x_ev2_ulink2_swd.cfg + create mode 100644 tcl/interface/stlink-dap.cfg + create mode 100644 tcl/target/stm32mp15x.cfg + create mode 100644 tcl/target/stm32mp15x_stpmu1.cfg + +diff --git a/contrib/60-openocd.rules b/contrib/60-openocd.rules +index 692e1b1..de47541 100644 +--- a/contrib/60-openocd.rules ++++ b/contrib/60-openocd.rules +@@ -63,6 +63,13 @@ ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", MODE="660", GROUP="plugdev", + + # STLink v2-1 + ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", MODE="660", GROUP="plugdev", TAG+="uaccess" ++ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3752", MODE="660", GROUP="plugdev", TAG+="uaccess" ++ ++# STLink v3 ++ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374d", MODE="660", GROUP="plugdev", TAG+="uaccess" ++ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374e", MODE="660", GROUP="plugdev", TAG+="uaccess" ++ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374f", MODE="660", GROUP="plugdev", TAG+="uaccess" ++ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3753", MODE="660", GROUP="plugdev", TAG+="uaccess" + + # Cypress KitProg in KitProg mode + ATTRS{idVendor}=="04b4", ATTRS{idProduct}=="f139", MODE="660", GROUP="plugdev", TAG+="uaccess" +diff --git a/src/helper/bits.h b/src/helper/bits.h +new file mode 100644 +index 0000000..988dde0 +--- /dev/null ++++ b/src/helper/bits.h +@@ -0,0 +1,88 @@ ++/* ++ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved ++ * Author(s): Antonio Borneo for STMicroelectronics ++ * ++ * This program is free software; you can redistribute it and/or modify ++ * it under the terms of the GNU General Public License as published by ++ * the Free Software Foundation; either version 2 of the License, or ++ * (at your option) any later version. ++ * ++ * This program is distributed in the hope that it will be useful ++ * but WITHOUT ANY WARRANTY; without even the implied warranty of ++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ++ * GNU General Public License for more details. ++ * ++ * You should have received a copy of the GNU General Public License ++ * along with this program. If not, see . ++ */ ++ ++/* ++ * The content of this file is mainly copied/inspired from Linux kernel ++ * code in include/linux/types.h include/linux/bitmap.h include/linux/bitops.h ++ */ ++ ++#ifndef OPENOCD_HELPER_BITS_H ++#define OPENOCD_HELPER_BITS_H ++ ++#include ++ ++#define BIT(nr) (1UL << (nr)) ++#define BITS_PER_BYTE 8 ++#define BITS_PER_LONG (BITS_PER_BYTE * sizeof(long)) ++#define BITS_TO_LONGS(nr) DIV_ROUND_UP(nr, BITS_PER_BYTE * sizeof(long)) ++#define BIT_MASK(nr) (1UL << ((nr) % BITS_PER_LONG)) ++#define BIT_WORD(nr) ((nr) / BITS_PER_LONG) ++#define DECLARE_BITMAP(name, bits) unsigned long name[BITS_TO_LONGS(bits)] ++ ++/** ++ * bitmap_zero - Clears all the bits in memory ++ * @dst: the address of the bitmap ++ * @nbits: the number of bits to clear ++ */ ++static inline void bitmap_zero(unsigned long *dst, unsigned int nbits) ++{ ++ if (nbits <= BITS_PER_LONG) ++ *dst = 0UL; ++ else { ++ unsigned int len = BITS_TO_LONGS(nbits) * sizeof(unsigned long); ++ memset(dst, 0, len); ++ } ++} ++ ++/** ++ * clear_bit - Clear a bit in memory ++ * @nr: the bit to set ++ * @addr: the address to start counting from ++ */ ++static inline void clear_bit(int nr, volatile unsigned long *addr) ++{ ++ unsigned long mask = BIT_MASK(nr); ++ unsigned long *p = ((unsigned long *)addr) + BIT_WORD(nr); ++ ++ *p &= ~mask; ++} ++ ++/** ++ * set_bit - Set a bit in memory ++ * @nr: the bit to set ++ * @addr: the address to start counting from ++ */ ++static inline void set_bit(int nr, volatile unsigned long *addr) ++{ ++ unsigned long mask = BIT_MASK(nr); ++ unsigned long *p = ((unsigned long *)addr) + BIT_WORD(nr); ++ ++ *p |= mask; ++} ++ ++/** ++ * test_bit - Determine whether a bit is set ++ * @nr: bit number to test ++ * @addr: Address to start counting from ++ */ ++static inline int test_bit(int nr, const volatile unsigned long *addr) ++{ ++ return 1UL & (addr[BIT_WORD(nr)] >> (nr & (BITS_PER_LONG-1))); ++} ++ ++#endif /* OPENOCD_HELPER_BITS_H */ +diff --git a/src/helper/log.h b/src/helper/log.h +index d60587f..751ea17 100644 +--- a/src/helper/log.h ++++ b/src/helper/log.h +@@ -151,6 +151,7 @@ extern int debug_level; + #define ERROR_WAIT (-5) + /* ERROR_TIMEOUT is already taken by winerror.h. */ + #define ERROR_TIMEOUT_REACHED (-6) ++#define ERROR_OP_NOT_SUPPORTED (-7) + + + #endif /* OPENOCD_HELPER_LOG_H */ +diff --git a/src/jtag/adapter.c b/src/jtag/adapter.c +index 2035788..d62c9a1 100644 +--- a/src/jtag/adapter.c ++++ b/src/jtag/adapter.c +@@ -31,6 +31,7 @@ + #endif + + #include "jtag.h" ++#include "swd.h" + #include "minidriver.h" + #include "interface.h" + #include "interfaces.h" +@@ -47,6 +48,26 @@ + + extern struct jtag_interface *jtag_interface; + const char * const jtag_only[] = { "jtag", NULL }; ++static const Jim_Nvp nvp_assert[] = { ++ { .name = "assert", NVP_ASSERT }, ++ { .name = "deassert", NVP_DEASSERT }, ++ { .name = "T", NVP_ASSERT }, ++ { .name = "F", NVP_DEASSERT }, ++ { .name = "t", NVP_ASSERT }, ++ { .name = "f", NVP_DEASSERT }, ++ { .name = NULL, .value = -1 } ++}; ++ ++#define RESET_TEST_INDEPENDENT_TRST -1 ++static const Jim_Nvp nvp_reset_config_includes[] = { ++ { .name = "srst", RESET_HAS_SRST }, ++ { .name = "trst", RESET_HAS_TRST }, ++ { .name = "srst_pulls_trst_or_combined", RESET_SRST_PULLS_TRST }, ++ { .name = "trst_pulls_srst_or_combined", RESET_TRST_PULLS_SRST }, ++ { .name = "srst_nogate", RESET_SRST_NO_GATING }, ++ { .name = "connect_assert_srst", RESET_CNCT_UNDER_SRST }, ++ { .name = "independent_trst", RESET_TEST_INDEPENDENT_TRST }, ++}; + + static int jim_adapter_name(Jim_Interp *interp, int argc, Jim_Obj * const *argv) + { +@@ -400,6 +421,42 @@ next: + return ERROR_OK; + } + ++static int jim_reset_config_includes(Jim_Interp *interp, int argc, Jim_Obj * const *argv) ++{ ++ Jim_GetOptInfo goi; ++ Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1); ++ ++ if (goi.argc < 1) { ++ Jim_WrongNumArgs(interp, 1, argv, "[srst_any][trst_any]..."); ++ return JIM_ERR; ++ } ++ ++ int cfg = jtag_get_reset_config(); ++ bool result = true; ++ ++ while (goi.argc) { ++ Jim_Nvp *n; ++ int e = Jim_GetOpt_Nvp(&goi, nvp_reset_config_includes, &n); ++ if (e != JIM_OK) { ++ Jim_GetOpt_NvpUnknown(&goi, nvp_reset_config_includes, 1); ++ return e; ++ } ++ ++ if (n->value == RESET_TEST_INDEPENDENT_TRST) { ++ bool indep_trst = cfg & RESET_HAS_TRST; ++ if (cfg & RESET_HAS_SRST && cfg & RESET_SRST_PULLS_TRST) ++ indep_trst = false; ++ ++ result = result && indep_trst; ++ } else { ++ result = result && cfg & n->value; ++ } ++ } ++ ++ Jim_SetResultString(interp, result ? "1" : "0", -1); ++ return JIM_OK; ++} ++ + COMMAND_HANDLER(handle_adapter_nsrst_delay_command) + { + if (CMD_ARGC > 1) +@@ -456,6 +513,57 @@ COMMAND_HANDLER(handle_adapter_khz_command) + return retval; + } + ++static int jim_arp_init_reset(Jim_Interp *interp, int argc, Jim_Obj*const *argv) ++{ ++ int e = ERROR_OK; ++ Jim_GetOptInfo goi; ++ Jim_GetOpt_Setup(&goi, interp, argc-1, argv + 1); ++ if (goi.argc != 0) { ++ Jim_WrongNumArgs(goi.interp, 1, goi.argv-1, "(no params)"); ++ return JIM_ERR; ++ } ++ struct command_context *context = current_command_context(interp); ++ if (transport_is_jtag() || transport_is_stlink_jtag()) ++ e = jtag_init_reset(context); ++ else if (transport_is_swd() || transport_is_stlink_swd()) ++ e = swd_init_reset(context); ++ ++ if (e != ERROR_OK) { ++ Jim_Obj *eObj = Jim_NewIntObj(goi.interp, e); ++ Jim_SetResultFormatted(goi.interp, "error: %#s", eObj); ++ Jim_FreeNewObj(goi.interp, eObj); ++ return JIM_ERR; ++ } ++ return JIM_OK; ++} ++ ++static int jim_arp_adapter_reset(Jim_Interp *interp, int argc, Jim_Obj *const *argv) ++{ ++ Jim_GetOptInfo goi; ++ Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1); ++ ++ if (goi.argc != 1) { ++ Jim_WrongNumArgs(interp, 0, argv, "([tT]|[fF]|assert|deassert)"); ++ return JIM_ERR; ++ } ++ ++ Jim_Nvp *n; ++ int e = Jim_GetOpt_Nvp(&goi, nvp_assert, &n); ++ if (e != JIM_OK) { ++ Jim_GetOpt_NvpUnknown(&goi, nvp_assert, 1); ++ return e; ++ } ++ ++ if (jtag_get_reset_config() & RESET_HAS_SRST) { ++ if (n->value == NVP_ASSERT) ++ adapter_assert_reset(); ++ else ++ adapter_deassert_reset(); ++ } ++ ++ return JIM_OK; ++} ++ + static const struct command_registration interface_command_handlers[] = { + { + .name = "adapter_khz", +@@ -509,6 +617,21 @@ static const struct command_registration interface_command_handlers[] = { + .help = "List all built-in debug adapter interfaces (drivers)", + }, + { ++ .name = "arp_init_reset", ++ .mode = COMMAND_ANY, ++ .jim_handler = jim_arp_init_reset, ++ .help = "Uses TRST and SRST to try resetting everything on the JTAG scan chain." ++ " If SWD transport is selected command uses SRST only." ++ " SRST is left asserted if 'reset_config srst_no_gating'." ++ }, ++ { ++ .name = "arp_adapter_reset", ++ .mode = COMMAND_ANY, ++ .jim_handler = jim_arp_adapter_reset, ++ .help = "Controls SRST line.", ++ .usage = "[assert|deassert]" ++ }, ++ { + .name = "reset_config", + .handler = handle_reset_config_command, + .mode = COMMAND_ANY, +@@ -520,6 +643,15 @@ static const struct command_registration interface_command_handlers[] = { + "[srst_push_pull|srst_open_drain] " + "[connect_deassert_srst|connect_assert_srst]", + }, ++ { ++ .name = "reset_config_includes", ++ .jim_handler = jim_reset_config_includes, ++ .mode = COMMAND_ANY, ++ .help = "Test adapter reset configuration", ++ .usage = "[trst] [srst] [srst_nogate] [connect_assert_srst]" ++ " [srst_pulls_trst_or_combined] [trst_pulls_srst_or_combined]" ++ " [independent_trst]" ++ }, + COMMAND_REGISTRATION_DONE + }; + +diff --git a/src/jtag/core.c b/src/jtag/core.c +index f90ae99..e11a633 100644 +--- a/src/jtag/core.c ++++ b/src/jtag/core.c +@@ -160,7 +160,7 @@ bool is_jtag_poll_safe(void) + * It is also implicitly disabled while TRST is active and + * while SRST is gating the JTAG clock. + */ +- if (!transport_is_jtag()) ++ if (!transport_is_jtag() && !transport_is_stlink_jtag()) + return jtag_poll; + + if (!jtag_poll || jtag_trst != 0) +@@ -640,11 +640,11 @@ void swd_add_reset(int req_srst) + /* SRST resets everything hooked up to that signal */ + jtag_srst = req_srst; + if (jtag_srst) { +- LOG_DEBUG("SRST line asserted"); ++ LOG_INFO("SRST line asserted"); + if (adapter_nsrst_assert_width) + jtag_add_sleep(adapter_nsrst_assert_width * 1000); + } else { +- LOG_DEBUG("SRST line released"); ++ LOG_INFO("SRST line released"); + if (adapter_nsrst_delay) + jtag_add_sleep(adapter_nsrst_delay * 1000); + } +@@ -722,11 +722,11 @@ void jtag_add_reset(int req_tlr_or_trst, int req_srst) + if (jtag_srst != new_srst) { + jtag_srst = new_srst; + if (jtag_srst) { +- LOG_DEBUG("SRST line asserted"); ++ LOG_INFO("SRST line asserted"); + if (adapter_nsrst_assert_width) + jtag_add_sleep(adapter_nsrst_assert_width * 1000); + } else { +- LOG_DEBUG("SRST line released"); ++ LOG_INFO("SRST line released"); + if (adapter_nsrst_delay) + jtag_add_sleep(adapter_nsrst_delay * 1000); + } +@@ -1404,7 +1404,7 @@ int jtag_init_inner(struct command_context *cmd_ctx) + int retval; + bool issue_setup = true; + +- LOG_DEBUG("Init JTAG chain"); ++ LOG_INFO("Init JTAG chain"); + + tap = jtag_tap_next_enabled(NULL); + if (tap == NULL) { +@@ -1505,9 +1505,10 @@ int swd_init_reset(struct command_context *cmd_ctx) + + LOG_DEBUG("Initializing with hard SRST reset"); + +- if (jtag_reset_config & RESET_HAS_SRST) ++ if ((jtag_reset_config & (RESET_HAS_SRST|RESET_SRST_NO_GATING)) == (RESET_HAS_SRST|RESET_SRST_NO_GATING)) + swd_add_reset(1); +- swd_add_reset(0); ++ /* leave SRST asserted if SWD trasport is usable in reset state */ ++ + retval = jtag_execute_queue(); + return retval; + } +@@ -1544,28 +1545,30 @@ int jtag_init_reset(struct command_context *cmd_ctx) + * REVISIT once Tcl code can read the reset_config modes, this won't + * need to be a C routine at all... + */ +- if (jtag_reset_config & RESET_HAS_SRST) { ++ if ((jtag_reset_config & RESET_HAS_SRST) ++ && (jtag_reset_config & RESET_SRST_NO_GATING)) { + jtag_add_reset(1, 1); +- if ((jtag_reset_config & RESET_SRST_PULLS_TRST) == 0) +- jtag_add_reset(0, 1); + } else { + jtag_add_reset(1, 0); /* TAP_RESET, using TMS+TCK or TRST */ + } + + /* some targets enable us to connect with srst asserted */ +- if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { +- if (jtag_reset_config & RESET_SRST_NO_GATING) +- jtag_add_reset(0, 1); +- else { ++ if (jtag_reset_config & RESET_SRST_NO_GATING) ++ jtag_add_reset(0, 1); ++ else { ++ if (jtag_reset_config & RESET_CNCT_UNDER_SRST) + LOG_WARNING("\'srst_nogate\' reset_config option is required"); +- jtag_add_reset(0, 0); +- } +- } else + jtag_add_reset(0, 0); ++ } ++ + retval = jtag_execute_queue(); + if (retval != ERROR_OK) + return retval; + ++ /* ST-Link cannot scan the JTAG chain */ ++ if (transport_is_stlink_jtag()) ++ return ERROR_OK; ++ + /* Check that we can communication on the JTAG chain + eventually we want to + * be able to perform enumeration only after OpenOCD has started + * telnet and GDB server +@@ -1816,30 +1819,33 @@ bool transport_is_jtag(void) + + void adapter_assert_reset(void) + { +- if (transport_is_jtag()) { ++ if (transport_is_jtag() || transport_is_stlink_jtag()) { + if (jtag_reset_config & RESET_SRST_PULLS_TRST) + jtag_add_reset(1, 1); + else + jtag_add_reset(0, 1); +- } else if (transport_is_swd()) ++ } else if (transport_is_swd() || transport_is_stlink_swd()) { + swd_add_reset(1); +- else if (get_current_transport() != NULL) +- LOG_ERROR("reset is not supported on %s", +- get_current_transport()->name); +- else ++ } else if (get_current_transport() != NULL) { ++ const char *name = get_current_transport()->name; ++ /* hla transports control srst through target assert_reset ++ * and deassert_reset calls so no action here ++ * Show error for other transports */ ++ if (strcmp(name, "hla_swd") && strcmp(name, "hla_jtag") ++ && strcmp(name, "stlink_swim")) ++ LOG_ERROR("reset is not supported on %s", name); ++ } else { + LOG_ERROR("transport is not selected"); ++ } + } + + void adapter_deassert_reset(void) + { +- if (transport_is_jtag()) ++ if (transport_is_jtag() || transport_is_stlink_jtag()) + jtag_add_reset(0, 0); +- else if (transport_is_swd()) ++ else if (transport_is_swd() || transport_is_stlink_swd()) + swd_add_reset(0); +- else if (get_current_transport() != NULL) +- LOG_ERROR("reset is not supported on %s", +- get_current_transport()->name); +- else ++ else if (get_current_transport() == NULL) + LOG_ERROR("transport is not selected"); + } + +diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c +index d9ca53e..f55a608 100644 +--- a/src/jtag/drivers/stlink_usb.c ++++ b/src/jtag/drivers/stlink_usb.c +@@ -30,17 +30,21 @@ + #endif + + /* project specific includes */ ++#include + #include + #include + #include + #include + #include ++#include + #include + + #include + + #include "libusb_common.h" + ++#define HLA_DEFAULT_APNUN (0) ++ + #define ENDPOINT_IN 0x80 + #define ENDPOINT_OUT 0x00 + +@@ -60,13 +64,32 @@ + #define STLINK_CMD_SIZE_V2 (16) + #define STLINK_CMD_SIZE_V1 (10) + ++#define STLINK_VID (0x0483) + #define STLINK_V1_PID (0x3744) + #define STLINK_V2_PID (0x3748) + #define STLINK_V2_1_PID (0x374B) ++#define STLINK_V2_1_NO_MSD_PID (0x3752) ++#define STLINK_V3_DFU_PID (0x374D) ++#define STLINK_V3E_PID (0x374E) ++#define STLINK_V3S_PID (0x374F) ++#define STLINK_V3_2VCP_PID (0x3753) + + /* the current implementation of the stlink limits +- * 8bit read/writes to max 64 bytes. */ +-#define STLINK_MAX_RW8 (64) ++ * 8bit read/writes to max 64 bytes. ++ * The limit is 512 bytes with stlink v3 on usb 2.0 high speed */ ++/* ++ * ST-Link/V1, ST-Link/V2 and ST-Link/V2.1 are full-speed USB devices and ++ * this limits the bulk packet size and the 8bit read/writes to max 64 bytes. ++ * ST-Link/V3 is a high speed USB 2.0 and the limit is 512 bytes. ++ */ ++#define STLINKV2_MAX_RW8 (64) ++#define STLINKV3_MAX_RW8 (512) ++ ++/* ++ * ARM IHI 0031E: TAR Automatic address increment is only guaranteed to ++ * operate on the 10 least significant bits of the address ++ */ ++#define TAR_AUTOINCR_BLOCK (1 << 10) + + /* "WAIT" responses will be retried (with exponential backoff) at + * most this many times before failing to caller. +@@ -76,6 +99,7 @@ + enum stlink_jtag_api_version { + STLINK_JTAG_API_V1 = 1, + STLINK_JTAG_API_V2, ++ STLINK_JTAG_API_V3, + }; + + /** */ +@@ -134,35 +158,57 @@ struct stlink_usb_handle_s { + bool reconnect_pending; + }; + ++/* status codes */ + #define STLINK_SWIM_ERR_OK 0x00 + #define STLINK_SWIM_BUSY 0x01 + #define STLINK_DEBUG_ERR_OK 0x80 + #define STLINK_DEBUG_ERR_FAULT 0x81 ++ ++#define STLINK_JTAG_SPI_ERROR 0x02 ++#define STLINK_JTAG_DMA_ERROR 0x03 ++#define STLINK_JTAG_UNKNOWN_JTAG_CHAIN 0x04 ++#define STLINK_JTAG_NO_DEVICE_CONNECTED 0x05 ++#define STLINK_JTAG_INTERNAL_ERROR 0x06 ++#define STLINK_JTAG_CMD_WAIT 0x07 ++#define STLINK_JTAG_CMD_ERROR 0x08 ++#define STLINK_JTAG_GET_IDCODE_ERROR 0x09 ++#define STLINK_JTAG_ALIGNMENT_ERROR 0x0A ++#define STLINK_JTAG_DBG_POWER_ERROR 0x0B ++#define STLINK_JTAG_WRITE_ERROR 0x0C ++#define STLINK_JTAG_WRITE_VERIF_ERROR 0x0D ++#define STLINK_JTAG_ALREADY_OPENED_IN_OTHER_MODE 0x0E ++ + #define STLINK_SWD_AP_WAIT 0x10 + #define STLINK_SWD_AP_FAULT 0x11 + #define STLINK_SWD_AP_ERROR 0x12 + #define STLINK_SWD_AP_PARITY_ERROR 0x13 +-#define STLINK_JTAG_WRITE_ERROR 0x0c +-#define STLINK_JTAG_WRITE_VERIF_ERROR 0x0d + #define STLINK_SWD_DP_WAIT 0x14 + #define STLINK_SWD_DP_FAULT 0x15 + #define STLINK_SWD_DP_ERROR 0x16 + #define STLINK_SWD_DP_PARITY_ERROR 0x17 +- + #define STLINK_SWD_AP_WDATA_ERROR 0x18 + #define STLINK_SWD_AP_STICKY_ERROR 0x19 +-#define STLINK_SWD_AP_STICKYORUN_ERROR 0x1a ++#define STLINK_SWD_AP_STICKYORUN_ERROR 0x1A ++#define STLINK_AP_ALREADY_USED 0x1B ++#define STLINK_TRACE_AP_TURNAROUND 0x1C ++#define STLINK_BAD_AP 0x1D ++#define STLINK_SWV_NOT_AVAILABLE 0x20 ++#define STLINK_NO_JUMP_TO_USB_LOADER 0x21 ++#define STLINK_JTAG_TCPID_NOT_FOUND 0x30 ++#define STLINK_JTAG_TCPID_MAX_REACHED 0x31 + + #define STLINK_CORE_RUNNING 0x80 + #define STLINK_CORE_HALTED 0x81 + #define STLINK_CORE_STAT_UNKNOWN -1 + ++/* stlink commands */ + #define STLINK_GET_VERSION 0xF1 + #define STLINK_DEBUG_COMMAND 0xF2 + #define STLINK_DFU_COMMAND 0xF3 + #define STLINK_SWIM_COMMAND 0xF4 + #define STLINK_GET_CURRENT_MODE 0xF5 + #define STLINK_GET_TARGET_VOLTAGE 0xF7 ++#define STLINK_APIV3_GET_VERSION_EX 0xFB + + #define STLINK_DEV_DFU_MODE 0x00 + #define STLINK_DEV_MASS_MODE 0x01 +@@ -204,7 +250,6 @@ struct stlink_usb_handle_s { + #define STLINK_SWIM_READMEM 0x0b + #define STLINK_SWIM_READBUF 0x0c + +-#define STLINK_DEBUG_ENTER_JTAG 0x00 + #define STLINK_DEBUG_GETSTATUS 0x01 + #define STLINK_DEBUG_FORCEDEBUG 0x02 + #define STLINK_DEBUG_APIV1_RESETSYS 0x03 +@@ -224,6 +269,7 @@ struct stlink_usb_handle_s { + + #define STLINK_DEBUG_ENTER_JTAG 0x00 + #define STLINK_DEBUG_ENTER_SWD 0xa3 ++#define STLINK_ENTER_JTAG_NO_CORE_RESET 0xA4 + + #define STLINK_DEBUG_APIV1_ENTER 0x20 + #define STLINK_DEBUG_EXIT 0x21 +@@ -236,24 +282,55 @@ struct stlink_usb_handle_s { + #define STLINK_DEBUG_APIV2_WRITEREG 0x34 + #define STLINK_DEBUG_APIV2_WRITEDEBUGREG 0x35 + #define STLINK_DEBUG_APIV2_READDEBUGREG 0x36 +- ++#define STLINK_DEBUG_APIV2_SETWATCHPOINT2 0x37 ++#define STLINK_DEBUG_APIV2_SETFP2 0x38 ++#define STLINK_DEBUG_APIV2_CLEARFP2 0x39 + #define STLINK_DEBUG_APIV2_READALLREGS 0x3A + #define STLINK_DEBUG_APIV2_GETLASTRWSTATUS 0x3B + #define STLINK_DEBUG_APIV2_DRIVE_NRST 0x3C ++#define STLINK_DEBUG_APIV2_READFPUREGS 0x3D ++#define STLINK_DEBUG_APIV3_GETLASTRWSTATUS 0x3E + + #define STLINK_DEBUG_APIV2_START_TRACE_RX 0x40 + #define STLINK_DEBUG_APIV2_STOP_TRACE_RX 0x41 + #define STLINK_DEBUG_APIV2_GET_TRACE_NB 0x42 + #define STLINK_DEBUG_APIV2_SWD_SET_FREQ 0x43 ++#define STLINK_DEBUG_APIV2_JTAG_SET_FREQ 0x44 ++#define STLINK_JTAG_READ_DAP_REG 0x45 ++#define STLINK_JTAG_WRITE_DAP_REG 0x46 ++#define STLINK_DEBUG_APIV2_READMEM_16BIT 0x47 ++#define STLINK_DEBUG_APIV2_WRITEMEM_16BIT 0x48 ++#define STLINK_DEBUG_APIV2_BLINK_LED 0x49 ++#define STLINK_DEBUG_APIV2_GET_DISK_NAME 0x4A ++#define STLINK_DEBUG_APIV2_INIT_AP 0x4B ++#define STLINK_DEBUG_APIV2_CLOSE_AP_DBG 0x4C ++ ++#define STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC 0x50 + ++#define STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC 0x54 ++ ++#define STLINK_APIV3_SET_COM_FREQ 0x61 ++#define STLINK_APIV3_GET_COM_FREQ 0x62 ++#define STLINK_APIV3_SWITCH_STLINK_FREQ 0x63 ++ ++/* parameters */ + #define STLINK_DEBUG_APIV2_DRIVE_NRST_LOW 0x00 + #define STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH 0x01 + #define STLINK_DEBUG_APIV2_DRIVE_NRST_PULSE 0x02 + ++#define STLINK_DEBUG_FLASHPATCH_LOWER 0 ++#define STLINK_DEBUG_FLASHPATCH_UPPER 1 ++#define STLINK_DEBUG_FLASHPATCH_ALL 2 ++ ++#define JTAG_AP_CORTEXM_CORE 1 ++#define JTAG_AP_NO_CORE 0 ++ + #define STLINK_TRACE_SIZE 4096 + #define STLINK_TRACE_MAX_HZ 2000000 + #define STLINK_TRACE_MIN_VERSION 13 + ++#define STLINK_V3_MAX_FREQ_NB 10 ++ + /** */ + enum stlink_mode { + STLINK_MODE_UNKNOWN = 0, +@@ -267,10 +344,13 @@ enum stlink_mode { + #define REQUEST_SENSE 0x03 + #define REQUEST_SENSE_LENGTH 18 + +-static const struct { ++struct speed_map { + int speed; + int speed_divisor; +-} stlink_khz_to_speed_map[] = { ++}; ++ ++/* SWD clock speed */ ++static const struct speed_map stlink_khz_to_speed_map[] = { + {4000, 0}, + {1800, 1}, /* default */ + {1200, 2}, +@@ -285,8 +365,37 @@ static const struct { + {5, 798} + }; + ++/* JTAG clock speed */ ++static const struct speed_map stlink_khz_to_speed_map_jtag[] = { ++ {18000, 2}, ++ {9000, 4}, ++ {4500, 8}, ++ {2250, 16}, ++ {1125, 32}, /* default */ ++ {562, 64}, ++ {281, 128}, ++ {140, 256} ++}; ++ + static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size); + static int stlink_swim_status(void *handle); ++static int stlink_usb_reset(void *handle); ++ ++ ++/** */ ++__attribute__((unused)) ++static unsigned int stlink_usb_block(void *handle) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ if (h->version.stlink == 3) ++ return STLINKV3_MAX_RW8; ++ else ++ return STLINKV2_MAX_RW8; ++} ++ + + /** */ + static int stlink_usb_xfer_v1_get_status(void *handle) +@@ -453,6 +562,36 @@ static int stlink_usb_error_check(void *handle) + case STLINK_SWD_DP_WAIT: + LOG_DEBUG("wait status SWD_DP_WAIT (0x%x)", STLINK_SWD_DP_WAIT); + return ERROR_WAIT; ++ case STLINK_JTAG_SPI_ERROR: ++ LOG_DEBUG("JTAG_INTERNAL_ERROR (SPI)"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_DMA_ERROR: ++ LOG_DEBUG("JTAG_INTERNAL_ERROR (DMA)"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_UNKNOWN_JTAG_CHAIN: ++ LOG_DEBUG("UNKNOWN_JTAG_CHAIN"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_NO_DEVICE_CONNECTED: ++ LOG_DEBUG("NO_DEVICE_CONNECTED"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_INTERNAL_ERROR: ++ LOG_DEBUG("JTAG_INTERNAL_ERROR"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_CMD_WAIT: ++ LOG_DEBUG("wait status STLINK_JTAG_CMD_WAIT"); ++ return ERROR_WAIT; ++ case STLINK_JTAG_CMD_ERROR: ++ LOG_DEBUG("JTAG_CMD_ERROR"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_GET_IDCODE_ERROR: ++ LOG_DEBUG("JTAG_GET_IDCODE_ERROR"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_ALIGNMENT_ERROR: ++ LOG_DEBUG("JTAG_ALIGNMENT_ERROR"); ++ return ERROR_FAIL; ++ case STLINK_JTAG_DBG_POWER_ERROR: ++ LOG_DEBUG("JTAG_DBG_POWER_ERROR"); ++ return ERROR_FAIL; + case STLINK_JTAG_WRITE_ERROR: + LOG_DEBUG("Write error"); + return ERROR_FAIL; +@@ -466,7 +605,7 @@ static int stlink_usb_error_check(void *handle) + * This fix allows CDT plugin to visualize memory. + */ + LOG_DEBUG("STLINK_SWD_AP_FAULT"); +- return ERROR_FAIL; ++ return ERROR_OK; + case STLINK_SWD_AP_ERROR: + LOG_DEBUG("STLINK_SWD_AP_ERROR"); + return ERROR_FAIL; +@@ -487,10 +626,25 @@ static int stlink_usb_error_check(void *handle) + return ERROR_FAIL; + case STLINK_SWD_AP_STICKY_ERROR: + LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR"); ++ /* Seen when reading address out of range (0xFFFFFFFF) */ ++ /* Seems usb_reset helps the following r/w accesses */ ++ stlink_usb_reset(h); + return ERROR_FAIL; + case STLINK_SWD_AP_STICKYORUN_ERROR: + LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR"); + return ERROR_FAIL; ++ case STLINK_JTAG_ALREADY_OPENED_IN_OTHER_MODE: ++ LOG_DEBUG("JTAG_ALREADY_OPENED_IN_OTHER_MODE"); ++ return ERROR_FAIL; ++ case STLINK_AP_ALREADY_USED: ++ LOG_DEBUG("AP_ALREADY_USED"); ++ return ERROR_FAIL; ++ case STLINK_TRACE_AP_TURNAROUND: ++ LOG_DEBUG("TRACE_AP_TURNAROUND"); ++ return ERROR_FAIL; ++ case STLINK_BAD_AP: ++ LOG_DEBUG("BAD_AP"); ++ return ERROR_FAIL; + default: + LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]); + return ERROR_FAIL; +@@ -603,40 +757,62 @@ static int stlink_usb_version(void *handle) + { + int res; + uint16_t v; ++ uint8_t m_version = 0; + struct stlink_usb_handle_s *h = handle; + + assert(handle != NULL); + +- stlink_usb_init_buffer(handle, h->rx_ep, 6); ++ /* FIXME: don't use PID, but first old command to get V3, then new command */ ++ if (h->pid == STLINK_V3E_PID || h->pid == STLINK_V3S_PID || h->pid == STLINK_V3_2VCP_PID) { ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_VERSION_EX; ++ res = stlink_usb_xfer(handle, h->databuf, 12); + +- h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION; ++ if (res != ERROR_OK) ++ return res; + +- res = stlink_usb_xfer(handle, h->databuf, 6); ++ h->version.stlink = h->databuf[0]; ++ h->version.swim = h->databuf[1]; ++ h->version.jtag = h->databuf[2]; ++ m_version = h->databuf[3]; + +- if (res != ERROR_OK) +- return res; ++ h->vid = (h->databuf[9] << 8) | h->databuf[8]; ++ h->pid = (h->databuf[11] << 8) | h->databuf[10]; + +- v = (h->databuf[0] << 8) | h->databuf[1]; ++ h->version.jtag_api_max = STLINK_JTAG_API_V3; ++ } else { ++ stlink_usb_init_buffer(handle, h->rx_ep, 6); ++ h->cmdbuf[h->cmdidx++] = STLINK_GET_VERSION; + +- h->version.stlink = (v >> 12) & 0x0f; +- h->version.jtag = (v >> 6) & 0x3f; +- h->version.swim = v & 0x3f; +- h->vid = buf_get_u32(h->databuf, 16, 16); +- h->pid = buf_get_u32(h->databuf, 32, 16); ++ res = stlink_usb_xfer(handle, h->databuf, 6); + +- /* set the supported jtag api version +- * API V2 is supported since JTAG V11 +- */ +- if (h->version.jtag >= 11) +- h->version.jtag_api_max = STLINK_JTAG_API_V2; +- else +- h->version.jtag_api_max = STLINK_JTAG_API_V1; ++ if (res != ERROR_OK) ++ return res; ++ ++ v = (h->databuf[0] << 8) | h->databuf[1]; ++ ++ h->version.stlink = (v >> 12) & 0x0f; ++ h->version.jtag = (v >> 6) & 0x3f; ++ h->version.swim = v & 0x3f; ++ h->vid = buf_get_u32(h->databuf, 16, 16); ++ h->pid = buf_get_u32(h->databuf, 32, 16); ++ ++ /* set the supported jtag api version ++ * API V2 is supported since JTAG V11 ++ */ ++ if (h->version.jtag >= 11) ++ h->version.jtag_api_max = STLINK_JTAG_API_V2; ++ else ++ h->version.jtag_api_max = STLINK_JTAG_API_V1; ++ } + +- LOG_INFO("STLINK v%d JTAG v%d API v%d SWIM v%d VID 0x%04X PID 0x%04X", ++ LOG_INFO("STLINK v%d%s JTAG v%d API v%d %s%d VID 0x%04X PID 0x%04X", + h->version.stlink, ++ (h->pid == STLINK_V2_1_PID) ? ".1" : "", + h->version.jtag, +- (h->version.jtag_api_max == STLINK_JTAG_API_V1) ? 1 : 2, +- h->version.swim, ++ h->version.jtag_api_max, ++ (h->pid == STLINK_V2_1_PID || (h->version.stlink == 3 && h->version.swim == 0)) ? "M" : "SWIM v", ++ (h->version.stlink == 3 && h->version.swim == 0) ? m_version : h->version.swim, + h->vid, + h->pid); + +@@ -648,8 +824,8 @@ static int stlink_usb_check_voltage(void *handle, float *target_voltage) + struct stlink_usb_handle_s *h = handle; + uint32_t adc_results[2]; + +- /* only supported by stlink/v2 and for firmware >= 13 */ +- if (h->version.stlink == 1 || h->version.jtag < 13) ++ /* only supported by stlink v2/v3 and for firmware >= 13 */ ++ if (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 13)) + return ERROR_COMMAND_NOTFOUND; + + stlink_usb_init_buffer(handle, h->rx_ep, 8); +@@ -682,7 +858,8 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor) + assert(handle != NULL); + + /* only supported by stlink/v2 and for firmware >= 22 */ +- if (h->version.stlink == 1 || h->version.jtag < 22) ++ if (h->version.stlink == 1 || h->version.stlink == 3 || \ ++ (h->version.stlink == 2 && h->version.jtag < 22)) + return ERROR_COMMAND_NOTFOUND; + + stlink_usb_init_buffer(handle, h->rx_ep, 2); +@@ -700,6 +877,32 @@ static int stlink_usb_set_swdclk(void *handle, uint16_t clk_divisor) + return ERROR_OK; + } + ++static int stlink_usb_set_jtagclk(void *handle, uint16_t clk_divisor) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* only supported by stlink/v2 and for firmware >= 24 */ ++ if (h->version.stlink == 1 || h->version.stlink == 3 || \ ++ (h->version.stlink == 2 && h->version.jtag < 24)) ++ return ERROR_COMMAND_NOTFOUND; ++ ++ stlink_usb_init_buffer(handle, h->rx_ep, 2); ++ ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_JTAG_SET_FREQ; ++ h_u16_to_le(h->cmdbuf+h->cmdidx, clk_divisor); ++ h->cmdidx += 2; ++ ++ int result = stlink_cmd_allow_retry(handle, h->databuf, 2); ++ ++ if (result != ERROR_OK) ++ return result; ++ ++ return ERROR_OK; ++} ++ + /** */ + static int stlink_usb_current_mode(void *handle, uint8_t *mode) + { +@@ -734,21 +937,23 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type) + * status + * TODO: we need the test on api V1 too + */ +- if (h->jtag_api == STLINK_JTAG_API_V2) ++ if (h->jtag_api == STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) + rx_size = 2; + + stlink_usb_init_buffer(handle, h->rx_ep, rx_size); + + switch (type) { + case STLINK_MODE_DEBUG_JTAG: ++ LOG_DEBUG("stlink_usb_mode_enter(JTAG)"); + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; + if (h->jtag_api == STLINK_JTAG_API_V1) + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER; + else + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_ENTER; +- h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_JTAG; ++ h->cmdbuf[h->cmdidx++] = STLINK_ENTER_JTAG_NO_CORE_RESET; + break; + case STLINK_MODE_DEBUG_SWD: ++ LOG_DEBUG("stlink_usb_mode_enter(SWD)"); + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; + if (h->jtag_api == STLINK_JTAG_API_V1) + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV1_ENTER; +@@ -757,6 +962,7 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type) + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_ENTER_SWD; + break; + case STLINK_MODE_DEBUG_SWIM: ++ LOG_DEBUG("stlink_usb_mode_enter(SWIM)"); + h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND; + h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER; + /* no answer for this function... */ +@@ -825,12 +1031,11 @@ static enum stlink_mode stlink_get_mode(enum hl_transports t) + } + + /** */ +-static int stlink_usb_init_mode(void *handle, bool connect_under_reset) ++static int stlink_exit_mode (void *handle) + { + int res; + uint8_t mode; + enum stlink_mode emode; +- struct stlink_usb_handle_s *h = handle; + + assert(handle != NULL); + +@@ -860,17 +1065,35 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset) + } + + if (emode != STLINK_MODE_UNKNOWN) { ++ LOG_DEBUG("E-MODE: 0x%02X", emode); + res = stlink_usb_mode_leave(handle, emode); +- + if (res != ERROR_OK) + return res; + } + +- res = stlink_usb_current_mode(handle, &mode); ++ return ERROR_OK; ++} ++ ++/** */ ++static int stlink_usb_init_mode(void *handle, bool connect_under_reset) ++{ ++ int res; ++ uint8_t mode; ++ enum stlink_mode emode; ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ res = stlink_exit_mode(handle); ++ if (res != ERROR_OK) ++ return res; + ++ res = stlink_usb_current_mode(handle, &mode); + if (res != ERROR_OK) + return res; + ++ LOG_DEBUG("MODE: 0x%02X", mode); ++ + /* we check the target voltage here as an aid to debugging connection problems. + * the stlink requires the target Vdd to be connected for reliable debugging. + * this cmd is supported in all modes except DFU +@@ -921,7 +1144,7 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset) + return res; + + /* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */ +- if (connect_under_reset) { ++ if (h->transport == HL_TRANSPORT_SWIM && connect_under_reset) { + res = stlink_usb_assert_srst(handle, 0); + if (res != ERROR_OK) + return res; +@@ -1174,6 +1397,7 @@ static int stlink_usb_v2_read_debug_reg(void *handle, uint32_t addr, uint32_t *v + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READDEBUGREG; + h_u32_to_le(h->cmdbuf+h->cmdidx, addr); + h->cmdidx += 4; ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; + + res = stlink_cmd_allow_retry(handle, h->databuf, 8); + if (res != ERROR_OK) +@@ -1200,6 +1424,7 @@ static int stlink_usb_write_debug_reg(void *handle, uint32_t addr, uint32_t val) + h->cmdidx += 4; + h_u32_to_le(h->cmdbuf+h->cmdidx, val); + h->cmdidx += 4; ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; + + return stlink_cmd_allow_retry(handle, h->databuf, 2); + } +@@ -1211,7 +1436,8 @@ static int stlink_usb_trace_read(void *handle, uint8_t *buf, size_t *size) + + assert(handle != NULL); + +- if (h->trace.enabled && h->version.jtag >= STLINK_TRACE_MIN_VERSION) { ++ if (h->trace.enabled && ++ (h->version.jtag >= STLINK_TRACE_MIN_VERSION || h->jtag_api == STLINK_JTAG_API_V3)) { + int res; + + stlink_usb_init_buffer(handle, h->rx_ep, 10); +@@ -1284,7 +1510,7 @@ static enum target_state stlink_usb_state(void *handle) + h->reconnect_pending = false; + } + +- if (h->jtag_api == STLINK_JTAG_API_V2) { ++ if (h->jtag_api == STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) { + res = stlink_usb_v2_get_status(handle); + if (res == TARGET_UNKNOWN) + h->reconnect_pending = true; +@@ -1340,7 +1566,7 @@ static void stlink_usb_trace_disable(void *handle) + + assert(handle != NULL); + +- assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION); ++ assert(h->version.jtag >= STLINK_TRACE_MIN_VERSION || h->jtag_api == STLINK_JTAG_API_V3); + + LOG_DEBUG("Tracing: disable"); + +@@ -1362,7 +1588,7 @@ static int stlink_usb_trace_enable(void *handle) + + assert(handle != NULL); + +- if (h->version.jtag >= STLINK_TRACE_MIN_VERSION) { ++ if (h->version.jtag >= STLINK_TRACE_MIN_VERSION || h->jtag_api == STLINK_JTAG_API_V3) { + stlink_usb_init_buffer(handle, h->rx_ep, 10); + + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; +@@ -1426,7 +1652,7 @@ static int stlink_usb_run(void *handle) + + assert(handle != NULL); + +- if (h->jtag_api == STLINK_JTAG_API_V2) { ++ if (h->jtag_api == STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) { + res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_DEBUGEN); + + return res; +@@ -1448,7 +1674,7 @@ static int stlink_usb_halt(void *handle) + + assert(handle != NULL); + +- if (h->jtag_api == STLINK_JTAG_API_V2) { ++ if (h->jtag_api == STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) { + res = stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_DEBUGEN); + + return res; +@@ -1469,7 +1695,7 @@ static int stlink_usb_step(void *handle) + + assert(handle != NULL); + +- if (h->jtag_api == STLINK_JTAG_API_V2) { ++ if (h->jtag_api == STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) { + /* TODO: this emulates the v1 api, it should really use a similar auto mask isr + * that the Cortex-M3 currently does. */ + stlink_usb_write_debug_reg(handle, DCB_DHCSR, DBGKEY|C_HALT|C_MASKINTS|C_DEBUGEN); +@@ -1501,6 +1727,9 @@ static int stlink_usb_read_regs(void *handle) + else + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READALLREGS; + ++ if (h->jtag_api >= STLINK_JTAG_API_V2 || h->jtag_api == STLINK_JTAG_API_V3) ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; ++ + res = stlink_usb_xfer(handle, h->databuf, 84); + + if (res != ERROR_OK) +@@ -1526,6 +1755,9 @@ static int stlink_usb_read_reg(void *handle, int num, uint32_t *val) + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_READREG; + h->cmdbuf[h->cmdidx++] = num; + ++ if ((h->jtag_api == STLINK_JTAG_API_V2 && h->version.jtag >= 28) || h->jtag_api == STLINK_JTAG_API_V3) ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; ++ + if (h->jtag_api == STLINK_JTAG_API_V1) { + res = stlink_usb_xfer(handle, h->databuf, 4); + if (res != ERROR_OK) +@@ -1559,6 +1791,9 @@ static int stlink_usb_write_reg(void *handle, int num, uint32_t val) + h_u32_to_le(h->cmdbuf+h->cmdidx, val); + h->cmdidx += 4; + ++ if ((h->jtag_api == STLINK_JTAG_API_V2 && h->version.jtag >= 28) || h->jtag_api == STLINK_JTAG_API_V3) ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; ++ + return stlink_cmd_allow_retry(handle, h->databuf, 2); + } + +@@ -1572,12 +1807,20 @@ static int stlink_usb_get_rw_status(void *handle) + if (h->jtag_api == STLINK_JTAG_API_V1) + return ERROR_OK; + +- stlink_usb_init_buffer(handle, h->rx_ep, 2); ++ stlink_usb_init_buffer(handle, h->rx_ep, 3); + + h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; +- h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS; +- +- res = stlink_usb_xfer(handle, h->databuf, 2); ++ /* FIXME: this is incorrect */ ++ if (h->jtag_api == STLINK_JTAG_API_V3) { ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV3_GETLASTRWSTATUS; ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; ++ res = stlink_usb_xfer(handle, h->databuf, 12); ++ } else { ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_GETLASTRWSTATUS; ++ if (h->version.jtag >= 28) ++ h->cmdbuf[h->cmdidx++] = HLA_DEFAULT_APNUN; ++ res = stlink_usb_xfer(handle, h->databuf, 2); ++ } + + if (res != ERROR_OK) + return res; +@@ -1586,7 +1829,7 @@ static int stlink_usb_get_rw_status(void *handle) + } + + /** */ +-static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, ++static int stlink_usb_read_mem8(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, + uint8_t *buffer) + { + int res; +@@ -1595,9 +1838,9 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, + + assert(handle != NULL); + +- /* max 8bit read/write is 64bytes */ +- if (len > STLINK_MAX_RW8) { +- LOG_DEBUG("max buffer length exceeded"); ++ /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */ ++ if (len > stlink_usb_block(h)) { ++ LOG_DEBUG("max buffer (%d) length exceeded", stlink_usb_block(h)); + return ERROR_FAIL; + } + +@@ -1609,6 +1852,7 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, + h->cmdidx += 4; + h_u16_to_le(h->cmdbuf+h->cmdidx, len); + h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; + + /* we need to fix read length for single bytes */ + if (read_len == 1) +@@ -1625,7 +1869,7 @@ static int stlink_usb_read_mem8(void *handle, uint32_t addr, uint16_t len, + } + + /** */ +-static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, ++static int stlink_usb_write_mem8(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, + const uint8_t *buffer) + { + int res; +@@ -1633,9 +1877,9 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, + + assert(handle != NULL); + +- /* max 8bit read/write is 64bytes */ +- if (len > STLINK_MAX_RW8) { +- LOG_DEBUG("max buffer length exceeded"); ++ /* max 8 bit read/write is 64 bytes or 512 bytes for v3 */ ++ if (len > stlink_usb_block(h)) { ++ LOG_DEBUG("max buffer length (%d) exceeded", stlink_usb_block(h)); + return ERROR_FAIL; + } + +@@ -1647,6 +1891,7 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, + h->cmdidx += 4; + h_u16_to_le(h->cmdbuf+h->cmdidx, len); + h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; + + res = stlink_usb_xfer(handle, buffer, len); + +@@ -1657,7 +1902,7 @@ static int stlink_usb_write_mem8(void *handle, uint32_t addr, uint16_t len, + } + + /** */ +-static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, ++static int stlink_usb_read_mem32(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, + uint8_t *buffer) + { + int res; +@@ -1679,6 +1924,7 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, + h->cmdidx += 4; + h_u16_to_le(h->cmdbuf+h->cmdidx, len); + h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; + + res = stlink_usb_xfer(handle, h->databuf, len); + +@@ -1691,7 +1937,7 @@ static int stlink_usb_read_mem32(void *handle, uint32_t addr, uint16_t len, + } + + /** */ +-static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, ++static int stlink_usb_write_mem32(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, + const uint8_t *buffer) + { + int res; +@@ -1713,6 +1959,75 @@ static int stlink_usb_write_mem32(void *handle, uint32_t addr, uint16_t len, + h->cmdidx += 4; + h_u16_to_le(h->cmdbuf+h->cmdidx, len); + h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; ++ ++ res = stlink_usb_xfer(handle, buffer, len); ++ ++ if (res != ERROR_OK) ++ return res; ++ ++ return stlink_usb_get_rw_status(handle); ++} ++ ++/** */ ++static int stlink_usb_read_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, ++ uint8_t *buffer) ++{ ++ int res; ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* data must be a multiple of 4 and word aligned */ ++ if (len % 4 || addr % 4) { ++ LOG_DEBUG("Invalid data alignment"); ++ return ERROR_TARGET_UNALIGNED_ACCESS; ++ } ++ ++ stlink_usb_init_buffer(handle, h->rx_ep, len); ++ ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_READMEM_32BIT_NO_ADDR_INC; ++ h_u32_to_le(h->cmdbuf+h->cmdidx, addr); ++ h->cmdidx += 4; ++ h_u16_to_le(h->cmdbuf+h->cmdidx, len); ++ h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; ++ ++ res = stlink_usb_xfer(handle, h->databuf, len); ++ ++ if (res != ERROR_OK) ++ return res; ++ ++ memcpy(buffer, h->databuf, len); ++ ++ return stlink_usb_get_rw_status(handle); ++} ++ ++/** */ ++static int stlink_usb_write_mem32_noaddrinc(void *handle, uint8_t ap_num, uint32_t addr, uint16_t len, ++ const uint8_t *buffer) ++{ ++ int res; ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* data must be a multiple of 4 and word aligned */ ++ if (len % 4 || addr % 4) { ++ LOG_DEBUG("Invalid data alignment"); ++ return ERROR_TARGET_UNALIGNED_ACCESS; ++ } ++ ++ stlink_usb_init_buffer(handle, h->tx_ep, len); ++ ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_WRITEMEM_32BIT_NO_ADDR_INC; ++ h_u32_to_le(h->cmdbuf+h->cmdidx, addr); ++ h->cmdidx += 4; ++ h_u16_to_le(h->cmdbuf+h->cmdidx, len); ++ h->cmdidx += 2; ++ h->cmdbuf[h->cmdidx++] = ap_num; + + res = stlink_usb_xfer(handle, buffer, len); + +@@ -1730,7 +2045,7 @@ static uint32_t stlink_max_block_size(uint32_t tar_autoincr_block, uint32_t addr + return max_tar_block; + } + +-static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, ++static int stlink_usb_read_ap_mem(void *handle, uint8_t ap_num, uint32_t addr, uint32_t size, + uint32_t count, uint8_t *buffer) + { + int retval = ERROR_OK; +@@ -1744,7 +2059,7 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, + while (count) { + + bytes_remaining = (size == 4) ? \ +- stlink_max_block_size(h->max_mem_packet, addr) : STLINK_MAX_RW8; ++ stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h); + + if (count < bytes_remaining) + bytes_remaining = count; +@@ -1770,7 +2085,7 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size, + if (addr % 4) { + + uint32_t head_bytes = 4 - (addr % 4); +- retval = stlink_usb_read_mem8(handle, addr, head_bytes, buffer); ++ retval = stlink_usb_read_mem8(handle, ap_num, addr, head_bytes, buffer); + if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { + usleep((1<max_mem_packet, addr) : STLINK_MAX_RW8; ++ stlink_max_block_size(h->max_mem_packet, addr) : stlink_usb_block(h); + + if (count < bytes_remaining) + bytes_remaining = count; +@@ -1845,7 +2166,7 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size, + if (addr % 4) { + + uint32_t head_bytes = 4 - (addr % 4); +- retval = stlink_usb_write_mem8(handle, addr, head_bytes, buffer); ++ retval = stlink_usb_write_mem8(handle, ap_num, addr, head_bytes, buffer); + if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { + usleep((1<transport == HL_TRANSPORT_SWIM)) { +- /* ++ /* + we dont care what the khz rate is + we only have low and high speed... + before changing speed the SWIM_CSR HS bit + must be updated +- */ +- if (khz == 0) +- stlink_swim_speed(handle, 0); +- else +- stlink_swim_speed(handle, 1); +- return khz; +- } ++ */ ++ if (khz == 0) ++ stlink_swim_speed(handle, 0); ++ else ++ stlink_swim_speed(handle, 1); ++ return khz; ++} + +- /* only supported by stlink/v2 and for firmware >= 22 */ +- if (h && (h->version.stlink == 1 || h->version.jtag < 22)) +- return khz; ++static int stlink_match_speed_map(const struct speed_map *map, unsigned int map_size, int khz, bool query) ++{ ++ unsigned int i; ++ int speed_index = -1; ++ int speed_diff = INT_MAX; ++ int last_valid_speed = -1; ++ bool match = true; + +- for (i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) { +- if (khz == stlink_khz_to_speed_map[i].speed) { ++ for (i = 0; i < map_size; i++) { ++ if (!map[i].speed) ++ continue; ++ last_valid_speed = i; ++ if (khz == map[i].speed) { + speed_index = i; + break; + } else { +- int current_diff = khz - stlink_khz_to_speed_map[i].speed; ++ int current_diff = khz - map[i].speed; + /* get abs value for comparison */ + current_diff = (current_diff > 0) ? current_diff : -current_diff; +- if ((current_diff < speed_diff) && khz >= stlink_khz_to_speed_map[i].speed) { ++ if ((current_diff < speed_diff) && khz >= map[i].speed) { + speed_diff = current_diff; + speed_index = i; + } + } + } + +- bool match = true; +- + if (speed_index == -1) { + /* this will only be here if we cannot match the slow speed. + * use the slowest speed we support.*/ +- speed_index = ARRAY_SIZE(stlink_khz_to_speed_map) - 1; ++ speed_index = last_valid_speed; + match = false; +- } else if (i == ARRAY_SIZE(stlink_khz_to_speed_map)) ++ } else if (i == map_size) + match = false; + + if (!match && query) { +@@ -1941,7 +2267,22 @@ static int stlink_speed(void *handle, int khz, bool query) + khz, stlink_khz_to_speed_map[speed_index].speed); + } + +- if (h && !query) { ++ return speed_index; ++} ++ ++static int stlink_speed_swd(void *handle, int khz, bool query) ++{ ++ int speed_index; ++ struct stlink_usb_handle_s *h = handle; ++ ++ /* only supported by stlink/v2 and for firmware >= 22 */ ++ if (h->version.stlink == 1 || h->version.jtag < 22) ++ return khz; ++ ++ speed_index = stlink_match_speed_map(stlink_khz_to_speed_map, ++ ARRAY_SIZE(stlink_khz_to_speed_map), khz, query); ++ ++ if (!query) { + int result = stlink_usb_set_swdclk(h, stlink_khz_to_speed_map[speed_index].speed_divisor); + if (result != ERROR_OK) { + LOG_ERROR("Unable to set adapter speed"); +@@ -1952,62 +2293,157 @@ static int stlink_speed(void *handle, int khz, bool query) + return stlink_khz_to_speed_map[speed_index].speed; + } + +-/** */ +-static int stlink_usb_close(void *handle) ++static int stlink_speed_jtag(void *handle, int khz, bool query) + { +- int res; +- uint8_t mode; +- enum stlink_mode emode; ++ int speed_index; + struct stlink_usb_handle_s *h = handle; + +- if (h && h->fd) +- res = stlink_usb_current_mode(handle, &mode); +- else +- res = ERROR_FAIL; +- /* do not exit if return code != ERROR_OK, +- it prevents us from closing jtag_libusb */ +- +- if (res == ERROR_OK) { +- /* try to exit current mode */ +- switch (mode) { +- case STLINK_DEV_DFU_MODE: +- emode = STLINK_MODE_DFU; +- break; +- case STLINK_DEV_DEBUG_MODE: +- emode = STLINK_MODE_DEBUG_SWD; +- break; +- case STLINK_DEV_SWIM_MODE: +- emode = STLINK_MODE_DEBUG_SWIM; +- break; +- case STLINK_DEV_BOOTLOADER_MODE: +- case STLINK_DEV_MASS_MODE: +- default: +- emode = STLINK_MODE_UNKNOWN; +- break; +- } ++ /* only supported by stlink/v2 and for firmware >= 24 */ ++ if (h->version.stlink == 1 || h->version.jtag < 24) ++ return khz; + +- if (emode != STLINK_MODE_UNKNOWN) +- stlink_usb_mode_leave(handle, emode); +- /* do not check return code, it prevent +- us from closing jtag_libusb */ ++ speed_index = stlink_match_speed_map(stlink_khz_to_speed_map_jtag, ++ ARRAY_SIZE(stlink_khz_to_speed_map_jtag), khz, query); ++ ++ if (!query) { ++ int result = stlink_usb_set_jtagclk(h, stlink_khz_to_speed_map_jtag[speed_index].speed_divisor); ++ if (result != ERROR_OK) { ++ LOG_ERROR("Unable to set adapter speed"); ++ return khz; ++ } + } + +- if (h && h->fd) +- jtag_libusb_close(h->fd); ++ return stlink_khz_to_speed_map_jtag[speed_index].speed; ++} + +- free(h); ++void stlink_dump_speed_map(const struct speed_map *map, unsigned int map_size) ++{ ++ unsigned int i; + +- return ERROR_OK; ++ LOG_DEBUG("Supported clock speeds are:"); ++ for (i = 0; i < map_size; i++) ++ if (map[i].speed) ++ LOG_DEBUG("%d kHz", map[i].speed); + } + +-/** */ +-static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) ++static int stlink_get_com_freq(void *handle, bool is_jtag, struct speed_map *map) + { +- int err, retry_count = 1; +- struct stlink_usb_handle_s *h; +- enum stlink_jtag_api_version api; ++ struct stlink_usb_handle_s *h = handle; ++ int i; + +- LOG_DEBUG("stlink_usb_open"); ++ if (h->version.stlink < 3) { ++ LOG_ERROR("Unknown command"); ++ return 0; ++ } ++ ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_APIV3_GET_COM_FREQ; ++ h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0; ++ ++ int res = stlink_usb_xfer(handle, h->databuf, 52); ++ ++ int size = h->databuf[8]; ++ ++ if (size > STLINK_V3_MAX_FREQ_NB) ++ size = STLINK_V3_MAX_FREQ_NB; ++ ++ for (i = 0; i < size; i++) { ++ map[i].speed = le_to_h_u32(&h->databuf[12 + 4 * i]); ++ map[i].speed_divisor = i; ++ } ++ ++ /* set to zero all the next entries */ ++ for (i = size; i < STLINK_V3_MAX_FREQ_NB; i++) ++ map[i].speed = 0; ++ ++ return res; ++} ++ ++static int stlink_set_com_freq(void *handle, bool is_jtag, unsigned int frequency) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ if (h->version.stlink < 3) { ++ LOG_ERROR("Unknown command"); ++ return 0; ++ } ++ ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_APIV3_SET_COM_FREQ; ++ h->cmdbuf[h->cmdidx++] = is_jtag ? 1 : 0; ++ h->cmdbuf[h->cmdidx++] = 0; ++ ++ h_u32_to_le(&h->cmdbuf[4], frequency); ++ ++ return stlink_usb_xfer(handle, h->databuf, 8); ++} ++ ++static int stlink_speed_v3(void *handle, int khz, bool query) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ int speed_index; ++ struct speed_map map[STLINK_V3_MAX_FREQ_NB]; ++ ++ stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map); ++ ++ speed_index = stlink_match_speed_map(map, ARRAY_SIZE(map), khz, query); ++ ++ if (!query) { ++ int result = stlink_set_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map[speed_index].speed); ++ if (result != ERROR_OK) { ++ LOG_ERROR("Unable to set adapter speed"); ++ return khz; ++ } ++ } ++ return map[speed_index].speed; ++} ++ ++static int stlink_speed(void *handle, int khz, bool query) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ if (!handle) ++ return khz; ++ ++ if (h->transport == HL_TRANSPORT_SWIM) ++ return stlink_speed_swim(handle, khz, query); ++ else if (h->version.stlink == 3) ++ return stlink_speed_v3(handle, khz, query); ++ else if (h->transport == HL_TRANSPORT_SWD) ++ return stlink_speed_swd(handle, khz, query); ++ else if (h->transport == HL_TRANSPORT_JTAG) ++ return stlink_speed_jtag(handle, khz, query); ++ ++ return khz; ++} ++ ++/** */ ++static int stlink_usb_close(void *handle) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ if (h && h->fd) { ++ stlink_exit_mode(h); ++ jtag_libusb_close(h->fd); ++ } ++ ++ free(h); ++ ++ return ERROR_OK; ++} ++ ++/** */ ++static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) ++{ ++ int err, retry_count = 1; ++ struct stlink_usb_handle_s *h; ++ enum stlink_jtag_api_version api; ++ ++ LOG_DEBUG("stlink_usb_open"); + + h = calloc(1, sizeof(struct stlink_usb_handle_s)); + +@@ -2062,7 +2498,16 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) + h->tx_ep = STLINK_TX_EP; + h->trace_ep = STLINK_TRACE_EP; + break; ++ case STLINK_V3_DFU_PID: ++ case STLINK_V3E_PID: ++ case STLINK_V3S_PID: ++ case STLINK_V3_2VCP_PID: ++ h->version.stlink = 3; ++ h->tx_ep = STLINK_V2_1_TX_EP; ++ h->trace_ep = STLINK_V2_1_TRACE_EP; ++ break; + case STLINK_V2_1_PID: ++ case STLINK_V2_1_NO_MSD_PID: + h->version.stlink = 2; + h->tx_ep = STLINK_V2_1_TX_EP; + h->trace_ep = STLINK_V2_1_TRACE_EP; +@@ -2075,6 +2520,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) + h->trace_ep = STLINK_TRACE_EP; + break; + } ++ h->pid = pid; + + /* get the device version */ + err = stlink_usb_version(h); +@@ -2157,13 +2603,25 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) + return ERROR_OK; + } + +- /* clock speed only supported by stlink/v2 and for firmware >= 22 */ +- if (h->version.stlink >= 2 && h->version.jtag >= 22) { +- LOG_DEBUG("Supported clock speeds are:"); ++ if (h->transport == HL_TRANSPORT_JTAG) { ++ /* jtag clock speed only supported by stlink/v2 and for firmware >= 24 */ ++ if (h->version.stlink == 2 && h->version.jtag >= 24) { ++ stlink_dump_speed_map(stlink_khz_to_speed_map_jtag, ARRAY_SIZE(stlink_khz_to_speed_map_jtag)); ++ stlink_speed(h, param->initial_interface_speed, false); ++ } ++ } else if (h->transport == HL_TRANSPORT_SWD) { ++ /* clock speed only supported by stlink/v2 and for firmware >= 22 */ ++ if (h->version.stlink == 2 && h->version.jtag >= 22) { ++ stlink_dump_speed_map(stlink_khz_to_speed_map, ARRAY_SIZE(stlink_khz_to_speed_map)); ++ stlink_speed(h, param->initial_interface_speed, false); ++ } ++ } + +- for (unsigned i = 0; i < ARRAY_SIZE(stlink_khz_to_speed_map); i++) +- LOG_DEBUG("%d kHz", stlink_khz_to_speed_map[i].speed); ++ if (h->version.stlink == 3) { ++ struct speed_map map[STLINK_V3_MAX_FREQ_NB]; + ++ stlink_get_com_freq(h, (h->transport == HL_TRANSPORT_JTAG), map); ++ stlink_dump_speed_map(map, ARRAY_SIZE(map)); + stlink_speed(h, param->initial_interface_speed, false); + } + +@@ -2171,8 +2629,9 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) + * start with a safe default */ + h->max_mem_packet = (1 << 10); + ++#if 0 + uint8_t buffer[4]; +- err = stlink_usb_read_mem32(h, CPUID, 4, buffer); ++ err = stlink_usb_read_mem32(h, 0, CPUID, 4, buffer); + if (err == ERROR_OK) { + uint32_t cpuid = le_to_h_u32(buffer); + int i = (cpuid >> 4) & 0xf; +@@ -2181,6 +2640,7 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd) + h->max_mem_packet = (1 << 12); + } + } ++#endif + + LOG_DEBUG("Using TAR autoincrement: %" PRIu32, h->max_mem_packet); + +@@ -2226,6 +2686,122 @@ int stlink_config_trace(void *handle, bool enabled, enum tpiu_pin_protocol pin_p + } + + /** */ ++static int stlink_usb_init_access_point(void *handle, ++ unsigned char access_point_id, unsigned char resource) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* only required by stlink/v2 and for firmware >= 28 */ ++ if (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 28)) ++ return ERROR_OK; ++ ++ LOG_DEBUG_IO("init apnum = %d, resource %d", access_point_id, resource); ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_INIT_AP; ++ h->cmdbuf[h->cmdidx++] = access_point_id; ++ h->cmdbuf[h->cmdidx++] = resource; ++ h_u32_to_le(&h->cmdbuf[12], 0); ++ ++ return stlink_usb_xfer(handle, h->databuf, 2); ++} ++ ++/** */ ++static int stlink_usb_close_access_point(void *handle, ++ unsigned char access_point_id) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* only required by stlink/v2 and for firmware >= 28 */ ++ if (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 28)) ++ return ERROR_OK; ++ ++ LOG_DEBUG_IO("close apnum = %d", access_point_id); ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_APIV2_CLOSE_AP_DBG; ++ h->cmdbuf[h->cmdidx++] = access_point_id; ++ h_u32_to_le(&h->cmdbuf[12], 0); ++ ++ return stlink_usb_xfer(handle, h->databuf, 2); ++} ++ ++/** */ ++static int stlink_internal_read_dap_register(void *handle, unsigned short dap_port, ++ unsigned short addr, uint32_t *val) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ int retval; ++ ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_JTAG_READ_DAP_REG; ++ h_u16_to_le(&h->cmdbuf[2], dap_port); ++ h_u16_to_le(&h->cmdbuf[4], addr); ++ h_u32_to_le(&h->cmdbuf[12], 0); ++ ++ retval = stlink_usb_xfer(handle, h->databuf, 8); ++ *val = le_to_h_u32(h->databuf + 4); ++ return retval; ++} ++ ++#define STLINK_DEBUG_PORT 0xffff ++static int stlink_read_dap_register(void *handle, unsigned short dap_port, ++ unsigned short addr, uint32_t *val) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ int retval; ++ uint32_t dummy; ++ ++ assert(handle != NULL); ++ ++ /* only supported by stlink/v2 and for firmware >= 24 */ ++ if (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 24)) ++ return ERROR_COMMAND_NOTFOUND; ++ ++ if (h->transport == HL_TRANSPORT_JTAG && dap_port == STLINK_DEBUG_PORT ++ && h->version.stlink == 2 && h->version.jtag < 32) { ++ /* workaround for V2J24 ~ V2J31 */ ++ retval = stlink_internal_read_dap_register(handle, STLINK_DEBUG_PORT, addr, &dummy); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = stlink_internal_read_dap_register(handle, STLINK_DEBUG_PORT, DP_RDBUFF, val); ++ } else { ++ retval = stlink_internal_read_dap_register(handle, dap_port, addr, val); ++ } ++ ++ LOG_DEBUG_IO("dap_port_read = %d, addr = 0x%x, value = 0x%x", dap_port, addr, *val); ++ return retval; ++} ++ ++/** */ ++static int stlink_write_dap_register(void *handle, unsigned short dap_port, ++ unsigned short addr, uint32_t val) ++{ ++ struct stlink_usb_handle_s *h = handle; ++ ++ assert(handle != NULL); ++ ++ /* only supported by stlink/v2 and for firmware >= 24 */ ++ if (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 24)) ++ return ERROR_COMMAND_NOTFOUND; ++ ++ LOG_DEBUG_IO("dap_port_write = %d, addr = 0x%x, value = 0x%x", dap_port, addr, val); ++ stlink_usb_init_buffer(handle, h->rx_ep, 16); ++ h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND; ++ h->cmdbuf[h->cmdidx++] = STLINK_JTAG_WRITE_DAP_REG; ++ h_u16_to_le(&h->cmdbuf[2], dap_port); ++ h_u16_to_le(&h->cmdbuf[4], addr); ++ h_u32_to_le(&h->cmdbuf[6], val); ++ h_u32_to_le(&h->cmdbuf[12], 0); ++ return stlink_usb_xfer(handle, h->databuf, 2); ++} ++ ++/** */ + struct hl_layout_api_s stlink_usb_layout_api = { + /** */ + .open = stlink_usb_open, +@@ -2266,3 +2842,536 @@ struct hl_layout_api_s stlink_usb_layout_api = { + /** */ + .poll_trace = stlink_usb_trace_read, + }; ++ ++/* Low-level interface */ ++ ++static struct stlink_usb_handle_s *stlink_dap_handle; ++static struct hl_interface_param_s stlink_dap_param = { ++ .transport = HL_TRANSPORT_JTAG, ++ .vid = {STLINK_VID, STLINK_VID, STLINK_VID, STLINK_VID, STLINK_VID, STLINK_VID, STLINK_VID, STLINK_VID, 0}, ++ .pid = {STLINK_V1_PID, STLINK_V2_PID, STLINK_V2_1_PID, STLINK_V2_1_NO_MSD_PID, STLINK_V3_DFU_PID, STLINK_V3E_PID, STLINK_V3S_PID, STLINK_V3_2VCP_PID, 0}, ++}; ++static DECLARE_BITMAP(opened_ap, DP_APSEL_MAX + 1); ++static uint8_t ap_csw_size_cached[DP_APSEL_MAX + 1]; ++ ++static int stlink_dap_open_ap(unsigned short apsel) ++{ ++ int retval; ++ ++ if (apsel > DP_APSEL_MAX) ++ return ERROR_OK; ++ ++ if (test_bit(apsel, opened_ap)) ++ return ERROR_OK; ++ ++ retval = stlink_usb_init_access_point(stlink_dap_handle, apsel, 0); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ LOG_DEBUG("AP %d enabled", apsel); ++ set_bit(apsel, opened_ap); ++ return ERROR_OK; ++} ++ ++static int stlink_dap_closeall_ap(void) ++{ ++ int retval, apsel; ++ ++ for (apsel = 0; apsel <= DP_APSEL_MAX; apsel++) { ++ if (!test_bit(apsel, opened_ap)) ++ continue; ++ retval = stlink_usb_close_access_point(stlink_dap_handle, apsel); ++ if (retval != ERROR_OK) ++ return retval; ++ clear_bit(apsel, opened_ap); ++ } ++ return ERROR_OK; ++} ++ ++int stlink_dap_dap_read(unsigned short dap_port, unsigned short addr, uint32_t *val) ++{ ++ uint32_t dummy; ++ int retval; ++ ++ /* Skip opening DAP if we are only scanning to look for an AP */ ++ if (dap_port <= DP_APSEL_MAX && addr != AP_REG_IDR) { ++ retval = stlink_dap_open_ap(dap_port); ++ if (retval != ERROR_OK) ++ return retval; ++ } ++ ++ if (!val) ++ val = &dummy; ++ return stlink_read_dap_register(stlink_dap_handle, dap_port, addr, val); ++} ++ ++int stlink_dap_dap_write(unsigned short dap_port, unsigned short addr, uint32_t val) ++{ ++ int retval; ++ ++ retval = stlink_dap_open_ap(dap_port); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ return stlink_write_dap_register(stlink_dap_handle, dap_port, addr, val); ++} ++ ++/* ++ * Workaround for setting CSW with ST-Link pre-versions V2J32 and V3J2. ++ * Before ST-Link versions above, high level API does not provide a method to ++ * set CSW. Same issue on every version of ST-Link V1. This forced us using ++ * the un-efficient low-level AP register API for every memory read/write. ++ * ++ * This workaround leverage the CSW caching operated by ST-Link. At every ++ * memory R/W, ST-Link computes the new CSW value based on word size. If it ++ * match the previous CSW value than it has wrote in CSW register, ST-Link ++ * will not write in CSW register again. ++ * ++ * Here we track the word size used in the last memory R/W. If it does not ++ * match with current word size, we first force ST-Link to update CSW register ++ * and its internal cache accordingly to the new size. Then we overwrite CSW ++ * register with the value we need. ++ * The following memory R/W will operate base on the new CSW and ST-Link will ++ * not change it. ++ */ ++static int stlink_dap_set_csw(struct adiv5_ap *ap, uint32_t size, bool addrinc) ++{ ++ uint32_t csw; ++ uint8_t dummy[4], ap_num; ++ int retval; ++ ++ ap_num = ap->ap_num; ++ ++ struct stlink_usb_handle_s *h = stlink_dap_handle; ++ if ((h->version.stlink == 2 && h->version.jtag >= 32) || (h->version.stlink == 3 && h->version.jtag >= 2)) { ++ csw = ap->csw_default; ++ if (csw != (ap->csw_value & ~(CSW_SIZE_MASK | CSW_ADDRINC_MASK))) { ++ retval = dap_queue_ap_write(ap, MEM_AP_REG_CSW, csw); ++ if (retval != ERROR_OK) { ++ ap->csw_value = 0; ++ return retval; ++ } ++ ap->csw_value = csw; ++ } ++ return ERROR_OK; ++ } ++ ++ switch (size) { ++ case 2: ++ /* current implementation only use 8 and 32 bits */ ++ size = 1; ++ /* fallthrough */ ++ case 1: ++ csw = CSW_8BIT; ++ break; ++ case 4: ++ default: ++ /* ST-Link sets autoinc only in 32 bits mode */ ++ /* ++ * ARM IHI 0031D: Note: ++ * It is IMPLEMENTATION DEFINED whether a MEM-AP supports transfer sizes other than Word. If a ++ * MEM-AP only supports word transfers and Increment single is selected, the TAR always ++ * increments by four after a successful DRW transaction. ++ */ ++ csw = addrinc ? (CSW_32BIT | CSW_ADDRINC_SINGLE) : CSW_32BIT; ++ break; ++ } ++ csw |= ap->csw_default; ++ ++ if (ap_csw_size_cached[ap_num] != size) { ++ ap_csw_size_cached[ap_num] = size; ++ ++ /* The mem read below will change CSW */ ++ ap->csw_value = 0; ++ stlink_usb_read_ap_mem(stlink_dap_handle, ap_num, 0x00000000, size, 1, dummy); ++ } ++ ++ if (ap->csw_value != csw) { ++ retval = dap_queue_ap_write(ap, MEM_AP_REG_CSW, csw); ++ if (retval != ERROR_OK) { ++ ap->csw_value = 0; ++ return retval; ++ } ++ ap->csw_value = csw; ++ } ++ return ERROR_OK; ++} ++ ++static int stlink_dap_reset_csw(struct adiv5_ap *ap) ++{ ++ uint32_t csw; ++ int retval; ++ ++ csw = ap->csw_value; ++ if (csw & CSW_ADDRINC_SINGLE) ++ return ERROR_OK; ++ ++ csw &= ~CSW_ADDRINC_SINGLE; ++ retval = dap_queue_ap_write(ap, MEM_AP_REG_CSW, csw); ++ if (retval != ERROR_OK) ++ return retval; ++ ap->csw_value = csw; ++ return ERROR_OK; ++} ++ ++int stlink_dap_ap_mem_read(struct adiv5_ap *ap, uint8_t *buffer, ++ uint32_t size, uint32_t count, uint32_t address, bool addrinc) ++{ ++ int retval; ++ uint8_t ap_num; ++ uint32_t partial; ++ int retries = 0; ++ uint32_t bytes_remaining; ++ ++ if (!addrinc && size != 4) ++ return ERROR_OP_NOT_SUPPORTED; ++ ++ struct stlink_usb_handle_s *h = stlink_dap_handle; ++ if (!addrinc && (h->version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 24))) ++ return ERROR_OP_NOT_SUPPORTED; ++ ++ ap_num = ap->ap_num; ++ retval = stlink_dap_open_ap(ap_num); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ if (size == 4 && address & 3) { ++ /* unaligned 32-bits read could be split in mix 32 & 8-bits. Force 8-bis only */ ++ count *= 4; ++ size = 1; ++ } ++ ++ /* here we do not track TAR, and next calls will change it */ ++ ap->tar_valid = false; ++ ++ retval = stlink_dap_set_csw(ap, size, addrinc); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ if (addrinc) ++ return stlink_usb_read_ap_mem(stlink_dap_handle, ap_num, address, size, ++ count, buffer); ++ ++ /* !addrinc && size == 4 */ ++ ++ if ((h->version.stlink == 2 && h->version.jtag >= 32) || (h->version.stlink == 3 && h->version.jtag >= 2)) { ++ count *= 4; ++ while (count) { ++ bytes_remaining = stlink_usb_block(h); ++ ++ if (count < bytes_remaining) ++ bytes_remaining = count; ++ ++ retval = stlink_usb_read_mem32_noaddrinc(stlink_dap_handle, ap_num, address, bytes_remaining, buffer); ++ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { ++ usleep((1< stlink_usb_block(stlink_dap_handle) / 4) ++ partial = stlink_usb_block(stlink_dap_handle) / 4; ++ while (count) { ++ if (partial > count) ++ partial = count; ++ retval = stlink_usb_read_mem32(stlink_dap_handle, ap_num, address, 4 * partial, buffer); ++ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { ++ usleep((1<version.stlink == 1 || (h->version.stlink == 2 && h->version.jtag < 24))) ++ return ERROR_OP_NOT_SUPPORTED; ++ ++ ap_num = ap->ap_num; ++ retval = stlink_dap_open_ap(ap_num); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ if (size == 4 && address & 3) { ++ /* unaligned 32-bits write could be split in mix 32 & 8-bits. Force 8-bis only */ ++ count *= 4; ++ size = 1; ++ } ++ ++ /* here we do not track TAR, and next calls will change it */ ++ ap->tar_valid = false; ++ ++ retval = stlink_dap_set_csw(ap, size, addrinc); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ if (addrinc) ++ return stlink_usb_write_ap_mem(stlink_dap_handle, ap_num, address, size, ++ count, buffer); ++ ++ /* !addrinc && size == 4 */ ++ ++ if ((h->version.stlink == 2 && h->version.jtag >= 32) || (h->version.stlink == 3 && h->version.jtag >= 2)) { ++ count *= 4; ++ while (count) { ++ bytes_remaining = stlink_usb_block(h); ++ ++ if (count < bytes_remaining) ++ bytes_remaining = count; ++ ++ retval = stlink_usb_write_mem32_noaddrinc(stlink_dap_handle, ap_num, address, bytes_remaining, buffer); ++ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { ++ usleep((1< stlink_usb_block(stlink_dap_handle) / 4) ++ partial = stlink_usb_block(stlink_dap_handle) / 4; ++ while (count) { ++ if (partial > count) ++ partial = count; ++ retval = stlink_usb_write_mem32(stlink_dap_handle, ap_num, address, 4 * partial, buffer); ++ if (retval == ERROR_WAIT && retries < MAX_WAIT_RETRIES) { ++ usleep((1< 0) ++ stlink_dap_speed(hz / 1000); ++ ++ return hz; ++} ++ ++static int stlink_dap_swd_switch_seq(enum swd_special_seq seq) ++{ ++ LOG_ERROR("stlink_dap_swd_switch_seq()"); ++ assert(0); ++ return ERROR_FAIL; ++} ++ ++static void stlink_dap_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay_clk) ++{ ++ LOG_ERROR("stlink_dap_swd_read_reg()"); ++ assert(0); ++} ++ ++static void stlink_dap_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk) ++{ ++ LOG_ERROR("stlink_dap_swd_write_reg()"); ++ assert(0); ++} ++ ++static int stlink_dap_swd_run_queue(void) ++{ ++ LOG_ERROR("stlink_dap_swd_run_queue()"); ++ assert(0); ++ return ERROR_FAIL; ++} ++ ++static void stlink_dap_execute_command(struct jtag_command *cmd) ++{ ++ switch (cmd->type) { ++ case JTAG_RESET: ++ LOG_DEBUG("stlink_usb_assert trst(%d) srst(%d)", !cmd->cmd.reset->trst, !cmd->cmd.reset->srst); ++ if (cmd->cmd.reset->trst) ++ stlink_usb_reset(stlink_dap_handle); ++ stlink_usb_assert_srst(stlink_dap_handle, cmd->cmd.reset->srst ? STLINK_DEBUG_APIV2_DRIVE_NRST_LOW : STLINK_DEBUG_APIV2_DRIVE_NRST_HIGH); ++ break; ++ case JTAG_SLEEP: ++ jtag_sleep(cmd->cmd.sleep->us); ++ break; ++ default: ++ LOG_ERROR("stlink_dap_execute_queue(%d)", cmd->type); ++ assert(0); ++ break; ++ } ++} ++ ++static int stlink_dap_execute_queue(void) ++{ ++ struct jtag_command *cmd = jtag_command_queue; ++ ++ LOG_DEBUG_IO("stlink_dap_execute_queue()"); ++ ++ while (cmd != NULL) { ++ stlink_dap_execute_command(cmd); ++ cmd = cmd->next; ++ } ++ ++ return ERROR_OK; ++} ++ ++static int stlink_dap_init(void) ++{ ++ enum reset_types jtag_reset_config = jtag_get_reset_config(); ++ ++ LOG_DEBUG("stlink_dap_init()"); ++ ++ if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { ++ if (jtag_reset_config & RESET_SRST_NO_GATING) ++ stlink_dap_param.connect_under_reset = true; ++ else ++ LOG_WARNING("\'srst_nogate\' reset_config option is required"); ++ } ++ return stlink_usb_open(&stlink_dap_param, (void **)&stlink_dap_handle); ++} ++ ++static int stlink_dap_quit(void) ++{ ++ int retval; ++ ++ LOG_DEBUG("stlink_dap_quit()"); ++ ++ retval = stlink_dap_closeall_ap(); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ free((void *)stlink_dap_param.serial); ++ stlink_dap_param.serial = NULL; ++ ++ return stlink_usb_close(stlink_dap_handle); ++} ++ ++COMMAND_HANDLER(stlink_dap_serial_command) ++{ ++ LOG_DEBUG("stlink_dap_serial_command"); ++ ++ if (CMD_ARGC != 1) { ++ LOG_ERROR("Expected exactly one argument for \"st-link serial \"."); ++ return ERROR_COMMAND_SYNTAX_ERROR; ++ } ++ ++ if (stlink_dap_param.serial) { ++ LOG_WARNING("Command \"stlink serial\" already used. Replace previous value"); ++ free((void *)stlink_dap_param.serial); ++ } ++ ++ stlink_dap_param.serial = strdup(CMD_ARGV[0]); ++ return ERROR_OK; ++} ++ ++static const struct command_registration stlink_dap_subcommand_handlers[] = { ++ { ++ .name = "serial", ++ .handler = &stlink_dap_serial_command, ++ .mode = COMMAND_CONFIG, ++ .help = "set the serial number of the device that should be used", ++ .usage = "", ++ }, ++ COMMAND_REGISTRATION_DONE ++}; ++ ++static const struct command_registration stlink_dap_command_handlers[] = { ++ { ++ .name = "st-link", ++ .mode = COMMAND_ANY, ++ .help = "perform st-link management", ++ .chain = stlink_dap_subcommand_handlers, ++ .usage = "", ++ }, ++ COMMAND_REGISTRATION_DONE ++}; ++ ++static const struct swd_driver stlink_dap_swd_driver = { ++ .init = stlink_dap_swd_init, ++ .frequency = stlink_dap_swd_frequency, ++ .switch_seq = stlink_dap_swd_switch_seq, ++ .read_reg = stlink_dap_swd_read_reg, ++ .write_reg = stlink_dap_swd_write_reg, ++ .run = stlink_dap_swd_run_queue, ++}; ++ ++static const char *const stlink_dap_transport[] = { "stlink_swd", "stlink_jtag", NULL }; ++ ++struct jtag_interface stlink_dap_interface = { ++ .name = "st-link", ++ .commands = stlink_dap_command_handlers, ++ .swd = &stlink_dap_swd_driver, ++ .transports = stlink_dap_transport, ++ ++ .execute_queue = stlink_dap_execute_queue, ++ .speed = stlink_dap_speed, ++ .speed_div = stlink_dap_speed_div, ++ .khz = stlink_dap_khz, ++ .init = stlink_dap_init, ++ .quit = stlink_dap_quit, ++}; +diff --git a/src/jtag/interfaces.c b/src/jtag/interfaces.c +index 286a73a..1ef422f 100644 +--- a/src/jtag/interfaces.c ++++ b/src/jtag/interfaces.c +@@ -135,6 +135,9 @@ extern struct jtag_interface imx_gpio_interface; + #if BUILD_XDS110 == 1 + extern struct jtag_interface xds110_interface; + #endif ++#if BUILD_HLADAPTER_STLINK == 1 ++extern struct jtag_interface stlink_dap_interface; ++#endif + #endif /* standard drivers */ + + /** +@@ -240,6 +243,9 @@ struct jtag_interface *jtag_interfaces[] = { + #if BUILD_XDS110 == 1 + &xds110_interface, + #endif ++#if BUILD_HLADAPTER_STLINK == 1 ++ &stlink_dap_interface, ++#endif + #endif /* standard drivers */ + NULL, + }; +diff --git a/src/jtag/startup.tcl b/src/jtag/startup.tcl +index d57cafb..765c417 100644 +--- a/src/jtag/startup.tcl ++++ b/src/jtag/startup.tcl +@@ -21,11 +21,19 @@ proc jtag_init {} { + # startup (at OpenOCD server startup, when JTAG may not yet work); and + # potentially more (for reset types like cold, warm, etc) + proc init_reset { mode } { +- if {[using_jtag]} { +- jtag arp_init-reset ++ if {![using_hla]} { ++ arp_init_reset + } + } + ++proc reset_assert_final { mode } { ++ arp_adapter_reset assert ++} ++ ++proc reset_deassert_initial { mode } { ++ arp_adapter_reset deassert ++} ++ + ######### + + # TODO: power_restore and power_dropout are currently neither +diff --git a/src/jtag/tcl.c b/src/jtag/tcl.c +index e32f0ca..734ef1e 100644 +--- a/src/jtag/tcl.c ++++ b/src/jtag/tcl.c +@@ -549,7 +549,7 @@ static int jim_newtap_cmd(Jim_GetOptInfo *goi) + LOG_DEBUG("Creating New Tap, Chip: %s, Tap: %s, Dotted: %s, %d params", + pTap->chip, pTap->tapname, pTap->dotted_name, goi->argc); + +- if (!transport_is_jtag()) { ++ if (!transport_is_jtag() && !transport_is_stlink_jtag()) { + /* SWD doesn't require any JTAG tap parameters */ + pTap->enabled = true; + jtag_tap_init(pTap); +@@ -684,9 +684,9 @@ static int jim_jtag_arp_init_reset(Jim_Interp *interp, int argc, Jim_Obj *const + return JIM_ERR; + } + struct command_context *context = current_command_context(interp); +- if (transport_is_jtag()) ++ if (transport_is_jtag() || transport_is_stlink_jtag()) + e = jtag_init_reset(context); +- else if (transport_is_swd()) ++ else if (transport_is_swd() || transport_is_stlink_swd()) + e = swd_init_reset(context); + + if (e != ERROR_OK) { +diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c +index a451273..fc5f967 100644 +--- a/src/server/gdb_server.c ++++ b/src/server/gdb_server.c +@@ -1395,11 +1395,6 @@ static int gdb_error(struct connection *connection, int retval) + return ERROR_OK; + } + +-/* We don't have to worry about the default 2 second timeout for GDB packets, +- * because GDB breaks up large memory reads into smaller reads. +- * +- * 8191 bytes by the looks of it. Why 8191 bytes instead of 8192????? +- */ + static int gdb_read_memory_packet(struct connection *connection, + char const *packet, int packet_size) + { +@@ -3089,6 +3084,42 @@ static void gdb_log_callback(void *priv, const char *file, unsigned line, + gdb_output_con(connection, string); + } + ++/* ++ * During long memory read/write, the default 2 seconds timeout of GDB can ++ * expire due to slow JTAG interface combined with high traffic on the USB bus. ++ * The usual O packets cannot be used during memory read/write. ++ * To restart the GDB timeout counter, send a custom notification packet. It ++ * would be silently dropped because is not recognized by GDB. ++ */ ++static void gdb_keepalive_callback(void *priv, const char *file, unsigned line, ++ const char *function, const char *string) ++{ ++ static unsigned int count = 0; ++ struct connection *connection = priv; ++ struct gdb_connection *gdb_con = connection->priv; ++ int i, len; ++ unsigned int my_checksum = 0; ++ char buf[17]; ++ ++ /* keep_alive() sends empty strings */ ++ if (gdb_con->busy || string[0]) ++ return; ++ ++ len = sprintf(buf, "%%keepalive:%2.2x", count); ++ count = (count + 1) & 255; ++ for (i = 1; i < len; i++) ++ my_checksum += buf[i]; ++ len += sprintf(buf + len, "#%2.2x", my_checksum & 255); ++ ++#ifdef _DEBUG_GDB_IO_ ++ LOG_DEBUG("sending packet '%s'", buf); ++#endif ++ ++ gdb_con->busy = true; ++ gdb_write(connection, buf, len); ++ gdb_con->busy = false; ++} ++ + static void gdb_sig_halted(struct connection *connection) + { + char sig_reply[4]; +@@ -3172,10 +3203,14 @@ static int gdb_input_inner(struct connection *connection) + retval = gdb_set_register_packet(connection, packet, packet_size); + break; + case 'm': ++ log_add_callback(gdb_keepalive_callback, connection); + retval = gdb_read_memory_packet(connection, packet, packet_size); ++ log_remove_callback(gdb_keepalive_callback, connection); + break; + case 'M': ++ log_add_callback(gdb_keepalive_callback, connection); + retval = gdb_write_memory_packet(connection, packet, packet_size); ++ log_remove_callback(gdb_keepalive_callback, connection); + break; + case 'z': + case 'Z': +@@ -3261,9 +3296,9 @@ static int gdb_input_inner(struct connection *connection) + extended_protocol = 0; + break; + case 'X': ++ log_add_callback(gdb_keepalive_callback, connection); + retval = gdb_write_memory_binary_packet(connection, packet, packet_size); +- if (retval != ERROR_OK) +- return retval; ++ log_remove_callback(gdb_keepalive_callback, connection); + break; + case 'k': + if (extended_protocol != 0) { +diff --git a/src/target/Makefile.am b/src/target/Makefile.am +index 4b7c8c0..bb305b7 100644 +--- a/src/target/Makefile.am ++++ b/src/target/Makefile.am +@@ -105,6 +105,10 @@ ARM_DEBUG_SRC = \ + %D%/etm_dummy.c \ + %D%/arm_cti.c + ++if HLADAPTER ++ARM_DEBUG_SRC += %D%/adi_v5_stlink.c ++endif ++ + AVR32_SRC = \ + %D%/avr32_ap7k.c \ + %D%/avr32_jtag.c \ +diff --git a/src/target/adi_v5_stlink.c b/src/target/adi_v5_stlink.c +new file mode 100644 +index 0000000..9e7ab0f +--- /dev/null ++++ b/src/target/adi_v5_stlink.c +@@ -0,0 +1,311 @@ ++/* ++ * Copyright (C) 2018, STMicroelectronics - All Rights Reserved ++ * Author(s): Antonio Borneo for STMicroelectronics ++ * ++ * This program is free software; you can redistribute it and/or modify ++ * it under the terms of the GNU General Public License as published by ++ * the Free Software Foundation; either version 2 of the License, or ++ * (at your option) any later version. ++ * ++ * This program is distributed in the hope that it will be useful ++ * but WITHOUT ANY WARRANTY; without even the implied warranty of ++ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ++ * GNU General Public License for more details. ++ * ++ * You should have received a copy of the GNU General Public License ++ * along with this program. If not, see . ++ */ ++ ++/** ++ * @file ++ * Utilities to support STMicroelectronics in-circuit debugger and programmer ++ * "ST-Link" (only V2 and newer versions) to access ARM DAP through either ++ * JTAG or ARM "Serial Wire Debug" (SWD). ++ * ++ * This implementation is complementary to the existing "hla" transport, which ++ * is too much focused at Cortex-M, and supports also Cortex-A and SMP nodes. ++ * ++ * Single-DAP support only. ++ * ++ * for details, see "ARM IHI 0031A" ++ * ARM Debug Interface v5 Architecture Specification ++ */ ++ ++#ifdef HAVE_CONFIG_H ++#include "config.h" ++#endif ++ ++#include "arm.h" ++#include "arm_adi_v5.h" ++#include ++ ++#include ++#include ++ ++#include ++ ++#define STLINK_DEBUG_PORT 0xffff ++ ++int stlink_dap_dap_read(unsigned short dap_port, unsigned short addr, uint32_t *val); ++int stlink_dap_dap_write(unsigned short dap_port, unsigned short addr, uint32_t val); ++int stlink_dap_ap_mem_read(struct adiv5_ap *ap, uint8_t *buffer, ++ uint32_t size, uint32_t count, uint32_t address, bool addrinc); ++int stlink_dap_ap_mem_write(struct adiv5_ap *ap, const uint8_t *buffer, ++ uint32_t size, uint32_t count, uint32_t address, bool addrinc); ++ ++static int stlink_check_reconnect(struct adiv5_dap *dap); ++ ++static int stlink_swd_queue_dp_read(struct adiv5_dap *dap, unsigned reg, ++ uint32_t *data) ++{ ++ int retval = stlink_check_reconnect(dap); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ retval = stlink_dap_dap_read(STLINK_DEBUG_PORT, reg, data); ++ if (retval != ERROR_OK) ++ dap->do_reconnect = true; ++ return retval; ++} ++ ++static int stlink_swd_queue_dp_write(struct adiv5_dap *dap, unsigned reg, ++ uint32_t data) ++{ ++ int retval = stlink_check_reconnect(dap); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* ST-Link does not like that we set CORUNDETECT */ ++ if (reg == DP_CTRL_STAT) ++ data &= ~CORUNDETECT; ++ ++ retval = stlink_dap_dap_write(STLINK_DEBUG_PORT, reg, data); ++ if (retval != ERROR_OK) ++ dap->do_reconnect = true; ++ return retval; ++} ++ ++static int stlink_swd_queue_ap_read(struct adiv5_ap *ap, unsigned reg, ++ uint32_t *data) ++{ ++ struct adiv5_dap *dap = ap->dap; ++ int retval = stlink_check_reconnect(dap); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ retval = stlink_dap_dap_read(ap->ap_num, reg, data); ++ if (retval != ERROR_OK) ++ dap->do_reconnect = true; ++ return retval; ++} ++ ++static int stlink_swd_queue_ap_write(struct adiv5_ap *ap, unsigned reg, ++ uint32_t data) ++{ ++ struct adiv5_dap *dap = ap->dap; ++ int retval = stlink_check_reconnect(dap); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ retval = stlink_dap_dap_write(ap->ap_num, reg, data); ++ if (retval != ERROR_OK) ++ dap->do_reconnect = true; ++ return retval; ++} ++ ++static int stlink_connect(struct adiv5_dap *dap) ++{ ++ uint32_t dpidr; ++ int retval; ++ ++ LOG_INFO("stlink_connect(%sconnect)", dap->do_reconnect ? "re" : ""); ++ ++ /* Check if we should reset srst already when connecting, but not if reconnecting. */ ++ if (!dap->do_reconnect) { ++ enum reset_types jtag_reset_config = jtag_get_reset_config(); ++ ++ if (jtag_reset_config & RESET_CNCT_UNDER_SRST) { ++ if (jtag_reset_config & RESET_SRST_NO_GATING) ++ adapter_assert_reset(); ++ else ++ LOG_WARNING("\'srst_nogate\' reset_config option is required"); ++ } ++ } ++ ++ dap->do_reconnect = false; ++ dap_invalidate_cache(dap); ++ ++ retval = dap->ops->queue_dp_read(dap, DP_DPIDR, &dpidr); ++ if (retval == ERROR_OK) { ++ LOG_INFO("SWD DPIDR %#8.8" PRIx32, dpidr); ++ retval = dap_dp_init(dap); ++ } else ++ dap->do_reconnect = true; ++ ++ return retval; ++} ++ ++static int stlink_check_reconnect(struct adiv5_dap *dap) ++{ ++ if (dap->do_reconnect) ++ return stlink_connect(dap); ++ ++ return ERROR_OK; ++} ++ ++static int stlink_swd_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack) ++{ ++ LOG_ERROR("stlink_swd_queue_ap_abort()"); ++ return ERROR_OK; ++} ++ ++static int stlink_swd_run(struct adiv5_dap *dap) ++{ ++ /* Here no LOG_DEBUG. This is called continuously! */ ++ ++ /* ++ * ST-Link returns immediately after a DAP write, without waiting for it ++ * to complete. ++ * FIXME: Here we should check if the last operation is a read or a ++ * write, and issue the dummy read only to complete a write! ++ * ++ * Run a dummy read to DP_RDBUFF, as suggested in ++ * http://infocenter.arm.com/help/topic/com.arm.doc.faqs/ka16363.html ++ */ ++ return stlink_swd_queue_dp_read(dap, DP_RDBUFF, NULL); ++} ++ ++#define stlink_ap_mem_read stlink_dap_ap_mem_read ++#define stlink_ap_mem_write stlink_dap_ap_mem_write ++ ++const struct dap_ops stlink_dap_swd_ops = { ++ .connect = stlink_connect, ++ .queue_dp_read = stlink_swd_queue_dp_read, ++ .queue_dp_write = stlink_swd_queue_dp_write, ++ .queue_ap_read = stlink_swd_queue_ap_read, ++ .queue_ap_write = stlink_swd_queue_ap_write, ++ .queue_ap_abort = stlink_swd_queue_ap_abort, ++ .ap_mem_read = stlink_ap_mem_read, ++ .ap_mem_write = stlink_ap_mem_write, ++ .run = stlink_swd_run, ++}; ++ ++const struct dap_ops stlink_dap_jtag_ops = { ++ .connect = stlink_connect, ++ .queue_dp_read = stlink_swd_queue_dp_read, ++ .queue_dp_write = stlink_swd_queue_dp_write, ++ .queue_ap_read = stlink_swd_queue_ap_read, ++ .queue_ap_write = stlink_swd_queue_ap_write, ++ .queue_ap_abort = stlink_swd_queue_ap_abort, ++ .ap_mem_read = stlink_ap_mem_read, ++ .ap_mem_write = stlink_ap_mem_write, ++ .run = stlink_swd_run, ++}; ++ ++static const struct command_registration stlink_commands[] = { ++ { ++ /* ++ * Set up SWD and JTAG targets identically, unless/until ++ * infrastructure improves ... meanwhile, ignore all ++ * JTAG-specific stuff like IR length for SWD. ++ * ++ * REVISIT can we verify "just one SWD DAP" here/early? ++ */ ++ .name = "newdap", ++ .jim_handler = jim_jtag_newtap, ++ .mode = COMMAND_CONFIG, ++ .help = "declare a new DAP" ++ }, ++ COMMAND_REGISTRATION_DONE ++}; ++ ++static const struct command_registration stlink_handlers[] = { ++ { ++ .name = "stlink_dap", ++ .mode = COMMAND_ANY, ++ .help = "ST-Link command group", ++ .chain = stlink_commands, ++ .usage = "", ++ }, ++ COMMAND_REGISTRATION_DONE ++}; ++ ++static int stlink_jtag_select(struct command_context *ctx) ++{ ++ LOG_DEBUG("stlink_jtag_select()"); ++ ++ return register_commands(ctx, NULL, stlink_handlers); ++} ++ ++static int stlink_swd_select(struct command_context *ctx) ++{ ++ /* FIXME: only place where global 'jtag_interface' is still needed */ ++ extern struct jtag_interface *jtag_interface; ++ const struct swd_driver *swd = jtag_interface->swd; ++ int retval; ++ ++ LOG_DEBUG("stlink_swd_select()"); ++ ++ retval = register_commands(ctx, NULL, stlink_handlers); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* be sure driver is in SWD mode; start ++ * with hardware default TRN (1), it can be changed later ++ */ ++ if (!swd || !swd->read_reg || !swd->write_reg || !swd->init) { ++ LOG_DEBUG("no SWD driver?"); ++ return ERROR_FAIL; ++ } ++ ++ retval = swd->init(); ++ if (retval != ERROR_OK) { ++ LOG_DEBUG("can't init SWD driver"); ++ return retval; ++ } ++ ++ return retval; ++} ++ ++static int stlink_init(struct command_context *ctx) ++{ ++ LOG_DEBUG("stlink_init()"); ++ ++ adapter_deassert_reset(); ++ return ERROR_OK; ++} ++ ++static struct transport stlink_jtag_transport = { ++ .name = "stlink_jtag", ++ .select = stlink_jtag_select, ++ .init = stlink_init, ++}; ++ ++static struct transport stlink_swd_transport = { ++ .name = "stlink_swd", ++ .select = stlink_swd_select, ++ .init = stlink_init, ++}; ++ ++static void stlink_constructor(void) __attribute__((constructor)); ++static void stlink_constructor(void) ++{ ++ transport_register(&stlink_jtag_transport); ++ transport_register(&stlink_swd_transport); ++} ++ ++/** Returns true if the current debug session ++ * is using SWD as its transport. ++ */ ++bool transport_is_stlink_jtag(void) ++{ ++ return get_current_transport() == &stlink_jtag_transport; ++} ++ ++/** Returns true if the current debug session ++ * is using SWD as its transport. ++ */ ++bool transport_is_stlink_swd(void) ++{ ++ return get_current_transport() == &stlink_swd_transport; ++} +diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c +index d9f3bd7..51d228b 100644 +--- a/src/target/arm_adi_v5.c ++++ b/src/target/arm_adi_v5.c +@@ -331,6 +331,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz + const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF; + uint32_t csw_size; + uint32_t addr_xor; ++ uint32_t j = 0; + int retval = ERROR_OK; + + /* TI BE-32 Quirks mode: +@@ -424,6 +425,9 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz + mem_ap_update_tar_cache(ap); + if (addrinc) + address += this_size; ++ ++ if ((j++ % 32) == 0) ++ keep_alive(); + } + + /* REVISIT: Might want to have a queued version of this function that does not run. */ +@@ -461,6 +465,7 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint + const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF; + uint32_t csw_size; + uint32_t address = adr; ++ uint32_t j = 0; + int retval = ERROR_OK; + + /* TI BE-32 Quirks mode: +@@ -523,6 +528,9 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint + address += this_size; + + mem_ap_update_tar_cache(ap); ++ ++ if ((j++ % 32) == 0) ++ keep_alive(); + } + + if (retval == ERROR_OK) +@@ -594,24 +602,44 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint + int mem_ap_read_buf(struct adiv5_ap *ap, + uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address) + { ++ if (ap->dap->ops->ap_mem_read) { ++ int retval = ap->dap->ops->ap_mem_read(ap, buffer, size, count, address, true); ++ if (retval != ERROR_OP_NOT_SUPPORTED) ++ return retval; ++ } + return mem_ap_read(ap, buffer, size, count, address, true); + } + + int mem_ap_write_buf(struct adiv5_ap *ap, + const uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address) + { ++ if (ap->dap->ops->ap_mem_write) { ++ int retval = ap->dap->ops->ap_mem_write(ap, buffer, size, count, address, true); ++ if (retval != ERROR_OP_NOT_SUPPORTED) ++ return retval; ++ } + return mem_ap_write(ap, buffer, size, count, address, true); + } + + int mem_ap_read_buf_noincr(struct adiv5_ap *ap, + uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address) + { ++ if (ap->dap->ops->ap_mem_read) { ++ int retval = ap->dap->ops->ap_mem_read(ap, buffer, size, count, address, false); ++ if (retval != ERROR_OP_NOT_SUPPORTED) ++ return retval; ++ } + return mem_ap_read(ap, buffer, size, count, address, false); + } + + int mem_ap_write_buf_noincr(struct adiv5_ap *ap, + const uint8_t *buffer, uint32_t size, uint32_t count, uint32_t address) + { ++ if (ap->dap->ops->ap_mem_write) { ++ int retval = ap->dap->ops->ap_mem_write(ap, buffer, size, count, address, false); ++ if (retval != ERROR_OP_NOT_SUPPORTED) ++ return retval; ++ } + return mem_ap_write(ap, buffer, size, count, address, false); + } + +diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h +index a340b76..3b04d0f 100644 +--- a/src/target/arm_adi_v5.h ++++ b/src/target/arm_adi_v5.h +@@ -292,6 +292,16 @@ struct dap_ops { + + /** Optional; called at OpenOCD exit */ + void (*quit)(struct adiv5_dap *dap); ++ ++ /** Optional interface-specific optimized AP memory read */ ++ int (*ap_mem_read)(struct adiv5_ap *ap, uint8_t *buffer, ++ uint32_t size, uint32_t count, uint32_t address, ++ bool addrinc); ++ ++ /** Optional interface-specific optimized AP memory write */ ++ int (*ap_mem_write)(struct adiv5_ap *ap, const uint8_t *buffer, ++ uint32_t size, uint32_t count, uint32_t address, ++ bool addrinc); + }; + + /* +diff --git a/src/target/arm_dap.c b/src/target/arm_dap.c +index 3adb4ed..277a804 100644 +--- a/src/target/arm_dap.c ++++ b/src/target/arm_dap.c +@@ -34,6 +34,8 @@ static LIST_HEAD(all_dap); + + extern const struct dap_ops swd_dap_ops; + extern const struct dap_ops jtag_dp_ops; ++extern const struct dap_ops stlink_dap_swd_ops; ++extern const struct dap_ops stlink_dap_jtag_ops; + extern struct jtag_interface *jtag_interface; + + /* DAP command support */ +@@ -118,6 +120,13 @@ static int dap_init_all(void) + if (transport_is_swd()) { + dap->ops = &swd_dap_ops; + obj->swd = jtag_interface->swd; ++#if BUILD_HLADAPTER_STLINK == 1 ++ } else if (transport_is_stlink_swd()) { ++ dap->ops = &stlink_dap_swd_ops; ++ obj->swd = jtag_interface->swd; ++ } else if (transport_is_stlink_jtag()) { ++ dap->ops = &stlink_dap_jtag_ops; ++#endif + } else + dap->ops = &jtag_dp_ops; + +diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h +index f8d1248..ac70ce3 100644 +--- a/src/target/arm_dpm.h ++++ b/src/target/arm_dpm.h +@@ -188,8 +188,6 @@ void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar); + #define DSCR_DTR_RX_FULL (0x1 << 30) /* bit 31 is reserved */ + + #define DSCR_ENTRY(dscr) ((dscr) & 0x3f) +-#define DSCR_RUN_MODE(dscr) ((dscr) & 0x03) +- + + /* Methods of entry into debug mode */ + #define DSCR_ENTRY_HALT_REQ (0x03) +@@ -240,4 +238,31 @@ void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr); + #define OSLSR_OSLM1 (1 << 3) + #define OSLSR_OSLM (OSLSR_OSLM0|OSLSR_OSLM1) + ++/* VCR (Vector Catch Register) bits */ ++#define VCR_RESET (1 << 0) ++#define VCR_UNDEFINED (1 << 1) ++#define VCR_SUPERVISOR_CALL (1 << 2) ++#define VCR_PREFETCH_ABORT (1 << 3) ++#define VCR_DATA_ABORT (1 << 4) ++#define VCR_IRQ (1 << 6) ++#define VCR_FIQ (1 << 7) ++#define VCR_MONITOR_SECURE_CALL (1 << 10) ++#define VCR_MONITOR_PREFETCH_ABORT (1 << 11) ++#define VCR_MONITOR_DATA_ABORT (1 << 12) ++#define VCR_MONITOR_IRQ (1 << 14) ++#define VCR_MONITOR_FIQ (1 << 15) ++#define VCR_HYP_UNDEFINED (1 << 17) ++#define VCR_HYPERVISOR_CALL (1 << 18) ++#define VCR_HYPERVISOR_PREFETCH_ABORT (1 << 19) ++#define VCR_HYPERVISOR_DATA_ABORT (1 << 20) ++#define VCR_HYPERVISOR_TRAP_ENTRY (1 << 21) ++#define VCR_HYPERVISOR_IRQ (1 << 22) ++#define VCR_HYPERVISOR_FIQ (1 << 23) ++#define VCR_NONSEC_UNDEFINED (1 << 25) ++#define VCR_NONSEC_SUPERVISOR_CALL (1 << 26) ++#define VCR_NONSEC_PREFETCH_ABORT (1 << 27) ++#define VCR_NONSEC_DATA_ABORT (1 << 28) ++#define VCR_NONSEC_IRQ (1 << 30) ++#define VCR_NONSEC_FIQ (1 << 31) ++ + #endif /* OPENOCD_TARGET_ARM_DPM_H */ +diff --git a/src/target/armv7a.c b/src/target/armv7a.c +index eecfa70..c4a3b05 100644 +--- a/src/target/armv7a.c ++++ b/src/target/armv7a.c +@@ -226,7 +226,8 @@ int armv7a_mmu_translate_va(struct target *target, uint32_t va, uint32_t *val) + LOG_DEBUG("1st lvl desc: %8.8" PRIx32 "", first_lvl_descriptor); + + if ((first_lvl_descriptor & 0x3) == 0) { +- LOG_ERROR("Address translation failure"); ++ /* Avoid LOG_ERROR, probably GDB is guessing the stack frame */ ++ LOG_WARNING("Address translation failure [1]: va %8.8" PRIx32 "", va); + return ERROR_TARGET_TRANSLATION_FAULT; + } + +@@ -258,7 +259,8 @@ int armv7a_mmu_translate_va(struct target *target, uint32_t va, uint32_t *val) + LOG_DEBUG("2nd lvl desc: %8.8" PRIx32 "", second_lvl_descriptor); + + if ((second_lvl_descriptor & 0x3) == 0) { +- LOG_ERROR("Address translation failure"); ++ /* Avoid LOG_ERROR, probably GDB is guessing the stack frame */ ++ LOG_WARNING("Address translation failure [2]: va %8.8" PRIx32 "", va); + return ERROR_TARGET_TRANSLATION_FAULT; + } + +@@ -665,6 +667,19 @@ done: + + } + ++/** ++ * Clears register cache during reset process ++ */ ++int armv7a_reset_clear_internal_state(struct target *target) ++{ ++ struct armv7a_common *armv7a = target_to_armv7a(target); ++ ++ if (target_was_examined(target)) ++ register_cache_invalidate(armv7a->arm.core_cache); ++ ++ return target_reset_clear_internal_state_default(target); ++} ++ + static int armv7a_setup_semihosting(struct target *target, int enable) + { + struct armv7a_common *armv7a = target_to_armv7a(target); +diff --git a/src/target/armv7a.h b/src/target/armv7a.h +index 57779c6..1a83522 100644 +--- a/src/target/armv7a.h ++++ b/src/target/armv7a.h +@@ -196,6 +196,8 @@ int armv7a_handle_cache_info_command(struct command_context *cmd_ctx, + struct armv7a_cache_common *armv7a_cache); + int armv7a_read_ttbcr(struct target *target); + ++int armv7a_reset_clear_internal_state(struct target *target); ++ + extern const struct command_registration armv7a_command_handlers[]; + + #endif /* OPENOCD_TARGET_ARMV7A_H */ +diff --git a/src/target/armv7m.c b/src/target/armv7m.c +index 7d3bd73..af394f8 100644 +--- a/src/target/armv7m.c ++++ b/src/target/armv7m.c +@@ -657,6 +657,19 @@ void armv7m_free_reg_cache(struct target *target) + arm->core_cache = NULL; + } + ++/** ++ * Clears register cache during reset process ++ */ ++int armv7m_reset_clear_internal_state(struct target *target) ++{ ++ struct armv7m_common *armv7m = target_to_armv7m(target); ++ ++ if (target_was_examined(target)) ++ register_cache_invalidate(armv7m->arm.core_cache); ++ ++ return target_reset_clear_internal_state_default(target); ++} ++ + static int armv7m_setup_semihosting(struct target *target, int enable) + { + /* nothing todo for armv7m */ +diff --git a/src/target/armv7m.h b/src/target/armv7m.h +index 01bf19e..f424c6c 100644 +--- a/src/target/armv7m.h ++++ b/src/target/armv7m.h +@@ -218,7 +218,7 @@ int armv7m_wait_algorithm(struct target *target, + target_addr_t exit_point, int timeout_ms, + void *arch_info); + +-int armv7m_invalidate_core_regs(struct target *target); ++int armv7m_reset_clear_internal_state(struct target *target); + + int armv7m_restore_context(struct target *target); + +diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c +index bc4aa33..f17eddd 100644 +--- a/src/target/cortex_a.c ++++ b/src/target/cortex_a.c +@@ -878,7 +878,7 @@ static int cortex_a_poll(struct target *target) + return retval; + cortex_a->cpudbg_dscr = dscr; + +- if (DSCR_RUN_MODE(dscr) == (DSCR_CORE_HALTED | DSCR_CORE_RESTARTED)) { ++ if (dscr & DSCR_CORE_HALTED) { + if (prev_target_state != TARGET_HALTED) { + /* We have a halting debug event */ + LOG_DEBUG("Target halted"); +@@ -1420,6 +1420,7 @@ static int cortex_a_step(struct target *target, int current, target_addr_t addre + + /* Setup single step breakpoint */ + stepbreakpoint.address = address; ++ stepbreakpoint.asid = 0; + stepbreakpoint.length = (arm->core_state == ARM_STATE_THUMB) + ? 2 : 4; + stepbreakpoint.type = BKPT_HARD; +@@ -1901,76 +1902,165 @@ static int cortex_a_remove_breakpoint(struct target *target, struct breakpoint * + return ERROR_OK; + } + +-/* +- * Cortex-A Reset functions ++/** ++ * Prepares debug state before reset or under active SRST if possible. ++ * ++ * @param halt sets vector catch to stop core at reset ++ * @param trigger triggers PRCR_WARM_RESET. + */ +- +-static int cortex_a_assert_reset(struct target *target) ++static int cortex_a_reset_prepare_trigger(struct target *target, bool halt, bool trigger) + { + struct armv7a_common *armv7a = target_to_armv7a(target); ++ uint32_t reg; ++ int retval; + +- LOG_DEBUG(" "); ++ /* cannot talk to target if it wasn't examined yet */ ++ if (!target_was_examined(target)) ++ return ERROR_OK; + +- /* FIXME when halt is requested, make it work somehow... */ ++ /* ++ * Some cores support connecting while SRST is asserted. ++ * If RESET_HAS_SRST and RESET_SRST_NO_GATING are configured, core is ++ * under SRST now ++ */ + +- /* This function can be called in "target not examined" state */ ++ if (halt) { ++ /* Enable debug requests */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, ®); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, reg | DSCR_HALT_DBG_MODE); ++ if (retval != ERROR_OK) ++ return retval; + +- /* Issue some kind of warm reset. */ +- if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) +- target_handle_event(target, TARGET_EVENT_RESET_ASSERT); +- else if (jtag_get_reset_config() & RESET_HAS_SRST) { +- /* REVISIT handle "pulls" cases, if there's +- * hardware that needs them to work. +- */ ++ /* set the reset vector catch */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, ®); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, reg | VCR_RESET); ++ if (retval != ERROR_OK) ++ return retval; + +- /* +- * FIXME: fix reset when transport is SWD. This is a temporary +- * work-around for release v0.10 that is not intended to stay! +- */ +- if (transport_is_swd() || +- (target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING))) +- jtag_add_reset(0, 1); ++ /* halt the core */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DRCR, DRCR_HALT); ++ if (retval != ERROR_OK) ++ return retval; + ++ /* hold core in reset */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_PRCR, PRCR_HOLD_NON_DEBUG_RESET); ++ if (retval != ERROR_OK) ++ return retval; + } else { +- LOG_ERROR("%s: how to reset?", target_name(target)); +- return ERROR_FAIL; ++ /* clear the reset vector catch */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, ®); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, reg & (~VCR_RESET)); ++ ++ /* resume the core if it was halted */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DRCR, DRCR_RESTART); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* resume the core if it was holding the reset */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_PRCR, 0); ++ if (retval != ERROR_OK) ++ return retval; + } + +- /* registers are now invalid */ +- if (target_was_examined(target)) +- register_cache_invalidate(armv7a->arm.core_cache); ++ if (trigger) { ++ /* ++ * Use a standard Cortex-A software reset mechanism. ++ * PRCR_WARM_RESET is 'recommended' but 'implementation defined'. ++ * See DDI0406C ARMv7-A/R Architecture Reference Manual. ++ * Usually this has the disadvantage of not resetting the peripherals, ++ * so a reset-init event handler is needed to perform any peripheral ++ * resets. ++ */ ++ reg = (halt ? PRCR_HOLD_NON_DEBUG_RESET : 0) | PRCR_WARM_RESET; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_PRCR, reg); ++ if (retval != ERROR_OK) ++ return retval; ++ } + +- target->state = TARGET_RESET; ++ armv7a_reset_clear_internal_state(target); + + return ERROR_OK; + } + +-static int cortex_a_deassert_reset(struct target *target) ++static int cortex_a_post_deassert_reset(struct target *target) + { ++ struct armv7a_common *armv7a = target_to_armv7a(target); ++ uint32_t reg; + int retval; + +- LOG_DEBUG(" "); ++ /* cannot talk to target if it wasn't examined yet */ ++ if (!target_was_examined(target)) ++ return ERROR_OK; ++ ++ if (!target->reset_halt) ++ return ERROR_OK; ++ ++ /* Enable debug requests */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, ®); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, reg | DSCR_HALT_DBG_MODE); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* halt the core while reset is on hold */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DRCR, DRCR_HALT); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* resume the core from reset hold */ ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_PRCR, 0); ++ if (retval != ERROR_OK) ++ return retval; ++ ++ /* clear the reset vector-catch */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, ®); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_VCR, reg & (~VCR_RESET)); ++ if (retval != ERROR_OK) ++ return retval; + +- /* be certain SRST is off */ +- jtag_add_reset(0, 0); ++ retval = cortex_a_poll(target); ++ if (retval != ERROR_OK) ++ return retval; + +- if (target_was_examined(target)) { ++ for (int i = 0; i < 50 && target->state != TARGET_HALTED; i++) { ++ alive_sleep(1); + retval = cortex_a_poll(target); + if (retval != ERROR_OK) + return retval; + } + +- if (target->reset_halt) { +- if (target->state != TARGET_HALTED) { +- LOG_WARNING("%s: ran after reset and before halt ...", +- target_name(target)); +- if (target_was_examined(target)) { +- retval = target_halt(target); +- if (retval != ERROR_OK) +- return retval; +- } else +- target->state = TARGET_UNKNOWN; +- } ++ if (target->state != TARGET_HALTED) { ++ LOG_WARNING("%s: ran after reset and before halt ...", ++ target_name(target)); ++ retval = target_halt(target); ++ if (retval != ERROR_OK) ++ return retval; + } + + return ERROR_OK; +@@ -2749,6 +2839,10 @@ static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address + LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, + address, size, count); + ++ /* flush the data cache */ ++ if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) ++ armv7a_l1_d_cache_clean_virt(target, address, size * count); ++ + /* determine if MMU was enabled on target stop */ + if (!armv7a->is_armv7r) { + retval = cortex_a_mmu(target, &mmu_enabled); +@@ -2756,24 +2850,42 @@ static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address + return retval; + } + +- if (mmu_enabled) { ++ if (!count || !buffer) ++ return ERROR_COMMAND_SYNTAX_ERROR; ++ ++ if (!mmu_enabled) ++ return mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address); ++ ++ /* ++ * TODO: here we are pessimistic and we use the smaller page of 4kB, but ++ * cortex_a_virt2phys() does a full table walk, so can return the actual ++ * size of page (4kB or 64kB) or section (1MB or 16Mb). ++ */ ++ while (count) { ++ target_addr_t page_size = 0x1000; ++ uint32_t current_count; ++ + virt = address; + retval = cortex_a_virt2phys(target, virt, &phys); + if (retval != ERROR_OK) + return retval; + +- LOG_DEBUG("Reading at virtual address. " +- "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, +- virt, phys); +- address = phys; +- } +- +- if (!count || !buffer) +- return ERROR_COMMAND_SYNTAX_ERROR; ++ current_count = (page_size - (address & (page_size - 1))) / size; ++ if (current_count > count) ++ current_count = count; + +- retval = mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address); ++ LOG_DEBUG("Reading at virtual address 0x%" PRIx32 " bytes. " ++ "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, ++ size * current_count, virt, phys); + +- return retval; ++ retval = mem_ap_read_buf(armv7a->memory_ap, buffer, size, current_count, phys); ++ if (retval != ERROR_OK) ++ return retval; ++ count -= current_count; ++ address += size * current_count; ++ buffer += size * current_count; ++ } ++ return ERROR_OK; + } + + static int cortex_a_write_phys_memory(struct target *target, +@@ -2837,6 +2949,12 @@ static int cortex_a_write_memory_ahb(struct target *target, target_addr_t addres + LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, + address, size, count); + ++ /* flush and invalidate the data cache */ ++ if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) ++ armv7a_cache_flush_virt(target, address, size * count); ++ /* invalidate instruction cache */ ++ armv7a_l1_i_cache_inval_virt(target, address, size * count); ++ + /* determine if MMU was enabled on target stop */ + if (!armv7a->is_armv7r) { + retval = cortex_a_mmu(target, &mmu_enabled); +@@ -2844,25 +2962,42 @@ static int cortex_a_write_memory_ahb(struct target *target, target_addr_t addres + return retval; + } + +- if (mmu_enabled) { ++ if (!count || !buffer) ++ return ERROR_COMMAND_SYNTAX_ERROR; ++ ++ if (!mmu_enabled) ++ return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address); ++ ++ /* ++ * TODO: here we are pessimistic and we use the smaller page of 4kB, but ++ * cortex_a_virt2phys() does a full table walk, so can return the actual ++ * size of page (4kB or 64kB) or section (1MB or 16Mb). ++ */ ++ while (count) { ++ target_addr_t page_size = 0x1000; ++ uint32_t current_count; ++ + virt = address; + retval = cortex_a_virt2phys(target, virt, &phys); + if (retval != ERROR_OK) + return retval; + +- LOG_DEBUG("Writing to virtual address. " +- "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, +- virt, +- phys); +- address = phys; +- } +- +- if (!count || !buffer) +- return ERROR_COMMAND_SYNTAX_ERROR; ++ current_count = (page_size - (address & (page_size - 1))) / size; ++ if (current_count > count) ++ current_count = count; + +- retval = mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address); ++ LOG_DEBUG("Writing to virtual address 0x%" PRIx32 " bytes. " ++ "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, ++ size * current_count, virt, phys); + +- return retval; ++ retval = mem_ap_write_buf(armv7a->memory_ap, buffer, size, current_count, phys); ++ if (retval != ERROR_OK) ++ return retval; ++ count -= current_count; ++ address += size * current_count; ++ buffer += size * current_count; ++ } ++ return ERROR_OK; + } + + static int cortex_a_read_buffer(struct target *target, target_addr_t address, +@@ -2982,7 +3117,21 @@ static int cortex_a_examine_first(struct target *target) + + int i; + int retval = ERROR_OK; +- uint32_t didr, cpuid, dbg_osreg; ++ uint32_t didr, cpuid, dbg_osreg, dp_ctrl_stat; ++ ++ /* ++ * Some device turn off CSYSPWRUPACK or CDBGPWRUPACK at reset. ++ * Check them and eventually re init the DP. ++ */ ++ retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dp_ctrl_stat); ++ if (retval != ERROR_OK ++ || ((dp_ctrl_stat & CDBGPWRUPACK) == 0) ++ || (!swjdp->ignore_syspwrupack && ((dp_ctrl_stat & CSYSPWRUPACK) == 0))) { ++ LOG_INFO("DP disabled after reset. Re-initialize it!"); ++ retval = dap_dp_init(swjdp); ++ if (retval != ERROR_OK) ++ LOG_ERROR("DP initialization failed"); ++ } + + /* Search for the APB-AP - it is needed for access to debug registers */ + retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv7a->debug_ap); +@@ -2999,19 +3148,23 @@ static int cortex_a_examine_first(struct target *target) + + armv7a->debug_ap->memaccess_tck = 80; + +- /* Search for the AHB-AB. +- * REVISIT: We should search for AXI-AP as well and make sure the AP's MEMTYPE says it ++ /* Search for the AXI-AP or the AHB-AP. ++ * REVISIT: We should make sure the AP's MEMTYPE says it + * can access system memory. */ + armv7a->memory_ap_available = false; +- retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7a->memory_ap); +- if (retval == ERROR_OK) { ++ retval = dap_find_ap(swjdp, AP_TYPE_AXI_AP, &armv7a->memory_ap); ++ if (retval == ERROR_OK) + retval = mem_ap_init(armv7a->memory_ap); ++ if (retval != ERROR_OK) { ++ retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7a->memory_ap); + if (retval == ERROR_OK) +- armv7a->memory_ap_available = true; ++ retval = mem_ap_init(armv7a->memory_ap); + } + if (retval != ERROR_OK) { +- /* AHB-AP not found or unavailable - use the CPU */ +- LOG_DEBUG("No AHB-AP available for memory access"); ++ /* AHB-AP and AXI-AP not found or unavailable - use the CPU */ ++ LOG_DEBUG("Nor AHB-AP nor AXI-AP available for memory access"); ++ } else { ++ armv7a->memory_ap_available = true; + } + + if (!target->dbgbase_set) { +@@ -3508,8 +3661,9 @@ struct target_type cortexa_target = { + .resume = cortex_a_resume, + .step = cortex_a_step, + +- .assert_reset = cortex_a_assert_reset, +- .deassert_reset = cortex_a_deassert_reset, ++ .reset_clear_internal_state = armv7a_reset_clear_internal_state, ++ .reset_prepare_trigger = cortex_a_reset_prepare_trigger, ++ .deassert_reset = cortex_a_post_deassert_reset, + + /* REVISIT allow exporting VFP3 registers ... */ + .get_gdb_reg_list = arm_get_gdb_reg_list, +@@ -3587,8 +3741,9 @@ struct target_type cortexr4_target = { + .resume = cortex_a_resume, + .step = cortex_a_step, + +- .assert_reset = cortex_a_assert_reset, +- .deassert_reset = cortex_a_deassert_reset, ++ .reset_clear_internal_state = armv7a_reset_clear_internal_state, ++ .reset_prepare_trigger = cortex_a_reset_prepare_trigger, ++ .deassert_reset = cortex_a_post_deassert_reset, + + /* REVISIT allow exporting VFP3 registers ... */ + .get_gdb_reg_list = arm_get_gdb_reg_list, +diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c +index 07fea51..42eb4b2 100644 +--- a/src/target/cortex_m.c ++++ b/src/target/cortex_m.c +@@ -948,49 +948,24 @@ static int cortex_m_step(struct target *target, int current, + return ERROR_OK; + } + +-static int cortex_m_assert_reset(struct target *target) ++/** ++ * Prepares debug state before reset or under active SRST if possible. ++ * ++ * @param halt sets vector catch to stop core at reset ++ * @param trigger triggers SYSRESETREQ or VECTRESET. ++ */ ++int cortex_m_reset_prepare_trigger(struct target *target, bool halt, bool trigger) + { + struct cortex_m_common *cortex_m = target_to_cm(target); + struct armv7m_common *armv7m = &cortex_m->armv7m; + enum cortex_m_soft_reset_config reset_config = cortex_m->soft_reset_config; + +- LOG_DEBUG("target->state: %s", +- target_state_name(target)); +- +- enum reset_types jtag_reset_config = jtag_get_reset_config(); +- +- if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) { +- /* allow scripts to override the reset event */ +- +- target_handle_event(target, TARGET_EVENT_RESET_ASSERT); +- register_cache_invalidate(cortex_m->armv7m.arm.core_cache); +- target->state = TARGET_RESET; +- ++ /* cannot talk to target if it wasn't examined yet */ ++ if (!target_was_examined(target)) + return ERROR_OK; +- } +- +- /* some cores support connecting while srst is asserted +- * use that mode is it has been configured */ +- +- bool srst_asserted = false; +- +- if (!target_was_examined(target)) { +- if (jtag_reset_config & RESET_HAS_SRST) { +- adapter_assert_reset(); +- if (target->reset_halt) +- LOG_ERROR("Target not examined, will not halt after reset!"); +- return ERROR_OK; +- } else { +- LOG_ERROR("Target not examined, reset NOT asserted!"); +- return ERROR_FAIL; +- } +- } + +- if ((jtag_reset_config & RESET_HAS_SRST) && +- (jtag_reset_config & RESET_SRST_NO_GATING)) { +- adapter_assert_reset(); +- srst_asserted = true; +- } ++ /* Some cores support connecting while srst is asserted. ++ * If RESET_HAS_SRST and RESET_SRST_NO_GATING are configured, core is under SRST now */ + + /* Enable debug requests */ + int retval; +@@ -1008,17 +983,7 @@ static int cortex_m_assert_reset(struct target *target) + mem_ap_write_u32(armv7m->debug_ap, DCB_DCRDR, 0); + /* Ignore less important errors */ + +- if (!target->reset_halt) { +- /* Set/Clear C_MASKINTS in a separate operation */ +- if (cortex_m->dcb_dhcsr & C_MASKINTS) +- cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS); +- +- /* clear any debug flags before resuming */ +- cortex_m_clear_halt(target); +- +- /* clear C_HALT in dhcsr reg */ +- cortex_m_write_debug_halt_mask(target, 0, C_HALT); +- } else { ++ if (halt) { + /* Halt in debug on reset; endreset_event() restores DEMCR. + * + * REVISIT catching BUSERR presumably helps to defend against +@@ -1030,16 +995,19 @@ static int cortex_m_assert_reset(struct target *target) + TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET); + if (retval != ERROR_OK || retval2 != ERROR_OK) + LOG_INFO("AP write error, reset will not halt"); +- } ++ } else { ++ /* Set/Clear C_MASKINTS in a separate operation */ ++ if (cortex_m->dcb_dhcsr & C_MASKINTS) ++ cortex_m_write_debug_halt_mask(target, 0, C_MASKINTS); + +- if (jtag_reset_config & RESET_HAS_SRST) { +- /* default to asserting srst */ +- if (!srst_asserted) +- adapter_assert_reset(); ++ /* clear any debug flags before resuming */ ++ cortex_m_clear_halt(target); + +- /* srst is asserted, ignore AP access errors */ +- retval = ERROR_OK; +- } else { ++ /* clear C_HALT in dhcsr reg */ ++ cortex_m_write_debug_halt_mask(target, 0, C_HALT); ++ } ++ ++ if (trigger) { + /* Use a standard Cortex-M3 software reset mechanism. + * We default to using VECRESET as it is supported on all current cores. + * This has the disadvantage of not resetting the peripherals, so a +@@ -1059,7 +1027,7 @@ static int cortex_m_assert_reset(struct target *target) + ? AIRCR_SYSRESETREQ : AIRCR_VECTRESET)); + if (retval3 != ERROR_OK) + LOG_DEBUG("Ignoring AP write error right after reset"); +- ++#if 0 + retval3 = dap_dp_init(armv7m->debug_ap->dap); + if (retval3 != ERROR_OK) + LOG_ERROR("DP initialisation failed"); +@@ -1072,41 +1040,66 @@ static int cortex_m_assert_reset(struct target *target) + uint32_t tmp; + mem_ap_read_atomic_u32(armv7m->debug_ap, NVIC_AIRCR, &tmp); + } ++#endif ++ } else { ++ /* srst is asserted, ignore AP access errors */ ++ retval = ERROR_OK; + } + +- target->state = TARGET_RESET; +- jtag_add_sleep(50000); +- +- register_cache_invalidate(cortex_m->armv7m.arm.core_cache); ++ armv7m_reset_clear_internal_state(target); + + /* now return stored error code if any */ + if (retval != ERROR_OK) + return retval; +- +- if (target->reset_halt) { ++#if 0 ++ if (halt) { + retval = target_halt(target); + if (retval != ERROR_OK) + return retval; + } +- ++#endif + return ERROR_OK; + } + +-static int cortex_m_deassert_reset(struct target *target) ++#if 0 ++static int cortex_m_assert_reset(struct target *target) ++{ ++ LOG_DEBUG("target->state: %s", ++ target_state_name(target)); ++ ++ if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT)) { ++ /* allow scripts to override the reset event */ ++ ++ target_handle_event(target, TARGET_EVENT_RESET_ASSERT); ++ ++ struct cortex_m_common *cortex_m = target_to_cm(target); ++ register_cache_invalidate(cortex_m->armv7m.arm.core_cache); ++ target->state = TARGET_RESET; ++ ++ return ERROR_OK; ++ } ++ ++ enum reset_types jtag_reset_config = jtag_get_reset_config(); ++ ++ return cortex_m_prepare_reset(target, target->reset_halt, ++ (jtag_reset_config & RESET_HAS_SRST) == 0); ++} ++ ++static int cortex_m_post_deassert_reset(struct target *target) + { + struct armv7m_common *armv7m = &target_to_cm(target)->armv7m; + + LOG_DEBUG("target->state: %s", + target_state_name(target)); + +- /* deassert reset lines */ +- adapter_deassert_reset(); ++ /* SRST was deasserted in the previous Tcl call to reset_deassert_initial */ ++ /* adapter_deassert_reset(); is no more needed here */ + + enum reset_types jtag_reset_config = jtag_get_reset_config(); + +- if ((jtag_reset_config & RESET_HAS_SRST) && +- !(jtag_reset_config & RESET_SRST_NO_GATING) && +- target_was_examined(target)) { ++ if ((jtag_reset_config & RESET_HAS_SRST) ++ && target->dbg_under_srst != DBG_UNDER_SRST_WORKING ++ && target_was_examined(target)) { + int retval = dap_dp_init(armv7m->debug_ap->dap); + if (retval != ERROR_OK) { + LOG_ERROR("DP initialisation failed"); +@@ -1116,6 +1109,7 @@ static int cortex_m_deassert_reset(struct target *target) + + return ERROR_OK; + } ++#endif + + int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint) + { +@@ -2495,8 +2489,10 @@ struct target_type cortexm_target = { + .resume = cortex_m_resume, + .step = cortex_m_step, + +- .assert_reset = cortex_m_assert_reset, +- .deassert_reset = cortex_m_deassert_reset, ++ .reset_clear_internal_state = armv7m_reset_clear_internal_state, ++ .reset_prepare_trigger = cortex_m_reset_prepare_trigger, ++/* .assert_reset = cortex_m_assert_reset,*/ ++/* .deassert_reset = cortex_m_post_deassert_reset,*/ + .soft_reset_halt = cortex_m_soft_reset_halt, + + .get_gdb_reg_list = armv7m_get_gdb_reg_list, +diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h +index 2daf4cb..e6a2ab3 100644 +--- a/src/target/cortex_m.h ++++ b/src/target/cortex_m.h +@@ -200,6 +200,7 @@ target_to_cm(struct target *target) + } + + int cortex_m_examine(struct target *target); ++int cortex_m_prepare_reset(struct target *target, bool halt, bool without_srst); + int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint); + int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoint); + int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint); +diff --git a/src/target/startup.tcl b/src/target/startup.tcl +index cf844e1..10ee27c 100644 +--- a/src/target/startup.tcl ++++ b/src/target/startup.tcl +@@ -10,12 +10,14 @@ set in_process_reset 0 + # Catch reset recursion + proc ocd_process_reset { MODE } { + global in_process_reset ++ global arp_reset_mode + if {$in_process_reset} { + set in_process_reset 0 + return -code error "'reset' can not be invoked recursively" + } + + set in_process_reset 1 ++ set arp_reset_mode $MODE + set success [expr [catch {ocd_process_reset_inner $MODE} result]==0] + set in_process_reset 0 + +@@ -26,6 +28,158 @@ proc ocd_process_reset { MODE } { + } + } + ++ ++proc arp_is_tap_enabled { target } { ++ if (![using_jtag]) { ++ return 1 ++ } ++ return [jtag tapisenabled [$target cget -chain-position]] ++} ++ ++# duplicate of target_examine_one(), keep in sync ++proc arp_examine_one { target } { ++ if [arp_is_tap_enabled $target] { ++ $target invoke-event examine-start ++ set err [catch "$target arp_examine allow-defer"] ++ if { $err } { ++ $target invoke-event examine-fail ++ } else { ++ $target invoke-event examine-end ++ } ++ } ++} ++ ++ ++proc arp_reset_plan_no_srst { phase target } { ++ global arp_reset_mode ++ switch $phase { ++ pre { ++ arp_examine_one $target ++ } ++ middle { ++ $target arp_reset trigger $arp_reset_mode ++ } ++ post { ++ $target arp_reset post_deassert $arp_reset_mode ++ } ++ } ++} ++ ++proc arp_reset_plan_srst_dbg_Working { phase target } { ++ global arp_reset_mode ++ switch $phase { ++ pre { ++ # srst_nogate: SRST is asserted now ++ # srst_gates_jtag: SRST has not been asserted yet ++ } ++ middle { ++ # SRST is asserted, target is responsive ++ arp_examine_one $target ++ $target arp_reset prepare $arp_reset_mode ++ } ++ post { ++ $target arp_reset post_deassert $arp_reset_mode ++ } ++ } ++} ++ ++proc arp_reset_plan_srst_dbg_gated { phase target } { ++ global arp_reset_mode ++ switch $phase { ++ pre { ++ # SRST has not been asserted yet ++ # srst_nogate mode is not supported ++ $target arp_reset prepare $arp_reset_mode ++ } ++ middle { ++ # SRST is asserted, target debug gated ++ } ++ post { ++ arp_examine_one $target ++ $target arp_reset post_deassert $arp_reset_mode ++ } ++ } ++} ++ ++proc arp_reset_plan_srst_dbg_cleared { phase target } { ++ global arp_reset_mode ++ switch $phase { ++ pre { ++ # target debug is going to be cleared ++ # no point to pre-configure is ++ } ++ middle { ++ # SRST is asserted, target debug not accessible ++ if { [using_hla] } { ++ # hla_target controls SRST on its own and even worse ++ # an hl adapter may not export SRST control in API (ST-Link 1) ++ # The only possible workaround is to let the target control SRST ++ $target arp_reset assert $arp_reset_mode ++ } else { ++ $target arp_reset clear_internal_state $arp_reset_mode ++ } ++ } ++ post { ++ arp_examine_one $target ++ $target arp_reset post_deassert $arp_reset_mode ++ } ++ } ++} ++ ++proc arp_reset_default_handler { phase target } { ++ if [arp_is_tap_enabled $target] { ++ set dbg_u_srst [$target cget -dbg-under-srst] ++ if [reset_config_includes srst] { ++ switch $dbg_u_srst { ++ working { ++ arp_reset_plan_srst_dbg_Working $phase $target ++ } ++ gated { ++ arp_reset_plan_srst_dbg_gated $phase $target ++ } ++ cleared { ++ arp_reset_plan_srst_dbg_cleared $phase $target ++ } ++ unknown { ++ if [reset_config_includes srst_nogate] { ++ arp_reset_plan_srst_dbg_Working $phase $target ++ } else { ++ arp_reset_plan_srst_dbg_gated $phase $target ++ } ++ } ++ } ++ } else { ++ arp_reset_plan_no_srst $phase $target ++ } ++ } ++} ++ ++proc arp_reset_halt_default_handler { target } { ++ # Wait upto 1 second for target to halt. Why 1sec? Cause ++ # the JTAG tap reset signal might be hooked to a slow ++ # resistor/capacitor circuit - and it might take a while ++ # to charge ++ catch { $target arp_waitstate halted 1000 } ++} ++ ++# Utility to make 'reset halt' work as reset;halt on a target ++# It does not prevent running code after reset ++proc arp_reset_simple_halter { target } { ++ $target arp_poll ++ set st [$target curstate] ++ if { $st eq "reset" } { ++ # we assume running state follows ++ # if reset accidentaly halt, waiting is useless ++ catch { $target arp_waitstate running 1000 } ++ set st [$target curstate] ++ } ++ if { $st eq "running" } { ++ echo "$target: Ran after reset and before halt..." ++ $target arp_halt ++ } ++ arp_reset_halt_default_handler $target ++} ++ + proc ocd_process_reset_inner { MODE } { + set targets [target names] + +@@ -53,51 +207,53 @@ proc ocd_process_reset_inner { MODE } { + # relative to a previous restrictive scheme + + foreach t $targets { +- # New event script. + $t invoke-event reset-start + } + +- # Use TRST or TMS/TCK operations to reset all the tap controllers. +- # TAP reset events get reported; they might enable some taps. +- init_reset $MODE +- +- # Examine all targets on enabled taps. +- foreach t $targets { +- if {![using_jtag] || [jtag tapisenabled [$t cget -chain-position]]} { +- $t invoke-event examine-start +- set err [catch "$t arp_examine allow-defer"] +- if { $err == 0 } { +- $t invoke-event examine-end ++ # If srst_nogate is set, check all targets whether they support it ++ if {[reset_config_includes srst srst_nogate]} { ++ foreach t $targets { ++ set dbg_u_srst [$t cget -dbg-under-srst] ++ if {$dbg_u_srst eq "gated"} { ++ reset_config srst_gates_jtag ++ echo "'srst_nogate' is not supported by at least target $t" ++ echo "Reset config changed to 'srst_gates_jtag'" ++ break; + } + } + } ++ set early_reset_init [expr {[reset_config_includes independent_trst] ++ || [reset_config_includes srst srst_nogate]}] + +- # Assert SRST, and report the pre/post events. +- # Note: no target sees SRST before "pre" or after "post". +- foreach t $targets { +- $t invoke-event reset-assert-pre ++ if $early_reset_init { ++ # We have an independent trst or no-gating srst ++ ++ # Use TRST or TMS/TCK operations to reset all the tap controllers. ++ # TAP reset events get reported; they might enable some taps. ++ init_reset $MODE + } ++ + foreach t $targets { +- # C code needs to know if we expect to 'halt' +- if {![using_jtag] || [jtag tapisenabled [$t cget -chain-position]]} { +- $t arp_reset assert $halt +- } ++ $t invoke-event reset-assert-pre + } ++ ++ # Assert SRST ++ reset_assert_final $MODE ++ + foreach t $targets { + $t invoke-event reset-assert-post + } + +- # Now de-assert SRST, and report the pre/post events. +- # Note: no target sees !SRST before "pre" or after "post". + foreach t $targets { + $t invoke-event reset-deassert-pre + } +- foreach t $targets { +- # Again, de-assert code needs to know if we 'halt' +- if {![using_jtag] || [jtag tapisenabled [$t cget -chain-position]]} { +- $t arp_reset deassert $halt +- } ++ ++ # Deassert SRST ++ reset_deassert_initial $MODE ++ if { !$early_reset_init } { ++ if [using_jtag] { jtag arp_init } + } ++ + foreach t $targets { + $t invoke-event reset-deassert-post + } +@@ -107,7 +263,7 @@ proc ocd_process_reset_inner { MODE } { + # first executing any instructions. + if { $halt } { + foreach t $targets { +- if {[using_jtag] && ![jtag tapisenabled [$t cget -chain-position]]} { ++ if {![arp_is_tap_enabled $t]} { + continue + } + +@@ -117,18 +273,10 @@ proc ocd_process_reset_inner { MODE } { + continue + } + +- # Wait upto 1 second for target to halt. Why 1sec? Cause +- # the JTAG tap reset signal might be hooked to a slow +- # resistor/capacitor circuit - and it might take a while +- # to charge +- +- # Catch, but ignore any errors. +- catch { $t arp_waitstate halted 1000 } ++ $t invoke-event reset-halt + + # Did we succeed? +- set s [$t curstate] +- +- if { 0 != [string compare $s "halted" ] } { ++ if { [$t curstate] ne "halted" } { + return -code error [format "TARGET: %s - Not halted" $t] + } + } +@@ -137,7 +285,7 @@ proc ocd_process_reset_inner { MODE } { + #Pass 2 - if needed "init" + if { 0 == [string compare init $MODE] } { + foreach t $targets { +- if {[using_jtag] && ![jtag tapisenabled [$t cget -chain-position]]} { ++ if {![arp_is_tap_enabled $t]} { + continue + } + +@@ -147,11 +295,7 @@ proc ocd_process_reset_inner { MODE } { + continue + } + +- set err [catch "$t arp_waitstate halted 5000"] +- # Did it halt? +- if { $err == 0 } { +- $t invoke-event reset-init +- } ++ $t invoke-event reset-init + } + } + +@@ -162,12 +306,12 @@ proc ocd_process_reset_inner { MODE } { + + proc using_jtag {} { + set _TRANSPORT [ transport select ] +- expr { [ string first "jtag" $_TRANSPORT ] != -1 } ++ expr { [ string equal "jtag" $_TRANSPORT ] != 0 || [ string equal "hla_jtag" $_TRANSPORT ] != 0 } + } + + proc using_swd {} { + set _TRANSPORT [ transport select ] +- expr { [ string first "swd" $_TRANSPORT ] != -1 } ++ expr { [ string equal "swd" $_TRANSPORT ] != 0 || [ string equal "hla_swd" $_TRANSPORT ] != 0} + } + + proc using_hla {} { +@@ -175,6 +319,16 @@ proc using_hla {} { + expr { [ string first "hla" $_TRANSPORT ] != -1 } + } + ++proc using_stlink_jtag {} { ++ set _TRANSPORT [ transport select ] ++ expr { [ string equal "stlink_jtag" $_TRANSPORT ] != 0 } ++} ++ ++proc using_stlink_swd {} { ++ set _TRANSPORT [ transport select ] ++ expr { [ string equal "stlink_swd" $_TRANSPORT ] != 0 } ++} ++ + ######### + + # Temporary migration aid. May be removed starting in January 2011. +@@ -204,6 +358,10 @@ proc init_target_events {} { + set_default_target_event $t gdb-flash-erase-start "reset init" + set_default_target_event $t gdb-flash-write-end "reset halt" + set_default_target_event $t gdb-attach "halt" ++ set_default_target_event $t reset-assert-pre "arp_reset_default_handler pre $t" ++ set_default_target_event $t reset-assert-post "arp_reset_default_handler middle $t" ++ set_default_target_event $t reset-deassert-post "arp_reset_default_handler post $t" ++ set_default_target_event $t reset-halt "arp_reset_halt_default_handler $t" + } + } + +diff --git a/src/target/target.c b/src/target/target.c +index 478c39d..906f1be 100644 +--- a/src/target/target.c ++++ b/src/target/target.c +@@ -74,6 +74,8 @@ static int target_gdb_fileio_end_default(struct target *target, int retcode, + int fileio_errno, bool ctrl_c); + static int target_profiling_default(struct target *target, uint32_t *samples, + uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds); ++static void target_free_all_working_areas_restore(struct target *target, int restore); ++static void target_reset_examined(struct target *target); + + /* targets */ + extern struct target_type arm7tdmi_target; +@@ -163,6 +165,10 @@ static const Jim_Nvp nvp_assert[] = { + { .name = "F", NVP_DEASSERT }, + { .name = "t", NVP_ASSERT }, + { .name = "f", NVP_DEASSERT }, ++ { .name = "prepare", NVP_PREPARE }, ++ { .name = "trigger", NVP_TRIGGER }, ++ { .name = "post_deassert", NVP_POST_DEASSERT }, ++ { .name = "clear_internal_state", NVP_CLEAR_INTERNAL_STATE }, + { .name = NULL, .value = -1 } + }; + +@@ -209,10 +215,12 @@ static const Jim_Nvp nvp_target_event[] = { + { .value = TARGET_EVENT_RESET_ASSERT_POST, .name = "reset-assert-post" }, + { .value = TARGET_EVENT_RESET_DEASSERT_PRE, .name = "reset-deassert-pre" }, + { .value = TARGET_EVENT_RESET_DEASSERT_POST, .name = "reset-deassert-post" }, ++ { .value = TARGET_EVENT_RESET_HALT, .name = "reset-halt" }, + { .value = TARGET_EVENT_RESET_INIT, .name = "reset-init" }, + { .value = TARGET_EVENT_RESET_END, .name = "reset-end" }, + + { .value = TARGET_EVENT_EXAMINE_START, .name = "examine-start" }, ++ { .value = TARGET_EVENT_EXAMINE_FAIL, .name = "examine-fail" }, + { .value = TARGET_EVENT_EXAMINE_END, .name = "examine-end" }, + + { .value = TARGET_EVENT_DEBUG_HALTED, .name = "debug-halted" }, +@@ -269,6 +277,14 @@ static const Jim_Nvp nvp_reset_modes[] = { + { .name = NULL , .value = -1 }, + }; + ++static const Jim_Nvp nvp_dbg_under_srst[] = { ++ { .name = "unknown", .value = DBG_UNDER_SRST_UNKNOWN }, ++ { .name = "working", .value = DBG_UNDER_SRST_WORKING }, ++ { .name = "gated", .value = DBG_UNDER_SRST_GATED }, ++ { .name = "cleared", .value = DBG_UNDER_SRST_CLEARED }, ++ { .name = NULL , .value = -1 }, ++}; ++ + const char *debug_reason_name(struct target *t) + { + const char *cp; +@@ -697,18 +713,23 @@ static int default_examine(struct target *target) + } + + /* no check by default */ ++/*TODO: remove and use reset_deassert_post or reset_end event */ + static int default_check_reset(struct target *target) + { + return ERROR_OK; + } + ++/* Equvivalent Tcl code arp_examine_one is in src/target/startup.tcl ++ * Keep in sync */ + int target_examine_one(struct target *target) + { + target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_START); + + int retval = target->type->examine(target); +- if (retval != ERROR_OK) ++ if (retval != ERROR_OK) { ++ target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_FAIL); + return retval; ++ } + + target_call_event_callbacks(target, TARGET_EVENT_EXAMINE_END); + +@@ -760,6 +781,41 @@ const char *target_type_name(struct target *target) + return target->type->name; + } + ++int target_reset_prepare_trigger(struct target *target, bool halt, bool trigger) ++{ ++ if (!target->type->reset_prepare_trigger) { ++ if (halt || trigger) { ++ LOG_ERROR("Target %s does not support reset_prepare_trigger", ++ target_name(target)); ++ return ERROR_FAIL; ++ } ++ LOG_DEBUG("Target %s does not support reset_prepare", ++ target_name(target)); ++ return ERROR_OK; ++ } ++ return target->type->reset_prepare_trigger(target, halt, trigger); ++} ++ ++int target_reset_clear_internal_state_default(struct target *target) ++{ ++ if (target->defer_examine) ++ target_reset_examined(target); ++ ++ target_free_all_working_areas_restore(target, 0); ++ target->state = TARGET_RESET; ++ return ERROR_OK; ++} ++ ++int target_reset_clear_internal_state(struct target *target) ++{ ++ if (!target->type->reset_clear_internal_state) { ++ LOG_DEBUG("Target %s does not support reset_clear_internal_state", ++ target_name(target)); ++ return ERROR_OK; ++ } ++ return target->type->reset_clear_internal_state(target); ++} ++ + static int target_soft_reset_halt(struct target *target) + { + if (!target_was_examined(target)) { +@@ -1252,7 +1308,6 @@ int target_profiling(struct target *target, uint32_t *samples, + + /** + * Reset the @c examined flag for the given target. +- * Pure paranoia -- targets are zeroed on allocation. + */ + static void target_reset_examined(struct target *target) + { +@@ -4551,6 +4606,7 @@ enum target_cfg_param { + TCFG_RTOS, + TCFG_DEFER_EXAMINE, + TCFG_GDB_PORT, ++ TCFG_DBG_UNDER_SRST, + }; + + static Jim_Nvp nvp_config_opts[] = { +@@ -4567,6 +4623,7 @@ static Jim_Nvp nvp_config_opts[] = { + { .name = "-rtos", .value = TCFG_RTOS }, + { .name = "-defer-examine", .value = TCFG_DEFER_EXAMINE }, + { .name = "-gdb-port", .value = TCFG_GDB_PORT }, ++ { .name = "-dbg-under-srst", .value = TCFG_DBG_UNDER_SRST }, + { .name = NULL, .value = -1 } + }; + +@@ -4868,6 +4925,27 @@ no_params: + Jim_SetResultString(goi->interp, target->gdb_port_override ? : "undefined", -1); + /* loop for more */ + break; ++ ++ case TCFG_DBG_UNDER_SRST: ++ if (goi->isconfigure) { ++ e = Jim_GetOpt_Nvp(goi, nvp_dbg_under_srst, &n); ++ if (e != JIM_OK) { ++ Jim_GetOpt_NvpUnknown(goi, nvp_dbg_under_srst, 1); ++ return e; ++ } ++ target->dbg_under_srst = n->value; ++ } else { ++ if (goi->argc != 0) ++ goto no_params; ++ } ++ n = Jim_Nvp_value2name_simple(nvp_dbg_under_srst, target->dbg_under_srst); ++ if (n->name == NULL) { ++ target->dbg_under_srst = DBG_UNDER_SRST_UNKNOWN; ++ n = Jim_Nvp_value2name_simple(nvp_dbg_under_srst, target->dbg_under_srst); ++ } ++ Jim_SetResultString(goi->interp, n->name, -1); ++ /* loop for more */ ++ break; + } + } /* while (goi->argc) */ + +@@ -5228,7 +5306,7 @@ static int jim_target_reset(Jim_Interp *interp, int argc, Jim_Obj *const *argv) + + if (goi.argc != 2) { + Jim_WrongNumArgs(interp, 0, argv, +- "([tT]|[fF]|assert|deassert) BOOL"); ++ "[prepare|trigger|post_deassert|clear_internal_state|assert|deassert] [run|halt|BOOL]"); + return JIM_ERR; + } + +@@ -5239,35 +5317,106 @@ static int jim_target_reset(Jim_Interp *interp, int argc, Jim_Obj *const *argv) + return e; + } + /* the halt or not param */ +- jim_wide a; +- e = Jim_GetOpt_Wide(&goi, &a); ++ Jim_Obj *o; ++ e = Jim_GetOpt_Obj(&goi, &o); + if (e != JIM_OK) + return e; + + struct target *target = Jim_CmdPrivData(goi.interp); ++ Jim_Nvp *n2; ++ e = Jim_Nvp_name2value_obj(interp, nvp_reset_modes, o, &n2); ++ if (e == JIM_OK) { ++ target->reset_halt = n2->value == RESET_HALT || n2->value == RESET_INIT; ++ } else { ++ jim_wide a; ++ e = Jim_GetWide(interp, o, &a); ++ if (e == JIM_OK) ++ target->reset_halt = a != 0; ++ } ++ if (e != JIM_OK) { ++ Jim_GetOpt_NvpUnknown(&goi, nvp_reset_modes, 1); ++ return e; ++ } ++ + if (!target->tap->enabled) + return jim_target_tap_disabled(interp); + +- if (!target->type->assert_reset || !target->type->deassert_reset) { +- Jim_SetResultFormatted(interp, +- "No target-specific reset for %s", ++ enum reset_types jtag_reset_config = jtag_get_reset_config(); ++ ++ e = ERROR_FAIL; ++ switch (n->value) { ++ case NVP_ASSERT: ++ if (!target->type->assert_reset) { ++ Jim_SetResultFormatted(interp, ++ "No target-specific reset assert for %s", + target_name(target)); +- return JIM_ERR; +- } ++ return JIM_ERR; ++ } ++ e = target->type->assert_reset(target); ++ break; + +- if (target->defer_examine) +- target_reset_examined(target); ++ case NVP_PREPARE: ++ if (target->type->reset_prepare_trigger) { ++ e = target->type->reset_prepare_trigger(target, target->reset_halt, false); ++ } else if (target->type->assert_reset ++ && jtag_reset_config & RESET_HAS_SRST) { ++ /* old target compatibility */ ++ LOG_DEBUG("No target-specific reset prepare for %s, using assert_reset", ++ target_name(target)); ++ e = target->type->assert_reset(target); ++ } else { ++ LOG_DEBUG("No target-specific reset prepare for %s", ++ target_name(target)); ++ e = ERROR_OK; ++ } ++ break; + +- /* determine if we should halt or not. */ +- target->reset_halt = !!a; +- /* When this happens - all workareas are invalid. */ +- target_free_all_working_areas_restore(target, 0); ++ case NVP_TRIGGER: ++ if (target->type->reset_prepare_trigger) { ++ e = target->type->reset_prepare_trigger(target, target->reset_halt, true); ++ } else if (target->type->assert_reset ++ && (jtag_reset_config & RESET_HAS_SRST) == 0) { ++ /* old target compatibility */ ++ LOG_DEBUG("No target-specific reset trigger for %s, using assert_reset", ++ target_name(target)); ++ e = target->type->assert_reset(target); ++ } else { ++ Jim_SetResultFormatted(interp, ++ "No target-specific reset trigger for %s", ++ target_name(target)); ++ return JIM_ERR; ++ } ++ break; + +- /* do the assert */ +- if (n->value == NVP_ASSERT) +- e = target->type->assert_reset(target); +- else ++ case NVP_DEASSERT: ++ if (!target->type->deassert_reset) { ++ Jim_SetResultFormatted(interp, ++ "No target-specific reset deassert for %s", ++ target_name(target)); ++ return JIM_ERR; ++ } + e = target->type->deassert_reset(target); ++ break; ++ ++ case NVP_POST_DEASSERT: ++ if (target->type->deassert_reset) { ++ e = target->type->deassert_reset(target); ++ } else { ++ LOG_DEBUG("No target-specific reset post_deassert for %s", ++ target_name(target)); ++ e = ERROR_OK; ++ } ++ break; ++ ++ case NVP_CLEAR_INTERNAL_STATE: ++ if (target->type->reset_clear_internal_state) { ++ e = target->type->reset_clear_internal_state(target); ++ } else { ++ LOG_DEBUG("No target-specific reset clear internal state for %s", ++ target_name(target)); ++ e = target_reset_clear_internal_state_default(target); ++ } ++ } + return (e == ERROR_OK) ? JIM_OK : JIM_ERR; + } + +diff --git a/src/target/target.h b/src/target/target.h +index fe7e1a7..b24abc0 100644 +--- a/src/target/target.h ++++ b/src/target/target.h +@@ -67,6 +67,20 @@ enum target_state { + enum nvp_assert { + NVP_DEASSERT, + NVP_ASSERT, ++ NVP_PREPARE, ++ NVP_TRIGGER, ++ NVP_POST_DEASSERT, ++ NVP_CLEAR_INTERNAL_STATE, ++}; ++ ++enum target_dbg_under_srst { ++ DBG_UNDER_SRST_UNKNOWN = 0, ++ DBG_UNDER_SRST_WORKING = 1, ++ /* Debug circuitry is working with SRST asserted */ ++ DBG_UNDER_SRST_GATED = 2, ++ /* Debug circuitry holds the setting but access to it is blocked under SRST */ ++ DBG_UNDER_SRST_CLEARED = 3, ++ /* SRST clears debug circuitry */ + }; + + enum target_reset_mode { +@@ -152,7 +166,8 @@ struct target { + + struct target_event_action *event_action; + +- int reset_halt; /* attempt resetting the CPU into the halted mode? */ ++ enum target_dbg_under_srst dbg_under_srst; /* how SRST signal influences the debug circuitry */ ++ int reset_halt; /* attempt resetting the CPU into the halted mode */ + target_addr_t working_area; /* working area (initialised RAM). Evaluated + * upon first allocation from virtual/physical address. */ + bool working_area_virt_spec; /* virtual address specified? */ +@@ -259,6 +274,7 @@ enum target_event { + TARGET_EVENT_RESET_ASSERT_POST, + TARGET_EVENT_RESET_DEASSERT_PRE, + TARGET_EVENT_RESET_DEASSERT_POST, ++ TARGET_EVENT_RESET_HALT, + TARGET_EVENT_RESET_INIT, + TARGET_EVENT_RESET_END, + +@@ -266,6 +282,7 @@ enum target_event { + TARGET_EVENT_DEBUG_RESUMED, /* target resumed to execute on behalf of the debugger */ + + TARGET_EVENT_EXAMINE_START, ++ TARGET_EVENT_EXAMINE_FAIL, + TARGET_EVENT_EXAMINE_END, + + TARGET_EVENT_GDB_ATTACH, +@@ -369,6 +386,9 @@ int target_poll(struct target *target); + int target_resume(struct target *target, int current, target_addr_t address, + int handle_breakpoints, int debug_execution); + int target_halt(struct target *target); ++int target_reset_prepare_trigger(struct target *target, bool halt, bool trigger); ++int target_reset_clear_internal_state_default(struct target *target); ++int target_reset_clear_internal_state(struct target *target); + int target_call_event_callbacks(struct target *target, enum target_event event); + int target_call_reset_callbacks(struct target *target, enum target_reset_mode reset_mode); + int target_call_trace_callbacks(struct target *target, size_t len, uint8_t *data); +diff --git a/src/target/target_type.h b/src/target/target_type.h +index fbbd57d..24b29ea 100644 +--- a/src/target/target_type.h ++++ b/src/target/target_type.h +@@ -86,7 +86,23 @@ struct target_type { + * reset run; halt + */ + int (*deassert_reset)(struct target *target); ++ /** ++ * Substitutes assert_reset ++ * Prepares debug circuitry before or during SRST ++ * Optionally triggers reset using a debug register ++ * DOES NOT CONTROL SRST LINE!!! ++ * ++ * @param target The target to work on ++ * @param halt Prepare halt after reset ++ * @param trigger Trigger reset by setting a debug register ++ */ ++ int (*reset_prepare_trigger)(struct target *target, bool halt, bool trigger); + int (*soft_reset_halt)(struct target *target); ++ /** ++ * Internal target adjustment after reset ++ * Typically sets target state and clears register cache ++ */ ++ int (*reset_clear_internal_state)(struct target *target); + + /** + * Target register access for GDB. Do @b not call this function +diff --git a/src/transport/transport.h b/src/transport/transport.h +index 140ef50..8fb2585 100644 +--- a/src/transport/transport.h ++++ b/src/transport/transport.h +@@ -96,6 +96,8 @@ bool transports_are_declared(void); + + bool transport_is_jtag(void); + bool transport_is_swd(void); ++bool transport_is_stlink_jtag(void); ++bool transport_is_stlink_swd(void); + + #if BUILD_HLADAPTER + bool transport_is_hla(void); +diff --git a/tcl/board/stm32mp15x_dk2.cfg b/tcl/board/stm32mp15x_dk2.cfg +new file mode 100644 +index 0000000..82617c5 +--- /dev/null ++++ b/tcl/board/stm32mp15x_dk2.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board MB1272B. ++ ++source [find interface/stlink-dap.cfg] ++ ++transport select stlink_swd ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg +new file mode 100644 +index 0000000..7d74fa6 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/jlink.cfg] ++ ++transport select jtag ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_jlink_swd.cfg b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg +new file mode 100644 +index 0000000..33e6ef5 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/jlink.cfg] ++ ++transport select swd ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg +new file mode 100644 +index 0000000..6fb1e31 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/stlink-dap.cfg] ++ ++transport select stlink_jtag ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_stlink_swd.cfg b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg +new file mode 100644 +index 0000000..3c561c9 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/stlink-dap.cfg] ++ ++transport select stlink_swd ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg +new file mode 100644 +index 0000000..6b10351 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/cmsis-dap.cfg] ++ ++transport select jtag ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg +new file mode 100644 +index 0000000..5495bc3 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1263B with PMIC STPMU1. ++ ++source [find interface/cmsis-dap.cfg] ++ ++transport select swd ++ ++source [find target/stm32mp15x_stpmu1.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev2_jlink_jtag.cfg b/tcl/board/stm32mp15x_ev2_jlink_jtag.cfg +new file mode 100644 +index 0000000..fcb0064 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_jlink_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/jlink.cfg] ++ ++transport select jtag ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev2_jlink_swd.cfg b/tcl/board/stm32mp15x_ev2_jlink_swd.cfg +new file mode 100644 +index 0000000..bbf981c +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_jlink_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/jlink.cfg] ++ ++transport select swd ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev2_stlink_jtag.cfg b/tcl/board/stm32mp15x_ev2_stlink_jtag.cfg +new file mode 100644 +index 0000000..4f2e0df +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_stlink_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/stlink-dap.cfg] ++ ++transport select stlink_jtag ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev2_stlink_swd.cfg b/tcl/board/stm32mp15x_ev2_stlink_swd.cfg +new file mode 100644 +index 0000000..182ed4b +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_stlink_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/stlink-dap.cfg] ++ ++transport select stlink_swd ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev2_ulink2_jtag.cfg b/tcl/board/stm32mp15x_ev2_ulink2_jtag.cfg +new file mode 100644 +index 0000000..8c320a4 +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_ulink2_jtag.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/cmsis-dap.cfg] ++ ++transport select jtag ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev2_ulink2_swd.cfg b/tcl/board/stm32mp15x_ev2_ulink2_swd.cfg +new file mode 100644 +index 0000000..25b87cc +--- /dev/null ++++ b/tcl/board/stm32mp15x_ev2_ulink2_swd.cfg +@@ -0,0 +1,9 @@ ++# This is a STM32MP15x board set MB1262C + MB1264A. ++ ++source [find interface/cmsis-dap.cfg] ++ ++transport select swd ++ ++source [find target/stm32mp15x.cfg] ++ ++reset_config srst_only +diff --git a/tcl/interface/stlink-dap.cfg b/tcl/interface/stlink-dap.cfg +new file mode 100644 +index 0000000..78a1b24 +--- /dev/null ++++ b/tcl/interface/stlink-dap.cfg +@@ -0,0 +1,18 @@ ++# ++# STMicroelectronics ST-LINK/V1, ST-LINK/V2, ST-LINK/V2-1 in-circuit ++# debugger/programmer ++# ++# This new interface drive creates a ST-Link wrapper for ARM-DAP ++# ++ ++interface st-link ++ ++# transport select stlink_swd ++# transport select stlink_jtag ++ ++# Optionally specify the serial number of ST-LINK/V2 usb device. ST-LINK/V2 ++# devices seem to have serial numbers with unreadable characters. ST-LINK/V2 ++# firmware version >= V2.J21.S4 recommended to avoid issues with adapter serial ++# number reset issues. ++# e.g. ++# st-link serial "\xaa\xbc\x6e\x06\x50\x75\xff\x55\x17\x42\x19\x3f" +diff --git a/tcl/target/atsamv.cfg b/tcl/target/atsamv.cfg +index 1d026aa..6626a3d 100644 +--- a/tcl/target/atsamv.cfg ++++ b/tcl/target/atsamv.cfg +@@ -35,7 +35,7 @@ swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPU + dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu + + set _TARGETNAME $_CHIPNAME.cpu +-target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap ++target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap -dbg-under-srst gated + + $_TARGETNAME configure -work-area-phys 0x20400000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +diff --git a/tcl/target/imx6.cfg b/tcl/target/imx6.cfg +index 5b59ecf..0b0e87d 100644 +--- a/tcl/target/imx6.cfg ++++ b/tcl/target/imx6.cfg +@@ -6,6 +6,12 @@ if { [info exists CHIPNAME] } { + set _CHIPNAME imx6 + } + ++if { [info exists SMP] } { ++ set _SMP $SMP ++} else { ++ set _SMP 4 ++} ++ + # CoreSight Debug Access Port + if { [info exists DAP_TAPID] } { + set _DAP_TAPID $DAP_TAPID +@@ -39,22 +45,40 @@ jtag newtap $_CHIPNAME sjc -irlen 5 -ircapture 0x01 -irmask 0x1f \ + # core 1 - 0x82152000 + # core 2 - 0x82154000 + # core 3 - 0x82156000 +-set _TARGETNAME $_CHIPNAME.cpu.0 ++set _TARGETNAME $_CHIPNAME.cpu + dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu +-target create $_TARGETNAME cortex_a -dap $_CHIPNAME.dap \ +- -coreid 0 -dbgbase 0x82150000 ++target create $_TARGETNAME.0 cortex_a -dap $_CHIPNAME.dap \ ++ -coreid 0 -dbgbase 0x82150000 -dbg-under-srst cleared + +-# some TCK cycles are required to activate the DEBUG power domain +-jtag configure $_CHIPNAME.sjc -event post-reset "runtest 100" ++set _CORES $_TARGETNAME.0 + +-proc imx6_dbginit {target} { +- # General Cortex-A8/A9 debug initialisation +- cortex_a dbginit ++if { $_SMP >= 2 } { ++ target create $_TARGETNAME.1 cortex_a -chain-position $_CHIPNAME.dap \ ++ -coreid 1 -dbgbase 0x82152000 -defer-examine \ ++ -dbg-under-srst cleared ++} ++if { $_SMP == 2 } { ++ set _CORES [list $_TARGETNAME.1 $_TARGETNAME.0] ++ target smp {*}$_CORES + } ++if { $_SMP == 4 } { ++ target create $_TARGETNAME.2 cortex_a -chain-position $_CHIPNAME.dap \ ++ -coreid 2 -dbgbase 0x82154000 -defer-examine \ ++ -dbg-under-srst cleared ++ target create $_TARGETNAME.3 cortex_a -chain-position $_CHIPNAME.dap \ ++ -coreid 3 -dbgbase 0x82156000 -defer-examine \ ++ -dbg-under-srst cleared + +-# Slow speed to be sure it will work +-adapter_khz 1000 +-$_TARGETNAME configure -event reset-start { adapter_khz 1000 } ++ set _CORES [list $_TARGETNAME.3 $_TARGETNAME.2 \ ++ $_TARGETNAME.1 $_TARGETNAME.0 ] ++ target smp {*}$_CORES ++} + +-$_TARGETNAME configure -event reset-assert-post "imx6_dbginit $_TARGETNAME" +-$_TARGETNAME configure -event gdb-attach { halt } ++# some TCK cycles are required to activate the DEBUG power domain ++jtag configure $_CHIPNAME.sjc -event post-reset "runtest 100" ++ ++foreach t $_CORES { ++ $t configure -event reset-halt \ ++ "arp_reset_simple_halter $t" ++ $t configure -event gdb-attach { halt } ++} +diff --git a/tcl/target/kx.cfg b/tcl/target/kx.cfg +index 73ee62a..f16e772 100644 +--- a/tcl/target/kx.cfg ++++ b/tcl/target/kx.cfg +@@ -33,7 +33,7 @@ swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPU + dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.cpu + + set _TARGETNAME $_CHIPNAME.cpu +-target create $_TARGETNAME cortex_m -dap $_CHIPNAME.dap ++target create $_TARGETNAME cortex_m -dap $_CHIPNAME.dap -dbg-under-srst working + + $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +@@ -60,7 +60,10 @@ if {[using_hla]} { + echo "" + } { + # Detect secured MCU or boot lock-up in RESET/WDOG loop +- $_CHIPNAME.cpu configure -event examine-start { ++ $_CHIPNAME.cpu configure -event examine-fail { ++ kinetis mdm check_security ++ } ++ $_CHIPNAME.cpu configure -event examine-end { + kinetis mdm check_security + } + +diff --git a/tcl/target/stm32f7x.cfg b/tcl/target/stm32f7x.cfg +index e06a345..486d137 100755 +--- a/tcl/target/stm32f7x.cfg ++++ b/tcl/target/stm32f7x.cfg +@@ -43,7 +43,7 @@ if {[using_jtag]} { + } + + set _TARGETNAME $_CHIPNAME.cpu +-target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap ++target create $_TARGETNAME cortex_m -endian $_ENDIAN -dap $_CHIPNAME.dap -dbg-under-srst working + + $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg +new file mode 100644 +index 0000000..0def780 +--- /dev/null ++++ b/tcl/target/stm32mp15x.cfg +@@ -0,0 +1,165 @@ ++# script for stm32mp15x family ++ ++# ++# stm32 devices support both JTAG and SWD transports. ++# ++source [find target/swj-dp.tcl] ++source [find mem_helper.tcl] ++ ++if { [info exists CHIPNAME] } { ++ set _CHIPNAME $CHIPNAME ++} else { ++ set _CHIPNAME stm32mp15x ++} ++ ++set _ENDIAN little ++ ++# Work-area is a space in RAM used for flash programming ++# By default use 32kB ++if { [info exists WORKAREASIZE] } { ++ set _WORKAREASIZE $WORKAREASIZE ++} else { ++ set _WORKAREASIZE 0x8000 ++} ++ ++# jtag scan chain ++if { [info exists CPUTAPID] } { ++ set _CPUTAPID $CPUTAPID ++} else { ++ if { [using_jtag] } { ++ # See STM Document RM0436 ++ # Section 66.8.3 ++ set _CPUTAPID 0x6ba00477 ++ } { ++ set _CPUTAPID 0x6ba02477 ++ } ++} ++ ++swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f ++dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap ++target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -coreid 0 -dbgbase 0xE00D0000 ++target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -coreid 1 -dbgbase 0xE00D2000 ++ ++target smp $_CHIPNAME.cpu0 $_CHIPNAME.cpu1 ++$_CHIPNAME.cpu0 cortex_a maskisr on ++$_CHIPNAME.cpu1 cortex_a maskisr on ++$_CHIPNAME.cpu0 cortex_a dacrfixup on ++$_CHIPNAME.cpu1 cortex_a dacrfixup on ++ ++# Only in ENG_MODE the CM4 is available at reset ++if { [info exists ENG_MODE] } { ++ target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap ++} else { ++ target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap -defer-examine ++} ++ ++$_CHIPNAME.cpu0 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++$_CHIPNAME.cpu1 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++$_CHIPNAME.cpu2 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++ ++if { [info exists GDB_PORT] } { ++ set _GDB_PORT $GDB_PORT ++} else { ++ set _GDB_PORT [ocd_gdb_port] ++} ++ ++# The two CA7 will require one or two gdb ports, depending on smp enabled or not ++# Set a fixed gdb port of CM4 ++$_CHIPNAME.cpu0 configure -gdb-port [expr $_GDB_PORT + 1] ++$_CHIPNAME.cpu1 configure -gdb-port [expr $_GDB_PORT + 2] ++$_CHIPNAME.cpu2 configure -gdb-port [expr $_GDB_PORT + 0] ++ ++$_CHIPNAME.cpu0 configure -event reset-deassert-post { delayed_reset_halt_cpu0 } ++ ++# Reset-halt stops in bootrom when only cpu0 can be attached. ++# Set a breakpoint to stop cpu0 when cpu1 can be attached too. ++proc delayed_reset_halt_cpu0 { } { ++ global _CHIPNAME ++ global arp_reset_mode ++ ++ set rom_halt_pc 0x0000688c ++ ++ # only "reset halt" or "reset init" ++ if { 0 != [ string compare "$arp_reset_mode" "run" ] } { ++ cortex_a smp_off ++ targets $_CHIPNAME.cpu0 ++ arp_reset_default_handler post $_CHIPNAME.cpu0 ++ ++ poll on ++ catch { $_CHIPNAME.cpu0 arp_waitstate halted 1000 } ++ bp $rom_halt_pc 2 hw ++ resume ++ catch { $_CHIPNAME.cpu0 arp_waitstate halted 1000 } ++ rbp $rom_halt_pc ++ ++ cortex_a smp_on ++ } ++} ++ ++targets $_CHIPNAME.cpu0 ++ ++# interface does not work while srst is asserted ++# this is target specific, valid for every board ++reset_config srst_gates_jtag ++ ++if { [info exists CLOCK_FREQ] } { ++ set _CLOCK_FREQ $CLOCK_FREQ ++} else { ++ set _CLOCK_FREQ 5000 ++} ++adapter_khz $_CLOCK_FREQ ++adapter_nsrst_assert_width 200 ++adapter_nsrst_delay 200 ++ ++ ++set clients_num 0 ++proc clients_inc {} { ++ global clients_num ++ ++ incr clients_num ++} ++ ++proc clients_dec_and_shutdown {} { ++ global clients_num ++ ++ incr clients_num -1 ++ if { $clients_num <= 0 } { shutdown } ++} ++ ++$_CHIPNAME.cpu0 configure -event gdb-attach { clients_inc; halt } ++$_CHIPNAME.cpu1 configure -event gdb-attach { clients_inc; halt } ++ ++$_CHIPNAME.cpu2 configure -event gdb-attach { ++ global _CHIPNAME ++ global ENG_MODE ++ ++ clients_inc ++ ++ # Only in ENG_MODE the CM4 is available at reset ++ if { ! [info exists ENG_MODE] } { ++ $_CHIPNAME.cpu2 arp_examine ++ $_CHIPNAME.cpu2 arp_poll ++ } ++ halt ++} ++ ++$_CHIPNAME.cpu0 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } ++$_CHIPNAME.cpu1 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } ++$_CHIPNAME.cpu2 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } ++ ++proc ap0_secure_access {} { ++ global _CHIPNAME ++ ++ $_CHIPNAME.dap apsel 0 ++ $_CHIPNAME.dap apcsw 0x10006000 ++} ++ ++proc ap0_non_secure_access {} { ++ global _CHIPNAME ++ ++ $_CHIPNAME.dap apsel 0 ++ $_CHIPNAME.dap apcsw 0x30006000 ++} ++ ++$_CHIPNAME.cpu0 configure -event examine-end { ap0_secure_access } ++$_CHIPNAME.cpu1 configure -event examine-end { ap0_secure_access } +diff --git a/tcl/target/stm32mp15x_stpmu1.cfg b/tcl/target/stm32mp15x_stpmu1.cfg +new file mode 100644 +index 0000000..413abf2 +--- /dev/null ++++ b/tcl/target/stm32mp15x_stpmu1.cfg +@@ -0,0 +1,113 @@ ++# script for stm32mp15x family with companion PMIC STPMU1 ++ ++source [find target/stm32mp15x.cfg] ++ ++$_CHIPNAME.cpu0 configure -event reset-halt { catch { pmic_init }} ++ ++# Wait for expression to be true with a timeout of 200ms ++proc wait_state {condition} { ++ for {set t 0} {$t < 200} {incr t} { ++ if {[uplevel 1 $condition] == 0} { ++ return ++ } ++ sleep 1 ++ } ++ return -code 1 "Timed out" ++} ++ ++proc mrw_phys {reg} { ++ set value "" ++ mem2array value 32 $reg 1 phys ++ return $value(0) ++} ++ ++proc mmw_phys {reg setbits clearbits} { ++ set old [mrw_phys $reg] ++ set new [expr ($old & ~$clearbits) | $setbits] ++ mww phys $reg $new ++} ++ ++# Set mask_reset bits in PMIC to keep all powers on at next reset event ++# Only the mask_reset of SD-card power is not enabled to guarantee boot from SD ++# This procedure requires Secure memory access ++proc pmic_init {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ set RCC_BASE 0x50000000 ++ set GPIO_BASE 0x54004000 ++ set I2C_BASE 0x5c002000 ++ ++ # i2c @ 100kHz, rise 100ns, fall 100ns ++ # timing depending from HSIDIV[1:0] = {0 1 2 3} means hsi_ker_ck = {64 32 16 8} MHz ++ set I2C_TIMING {0x10b17db5 0x00b07cb4 0x00503d58 0x00201d2b} ++ ++ mww phys [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI) ++ mww phys [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck) ++ ++ mww phys [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1 ++ mww phys [expr $RCC_BASE + 0x188] 4; # assert I2C4RST ++ mww phys [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST ++ ++ mww phys [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ) ++ mmw_phys [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF ++ mmw_phys [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain ++ mmw_phys [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed ++ mmw_phys [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down ++ mmw_phys [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6 ++ ++ set hsidiv [expr [mrw_phys [expr $RCC_BASE + 0x018]] & 0x3] ++ mww phys [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] ++ mww phys [expr $I2C_BASE + 0x000] 1; # set PE = 1 ++ mww phys [expr $I2C_BASE + 0x004] 0x02043066 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x18 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x0f ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x00 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x6f ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF ++ mww phys [expr $I2C_BASE + 0x01c] 0x00000020 ++ mww phys [expr $I2C_BASE + 0x004] 0x01ff14fe ++ ++# mww phys [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4) ++# mww phys [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) ++} ++ ++# Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we ++# are in a debug session. This will force them (at next reboot) to program ++# the PMIC for keeping powered-on the debug unit during reset. ++proc set_pmic_in_backup_reg {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ mmw_phys 0x5c00a150 0x00010000 0x00000000 ++} ++ ++proc clear_pmic_in_backup_reg {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ mmw_phys 0x5c00a150 0x00000000 0x00010000 ++} ++ ++# Wrap around init/shutdown. Attention: CTRL-C will not invoke shutdown ++rename init _init ++proc init {} { ++ _init ++ set_pmic_in_backup_reg ++} ++ ++rename shutdown _shutdown ++proc shutdown {} { ++ clear_pmic_in_backup_reg ++ _shutdown ++} +diff --git a/tcl/target/swj-dp.tcl b/tcl/target/swj-dp.tcl +index 1d274cb..e42a168 100644 +--- a/tcl/target/swj-dp.tcl ++++ b/tcl/target/swj-dp.tcl +@@ -30,5 +30,7 @@ proc swj_newdap {chip tag args} { + eval jtag newtap $chip $tag $args + } elseif [using_swd] { + eval swd newdap $chip $tag $args ++ } elseif { [using_stlink_jtag] || [using_stlink_swd] } { ++ eval stlink_dap newdap $chip $tag $args + } + } +-- +2.7.4 + diff --git a/recipes-devtools/openocd/openocd-stm32mp/0002-Add-support-for-silicon-revB.patch b/recipes-devtools/openocd/openocd-stm32mp/0002-Add-support-for-silicon-revB.patch new file mode 100644 index 0000000..f468d30 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp/0002-Add-support-for-silicon-revB.patch @@ -0,0 +1,458 @@ +From df2861a1384f3bf2ab8236ee51f8a5ff40e8f45b Mon Sep 17 00:00:00 2001 +From: Antonio Borneo +Date: Fri, 12 Oct 2018 11:49:18 +0200 +Subject: [PATCH] Add support for silicon revB +--- + src/jtag/drivers/cmsis_dap_usb.c | 4 +- + src/jtag/drivers/libusb0_common.c | 7 ++ + src/jtag/drivers/libusb1_common.c | 7 ++ + tcl/board/stm32mp15x_dk2.cfg | 2 +- + tcl/board/stm32mp15x_ev1_jlink_jtag.cfg | 2 +- + tcl/board/stm32mp15x_ev1_jlink_swd.cfg | 2 +- + tcl/board/stm32mp15x_ev1_stlink_jtag.cfg | 2 +- + tcl/board/stm32mp15x_ev1_stlink_swd.cfg | 2 +- + tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg | 2 +- + tcl/board/stm32mp15x_ev1_ulink2_swd.cfg | 2 +- + tcl/target/stm32mp15x.cfg | 11 ++- + tcl/target/stm32mp15x_stpmic1.cfg | 113 +++++++++++++++++++++++++++++++ + tcl/target/stm32mp15x_stpmu1.cfg | 113 ------------------------------- + 13 files changed, 145 insertions(+), 124 deletions(-) + +diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c +index 4ee4836..9e723b5 100644 +--- a/src/jtag/drivers/cmsis_dap_usb.c ++++ b/src/jtag/drivers/cmsis_dap_usb.c +@@ -895,9 +895,7 @@ static int cmsis_dap_init(void) + retval = cmsis_dap_swd_open(); + if (retval != ERROR_OK) + return retval; +- } +- +- if (cmsis_dap_handle == NULL) { ++ } else { + /* Connect in JTAG mode */ + if (!(cmsis_dap_handle->caps & INFO_CAPS_JTAG)) { + LOG_ERROR("CMSIS-DAP: JTAG not supported"); +diff --git a/src/jtag/drivers/libusb0_common.c b/src/jtag/drivers/libusb0_common.c +index 1825543..04f88b6 100644 +--- a/src/jtag/drivers/libusb0_common.c ++++ b/src/jtag/drivers/libusb0_common.c +@@ -68,6 +68,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[], + struct jtag_libusb_device_handle **out) + { + int retval = -ENODEV; ++ bool serial_mismatch = false; + struct jtag_libusb_device_handle *libusb_handle; + usb_init(); + +@@ -90,14 +91,20 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[], + /* Device must be open to use libusb_get_string_descriptor_ascii. */ + if (serial != NULL && + !string_descriptor_equal(libusb_handle, dev->descriptor.iSerialNumber, serial)) { ++ serial_mismatch = true; + usb_close(libusb_handle); + continue; + } + *out = libusb_handle; + retval = 0; ++ serial_mismatch = false; + break; + } + } ++ ++ if (serial_mismatch) ++ LOG_DEBUG("No device matches the serial string"); ++ + return retval; + } + +diff --git a/src/jtag/drivers/libusb1_common.c b/src/jtag/drivers/libusb1_common.c +index a1db86f..0ec5fb9 100644 +--- a/src/jtag/drivers/libusb1_common.c ++++ b/src/jtag/drivers/libusb1_common.c +@@ -72,6 +72,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[], + { + int cnt, idx, errCode; + int retval = ERROR_FAIL; ++ bool serial_mismatch = false; + struct jtag_libusb_device_handle *libusb_handle = NULL; + + if (libusb_init(&jtag_libusb_context) < 0) +@@ -99,6 +100,7 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[], + /* Device must be open to use libusb_get_string_descriptor_ascii. */ + if (serial != NULL && + !string_descriptor_equal(libusb_handle, dev_desc.iSerialNumber, serial)) { ++ serial_mismatch = true; + libusb_close(libusb_handle); + continue; + } +@@ -106,10 +108,15 @@ int jtag_libusb_open(const uint16_t vids[], const uint16_t pids[], + /* Success. */ + *out = libusb_handle; + retval = ERROR_OK; ++ serial_mismatch = false; + break; + } + if (cnt >= 0) + libusb_free_device_list(devs, 1); ++ ++ if (serial_mismatch) ++ LOG_DEBUG("No device matches the serial string"); ++ + return retval; + } + +diff --git a/tcl/board/stm32mp15x_dk2.cfg b/tcl/board/stm32mp15x_dk2.cfg +index 82617c5..8d673fa 100644 +--- a/tcl/board/stm32mp15x_dk2.cfg ++++ b/tcl/board/stm32mp15x_dk2.cfg +@@ -4,6 +4,6 @@ source [find interface/stlink-dap.cfg] + + transport select stlink_swd + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg +index 7d74fa6..665dc82 100644 +--- a/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg ++++ b/tcl/board/stm32mp15x_ev1_jlink_jtag.cfg +@@ -4,6 +4,6 @@ source [find interface/jlink.cfg] + + transport select jtag + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_jlink_swd.cfg b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg +index 33e6ef5..7ca27af 100644 +--- a/tcl/board/stm32mp15x_ev1_jlink_swd.cfg ++++ b/tcl/board/stm32mp15x_ev1_jlink_swd.cfg +@@ -4,6 +4,6 @@ source [find interface/jlink.cfg] + + transport select swd + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg +index 6fb1e31..9364c92 100644 +--- a/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg ++++ b/tcl/board/stm32mp15x_ev1_stlink_jtag.cfg +@@ -4,6 +4,6 @@ source [find interface/stlink-dap.cfg] + + transport select stlink_jtag + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_stlink_swd.cfg b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg +index 3c561c9..d8527ee 100644 +--- a/tcl/board/stm32mp15x_ev1_stlink_swd.cfg ++++ b/tcl/board/stm32mp15x_ev1_stlink_swd.cfg +@@ -4,6 +4,6 @@ source [find interface/stlink-dap.cfg] + + transport select stlink_swd + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config srst_only +diff --git a/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg +index 6b10351..c7954ea 100644 +--- a/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg ++++ b/tcl/board/stm32mp15x_ev1_ulink2_jtag.cfg +@@ -4,6 +4,6 @@ source [find interface/cmsis-dap.cfg] + + transport select jtag + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config trst_and_srst separate +diff --git a/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg +index 5495bc3..b00cc61 100644 +--- a/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg ++++ b/tcl/board/stm32mp15x_ev1_ulink2_swd.cfg +@@ -4,6 +4,6 @@ source [find interface/cmsis-dap.cfg] + + transport select swd + +-source [find target/stm32mp15x_stpmu1.cfg] ++source [find target/stm32mp15x_stpmic1.cfg] + + reset_config srst_only +diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg +index 0def780..5d7e230 100644 +--- a/tcl/target/stm32mp15x.cfg ++++ b/tcl/target/stm32mp15x.cfg +@@ -77,7 +77,7 @@ proc delayed_reset_halt_cpu0 { } { + global _CHIPNAME + global arp_reset_mode + +- set rom_halt_pc 0x0000688c ++ set rom_halt_pc 0x000079ac + + # only "reset halt" or "reset init" + if { 0 != [ string compare "$arp_reset_mode" "run" ] } { +@@ -85,6 +85,15 @@ proc delayed_reset_halt_cpu0 { } { + targets $_CHIPNAME.cpu0 + arp_reset_default_handler post $_CHIPNAME.cpu0 + ++ # quick test for cut1.0, check bootrom ++ set v "" ++ ap0_non_secure_access ++ mem2array v 32 0x0000a000 1 phys ++ ap0_secure_access ++ if { $v(0) == 0x1e494610 } { ++ set rom_halt_pc 0x0000688c ++ } ++ + poll on + catch { $_CHIPNAME.cpu0 arp_waitstate halted 1000 } + bp $rom_halt_pc 2 hw +diff --git a/tcl/target/stm32mp15x_stpmic1.cfg b/tcl/target/stm32mp15x_stpmic1.cfg +new file mode 100644 +index 0000000..c080441 +--- /dev/null ++++ b/tcl/target/stm32mp15x_stpmic1.cfg +@@ -0,0 +1,113 @@ ++# script for stm32mp15x family with companion PMIC STPMIC1 ++ ++source [find target/stm32mp15x.cfg] ++ ++$_CHIPNAME.cpu0 configure -event reset-halt { catch { pmic_init }} ++ ++# Wait for expression to be true with a timeout of 200ms ++proc wait_state {condition} { ++ for {set t 0} {$t < 200} {incr t} { ++ if {[uplevel 1 $condition] == 0} { ++ return ++ } ++ sleep 1 ++ } ++ return -code 1 "Timed out" ++} ++ ++proc mrw_phys {reg} { ++ set value "" ++ mem2array value 32 $reg 1 phys ++ return $value(0) ++} ++ ++proc mmw_phys {reg setbits clearbits} { ++ set old [mrw_phys $reg] ++ set new [expr ($old & ~$clearbits) | $setbits] ++ mww phys $reg $new ++} ++ ++# Set mask_reset bits in PMIC to keep all powers on at next reset event ++# Only the mask_reset of SD-card power is not enabled to guarantee boot from SD ++# This procedure requires Secure memory access ++proc pmic_init {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ set RCC_BASE 0x50000000 ++ set GPIO_BASE 0x54004000 ++ set I2C_BASE 0x5c002000 ++ ++ # i2c @ 100kHz, rise 100ns, fall 100ns ++ # timing depending from HSIDIV[1:0] = {0 1 2 3} means hsi_ker_ck = {64 32 16 8} MHz ++ set I2C_TIMING {0x10b17db5 0x00b07cb4 0x00503d58 0x00201d2b} ++ ++ mww phys [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI) ++ mww phys [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck) ++ ++ mww phys [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1 ++ mww phys [expr $RCC_BASE + 0x188] 4; # assert I2C4RST ++ mww phys [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST ++ ++ mww phys [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ) ++ mmw_phys [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF ++ mmw_phys [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain ++ mmw_phys [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed ++ mmw_phys [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down ++ mmw_phys [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6 ++ ++ set hsidiv [expr [mrw_phys [expr $RCC_BASE + 0x018]] & 0x3] ++ mww phys [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] ++ mww phys [expr $I2C_BASE + 0x000] 1; # set PE = 1 ++ mww phys [expr $I2C_BASE + 0x004] 0x02043066 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x18 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x0f ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x00 ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ mww phys [expr $I2C_BASE + 0x028] 0x6f ++ wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF ++ mww phys [expr $I2C_BASE + 0x01c] 0x00000020 ++ mww phys [expr $I2C_BASE + 0x004] 0x01ff14fe ++ ++# mww phys [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4) ++# mww phys [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) ++} ++ ++# Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we ++# are in a debug session. This will force them (at next reboot) to program ++# the PMIC for keeping powered-on the debug unit during reset. ++proc set_pmic_in_backup_reg {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ mmw_phys 0x5c00a150 0x00010000 0x00000000 ++} ++ ++proc clear_pmic_in_backup_reg {} { ++ global _CHIPNAME ++ ++ targets $_CHIPNAME.cpu0 ++ $_CHIPNAME.dap apsel 0 ++ ++ mmw_phys 0x5c00a150 0x00000000 0x00010000 ++} ++ ++# Wrap around init/shutdown. Attention: CTRL-C will not invoke shutdown ++rename init _init ++proc init {} { ++ _init ++ set_pmic_in_backup_reg ++} ++ ++rename shutdown _shutdown ++proc shutdown {} { ++ clear_pmic_in_backup_reg ++ _shutdown ++} +diff --git a/tcl/target/stm32mp15x_stpmu1.cfg b/tcl/target/stm32mp15x_stpmu1.cfg +deleted file mode 100644 +index 413abf2..0000000 +--- a/tcl/target/stm32mp15x_stpmu1.cfg ++++ /dev/null +@@ -1,113 +0,0 @@ +-# script for stm32mp15x family with companion PMIC STPMU1 +- +-source [find target/stm32mp15x.cfg] +- +-$_CHIPNAME.cpu0 configure -event reset-halt { catch { pmic_init }} +- +-# Wait for expression to be true with a timeout of 200ms +-proc wait_state {condition} { +- for {set t 0} {$t < 200} {incr t} { +- if {[uplevel 1 $condition] == 0} { +- return +- } +- sleep 1 +- } +- return -code 1 "Timed out" +-} +- +-proc mrw_phys {reg} { +- set value "" +- mem2array value 32 $reg 1 phys +- return $value(0) +-} +- +-proc mmw_phys {reg setbits clearbits} { +- set old [mrw_phys $reg] +- set new [expr ($old & ~$clearbits) | $setbits] +- mww phys $reg $new +-} +- +-# Set mask_reset bits in PMIC to keep all powers on at next reset event +-# Only the mask_reset of SD-card power is not enabled to guarantee boot from SD +-# This procedure requires Secure memory access +-proc pmic_init {} { +- global _CHIPNAME +- +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- +- set RCC_BASE 0x50000000 +- set GPIO_BASE 0x54004000 +- set I2C_BASE 0x5c002000 +- +- # i2c @ 100kHz, rise 100ns, fall 100ns +- # timing depending from HSIDIV[1:0] = {0 1 2 3} means hsi_ker_ck = {64 32 16 8} MHz +- set I2C_TIMING {0x10b17db5 0x00b07cb4 0x00503d58 0x00201d2b} +- +- mww phys [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI) +- mww phys [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck) +- +- mww phys [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1 +- mww phys [expr $RCC_BASE + 0x188] 4; # assert I2C4RST +- mww phys [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST +- +- mww phys [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ) +- mmw_phys [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF +- mmw_phys [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain +- mmw_phys [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed +- mmw_phys [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down +- mmw_phys [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6 +- +- set hsidiv [expr [mrw_phys [expr $RCC_BASE + 0x018]] & 0x3] +- mww phys [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] +- mww phys [expr $I2C_BASE + 0x000] 1; # set PE = 1 +- mww phys [expr $I2C_BASE + 0x004] 0x02043066 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x18 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x0f +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x00 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x6f +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF +- mww phys [expr $I2C_BASE + 0x01c] 0x00000020 +- mww phys [expr $I2C_BASE + 0x004] 0x01ff14fe +- +-# mww phys [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4) +-# mww phys [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) +-} +- +-# Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we +-# are in a debug session. This will force them (at next reboot) to program +-# the PMIC for keeping powered-on the debug unit during reset. +-proc set_pmic_in_backup_reg {} { +- global _CHIPNAME +- +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- +- mmw_phys 0x5c00a150 0x00010000 0x00000000 +-} +- +-proc clear_pmic_in_backup_reg {} { +- global _CHIPNAME +- +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- +- mmw_phys 0x5c00a150 0x00000000 0x00010000 +-} +- +-# Wrap around init/shutdown. Attention: CTRL-C will not invoke shutdown +-rename init _init +-proc init {} { +- _init +- set_pmic_in_backup_reg +-} +- +-rename shutdown _shutdown +-proc shutdown {} { +- clear_pmic_in_backup_reg +- _shutdown +-} +-- +2.7.4 + diff --git a/recipes-devtools/openocd/openocd-stm32mp/0003-Align-to-community-code-for-cache-coherency-and-rese.patch b/recipes-devtools/openocd/openocd-stm32mp/0003-Align-to-community-code-for-cache-coherency-and-rese.patch new file mode 100644 index 0000000..0629d49 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp/0003-Align-to-community-code-for-cache-coherency-and-rese.patch @@ -0,0 +1,1563 @@ +From 89e86fd65a59f269ae5e0dd1027a427ced86c93e Mon Sep 17 00:00:00 2001 +From: Antonio Borneo +Date: Fri, 9 Nov 2018 11:24:37 +0100 +Subject: [PATCH 3/4] Align to community code for cache coherency and reset + framework + +--- + src/helper/startup.tcl | 9 +- + src/jtag/drivers/stlink_usb.c | 56 +++- + src/server/gdb_server.c | 15 +- + src/target/aarch64.c | 1 + + src/target/arm.h | 2 + + src/target/arm11.c | 1 + + src/target/arm720t.c | 1 + + src/target/arm7tdmi.c | 1 + + src/target/arm920t.c | 1 + + src/target/arm926ejs.c | 1 + + src/target/arm946e.c | 1 + + src/target/arm966e.c | 1 + + src/target/arm9tdmi.c | 1 + + src/target/arm_adi_v5.c | 23 +- + src/target/arm_dpm.c | 2 +- + src/target/arm_dpm.h | 1 + + src/target/armv4_5.c | 14 + + src/target/armv7a.h | 2 - + src/target/armv8.c | 5 + + src/target/cortex_a.c | 458 ++---------------------------- + src/target/cortex_a.h | 3 - + src/target/cortex_m.c | 1 + + src/target/fa526.c | 1 + + src/target/feroceon.c | 2 + + src/target/hla_target.c | 1 + + src/target/startup.tcl | 10 +- + src/target/stm8.c | 6 + + src/target/target.c | 7 + + src/target/target.h | 7 + + src/target/target_type.h | 9 + + src/target/xscale.c | 1 + + tcl/target/stm32mp15x.cfg | 53 ++-- + tcl/target/stm32mp15x_stpmic1.cfg | 97 +++---- + 33 files changed, 265 insertions(+), 529 deletions(-) + +diff --git a/src/helper/startup.tcl b/src/helper/startup.tcl +index 4ca2cabc2..2578de930 100644 +--- a/src/helper/startup.tcl ++++ b/src/helper/startup.tcl +@@ -12,15 +12,18 @@ proc exit {} { + + # All commands are registered with an 'ocd_' prefix, while the "real" + # command is a wrapper that calls this function. Its primary purpose is +-# to discard 'handler' command output, ++# to discard 'handler' command output. ++# Due to the two nested proc calls, this wrapper has to explicitly run ++# the wrapped command in the stack frame two levels above. + proc ocd_bouncer {name args} { + set cmd [format "ocd_%s" $name] + set type [eval ocd_command type $cmd $args] + set errcode error ++ set skiplevel [expr [eval info level] > 1 ? 2 : 1] + if {$type == "native"} { +- return [eval $cmd $args] ++ return [uplevel $skiplevel $cmd $args] + } else {if {$type == "simple"} { +- set errcode [catch {eval $cmd $args}] ++ set errcode [catch {uplevel $skiplevel $cmd $args}] + if {$errcode == 0} { + return "" + } else { +diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c +index f55a608a1..b27d04318 100644 +--- a/src/jtag/drivers/stlink_usb.c ++++ b/src/jtag/drivers/stlink_usb.c +@@ -524,6 +524,7 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size) + return ERROR_OK; + } + ++COMMAND_HANDLER(stlink_dap_arp_init); + /** + Converts an STLINK status code held in the first byte of a response + to an openocd error, logs any error/wait status as debug output. +@@ -628,7 +629,8 @@ static int stlink_usb_error_check(void *handle) + LOG_DEBUG("STLINK_SWD_AP_STICKY_ERROR"); + /* Seen when reading address out of range (0xFFFFFFFF) */ + /* Seems usb_reset helps the following r/w accesses */ +- stlink_usb_reset(h); ++ // stlink_usb_reset(h); ++ stlink_dap_arp_init(NULL); + return ERROR_FAIL; + case STLINK_SWD_AP_STICKYORUN_ERROR: + LOG_DEBUG("STLINK_SWD_AP_STICKYORUN_ERROR"); +@@ -758,6 +760,9 @@ static int stlink_usb_version(void *handle) + int res; + uint16_t v; + uint8_t m_version = 0; ++ uint8_t bridge = 0; ++ char v_str[5 * (1 + 3) + 1]; /* VvJjMmBbSs */ ++ char *p; + struct stlink_usb_handle_s *h = handle; + + assert(handle != NULL); +@@ -775,6 +780,7 @@ static int stlink_usb_version(void *handle) + h->version.swim = h->databuf[1]; + h->version.jtag = h->databuf[2]; + m_version = h->databuf[3]; ++ bridge = h->databuf[4]; + + h->vid = (h->databuf[9] << 8) | h->databuf[8]; + h->pid = (h->databuf[11] << 8) | h->databuf[10]; +@@ -806,13 +812,20 @@ static int stlink_usb_version(void *handle) + h->version.jtag_api_max = STLINK_JTAG_API_V1; + } + +- LOG_INFO("STLINK v%d%s JTAG v%d API v%d %s%d VID 0x%04X PID 0x%04X", +- h->version.stlink, +- (h->pid == STLINK_V2_1_PID) ? ".1" : "", +- h->version.jtag, ++ p = v_str; ++ p += sprintf(p, "V%d", h->version.stlink); ++ if (h->version.jtag || !m_version) ++ p += sprintf(p, "J%d", h->version.jtag); ++ if (m_version) ++ p += sprintf(p, "M%d", m_version); ++ if (bridge) ++ p += sprintf(p, "B%d", bridge); ++ if (h->version.swim || !m_version) ++ p += sprintf(p, "S%d", h->version.swim); ++ ++ LOG_INFO("STLINK %s (API v%d) VID:PID %04X:%04X", ++ v_str, + h->version.jtag_api_max, +- (h->pid == STLINK_V2_1_PID || (h->version.stlink == 3 && h->version.swim == 0)) ? "M" : "SWIM v", +- (h->version.stlink == 3 && h->version.swim == 0) ? m_version : h->version.swim, + h->vid, + h->pid); + +@@ -3329,6 +3342,27 @@ COMMAND_HANDLER(stlink_dap_serial_command) + return ERROR_OK; + } + ++COMMAND_HANDLER(stlink_dap_arp_init) ++{ ++ if (stlink_dap_param.transport == HL_TRANSPORT_JTAG || ++ (stlink_dap_param.transport == HL_TRANSPORT_SWD && stlink_dap_handle->version.stlink == 3)) { ++ //stlink_usb_reset(stlink_dap_handle); ++ stlink_usb_mode_leave(stlink_dap_handle, STLINK_MODE_DEBUG_JTAG); ++ stlink_usb_mode_enter(stlink_dap_handle, stlink_get_mode(stlink_dap_param.transport)); ++ /* exit jtag closes all the opened AP; reopen them! */ ++ for (int apsel = 0; apsel <= DP_APSEL_MAX; apsel++) ++ if (test_bit(apsel, opened_ap)) { ++ clear_bit(apsel, opened_ap); ++ stlink_dap_open_ap(apsel); ++ } ++ /* equivalent to ++ jtag_add_tlr(); ++ jtag_execute_queue(); ++ but let's stay away from jtag code */ ++ } ++ return ERROR_OK; ++} ++ + static const struct command_registration stlink_dap_subcommand_handlers[] = { + { + .name = "serial", +@@ -3337,6 +3371,14 @@ static const struct command_registration stlink_dap_subcommand_handlers[] = { + .help = "set the serial number of the device that should be used", + .usage = "", + }, ++ { ++ .name = "arp_init", ++ .handler = &stlink_dap_arp_init, ++ .mode = COMMAND_ANY, ++ .help = "Validates JTAG scan chain against the list of " ++ "declared TAPs.", ++ .usage = "", ++ }, + COMMAND_REGISTRATION_DONE + }; + +diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c +index fc5f967ef..8284968d3 100644 +--- a/src/server/gdb_server.c ++++ b/src/server/gdb_server.c +@@ -2183,6 +2183,7 @@ static int gdb_generate_target_description(struct target *target, char **tdesc_o + int retval = ERROR_OK; + struct reg **reg_list = NULL; + int reg_list_size; ++ char const *architecture; + char const **features = NULL; + char const **arch_defined_types = NULL; + int feature_list_size = 0; +@@ -2224,6 +2225,12 @@ static int gdb_generate_target_description(struct target *target, char **tdesc_o + "\n" + "\n"); + ++ /* generate architecture element if supported by target */ ++ architecture = target_get_gdb_arch(target); ++ if (architecture != NULL) ++ xml_printf(&retval, &tdesc, &pos, &size, ++ "%s\n", architecture); ++ + /* generate target description according to register list */ + if (features != NULL) { + while (features[current_feature]) { +@@ -2373,6 +2380,8 @@ static int gdb_target_description_supported(struct target *target, int *supporte + char const **features = NULL; + int feature_list_size = 0; + ++ char const *architecture = target_get_gdb_arch(target); ++ + retval = target_get_gdb_reg_list(target, ®_list, + ®_list_size, REG_CLASS_ALL); + if (retval != ERROR_OK) { +@@ -2394,7 +2403,7 @@ static int gdb_target_description_supported(struct target *target, int *supporte + } + + if (supported) { +- if (feature_list_size) ++ if (architecture || feature_list_size) + *supported = 1; + else + *supported = 0; +@@ -2826,7 +2835,7 @@ static bool gdb_handle_vcont_packet(struct connection *connection, const char *p + if (gdb_connection->sync) { + gdb_connection->sync = false; + if (ct->state == TARGET_HALTED) { +- LOG_WARNING("stepi ignored. GDB will now fetch the register state " \ ++ LOG_DEBUG("stepi ignored. GDB will now fetch the register state " \ + "from the target."); + gdb_sig_halted(connection); + log_remove_callback(gdb_log_callback, connection); +@@ -3250,7 +3259,7 @@ static int gdb_input_inner(struct connection *connection) + * make only the single stepping have the sync feature... + */ + nostep = true; +- LOG_WARNING("stepi ignored. GDB will now fetch the register state " \ ++ LOG_DEBUG("stepi ignored. GDB will now fetch the register state " \ + "from the target."); + } + gdb_con->sync = false; +diff --git a/src/target/aarch64.c b/src/target/aarch64.c +index 454de9e92..8fab3b5d7 100644 +--- a/src/target/aarch64.c ++++ b/src/target/aarch64.c +@@ -2824,6 +2824,7 @@ struct target_type aarch64_target = { + .deassert_reset = aarch64_deassert_reset, + + /* REVISIT allow exporting VFP3 registers ... */ ++ .get_gdb_arch = armv8_get_gdb_arch, + .get_gdb_reg_list = armv8_get_gdb_reg_list, + + .read_memory = aarch64_read_memory, +diff --git a/src/target/arm.h b/src/target/arm.h +index 316ff9ab8..ea83d3867 100644 +--- a/src/target/arm.h ++++ b/src/target/arm.h +@@ -263,9 +263,11 @@ struct reg_cache *armv8_build_reg_cache(struct target *target); + extern const struct command_registration arm_command_handlers[]; + + int arm_arch_state(struct target *target); ++const char *arm_get_gdb_arch(struct target *target); + int arm_get_gdb_reg_list(struct target *target, + struct reg **reg_list[], int *reg_list_size, + enum target_register_class reg_class); ++const char *armv8_get_gdb_arch(struct target *target); + int armv8_get_gdb_reg_list(struct target *target, + struct reg **reg_list[], int *reg_list_size, + enum target_register_class reg_class); +diff --git a/src/target/arm11.c b/src/target/arm11.c +index 13fbd207a..443866149 100644 +--- a/src/target/arm11.c ++++ b/src/target/arm11.c +@@ -1362,6 +1362,7 @@ struct target_type arm11_target = { + .assert_reset = arm11_assert_reset, + .deassert_reset = arm11_deassert_reset, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm11_read_memory, +diff --git a/src/target/arm720t.c b/src/target/arm720t.c +index bcbfa9dff..3d12aba22 100644 +--- a/src/target/arm720t.c ++++ b/src/target/arm720t.c +@@ -560,6 +560,7 @@ struct target_type arm720t_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm720t_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm720t_read_memory, +diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c +index 9dcb302d9..e1e91c3a0 100644 +--- a/src/target/arm7tdmi.c ++++ b/src/target/arm7tdmi.c +@@ -699,6 +699,7 @@ struct target_type arm7tdmi_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm7_9_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +diff --git a/src/target/arm920t.c b/src/target/arm920t.c +index 7927a2bea..39d75902c 100644 +--- a/src/target/arm920t.c ++++ b/src/target/arm920t.c +@@ -1693,6 +1693,7 @@ struct target_type arm920t_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm920t_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm920t_read_memory, +diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c +index 58de77850..07c519a53 100644 +--- a/src/target/arm926ejs.c ++++ b/src/target/arm926ejs.c +@@ -804,6 +804,7 @@ struct target_type arm926ejs_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm926ejs_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +diff --git a/src/target/arm946e.c b/src/target/arm946e.c +index 06c9fc30d..5e25d71ec 100644 +--- a/src/target/arm946e.c ++++ b/src/target/arm946e.c +@@ -756,6 +756,7 @@ struct target_type arm946e_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm7_9_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + /* .read_memory = arm7_9_read_memory, */ +diff --git a/src/target/arm966e.c b/src/target/arm966e.c +index 0429c54b5..c9d7f01aa 100644 +--- a/src/target/arm966e.c ++++ b/src/target/arm966e.c +@@ -259,6 +259,7 @@ struct target_type arm966e_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm7_9_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c +index 82b430f88..6425027f4 100644 +--- a/src/target/arm9tdmi.c ++++ b/src/target/arm9tdmi.c +@@ -902,6 +902,7 @@ struct target_type arm9tdmi_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm7_9_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c +index 51d228b8a..7d5353a32 100644 +--- a/src/target/arm_adi_v5.c ++++ b/src/target/arm_adi_v5.c +@@ -680,6 +680,15 @@ int dap_dp_init(struct adiv5_dap *dap) + + dap_invalidate_cache(dap); + ++ /* ++ * Early initialize dap->dp_ctrl_stat. ++ * In jtag mode only, if the following atomic reads fail and set the ++ * sticky error, it will trigger the clearing of the sticky. Without this ++ * initialization system and debug power would be disabled while clearing ++ * the sticky error bit. ++ */ ++ dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ; ++ + for (size_t i = 0; i < 30; i++) { + /* DP initialization */ + +@@ -688,7 +697,18 @@ int dap_dp_init(struct adiv5_dap *dap) + break; + } + +- retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR); ++ /* ++ * This write operation clears the sticky error bit in jtag mode only and ++ * is ignored in swd mode. It also powers-up system and debug domains in ++ * both jtag and swd modes, if not done before. ++ * Actually we do not need to clear the sticky error here because it has ++ * been already cleared (if it was set) in the previous atomic read. This ++ * write could be removed, but this initial part of dap_dp_init() is the ++ * result of years of fine tuning and there are strong concerns about any ++ * unnecessary code change. It doesn't harm, so let's keep it here and ++ * preserve the historical sequence of read/write operations! ++ */ ++ retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat | SSTICKYERR); + if (retval != ERROR_OK) + return retval; + +@@ -696,7 +716,6 @@ int dap_dp_init(struct adiv5_dap *dap) + if (retval != ERROR_OK) + return retval; + +- dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ; + retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat); + if (retval != ERROR_OK) + return retval; +diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c +index f9b30c187..89c487ae3 100644 +--- a/src/target/arm_dpm.c ++++ b/src/target/arm_dpm.c +@@ -168,7 +168,7 @@ static int dpm_read_reg_u64(struct arm_dpm *dpm, struct reg *r, unsigned regnum) + } + + /* just read the register -- rely on the core mode being right */ +-static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum) ++int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum) + { + uint32_t value; + int retval; +diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h +index ac70ce3e1..0901611d2 100644 +--- a/src/target/arm_dpm.h ++++ b/src/target/arm_dpm.h +@@ -152,6 +152,7 @@ struct arm_dpm { + int arm_dpm_setup(struct arm_dpm *dpm); + int arm_dpm_initialize(struct arm_dpm *dpm); + ++int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum); + int arm_dpm_read_current_registers(struct arm_dpm *); + int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode); + +diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c +index 96a63e497..af8fd9a7f 100644 +--- a/src/target/armv4_5.c ++++ b/src/target/armv4_5.c +@@ -1179,6 +1179,20 @@ const struct command_registration arm_command_handlers[] = { + COMMAND_REGISTRATION_DONE + }; + ++/* ++ * gdb for arm targets (e.g. arm-none-eabi-gdb) supports several variants ++ * of arm architecture. You can list them using the autocompletion of gdb ++ * command prompt by typing "set architecture " and then press TAB key. ++ * The default, selected automatically, is "arm". ++ * Let's use the default value, here, to make gdb-multiarch behave in the ++ * same way as a gdb for arm. This can be changed later on. User can still ++ * set the specific architecture variant with the gdb command. ++ */ ++const char *arm_get_gdb_arch(struct target *target) ++{ ++ return "arm"; ++} ++ + int arm_get_gdb_reg_list(struct target *target, + struct reg **reg_list[], int *reg_list_size, + enum target_register_class reg_class) +diff --git a/src/target/armv7a.h b/src/target/armv7a.h +index 1a83522b6..46cf49799 100644 +--- a/src/target/armv7a.h ++++ b/src/target/armv7a.h +@@ -106,8 +106,6 @@ struct armv7a_common { + struct arm_dpm dpm; + uint32_t debug_base; + struct adiv5_ap *debug_ap; +- struct adiv5_ap *memory_ap; +- bool memory_ap_available; + /* mdir */ + uint8_t multi_processor_system; + uint8_t cluster_id; +diff --git a/src/target/armv8.c b/src/target/armv8.c +index 75ada896d..39ce7e129 100644 +--- a/src/target/armv8.c ++++ b/src/target/armv8.c +@@ -1668,6 +1668,11 @@ const struct command_registration armv8_command_handlers[] = { + COMMAND_REGISTRATION_DONE + }; + ++const char *armv8_get_gdb_arch(struct target *target) ++{ ++ return "aarch64"; ++} ++ + int armv8_get_gdb_reg_list(struct target *target, + struct reg **reg_list[], int *reg_list_size, + enum target_register_class reg_class) +diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c +index f17edddb2..b281233ca 100644 +--- a/src/target/cortex_a.c ++++ b/src/target/cortex_a.c +@@ -71,10 +71,6 @@ static int cortex_a_set_hybrid_breakpoint(struct target *target, + struct breakpoint *breakpoint); + static int cortex_a_unset_breakpoint(struct target *target, + struct breakpoint *breakpoint); +-static int cortex_a_dap_read_coreregister_u32(struct target *target, +- uint32_t *value, int regnum); +-static int cortex_a_dap_write_coreregister_u32(struct target *target, +- uint32_t value, int regnum); + static int cortex_a_mmu(struct target *target, int *enabled); + static int cortex_a_mmu_modify(struct target *target, int enable); + static int cortex_a_virt2phys(struct target *target, +@@ -307,172 +303,6 @@ static int cortex_a_exec_opcode(struct target *target, + return retval; + } + +-/************************************************************************** +-Read core register with very few exec_opcode, fast but needs work_area. +-This can cause problems with MMU active. +-**************************************************************************/ +-static int cortex_a_read_regs_through_mem(struct target *target, uint32_t address, +- uint32_t *regfile) +-{ +- int retval = ERROR_OK; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- +- retval = cortex_a_dap_read_coreregister_u32(target, regfile, 0); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_dap_write_coreregister_u32(target, address, 0); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_exec_opcode(target, ARMV4_5_STMIA(0, 0xFFFE, 0, 0), NULL); +- if (retval != ERROR_OK) +- return retval; +- +- retval = mem_ap_read_buf(armv7a->memory_ap, +- (uint8_t *)(®file[1]), 4, 15, address); +- +- return retval; +-} +- +-static int cortex_a_dap_read_coreregister_u32(struct target *target, +- uint32_t *value, int regnum) +-{ +- int retval = ERROR_OK; +- uint8_t reg = regnum&0xFF; +- uint32_t dscr = 0; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- +- if (reg > 17) +- return retval; +- +- if (reg < 15) { +- /* Rn to DCCTX, "MCR p14, 0, Rn, c0, c5, 0" 0xEE00nE15 */ +- retval = cortex_a_exec_opcode(target, +- ARMV4_5_MCR(14, 0, reg, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- } else if (reg == 15) { +- /* "MOV r0, r15"; then move r0 to DCCTX */ +- retval = cortex_a_exec_opcode(target, 0xE1A0000F, &dscr); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_exec_opcode(target, +- ARMV4_5_MCR(14, 0, 0, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- } else { +- /* "MRS r0, CPSR" or "MRS r0, SPSR" +- * then move r0 to DCCTX +- */ +- retval = cortex_a_exec_opcode(target, ARMV4_5_MRS(0, reg & 1), &dscr); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_exec_opcode(target, +- ARMV4_5_MCR(14, 0, 0, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- } +- +- /* Wait for DTRRXfull then read DTRRTX */ +- int64_t then = timeval_ms(); +- while ((dscr & DSCR_DTR_TX_FULL) == 0) { +- retval = mem_ap_read_atomic_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DSCR, &dscr); +- if (retval != ERROR_OK) +- return retval; +- if (timeval_ms() > then + 1000) { +- LOG_ERROR("Timeout waiting for cortex_a_exec_opcode"); +- return ERROR_FAIL; +- } +- } +- +- retval = mem_ap_read_atomic_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DTRTX, value); +- LOG_DEBUG("read DCC 0x%08" PRIx32, *value); +- +- return retval; +-} +- +-static int cortex_a_dap_write_coreregister_u32(struct target *target, +- uint32_t value, int regnum) +-{ +- int retval = ERROR_OK; +- uint8_t Rd = regnum&0xFF; +- uint32_t dscr; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- +- LOG_DEBUG("register %i, value 0x%08" PRIx32, regnum, value); +- +- /* Check that DCCRX is not full */ +- retval = mem_ap_read_atomic_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DSCR, &dscr); +- if (retval != ERROR_OK) +- return retval; +- if (dscr & DSCR_DTR_RX_FULL) { +- LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr); +- /* Clear DCCRX with MRC(p14, 0, Rd, c0, c5, 0), opcode 0xEE100E15 */ +- retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- } +- +- if (Rd > 17) +- return retval; +- +- /* Write DTRRX ... sets DSCR.DTRRXfull but exec_opcode() won't care */ +- LOG_DEBUG("write DCC 0x%08" PRIx32, value); +- retval = mem_ap_write_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DTRRX, value); +- if (retval != ERROR_OK) +- return retval; +- +- if (Rd < 15) { +- /* DCCRX to Rn, "MRC p14, 0, Rn, c0, c5, 0", 0xEE10nE15 */ +- retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, Rd, 0, 5, 0), +- &dscr); +- +- if (retval != ERROR_OK) +- return retval; +- } else if (Rd == 15) { +- /* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15 +- * then "mov r15, r0" +- */ +- retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_exec_opcode(target, 0xE1A0F000, &dscr); +- if (retval != ERROR_OK) +- return retval; +- } else { +- /* DCCRX to R0, "MRC p14, 0, R0, c0, c5, 0", 0xEE100E15 +- * then "MSR CPSR_cxsf, r0" or "MSR SPSR_cxsf, r0" (all fields) +- */ +- retval = cortex_a_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- retval = cortex_a_exec_opcode(target, ARMV4_5_MSR_GP(0, 0xF, Rd & 1), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- +- /* "Prefetch flush" after modifying execution status in CPSR */ +- if (Rd == 16) { +- retval = cortex_a_exec_opcode(target, +- ARMV4_5_MCR(15, 0, 0, 7, 5, 4), +- &dscr); +- if (retval != ERROR_OK) +- return retval; +- } +- } +- +- return retval; +-} +- + /* Write to memory mapped registers directly with no cache or mmu handling */ + static int cortex_a_dap_write_memap_register_u32(struct target *target, + uint32_t address, +@@ -1183,14 +1013,11 @@ static int cortex_a_resume(struct target *target, int current, + + static int cortex_a_debug_entry(struct target *target) + { +- int i; +- uint32_t regfile[16], cpsr, spsr, dscr; ++ uint32_t dscr; + int retval = ERROR_OK; +- struct working_area *regfile_working_area = NULL; + struct cortex_a_common *cortex_a = target_to_cortex_a(target); + struct armv7a_common *armv7a = target_to_armv7a(target); + struct arm *arm = &armv7a->arm; +- struct reg *reg; + + LOG_DEBUG("dscr = 0x%08" PRIx32, cortex_a->cpudbg_dscr); + +@@ -1227,68 +1054,16 @@ static int cortex_a_debug_entry(struct target *target) + arm_dpm_report_wfar(&armv7a->dpm, wfar); + } + +- /* REVISIT fast_reg_read is never set ... */ +- +- /* Examine target state and mode */ +- if (cortex_a->fast_reg_read) +- target_alloc_working_area(target, 64, ®file_working_area); +- +- +- /* First load register acessible through core debug port*/ +- if (!regfile_working_area) +- retval = arm_dpm_read_current_registers(&armv7a->dpm); +- else { +- retval = cortex_a_read_regs_through_mem(target, +- regfile_working_area->address, regfile); +- +- target_free_working_area(target, regfile_working_area); +- if (retval != ERROR_OK) +- return retval; +- +- /* read Current PSR */ +- retval = cortex_a_dap_read_coreregister_u32(target, &cpsr, 16); +- /* store current cpsr */ +- if (retval != ERROR_OK) +- return retval; +- +- LOG_DEBUG("cpsr: %8.8" PRIx32, cpsr); +- +- arm_set_cpsr(arm, cpsr); +- +- /* update cache */ +- for (i = 0; i <= ARM_PC; i++) { +- reg = arm_reg_current(arm, i); +- +- buf_set_u32(reg->value, 0, 32, regfile[i]); +- reg->valid = 1; +- reg->dirty = 0; +- } +- +- /* Fixup PC Resume Address */ +- if (cpsr & (1 << 5)) { +- /* T bit set for Thumb or ThumbEE state */ +- regfile[ARM_PC] -= 4; +- } else { +- /* ARM state */ +- regfile[ARM_PC] -= 8; +- } +- +- reg = arm->pc; +- buf_set_u32(reg->value, 0, 32, regfile[ARM_PC]); +- reg->dirty = reg->valid; +- } ++ /* First load register accessible through core debug port */ ++ retval = arm_dpm_read_current_registers(&armv7a->dpm); ++ if (retval != ERROR_OK) ++ return retval; + + if (arm->spsr) { +- /* read Saved PSR */ +- retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17); +- /* store current spsr */ ++ /* read SPSR */ ++ retval = dpm_read_reg(&armv7a->dpm, arm->spsr, 17); + if (retval != ERROR_OK) + return retval; +- +- reg = arm->spsr; +- buf_set_u32(reg->value, 0, 32, spsr); +- reg->valid = 1; +- reg->dirty = 0; + } + + #if 0 +@@ -2784,9 +2559,6 @@ static int cortex_a_read_phys_memory(struct target *target, + target_addr_t address, uint32_t size, + uint32_t count, uint8_t *buffer) + { +- struct armv7a_common *armv7a = target_to_armv7a(target); +- struct adiv5_dap *swjdp = armv7a->arm.dap; +- uint8_t apsel = swjdp->apsel; + int retval; + + if (!count || !buffer) +@@ -2795,9 +2567,6 @@ static int cortex_a_read_phys_memory(struct target *target, + LOG_DEBUG("Reading memory at real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, + address, size, count); + +- if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num)) +- return mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address); +- + /* read memory through the CPU */ + cortex_a_prep_memaccess(target, 1); + retval = cortex_a_read_cpu_memory(target, address, size, count, buffer); +@@ -2822,79 +2591,10 @@ static int cortex_a_read_memory(struct target *target, target_addr_t address, + return retval; + } + +-static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address, +- uint32_t size, uint32_t count, uint8_t *buffer) +-{ +- int mmu_enabled = 0; +- target_addr_t virt, phys; +- int retval; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- struct adiv5_dap *swjdp = armv7a->arm.dap; +- uint8_t apsel = swjdp->apsel; +- +- if (!armv7a->memory_ap_available || (apsel != armv7a->memory_ap->ap_num)) +- return target_read_memory(target, address, size, count, buffer); +- +- /* cortex_a handles unaligned memory access */ +- LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, +- address, size, count); +- +- /* flush the data cache */ +- if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) +- armv7a_l1_d_cache_clean_virt(target, address, size * count); +- +- /* determine if MMU was enabled on target stop */ +- if (!armv7a->is_armv7r) { +- retval = cortex_a_mmu(target, &mmu_enabled); +- if (retval != ERROR_OK) +- return retval; +- } +- +- if (!count || !buffer) +- return ERROR_COMMAND_SYNTAX_ERROR; +- +- if (!mmu_enabled) +- return mem_ap_read_buf(armv7a->memory_ap, buffer, size, count, address); +- +- /* +- * TODO: here we are pessimistic and we use the smaller page of 4kB, but +- * cortex_a_virt2phys() does a full table walk, so can return the actual +- * size of page (4kB or 64kB) or section (1MB or 16Mb). +- */ +- while (count) { +- target_addr_t page_size = 0x1000; +- uint32_t current_count; +- +- virt = address; +- retval = cortex_a_virt2phys(target, virt, &phys); +- if (retval != ERROR_OK) +- return retval; +- +- current_count = (page_size - (address & (page_size - 1))) / size; +- if (current_count > count) +- current_count = count; +- +- LOG_DEBUG("Reading at virtual address 0x%" PRIx32 " bytes. " +- "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, +- size * current_count, virt, phys); +- +- retval = mem_ap_read_buf(armv7a->memory_ap, buffer, size, current_count, phys); +- if (retval != ERROR_OK) +- return retval; +- count -= current_count; +- address += size * current_count; +- buffer += size * current_count; +- } +- return ERROR_OK; +-} +- + static int cortex_a_write_phys_memory(struct target *target, + target_addr_t address, uint32_t size, + uint32_t count, const uint8_t *buffer) + { +- struct armv7a_common *armv7a = target_to_armv7a(target); +- struct adiv5_dap *swjdp = armv7a->arm.dap; +- uint8_t apsel = swjdp->apsel; + int retval; + + if (!count || !buffer) +@@ -2903,9 +2603,6 @@ static int cortex_a_write_phys_memory(struct target *target, + LOG_DEBUG("Writing memory to real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, + address, size, count); + +- if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num)) +- return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address); +- + /* write memory through the CPU */ + cortex_a_prep_memaccess(target, 1); + retval = cortex_a_write_cpu_memory(target, address, size, count, buffer); +@@ -2932,74 +2629,6 @@ static int cortex_a_write_memory(struct target *target, target_addr_t address, + return retval; + } + +-static int cortex_a_write_memory_ahb(struct target *target, target_addr_t address, +- uint32_t size, uint32_t count, const uint8_t *buffer) +-{ +- int mmu_enabled = 0; +- target_addr_t virt, phys; +- int retval; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- struct adiv5_dap *swjdp = armv7a->arm.dap; +- uint8_t apsel = swjdp->apsel; +- +- if (!armv7a->memory_ap_available || (apsel != armv7a->memory_ap->ap_num)) +- return target_write_memory(target, address, size, count, buffer); +- +- /* cortex_a handles unaligned memory access */ +- LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32, +- address, size, count); +- +- /* flush and invalidate the data cache */ +- if (armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) +- armv7a_cache_flush_virt(target, address, size * count); +- /* invalidate instruction cache */ +- armv7a_l1_i_cache_inval_virt(target, address, size * count); +- +- /* determine if MMU was enabled on target stop */ +- if (!armv7a->is_armv7r) { +- retval = cortex_a_mmu(target, &mmu_enabled); +- if (retval != ERROR_OK) +- return retval; +- } +- +- if (!count || !buffer) +- return ERROR_COMMAND_SYNTAX_ERROR; +- +- if (!mmu_enabled) +- return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address); +- +- /* +- * TODO: here we are pessimistic and we use the smaller page of 4kB, but +- * cortex_a_virt2phys() does a full table walk, so can return the actual +- * size of page (4kB or 64kB) or section (1MB or 16Mb). +- */ +- while (count) { +- target_addr_t page_size = 0x1000; +- uint32_t current_count; +- +- virt = address; +- retval = cortex_a_virt2phys(target, virt, &phys); +- if (retval != ERROR_OK) +- return retval; +- +- current_count = (page_size - (address & (page_size - 1))) / size; +- if (current_count > count) +- current_count = count; +- +- LOG_DEBUG("Writing to virtual address 0x%" PRIx32 " bytes. " +- "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT, +- size * current_count, virt, phys); +- +- retval = mem_ap_write_buf(armv7a->memory_ap, buffer, size, current_count, phys); +- if (retval != ERROR_OK) +- return retval; +- count -= current_count; +- address += size * current_count; +- buffer += size * current_count; +- } +- return ERROR_OK; +-} +- + static int cortex_a_read_buffer(struct target *target, target_addr_t address, + uint32_t count, uint8_t *buffer) + { +@@ -3009,7 +2638,7 @@ static int cortex_a_read_buffer(struct target *target, target_addr_t address, + * will have something to do with the size we leave to it. */ + for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) { + if (address & size) { +- int retval = cortex_a_read_memory_ahb(target, address, size, 1, buffer); ++ int retval = target_read_memory(target, address, size, 1, buffer); + if (retval != ERROR_OK) + return retval; + address += size; +@@ -3022,7 +2651,7 @@ static int cortex_a_read_buffer(struct target *target, target_addr_t address, + for (; size > 0; size /= 2) { + uint32_t aligned = count - count % size; + if (aligned > 0) { +- int retval = cortex_a_read_memory_ahb(target, address, size, aligned / size, buffer); ++ int retval = target_read_memory(target, address, size, aligned / size, buffer); + if (retval != ERROR_OK) + return retval; + address += aligned; +@@ -3043,7 +2672,7 @@ static int cortex_a_write_buffer(struct target *target, target_addr_t address, + * will have something to do with the size we leave to it. */ + for (size = 1; size < 4 && count >= size * 2 + (address & size); size *= 2) { + if (address & size) { +- int retval = cortex_a_write_memory_ahb(target, address, size, 1, buffer); ++ int retval = target_write_memory(target, address, size, 1, buffer); + if (retval != ERROR_OK) + return retval; + address += size; +@@ -3056,7 +2685,7 @@ static int cortex_a_write_buffer(struct target *target, target_addr_t address, + for (; size > 0; size /= 2) { + uint32_t aligned = count - count % size; + if (aligned > 0) { +- int retval = cortex_a_write_memory_ahb(target, address, size, aligned / size, buffer); ++ int retval = target_write_memory(target, address, size, aligned / size, buffer); + if (retval != ERROR_OK) + return retval; + address += aligned; +@@ -3117,21 +2746,7 @@ static int cortex_a_examine_first(struct target *target) + + int i; + int retval = ERROR_OK; +- uint32_t didr, cpuid, dbg_osreg, dp_ctrl_stat; +- +- /* +- * Some device turn off CSYSPWRUPACK or CDBGPWRUPACK at reset. +- * Check them and eventually re init the DP. +- */ +- retval = dap_queue_dp_read(swjdp, DP_CTRL_STAT, &dp_ctrl_stat); +- if (retval != ERROR_OK +- || ((dp_ctrl_stat & CDBGPWRUPACK) == 0) +- || (!swjdp->ignore_syspwrupack && ((dp_ctrl_stat & CSYSPWRUPACK) == 0))) { +- LOG_INFO("DP disabled after reset. Re-initialize it!"); +- retval = dap_dp_init(swjdp); +- if (retval != ERROR_OK) +- LOG_ERROR("DP initialization failed"); +- } ++ uint32_t didr, cpuid, dbg_osreg; + + /* Search for the APB-AP - it is needed for access to debug registers */ + retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv7a->debug_ap); +@@ -3148,25 +2763,6 @@ static int cortex_a_examine_first(struct target *target) + + armv7a->debug_ap->memaccess_tck = 80; + +- /* Search for the AXI-AP or the AHB-AP. +- * REVISIT: We should make sure the AP's MEMTYPE says it +- * can access system memory. */ +- armv7a->memory_ap_available = false; +- retval = dap_find_ap(swjdp, AP_TYPE_AXI_AP, &armv7a->memory_ap); +- if (retval == ERROR_OK) +- retval = mem_ap_init(armv7a->memory_ap); +- if (retval != ERROR_OK) { +- retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7a->memory_ap); +- if (retval == ERROR_OK) +- retval = mem_ap_init(armv7a->memory_ap); +- } +- if (retval != ERROR_OK) { +- /* AHB-AP and AXI-AP not found or unavailable - use the CPU */ +- LOG_DEBUG("Nor AHB-AP nor AXI-AP available for memory access"); +- } else { +- armv7a->memory_ap_available = true; +- } +- + if (!target->dbgbase_set) { + uint32_t dbgbase; + /* Get ROM Table base */ +@@ -3326,8 +2922,6 @@ static int cortex_a_init_arch_info(struct target *target, + cortex_a->common_magic = CORTEX_A_COMMON_MAGIC; + armv7a->arm.dap = dap; + +- cortex_a->fast_reg_read = 0; +- + /* register arch-specific functions */ + armv7a->examine_debug_reason = NULL; + +@@ -3412,10 +3006,7 @@ static int cortex_a_mmu(struct target *target, int *enabled) + static int cortex_a_virt2phys(struct target *target, + target_addr_t virt, target_addr_t *phys) + { +- int retval = ERROR_FAIL; +- struct armv7a_common *armv7a = target_to_armv7a(target); +- struct adiv5_dap *swjdp = armv7a->arm.dap; +- uint8_t apsel = swjdp->apsel; ++ int retval; + int mmu_enabled = 0; + + /* +@@ -3430,23 +3021,12 @@ static int cortex_a_virt2phys(struct target *target, + return ERROR_OK; + } + +- if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num)) { +- uint32_t ret; +- retval = armv7a_mmu_translate_va(target, +- virt, &ret); +- if (retval != ERROR_OK) +- goto done; +- *phys = ret; +- } else {/* use this method if armv7a->memory_ap not selected +- * mmu must be enable in order to get a correct translation */ +- retval = cortex_a_mmu_modify(target, 1); +- if (retval != ERROR_OK) +- goto done; +- retval = armv7a_mmu_translate_va_pa(target, (uint32_t)virt, ++ /* mmu must be enable in order to get a correct translation */ ++ retval = cortex_a_mmu_modify(target, 1); ++ if (retval != ERROR_OK) ++ return retval; ++ return armv7a_mmu_translate_va_pa(target, (uint32_t)virt, + (uint32_t *)phys, 1); +- } +-done: +- return retval; + } + + COMMAND_HANDLER(cortex_a_handle_cache_info_command) +@@ -3666,6 +3246,7 @@ struct target_type cortexa_target = { + .deassert_reset = cortex_a_post_deassert_reset, + + /* REVISIT allow exporting VFP3 registers ... */ ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = cortex_a_read_memory, +@@ -3746,6 +3327,7 @@ struct target_type cortexr4_target = { + .deassert_reset = cortex_a_post_deassert_reset, + + /* REVISIT allow exporting VFP3 registers ... */ ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = cortex_a_read_phys_memory, +diff --git a/src/target/cortex_a.h b/src/target/cortex_a.h +index ff0343208..197a5992d 100644 +--- a/src/target/cortex_a.h ++++ b/src/target/cortex_a.h +@@ -93,9 +93,6 @@ struct cortex_a_common { + int brp_num_available; + struct cortex_a_brp *brp_list; + +- /* Use cortex_a_read_regs_through_mem for fast register reads */ +- int fast_reg_read; +- + uint32_t cpuid; + uint32_t didr; + +diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c +index 42eb4b299..79975aa89 100644 +--- a/src/target/cortex_m.c ++++ b/src/target/cortex_m.c +@@ -2495,6 +2495,7 @@ struct target_type cortexm_target = { + /* .deassert_reset = cortex_m_post_deassert_reset,*/ + .soft_reset_halt = cortex_m_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = armv7m_get_gdb_reg_list, + + .read_memory = cortex_m_read_memory, +diff --git a/src/target/fa526.c b/src/target/fa526.c +index 9f6b80551..bb9f7353a 100644 +--- a/src/target/fa526.c ++++ b/src/target/fa526.c +@@ -364,6 +364,7 @@ struct target_type fa526_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm920t_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm920t_read_memory, +diff --git a/src/target/feroceon.c b/src/target/feroceon.c +index 6b14ab6a8..21963e55e 100644 +--- a/src/target/feroceon.c ++++ b/src/target/feroceon.c +@@ -710,6 +710,7 @@ struct target_type feroceon_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm926ejs_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +@@ -747,6 +748,7 @@ struct target_type dragonite_target = { + .deassert_reset = arm7_9_deassert_reset, + .soft_reset_halt = arm7_9_soft_reset_halt, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = arm7_9_read_memory, +diff --git a/src/target/hla_target.c b/src/target/hla_target.c +index 9ebf24168..567a61dc3 100644 +--- a/src/target/hla_target.c ++++ b/src/target/hla_target.c +@@ -820,6 +820,7 @@ struct target_type hla_target = { + .resume = adapter_resume, + .step = adapter_step, + ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = armv7m_get_gdb_reg_list, + + .read_memory = adapter_read_memory, +diff --git a/src/target/startup.tcl b/src/target/startup.tcl +index 10ee27c2d..416c20871 100644 +--- a/src/target/startup.tcl ++++ b/src/target/startup.tcl +@@ -231,6 +231,9 @@ proc ocd_process_reset_inner { MODE } { + # Use TRST or TMS/TCK operations to reset all the tap controllers. + # TAP reset events get reported; they might enable some taps. + init_reset $MODE ++ ++ # after resetting the JTAG chain, re-initialize all existing DAPs ++ dap init + } + + foreach t $targets { +@@ -250,9 +253,10 @@ proc ocd_process_reset_inner { MODE } { + + # Deassert SRST + reset_deassert_initial $MODE +- if { !$early_reset_init } { +- if [using_jtag] { jtag arp_init } +- } ++ if [using_stlink_jtag] { st-link arp_init } ++ if [using_stlink_swd] { st-link arp_init } ++ if [using_jtag] { jtag arp_init } ++ dap init + + foreach t $targets { + $t invoke-event reset-deassert-post +diff --git a/src/target/stm8.c b/src/target/stm8.c +index 5a3438a64..9a57e4490 100644 +--- a/src/target/stm8.c ++++ b/src/target/stm8.c +@@ -1189,6 +1189,11 @@ static int stm8_write_core_reg(struct target *target, unsigned int num) + return ERROR_OK; + } + ++static const char *stm8_get_gdb_arch(struct target *target) ++{ ++ return "stm8"; ++} ++ + static int stm8_get_gdb_reg_list(struct target *target, struct reg **reg_list[], + int *reg_list_size, enum target_register_class reg_class) + { +@@ -2199,6 +2204,7 @@ struct target_type stm8_target = { + .assert_reset = stm8_reset_assert, + .deassert_reset = stm8_reset_deassert, + ++ .get_gdb_arch = stm8_get_gdb_arch, + .get_gdb_reg_list = stm8_get_gdb_reg_list, + + .read_memory = stm8_read_memory, +diff --git a/src/target/target.c b/src/target/target.c +index 906f1be9d..4a20e0edb 100644 +--- a/src/target/target.c ++++ b/src/target/target.c +@@ -1255,6 +1255,13 @@ int target_hit_watchpoint(struct target *target, + return target->type->hit_watchpoint(target, hit_watchpoint); + } + ++const char *target_get_gdb_arch(struct target *target) ++{ ++ if (target->type->get_gdb_arch == NULL) ++ return NULL; ++ return target->type->get_gdb_arch(target); ++} ++ + int target_get_gdb_reg_list(struct target *target, + struct reg **reg_list[], int *reg_list_size, + enum target_register_class reg_class) +diff --git a/src/target/target.h b/src/target/target.h +index b24abc0f1..2532bfbbf 100644 +--- a/src/target/target.h ++++ b/src/target/target.h +@@ -492,6 +492,13 @@ int target_remove_watchpoint(struct target *target, + int target_hit_watchpoint(struct target *target, + struct watchpoint **watchpoint); + ++/** ++ * Obtain the architecture for GDB. ++ * ++ * This routine is a wrapper for target->type->get_gdb_arch. ++ */ ++const char *target_get_gdb_arch(struct target *target); ++ + /** + * Obtain the registers for GDB. + * +diff --git a/src/target/target_type.h b/src/target/target_type.h +index 24b29ea2f..e041bbd7f 100644 +--- a/src/target/target_type.h ++++ b/src/target/target_type.h +@@ -104,6 +104,15 @@ struct target_type { + */ + int (*reset_clear_internal_state)(struct target *target); + ++ /** ++ * Target architecture for GDB. ++ * ++ * The string returned by this function will not be automatically freed; ++ * if dynamic allocation is used for this value, it must be managed by ++ * the target, ideally by caching the result for subsequent calls. ++ */ ++ const char *(*get_gdb_arch)(struct target *target); ++ + /** + * Target register access for GDB. Do @b not call this function + * directly, use target_get_gdb_reg_list() instead. +diff --git a/src/target/xscale.c b/src/target/xscale.c +index 87a3d0f78..3ac455390 100644 +--- a/src/target/xscale.c ++++ b/src/target/xscale.c +@@ -3703,6 +3703,7 @@ struct target_type xscale_target = { + .deassert_reset = xscale_deassert_reset, + + /* REVISIT on some cores, allow exporting iwmmxt registers ... */ ++ .get_gdb_arch = arm_get_gdb_arch, + .get_gdb_reg_list = arm_get_gdb_reg_list, + + .read_memory = xscale_read_memory, +diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg +index 5d7e230c1..0d10021ef 100644 +--- a/tcl/target/stm32mp15x.cfg ++++ b/tcl/target/stm32mp15x.cfg +@@ -35,10 +35,22 @@ if { [info exists CPUTAPID] } { + } + } + ++# Chip Level TAP Controller, only in jtag mode ++if { [info exists CLCTAPID] } { ++ set _CLCTAPID $CLCTAPID ++} else { ++ set _CLCTAPID 0x06500041 ++} ++ + swj_newdap $_CHIPNAME tap -expected-id $_CPUTAPID -irlen 4 -ircapture 0x01 -irmask 0x0f +-dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap ++if { [using_jtag] } { ++ swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5 ++} ++dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap -ignore-syspwrupack + target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -coreid 0 -dbgbase 0xE00D0000 + target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -coreid 1 -dbgbase 0xE00D2000 ++target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 0 ++target create $_CHIPNAME.ap1 mem_ap -dap $_CHIPNAME.dap -ap-num 1 + + target smp $_CHIPNAME.cpu0 $_CHIPNAME.cpu1 + $_CHIPNAME.cpu0 cortex_a maskisr on +@@ -49,13 +61,15 @@ $_CHIPNAME.cpu1 cortex_a dacrfixup on + # Only in ENG_MODE the CM4 is available at reset + if { [info exists ENG_MODE] } { + target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap ++ target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2 + } else { + target create $_CHIPNAME.cpu2 cortex_m -dap $_CHIPNAME.dap -defer-examine ++ target create $_CHIPNAME.ap2 mem_ap -dap $_CHIPNAME.dap -ap-num 2 -defer-examine + } + +-$_CHIPNAME.cpu0 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 +-$_CHIPNAME.cpu1 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 +-$_CHIPNAME.cpu2 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++#$_CHIPNAME.cpu0 configure -work-area-phys 0x2ffc0000 -work-area-virt 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++#$_CHIPNAME.cpu1 configure -work-area-phys 0x2ffc0000 -work-area-virt 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 ++#$_CHIPNAME.cpu2 configure -work-area-phys 0x2ffc0000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + + if { [info exists GDB_PORT] } { + set _GDB_PORT $GDB_PORT +@@ -71,25 +85,29 @@ $_CHIPNAME.cpu2 configure -gdb-port [expr $_GDB_PORT + 0] + + $_CHIPNAME.cpu0 configure -event reset-deassert-post { delayed_reset_halt_cpu0 } + +-# Reset-halt stops in bootrom when only cpu0 can be attached. ++# In production mode, reset-halt stops in bootrom when only ++# cpu0 can be attached due to security mode. + # Set a breakpoint to stop cpu0 when cpu1 can be attached too. + proc delayed_reset_halt_cpu0 { } { + global _CHIPNAME ++ global ENG_MODE + global arp_reset_mode + +- set rom_halt_pc 0x000079ac +- +- # only "reset halt" or "reset init" +- if { 0 != [ string compare "$arp_reset_mode" "run" ] } { ++ if { [info exists ENG_MODE] || ([string compare "$arp_reset_mode" "run"] == 0) } { ++ # default reset-deassert-post handler ++ arp_reset_default_handler post $_CHIPNAME.cpu0 ++ } else { ++ # only in production mode for "reset halt" or "reset init" + cortex_a smp_off + targets $_CHIPNAME.cpu0 + arp_reset_default_handler post $_CHIPNAME.cpu0 + +- # quick test for cut1.0, check bootrom ++ set rom_halt_pc 0x000079ac ++ # quick test for rev.A, check bootrom + set v "" +- ap0_non_secure_access +- mem2array v 32 0x0000a000 1 phys +- ap0_secure_access ++ axi_non_secure_access ++ $_CHIPNAME.axi mem2array v 32 0x0000a000 1 phys ++ axi_secure_access + if { $v(0) == 0x1e494610 } { + set rom_halt_pc 0x0000688c + } +@@ -148,6 +166,8 @@ $_CHIPNAME.cpu2 configure -event gdb-attach { + if { ! [info exists ENG_MODE] } { + $_CHIPNAME.cpu2 arp_examine + $_CHIPNAME.cpu2 arp_poll ++ $_CHIPNAME.ap2 arp_examine ++ $_CHIPNAME.ap2 arp_poll + } + halt + } +@@ -156,19 +176,18 @@ $_CHIPNAME.cpu0 configure -event gdb-detach { catch { resume }; clients_dec_and_ + $_CHIPNAME.cpu1 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } + $_CHIPNAME.cpu2 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } + +-proc ap0_secure_access {} { ++proc axi_secure_access {} { + global _CHIPNAME + + $_CHIPNAME.dap apsel 0 + $_CHIPNAME.dap apcsw 0x10006000 + } + +-proc ap0_non_secure_access {} { ++proc axi_non_secure_access {} { + global _CHIPNAME + + $_CHIPNAME.dap apsel 0 + $_CHIPNAME.dap apcsw 0x30006000 + } + +-$_CHIPNAME.cpu0 configure -event examine-end { ap0_secure_access } +-$_CHIPNAME.cpu1 configure -event examine-end { ap0_secure_access } ++axi_secure_access +diff --git a/tcl/target/stm32mp15x_stpmic1.cfg b/tcl/target/stm32mp15x_stpmic1.cfg +index c08044196..a6446c640 100644 +--- a/tcl/target/stm32mp15x_stpmic1.cfg ++++ b/tcl/target/stm32mp15x_stpmic1.cfg +@@ -15,16 +15,20 @@ proc wait_state {condition} { + return -code 1 "Timed out" + } + +-proc mrw_phys {reg} { ++proc $_CHIPNAME.axi_mrw {reg} { ++ global _CHIPNAME ++ + set value "" +- mem2array value 32 $reg 1 phys ++ $_CHIPNAME.axi mem2array value 32 $reg 1 + return $value(0) + } + +-proc mmw_phys {reg setbits clearbits} { +- set old [mrw_phys $reg] ++proc $_CHIPNAME.axi_mmw {reg setbits clearbits} { ++ global _CHIPNAME ++ ++ set old [$_CHIPNAME.axi_mrw $reg] + set new [expr ($old & ~$clearbits) | $setbits] +- mww phys $reg $new ++ $_CHIPNAME.axi mww $reg $new + } + + # Set mask_reset bits in PMIC to keep all powers on at next reset event +@@ -33,9 +37,6 @@ proc mmw_phys {reg setbits clearbits} { + proc pmic_init {} { + global _CHIPNAME + +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- + set RCC_BASE 0x50000000 + set GPIO_BASE 0x54004000 + set I2C_BASE 0x5c002000 +@@ -44,38 +45,38 @@ proc pmic_init {} { + # timing depending from HSIDIV[1:0] = {0 1 2 3} means hsi_ker_ck = {64 32 16 8} MHz + set I2C_TIMING {0x10b17db5 0x00b07cb4 0x00503d58 0x00201d2b} + +- mww phys [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI) +- mww phys [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck) +- +- mww phys [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1 +- mww phys [expr $RCC_BASE + 0x188] 4; # assert I2C4RST +- mww phys [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST +- +- mww phys [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ) +- mmw_phys [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF +- mmw_phys [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain +- mmw_phys [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed +- mmw_phys [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down +- mmw_phys [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6 +- +- set hsidiv [expr [mrw_phys [expr $RCC_BASE + 0x018]] & 0x3] +- mww phys [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] +- mww phys [expr $I2C_BASE + 0x000] 1; # set PE = 1 +- mww phys [expr $I2C_BASE + 0x004] 0x02043066 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x18 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x0f +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x00 +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- mww phys [expr $I2C_BASE + 0x028] 0x6f +- wait_state {expr {([mrw_phys [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF +- mww phys [expr $I2C_BASE + 0x01c] 0x00000020 +- mww phys [expr $I2C_BASE + 0x004] 0x01ff14fe +- +-# mww phys [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4) +-# mww phys [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x00c] 1; # set HSION = 1 (enable HSI) ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x0c0] 2; # set I2C46SRC[2:0] = 2 (hsi_ker_ck) ++ ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x208] 4; # set I2C4EN = 1 ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x188] 4; # assert I2C4RST ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x18c] 4; # deassert I2C4RST ++ ++ $_CHIPNAME.axi mww [expr $RCC_BASE + 0x210] 1; # set GPIOZEN = 1 (enable GPIOZ) ++ $_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x00] 0x00000a00 0x00000f00; # GPIOZ4/5 AF ++ $_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x04] 0x00000030 0x00000000; # GPIOZ4/5 open drain ++ $_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x08] 0x00000000 0x00000f00; # GPIOZ4/5 low speed ++ $_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x0c] 0x00000000 0x00000f00; # GPIOZ4/5 no pull-up/down ++ $_CHIPNAME.axi_mmw [expr $GPIO_BASE + 0x20] 0x00660000 0x00ff0000; # GPIOZ4/5 AF6 ++ ++ set hsidiv [expr [$_CHIPNAME.axi_mrw [expr $RCC_BASE + 0x018]] & 0x3] ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x000] 1; # set PE = 1 ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x02043066 ++ wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x18 ++ wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x0f ++ wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x00 ++ wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x6f ++ wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x01c] 0x00000020 ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x01ff14fe ++ ++# $_CHIPNAME.axi mww [expr $RCC_BASE + 0x20c] 4; # set I2C4EN = 0 (disable I2C4) ++# $_CHIPNAME.axi mww [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) + } + + # Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we +@@ -84,22 +85,16 @@ proc pmic_init {} { + proc set_pmic_in_backup_reg {} { + global _CHIPNAME + +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- +- mmw_phys 0x5c00a150 0x00010000 0x00000000 ++ $_CHIPNAME.axi_mmw 0x5c00a150 0x00010000 0x00000000 + } + + proc clear_pmic_in_backup_reg {} { + global _CHIPNAME + +- targets $_CHIPNAME.cpu0 +- $_CHIPNAME.dap apsel 0 +- +- mmw_phys 0x5c00a150 0x00000000 0x00010000 ++ $_CHIPNAME.axi_mmw 0x5c00a150 0x00000000 0x00010000 + } + +-# Wrap around init/shutdown. Attention: CTRL-C will not invoke shutdown ++# Wrap around init/shutdown. Typing CTRL-C will also invoke shutdown + rename init _init + proc init {} { + _init +@@ -108,6 +103,8 @@ proc init {} { + + rename shutdown _shutdown + proc shutdown {} { +- clear_pmic_in_backup_reg ++ catch { clear_pmic_in_backup_reg } + _shutdown + } ++ ++reset_config srst_pulls_trst +-- +2.19.1 + diff --git a/recipes-devtools/openocd/openocd-stm32mp/0004-Fix-init-command.patch b/recipes-devtools/openocd/openocd-stm32mp/0004-Fix-init-command.patch new file mode 100644 index 0000000..3a38484 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp/0004-Fix-init-command.patch @@ -0,0 +1,38 @@ +From 4b82bf719f303b77df9a2310f75da4ce81098b17 Mon Sep 17 00:00:00 2001 +From: Antonio Borneo +Date: Fri, 9 Nov 2018 11:26:47 +0100 +Subject: [PATCH 4/4] Fix init command + +--- + tcl/target/stm32mp15x_stpmic1.cfg | 8 ++++++++ + 1 file changed, 8 insertions(+) + +diff --git a/tcl/target/stm32mp15x_stpmic1.cfg b/tcl/target/stm32mp15x_stpmic1.cfg +index a6446c640..2a32610fc 100644 +--- a/tcl/target/stm32mp15x_stpmic1.cfg ++++ b/tcl/target/stm32mp15x_stpmic1.cfg +@@ -85,13 +85,21 @@ proc pmic_init {} { + proc set_pmic_in_backup_reg {} { + global _CHIPNAME + ++ $_CHIPNAME.dap apsel 0 ++ set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] ++ axi_non_secure_access + $_CHIPNAME.axi_mmw 0x5c00a150 0x00010000 0x00000000 ++ $_CHIPNAME.dap apcsw $old_csw + } + + proc clear_pmic_in_backup_reg {} { + global _CHIPNAME + ++ $_CHIPNAME.dap apsel 0 ++ set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] ++ axi_non_secure_access + $_CHIPNAME.axi_mmw 0x5c00a150 0x00000000 0x00010000 ++ $_CHIPNAME.dap apcsw $old_csw + } + + # Wrap around init/shutdown. Typing CTRL-C will also invoke shutdown +-- +2.19.1 + diff --git a/recipes-devtools/openocd/openocd-stm32mp/0005-Add-CTI-plus-fixes.patch b/recipes-devtools/openocd/openocd-stm32mp/0005-Add-CTI-plus-fixes.patch new file mode 100644 index 0000000..32d1c63 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp/0005-Add-CTI-plus-fixes.patch @@ -0,0 +1,424 @@ +From 4e6c43b1054ee6544f2f1758963c886fa03136ae Mon Sep 17 00:00:00 2001 +From: Antonio Borneo +Date: Fri, 18 Jan 2019 16:52:07 +0100 +Subject: [PATCH] Add CTI plus fixes + +--- + src/target/arm_cti.c | 4 +- + src/target/cortex_a.c | 39 ++++--- + tcl/target/stm32mp15x.cfg | 162 ++++++++++++++++++++++++++++-- + tcl/target/stm32mp15x_stpmic1.cfg | 62 +++--------- + 4 files changed, 196 insertions(+), 71 deletions(-) + +diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c +index dcaf21e50..e6cb3cde3 100644 +--- a/src/target/arm_cti.c ++++ b/src/target/arm_cti.c +@@ -173,7 +173,7 @@ int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel) + return arm_cti_write_reg(self, CTI_APPCLEAR, CTI_CHNL(channel)); + } + +-static uint32_t cti_regs[26]; ++static uint32_t cti_regs[28]; + + static const struct { + uint32_t offset; +@@ -206,6 +206,8 @@ static const struct { + { CTI_CHOU_STATUS, "CHOUT", &cti_regs[23] }, + { CTI_APPSET, "APPSET", &cti_regs[24] }, + { CTI_APPCLEAR, "APPCLR", &cti_regs[25] }, ++ { CTI_APPPULSE, "APPPULSE", &cti_regs[26] }, ++ { CTI_INACK, "INACK", &cti_regs[27] }, + }; + + static int cti_find_reg_offset(const char *name) +diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c +index b281233ca..499dacc9b 100644 +--- a/src/target/cortex_a.c ++++ b/src/target/cortex_a.c +@@ -201,6 +201,7 @@ static int cortex_a_mmu_modify(struct target *target, int enable) + static int cortex_a_init_debug_access(struct target *target) + { + struct armv7a_common *armv7a = target_to_armv7a(target); ++ uint32_t dscr; + int retval; + + /* lock memory-mapped access to debug registers to prevent +@@ -230,6 +231,16 @@ static int cortex_a_init_debug_access(struct target *target) + + /* Resync breakpoint registers */ + ++ /* Enable halt for breakpoint, watchpoint and vector catch */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, &dscr); ++ if (retval != ERROR_OK) ++ return retval; ++ retval = mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE); ++ if (retval != ERROR_OK) ++ return retval; ++ + /* Since this is likely called from init or reset, update target state information*/ + return cortex_a_poll(target); + } +@@ -768,19 +779,6 @@ static int cortex_a_halt(struct target *target) + if (retval != ERROR_OK) + return retval; + +- /* +- * enter halting debug mode +- */ +- retval = mem_ap_read_atomic_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DSCR, &dscr); +- if (retval != ERROR_OK) +- return retval; +- +- retval = mem_ap_write_atomic_u32(armv7a->debug_ap, +- armv7a->debug_base + CPUDBG_DSCR, dscr | DSCR_HALT_DBG_MODE); +- if (retval != ERROR_OK) +- return retval; +- + int64_t then = timeval_ms(); + for (;; ) { + retval = mem_ap_read_atomic_u32(armv7a->debug_ap, +@@ -2977,7 +2975,20 @@ static int cortex_r4_target_create(struct target *target, Jim_Interp *interp) + static void cortex_a_deinit_target(struct target *target) + { + struct cortex_a_common *cortex_a = target_to_cortex_a(target); +- struct arm_dpm *dpm = &cortex_a->armv7a_common.dpm; ++ struct armv7a_common *armv7a = &cortex_a->armv7a_common; ++ struct arm_dpm *dpm = &armv7a->dpm; ++ uint32_t dscr; ++ int retval; ++ ++ if (target_was_examined(target)) { ++ /* Disable halt for breakpoint, watchpoint and vector catch */ ++ retval = mem_ap_read_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, &dscr); ++ if (retval == ERROR_OK) ++ mem_ap_write_atomic_u32(armv7a->debug_ap, ++ armv7a->debug_base + CPUDBG_DSCR, ++ dscr & ~DSCR_HALT_DBG_MODE); ++ } + + free(cortex_a->brp_list); + free(dpm->dbp); +diff --git a/tcl/target/stm32mp15x.cfg b/tcl/target/stm32mp15x.cfg +index 0d10021ef..992975f87 100644 +--- a/tcl/target/stm32mp15x.cfg ++++ b/tcl/target/stm32mp15x.cfg +@@ -47,6 +47,69 @@ if { [using_jtag] } { + swj_newdap $_CHIPNAME.clc tap -expected-id $_CLCTAPID -irlen 5 + } + dap create $_CHIPNAME.dap -chain-position $_CHIPNAME.tap -ignore-syspwrupack ++ ++# in smp mode, if one core is halted the event 'halted' get fired for the selected core only. ++# so the halt event remains unacknowledged for other cores. ++proc cti_prepare_restart_all {} { ++ global _CHIPNAME ++ ++ cti_prepare_restart cti0 ++ cti_prepare_restart cti1 ++ if { [$_CHIPNAME.cpu2 curstate] ne "examine deferred" } { ++ cti_prepare_restart cti2 ++ } ++} ++ ++proc cti_prepare_restart {cti} { ++ global _CHIPNAME ++ ++ # Acknowlodge EDBGRQ at TRIGOUT0 ++ $_CHIPNAME.$cti write INACK 0x01 ++ $_CHIPNAME.$cti write INACK 0x00 ++} ++ ++proc cti_start {} { ++ global _CHIPNAME ++ ++ # Configure both Cortex-A CTIs to halt each other ++ # TRIGIN0 (DBGTRIGGER) and TRIGOUT0 (EDBGRQ) at CTM_CHANNEL_0 ++ $_CHIPNAME.cti0 write INEN0 0x1 ++ $_CHIPNAME.cti1 write INEN0 0x1 ++ $_CHIPNAME.cti0 write OUTEN0 0x1 ++ $_CHIPNAME.cti1 write OUTEN0 0x1 ++ ++ $_CHIPNAME.cpu0 configure -event halted { cti_prepare_restart_all } ++ $_CHIPNAME.cpu1 configure -event halted { cti_prepare_restart_all } ++ ++ # enable CTIs ++ $_CHIPNAME.cti0 enable on ++ $_CHIPNAME.cti1 enable on ++ ++ # Cortex-M4 CTI configuration (enable CM4.CTI and join same CTM channels) ++ if { [$_CHIPNAME.cpu2 curstate] ne "examine deferred" } { ++ $_CHIPNAME.cti2 write INEN0 0x1 ++ $_CHIPNAME.cti2 write OUTEN0 0x1 ++ $_CHIPNAME.cpu2 configure -event halted { cti_prepare_restart_all } ++ $_CHIPNAME.cti2 enable on ++ } ++} ++ ++proc cti_stop {} { ++ global _CHIPNAME ++ ++ $_CHIPNAME.cti0 enable off ++ $_CHIPNAME.cti1 enable off ++ ++ if { [$_CHIPNAME.cpu2 curstate] ne "examine deferred" } { ++ $_CHIPNAME.cti2 enable off ++ } ++} ++ ++cti create $_CHIPNAME.cti -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE0094000 ++cti create $_CHIPNAME.cti0 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D8000 ++cti create $_CHIPNAME.cti1 -dap $_CHIPNAME.dap -ap-num 1 -ctibase 0xE00D9000 ++cti create $_CHIPNAME.cti2 -dap $_CHIPNAME.dap -ap-num 2 -ctibase 0xE0043000 ++ + target create $_CHIPNAME.cpu0 cortex_a -dap $_CHIPNAME.dap -coreid 0 -dbgbase 0xE00D0000 + target create $_CHIPNAME.cpu1 cortex_a -dap $_CHIPNAME.dap -coreid 1 -dbgbase 0xE00D2000 + target create $_CHIPNAME.axi mem_ap -dap $_CHIPNAME.dap -ap-num 0 +@@ -83,8 +146,23 @@ $_CHIPNAME.cpu0 configure -gdb-port [expr $_GDB_PORT + 1] + $_CHIPNAME.cpu1 configure -gdb-port [expr $_GDB_PORT + 2] + $_CHIPNAME.cpu2 configure -gdb-port [expr $_GDB_PORT + 0] + ++$_CHIPNAME.cpu0 configure -event reset-assert-pre { pre_reset_halt_cpu0 } + $_CHIPNAME.cpu0 configure -event reset-deassert-post { delayed_reset_halt_cpu0 } + ++proc pre_reset_halt_cpu0 {} { ++ global _CHIPNAME ++ global ENG_MODE ++ global arp_reset_mode ++ global workaround_revision_0x2000 ++ ++ catch { unset workaround_revision_0x2000 } ++ if { ([eval chip_revision] == 0x2000) && ![info exists ENG_MODE] && ([string compare "$arp_reset_mode" "run"] != 0) } { ++ set workaround_revision_0x2000 1 ++ set_debugflag_in_backup_reg ++ } ++ arp_reset_default_handler pre $_CHIPNAME.cpu0 ++} ++ + # In production mode, reset-halt stops in bootrom when only + # cpu0 can be attached due to security mode. + # Set a breakpoint to stop cpu0 when cpu1 can be attached too. +@@ -92,10 +170,27 @@ proc delayed_reset_halt_cpu0 { } { + global _CHIPNAME + global ENG_MODE + global arp_reset_mode ++ global workaround_revision_0x2000 + + if { [info exists ENG_MODE] || ([string compare "$arp_reset_mode" "run"] == 0) } { + # default reset-deassert-post handler + arp_reset_default_handler post $_CHIPNAME.cpu0 ++ } elseif { [info exists workaround_revision_0x2000] } { ++ unset workaround_revision_0x2000 ++ cortex_a smp_off ++ $_CHIPNAME.cpu0 arp_examine ++ catch { $_CHIPNAME.cpu0 arp_waitstate halted 500 } ++ halt ++ cortex_a smp_on ++ catch { clear_debugflag_in_backup_reg } ++ ++ # incr PC if insn is "BKPT 5" ++ set pc [ocd_reg pc] ++ regsub {pc[^:]*: } $pc "" pc ++ if { ([expr $pc & 3] == 0) && ([mrw $pc] == 0xe1200075) } { ++ reg pc [expr $pc + 4] ++ } ++ arp_reset_halt_default_handler $_CHIPNAME.cpu0 + } else { + # only in production mode for "reset halt" or "reset init" + cortex_a smp_off +@@ -103,12 +198,7 @@ proc delayed_reset_halt_cpu0 { } { + arp_reset_default_handler post $_CHIPNAME.cpu0 + + set rom_halt_pc 0x000079ac +- # quick test for rev.A, check bootrom +- set v "" +- axi_non_secure_access +- $_CHIPNAME.axi mem2array v 32 0x0000a000 1 phys +- axi_secure_access +- if { $v(0) == 0x1e494610 } { ++ if { [eval chip_revision] == 0x1000 } { + set rom_halt_pc 0x0000688c + } + +@@ -176,6 +266,57 @@ $_CHIPNAME.cpu0 configure -event gdb-detach { catch { resume }; clients_dec_and_ + $_CHIPNAME.cpu1 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } + $_CHIPNAME.cpu2 configure -event gdb-detach { catch { resume }; clients_dec_and_shutdown } + ++proc chip_revision {} { ++ global _CHIPNAME ++ ++ # read DBGMCU_IDC ++ $_CHIPNAME.ap1 mem2array v 32 0xe0081000 1 ++ return [expr $v(0) >> 16] ++} ++ ++proc $_CHIPNAME.axi_mrw {reg} { ++ global _CHIPNAME ++ ++ set value "" ++ $_CHIPNAME.axi mem2array value 32 $reg 1 ++ return $value(0) ++} ++ ++proc $_CHIPNAME.axi_mmw {reg setbits clearbits} { ++ global _CHIPNAME ++ ++ set old [$_CHIPNAME.axi_mrw $reg] ++ set new [expr ($old & ~$clearbits) | $setbits] ++ $_CHIPNAME.axi mww $reg $new ++} ++ ++# Backup registers are battery powered and keep the value both across power ++# cycles and reset pulses on NRST_CORE pin. ++# Bit 16 in TAMP_BACKUP_REGISTER(20) is used as special signal between the ++# debugger and the application across a reset. ++proc set_debugflag_in_backup_reg {} { ++ global _CHIPNAME ++ ++ $_CHIPNAME.dap apsel 0 ++ set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] ++ axi_secure_access ++ catch { $_CHIPNAME.axi_mmw 0x50001000 0x00000100 0x00000000 } ++ catch { $_CHIPNAME.axi_mmw 0x50000208 0x00000100 0x00000000 } ++ axi_non_secure_access ++ $_CHIPNAME.axi_mmw 0x5c00a150 0x00010000 0x00000000 ++ $_CHIPNAME.dap apcsw $old_csw ++} ++ ++proc clear_debugflag_in_backup_reg {} { ++ global _CHIPNAME ++ ++ $_CHIPNAME.dap apsel 0 ++ set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] ++ axi_non_secure_access ++ $_CHIPNAME.axi_mmw 0x5c00a150 0x00000000 0x00010000 ++ $_CHIPNAME.dap apcsw $old_csw ++} ++ + proc axi_secure_access {} { + global _CHIPNAME + +@@ -191,3 +332,12 @@ proc axi_non_secure_access {} { + } + + axi_secure_access ++ ++rename init __init ++proc init {} { ++ __init ++ if { [eval chip_revision] == 0x2000 } { ++ # srst pulse causes a reset of the debug port ++ reset_config srst_pulls_trst ++ } ++} +diff --git a/tcl/target/stm32mp15x_stpmic1.cfg b/tcl/target/stm32mp15x_stpmic1.cfg +index 2a32610fc..16cf9b0ae 100644 +--- a/tcl/target/stm32mp15x_stpmic1.cfg ++++ b/tcl/target/stm32mp15x_stpmic1.cfg +@@ -2,7 +2,7 @@ + + source [find target/stm32mp15x.cfg] + +-$_CHIPNAME.cpu0 configure -event reset-halt { catch { pmic_init }} ++$_CHIPNAME.cpu0 configure -event reset-halt { catch { if { [eval chip_revision] != 0x2000 } { pmic_init } } } + + # Wait for expression to be true with a timeout of 200ms + proc wait_state {condition} { +@@ -15,24 +15,8 @@ proc wait_state {condition} { + return -code 1 "Timed out" + } + +-proc $_CHIPNAME.axi_mrw {reg} { +- global _CHIPNAME +- +- set value "" +- $_CHIPNAME.axi mem2array value 32 $reg 1 +- return $value(0) +-} +- +-proc $_CHIPNAME.axi_mmw {reg setbits clearbits} { +- global _CHIPNAME +- +- set old [$_CHIPNAME.axi_mrw $reg] +- set new [expr ($old & ~$clearbits) | $setbits] +- $_CHIPNAME.axi mww $reg $new +-} +- +-# Set mask_reset bits in PMIC to keep all powers on at next reset event +-# Only the mask_reset of SD-card power is not enabled to guarantee boot from SD ++# Set mask_reset bits in PMIC to keep VDD and VDD_CORE on at next reset event ++# Mask_reset for SD-card power "must" not be enabled to guarantee boot from SD + # This procedure requires Secure memory access + proc pmic_init {} { + global _CHIPNAME +@@ -62,15 +46,11 @@ proc pmic_init {} { + set hsidiv [expr [$_CHIPNAME.axi_mrw [expr $RCC_BASE + 0x018]] & 0x3] + $_CHIPNAME.axi mww [expr $I2C_BASE + 0x010] [lindex $I2C_TIMING $hsidiv] + $_CHIPNAME.axi mww [expr $I2C_BASE + 0x000] 1; # set PE = 1 +- $_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x02043066 ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x02023066 + wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS + $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x18 + wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x0f +- wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x00 +- wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000002) == 0}}; # wait TXIS +- $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x6f ++ $_CHIPNAME.axi mww [expr $I2C_BASE + 0x028] 0x05 + wait_state {expr {([$_CHIPNAME.axi_mrw [expr $I2C_BASE + 0x018]] & 0x00000020) == 0}}; # wait STOPF + $_CHIPNAME.axi mww [expr $I2C_BASE + 0x01c] 0x00000020 + $_CHIPNAME.axi mww [expr $I2C_BASE + 0x004] 0x01ff14fe +@@ -79,39 +59,21 @@ proc pmic_init {} { + # $_CHIPNAME.axi mww [expr $RCC_BASE + 0x214] 1; # set GPIOZEN = 0 (disable GPIOZ) + } + +-# Set bit 16 in TAMP_BACKUP_REGISTER(20) to signal to SPL and TF-A that we +-# are in a debug session. This will force them (at next reboot) to program +-# the PMIC for keeping powered-on the debug unit during reset. +-proc set_pmic_in_backup_reg {} { +- global _CHIPNAME +- +- $_CHIPNAME.dap apsel 0 +- set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] +- axi_non_secure_access +- $_CHIPNAME.axi_mmw 0x5c00a150 0x00010000 0x00000000 +- $_CHIPNAME.dap apcsw $old_csw +-} +- +-proc clear_pmic_in_backup_reg {} { +- global _CHIPNAME +- +- $_CHIPNAME.dap apsel 0 +- set old_csw [lindex [ocd_$_CHIPNAME.dap apcsw] 4] +- axi_non_secure_access +- $_CHIPNAME.axi_mmw 0x5c00a150 0x00000000 0x00010000 +- $_CHIPNAME.dap apcsw $old_csw +-} +- + # Wrap around init/shutdown. Typing CTRL-C will also invoke shutdown + rename init _init + proc init {} { + _init +- set_pmic_in_backup_reg ++ if { [eval chip_revision] != 0x2000 } { ++ # Use debug flag to signal to SPL and TF-A that we are in a debug ++ # session. This will force them (at next reboot) to program the PMIC ++ # for keeping powered-on the debug unit during reset. ++ set_debugflag_in_backup_reg ++ } + } + + rename shutdown _shutdown + proc shutdown {} { +- catch { clear_pmic_in_backup_reg } ++ catch { clear_debugflag_in_backup_reg } + _shutdown + } + +-- +2.20.0 + diff --git a/recipes-devtools/openocd/openocd-stm32mp_0.10.0.bb b/recipes-devtools/openocd/openocd-stm32mp_0.10.0.bb new file mode 100644 index 0000000..73977d0 --- /dev/null +++ b/recipes-devtools/openocd/openocd-stm32mp_0.10.0.bb @@ -0,0 +1,20 @@ +SUMMARY = "Free and Open On-Chip Debugging, In-System Programming and Boundary-Scan Testing" +HOMEPAGE = "http://openocd.org" +LICENSE = "GPLv2" +LIC_FILES_CHKSUM = "file://COPYING;md5=b234ee4d69f5fce4486a80fdaf4a4263" + +require openocd-stm32mp.inc + +SRC_URI_prepend = " git://repo.or.cz/openocd.git;name=openocd " + +SRCREV_FORMAT = "openocd" +SRCREV_openocd = "1afec4f561392539197fae678de4cd2ca01c127d" + +PV = "0.10.0" +PR = "release.${SRCPV}" + +SRC_URI += "file://0001-Add-support-of-STLINK-for-stm32mp1.patch" +SRC_URI += "file://0002-Add-support-for-silicon-revB.patch" +SRC_URI += "file://0003-Align-to-community-code-for-cache-coherency-and-rese.patch" +SRC_URI += "file://0004-Fix-init-command.patch" +SRC_URI += "file://0005-Add-CTI-plus-fixes.patch"