Merge pull request #1036 from en-sc/en-sc/from_upstream

Merge up to a35e254c5383008cdacf7838a777f7f17af5eeb1 from upstream

32 files changed, 1725 insertions, 1243 deletions

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 384f6d2..832ec77 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -7348,10 +7348,6 @@ works only for chips that do not have factory pre-programmed region 0
 code.
 @end deffn
 
-@deffn {Command} {nrf5 info}
-Decodes and shows information from FICR and UICR registers.
-@end deffn
-
 @end deffn
 
 @deffn {Flash Driver} {ocl}
@@ -11103,6 +11099,11 @@ EJTAG Register Specification could be found in MIPS Document MD00047F, for
 core specific EJTAG Register definition, please check Core Specific SUM manual.
 @end deffn
 
+@deffn {Command} {mips32 dsp} [[register_name] [value]]
+Displays all DSP registers' contents or get/set value by register name. Will display
+an error if current CPU does not support DSP.
+@end deffn
+
 @section RISC-V Architecture
 
 @uref{http://riscv.org/, RISC-V} is a free and open ISA. OpenOCD supports JTAG
@@ -12353,59 +12354,102 @@ waveform generator. These are synthesize-able hardware descriptions of
 logic circuits in addition to software for control, visualization and further analysis.
 In a session using JTAG for its transport protocol, OpenOCD supports the function
 of a JTAG-Host. The JTAG-Host is needed to connect the circuit over JTAG to the
-control-software. For more details see @url{http://ipdbg.org}.
+control-software. The JTAG-Hub is the circuit which transfers the data from JTAG to the
+different tools connected to the Hub. Hub implementations for most major FPGA vendors/families
+are provided. For more details see @url{http://ipdbg.org}.
+
+@deffn {Command} {ipdbg create-hub} @var{hub_name} @option{-tap @var{tapname}} @option{-ir @var{ir_value} [@var{dr_length}]} [@option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]}]
+@deffnx {Command} {ipdbg create-hub} @var{hub_name} @option{-pld @var{pld_name} [@var{user}]} [@option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]}]
+Creates a IPDBG JTAG Hub. The created hub is later used to start, stop and configure IPDBG JTAG Host servers.
+The first argument @var{hub_name} is the name of the created hub. It can be used later as a reference.
 
-@deffn {Command} {ipdbg} [@option{-start|-stop}] @option{-tap @var{tapname}} @option{-hub @var{ir_value} [@var{dr_length}]} [@option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]}] [@option{-port @var{number}}] [@option{-tool @var{number}}]
-Starts or stops a IPDBG JTAG-Host server. Arguments can be specified in any order.
+The pld drivers are able to provide the tap and ir_value for the IPDBG JTAG-Host server. This will be used with the second variant with option @option{-pld}.
 
 Command options:
 @itemize @bullet
-@item @option{-start|-stop} starts or stops a IPDBG JTAG-Host server (default: start).
+@item @var{hub_name} the name of the IPDBG hub.
+This name is also used to create the object's command, referred to here
+as @command{$hub_name}, and in other places where the Hub needs to be identified.
+
 @item @option{-tap @var{tapname}} targeting the TAP @var{tapname}.
-@item @option{-hub @var{ir_value}} states that the JTAG hub is
-reachable with dr-scans while the JTAG instruction register has the value @var{ir_value}.
-@item @option{-port @var{number}} tcp port number where the JTAG-Host will listen. The default is 4242 which is used when the option is not given.
-@item @option{-tool @var{number}} number of the tool/feature. These corresponds to the ports "data_(up/down)_(0..6)" at the JtagHub. The default is 1 which is used when the option is not given.
-@item @option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]} On some devices, the user data-register is reachable if there is a
-specific value in a second dr. This second dr is called vir (virtual ir). With this parameter given, the IPDBG satisfies this condition prior an
+
+@item @option{-ir @var{ir_value}} states that the JTAG hub is
+reachable with dr-scans while the JTAG instruction register has the value @var{ir_value}. Also known as  @verb{|USERx|} instructions.
+The optional @var{dr_length} is the length of the dr.
+Current JTAG-Hub implementation only supports dr_length=13, which is also the default value.
+
+@item @option{-vir [@var{vir_value} [@var{length} [@var{instr_code}]]]} To support more Hubs than USER registers in a single FPGA it is possible to
+use a mechanism known as virtual-ir where the user data-register is reachable if there is a specific value in a second dr.
+This second dr is called vir (virtual ir). With this parameter given, the IPDBG satisfies this condition prior an
 access to the IPDBG-Hub. The value shifted into the vir is given by the first parameter @var{vir_value} (default: 0x11). The second
 parameter @var{length} is the length of the vir data register (default: 5). With the @var{instr_code} (default: 0x00e) parameter the ir value to
 shift data through vir can be configured.
+
+@item @option{-pld @var{pld_name} [@var{user}]} The defined driver for the pld @var{pld_name} is used to get the tap and user instruction.
+The pld devices names can be shown by the command @command{pld devices}. With [@var{user}] one can select a different @verb{|USERx|}-Instruction.
+If the IPDBG JTAG-Hub is used without modification the default value of 1 which selects the first @verb{|USERx|} instruction is adequate.
+The @verb{|USERx|} instructions are vendor specific and don't change between families of the same vendor.
+So if there's a pld driver for your vendor it should work with your FPGA even when the driver is not compatible with your device for the remaining features.
+If your device/vendor is not supported you have to use the first variant.
+
 @end itemize
+
 @end deffn
-or
-@deffn {Command} {ipdbg} [@option{-start|-stop}] @option{-pld @var{name} [@var{user}]} [@option{-port @var{number}}] [@option{-tool @var{number}}]
-Also starts or stops a IPDBG JTAG-Host server. The pld drivers are able to provide the tap and hub/IR for the IPDBG JTAG-Host server.
-With the @option{-pld @var{name} [@var{user}]} the information from the pld-driver is used and the options @option{-tap} and @option{-hub} are not required.
-The defined driver for the pld @var{name} gets selected. (The pld devices names can be shown by the command @command{pld devices}).
 
-The @verb{|USERx|} instructions are vendor specific and don't change between families of the same vendor.
-So if there's a pld driver for your vendor it should work with your FPGA even when the driver is not compatible with your device for the remaining features. If your device/vendor is not supported you have to use the previous command.
+@deffn {Command} {$hub_name ipdbg start} @option{-tool @var{number}} @option{-port @var{number}}
+Starts a IPDBG JTAG-Host server. The remaining arguments can be specified in any order.
 
-With [@var{user}] one can select a different @verb{|USERx|}-Instruction. If the IPDBG JTAG-Hub is used without modification the default value of 1 which selects the first @verb{|USERx|} instruction is adequate.
+Command options:
+@itemize @bullet
+@item @option{-port @var{number}} tcp port number where the JTAG-Host will listen. The default is 4242 which is used when the option is not given.
+@item @option{-tool @var{number}} number of the tool/feature. These corresponds to the ports "data_(up/down)_(0..6)" at the JtagHub. The default is 1 which is used when the option is not given.
+@end itemize
+@end deffn
 
-The remaining options are described in the previous command.
+@deffn {Command} {$hub_name ipdbg stop} @option{-tool @var{number}}
+Stops a IPDBG JTAG-Host server.
+Command options:
+@itemize @bullet
+@item @option{-tool @var{number}} number of the tool/feature. These corresponds to the ports "data_(up/down)_(0..6)" at the JtagHub. The default is 1 which is used when the option is not given.
+@end itemize
 @end deffn
 
 Examples:
 @example
-ipdbg -start -tap xc6s.tap -hub 0x02 -port 4242 -tool 4
+ipdbg create-hub xc6s.ipdbghub -tap xc6s.tap -hub 0x02
+xc6s.ipdbghub ipdbg start -port 4242 -tool 4
 @end example
-Starts a server listening on tcp-port 4242 which connects to tool 4.
+Creates a IPDBG Hub and starts a server listening on tcp-port 4242 which connects to tool 4.
 The connection is through the TAP of a Xilinx Spartan 6 on USER1 instruction (tested with a papillion pro board).
 
 @example
-ipdbg -start -tap 10m50.tap -hub 0x00C -vir -port 60000 -tool 1
+ipdbg create-hub max10m50.ipdbghub -tap max10m50.tap -hub 0x00C -vir
+max10m50.ipdbghub ipdbg start -tool 1 -port 60000
 @end example
 Starts a server listening on tcp-port 60000 which connects to tool 1 (data_up_1/data_down_1).
 The connection is through the TAP of a Intel MAX10 virtual jtag component (sld_instance_index is 0; sld_ir_width is smaller than 5).
 
 @example
-ipdbg -start -pld xc7.pld -port 5555 -tool 0
+ipdbg create-hub xc7.ipdbghub -pld xc7.pld
+xc7.ipdbghub ipdbg start -port 5555 -tool 0
 @end example
 Starts a server listening on tcp-port 5555 which connects to tool 0 (data_up_0/data_down_0).
 The TAP and ir value used to reach the JTAG Hub is given by the pld driver.
 
+@deffn {Command} {$hub_name queuing} @option{-size @var{size}}
+Configure the queuing between IPDBG JTAG-Host and Hub.
+The maximum possible queue size is 1024 which is also the default.
+
+@itemize @bullet
+@item @option{-size @var{size}} max number of transfers in the queue.
+@end itemize
+@end deffn
+
+@example
+bitbang.ibdbghub queuing -size 32
+@end example
+Send a maximum of 32 transfers to the queue before executing them.
+
 
 @node Utility Commands
 @chapter Utility Commands
diff --git a/src/flash/nor/nrf5.c b/src/flash/nor/nrf5.c
index d5de4a4..bf8c9da 100644
--- a/src/flash/nor/nrf5.c
+++ b/src/flash/nor/nrf5.c
@@ -17,6 +17,7 @@
 #include <target/armv7m.h>
 #include <helper/types.h>
 #include <helper/time_support.h>
+#include <helper/bits.h>
 
 /* Both those values are constant across the current spectrum ofr nRF5 devices */
 #define WATCHDOG_REFRESH_REGISTER       0x40010600
@@ -42,32 +43,9 @@ enum nrf5_ficr_registers {
 	NRF51_FICR_SIZERAMBLOCK2	= NRF5_FICR_REG(0x040),
 	NRF51_FICR_SIZERAMBLOCK3	= NRF5_FICR_REG(0x044),
 
+	/* CONFIGID is documented on nRF51 series only.
+	 * On nRF52 is present but not documented */
 	NRF5_FICR_CONFIGID		= NRF5_FICR_REG(0x05C),
-	NRF5_FICR_DEVICEID0		= NRF5_FICR_REG(0x060),
-	NRF5_FICR_DEVICEID1		= NRF5_FICR_REG(0x064),
-	NRF5_FICR_ER0			= NRF5_FICR_REG(0x080),
-	NRF5_FICR_ER1			= NRF5_FICR_REG(0x084),
-	NRF5_FICR_ER2			= NRF5_FICR_REG(0x088),
-	NRF5_FICR_ER3			= NRF5_FICR_REG(0x08C),
-	NRF5_FICR_IR0			= NRF5_FICR_REG(0x090),
-	NRF5_FICR_IR1			= NRF5_FICR_REG(0x094),
-	NRF5_FICR_IR2			= NRF5_FICR_REG(0x098),
-	NRF5_FICR_IR3			= NRF5_FICR_REG(0x09C),
-	NRF5_FICR_DEVICEADDRTYPE	= NRF5_FICR_REG(0x0A0),
-	NRF5_FICR_DEVICEADDR0		= NRF5_FICR_REG(0x0A4),
-	NRF5_FICR_DEVICEADDR1		= NRF5_FICR_REG(0x0A8),
-
-	NRF51_FICR_OVERRIDEN		= NRF5_FICR_REG(0x0AC),
-	NRF51_FICR_NRF_1MBIT0		= NRF5_FICR_REG(0x0B0),
-	NRF51_FICR_NRF_1MBIT1		= NRF5_FICR_REG(0x0B4),
-	NRF51_FICR_NRF_1MBIT2		= NRF5_FICR_REG(0x0B8),
-	NRF51_FICR_NRF_1MBIT3		= NRF5_FICR_REG(0x0BC),
-	NRF51_FICR_NRF_1MBIT4		= NRF5_FICR_REG(0x0C0),
-	NRF51_FICR_BLE_1MBIT0		= NRF5_FICR_REG(0x0EC),
-	NRF51_FICR_BLE_1MBIT1		= NRF5_FICR_REG(0x0F0),
-	NRF51_FICR_BLE_1MBIT2		= NRF5_FICR_REG(0x0F4),
-	NRF51_FICR_BLE_1MBIT3		= NRF5_FICR_REG(0x0F8),
-	NRF51_FICR_BLE_1MBIT4		= NRF5_FICR_REG(0x0FC),
 
 	/* Following registers are available on nRF52 and on nRF51 since rev 3 */
 	NRF5_FICR_INFO_PART			= NRF5_FICR_REG(0x100),
@@ -84,9 +62,6 @@ enum nrf5_uicr_registers {
 #define NRF5_UICR_REG(offset) (NRF5_UICR_BASE + offset)
 
 	NRF51_UICR_CLENR0	= NRF5_UICR_REG(0x000),
-	NRF51_UICR_RBPCONF	= NRF5_UICR_REG(0x004),
-	NRF51_UICR_XTALFREQ	= NRF5_UICR_REG(0x008),
-	NRF51_UICR_FWID		= NRF5_UICR_REG(0x010),
 };
 
 enum nrf5_nvmc_registers {
@@ -120,10 +95,10 @@ struct nrf52_ficr_info {
 };
 
 enum nrf5_features {
-	NRF5_FEATURE_SERIES_51	= 1 << 0,
-	NRF5_FEATURE_SERIES_52	= 1 << 1,
-	NRF5_FEATURE_BPROT		= 1 << 2,
-	NRF5_FEATURE_ACL_PROT	= 1 << 3,
+	NRF5_FEATURE_SERIES_51	= BIT(0),
+	NRF5_FEATURE_SERIES_52	= BIT(1),
+	NRF5_FEATURE_BPROT		= BIT(2),
+	NRF5_FEATURE_ACL_PROT	= BIT(3),
 };
 
 struct nrf5_device_spec {
@@ -164,26 +139,20 @@ struct nrf5_info {
 .features      = NRF5_FEATURE_SERIES_51,            \
 }
 
-#define NRF5_DEVICE_DEF(id, pt, var, bcode, fsize, features) \
-{                                                   \
-.hwid          = (id),                              \
-.part          = pt,                                \
-.variant       = var,                               \
-.build_code    = bcode,                             \
-.flash_size_kb = (fsize),                           \
-.features      = features,                          \
-}
-
-/* The known devices table below is derived from the "nRF5x series
- * compatibility matrix" documents, which can be found in the "DocLib" of
- * nordic:
+/*
+ * The table maps known HWIDs to the part numbers, variant
+ * build code and some other info. For nRF51 rev 1 and 2 devices
+ * this is the only way how to get the part number and variant.
  *
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF51/latest/COMP/nrf51/nRF51422_ic_revision_overview
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF51/latest/COMP/nrf51/nRF51822_ic_revision_overview
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF51/latest/COMP/nrf51/nRF51824_ic_revision_overview
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF52810/latest/COMP/nrf52810/nRF52810_ic_revision_overview
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF52832/latest/COMP/nrf52832/ic_revision_overview
- * https://www.nordicsemi.com/DocLib/Content/Comp_Matrix/nRF52840/latest/COMP/nrf52840/nRF52840_ic_revision_overview
+ * All tested nRF51 rev 3 devices have FICR INFO fields
+ * but the fields are not documented in RM so we keep HWIDs in
+ * this table.
+ *
+ * nRF52 and newer devices have FICR INFO documented, the autodetection
+ * can rely on it and HWIDs table is not used.
+ *
+ * The known devices table below is derived from the "nRF5x series
+ * compatibility matrix" documents.
  *
  * Up to date with Matrix v2.0, plus some additional HWIDs.
  *
@@ -248,19 +217,6 @@ static const struct nrf5_device_spec nrf5_known_devices_table[] = {
 
 	/* The driver fully autodetects nRF52 series devices by FICR INFO,
 	 * no need for nRF52xxx HWIDs in this table */
-#if 0
-	/* nRF52810 Devices */
-	NRF5_DEVICE_DEF(0x0142, "52810", "QFAA", "B0",    192,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_BPROT),
-	NRF5_DEVICE_DEF(0x0143, "52810", "QCAA", "C0",    192,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_BPROT),
-
-	/* nRF52832 Devices */
-	NRF5_DEVICE_DEF(0x00C7, "52832", "QFAA", "B0",    512,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_BPROT),
-	NRF5_DEVICE_DEF(0x0139, "52832", "QFAA", "E0",    512,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_BPROT),
-	NRF5_DEVICE_DEF(0x00E3, "52832", "CIAA", "B0",    512,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_BPROT),
-
-	/* nRF52840 Devices */
-	NRF5_DEVICE_DEF(0x0150, "52840", "QIAA", "C0",    1024,	NRF5_FEATURE_SERIES_52 | NRF5_FEATURE_ACL_PROT),
-#endif
 };
 
 struct nrf5_device_package {
@@ -270,11 +226,16 @@ struct nrf5_device_package {
 
 /* Newer devices have FICR INFO.PACKAGE.
  * This table converts its value to two character code */
-static const struct nrf5_device_package nrf5_packages_table[] = {
+static const struct nrf5_device_package nrf52_packages_table[] = {
 	{ 0x2000, "QF" },
 	{ 0x2001, "CH" },
 	{ 0x2002, "CI" },
+	{ 0x2003, "QC" },
+	{ 0x2004, "QI/CA" },	/* differs nRF52805, 810, 811: CA, nRF52833, 840: QI */
 	{ 0x2005, "CK" },
+	{ 0x2007, "QD" },
+	{ 0x2008, "CJ" },
+	{ 0x2009, "CF" },
 };
 
 const struct flash_driver nrf5_flash, nrf51_flash;
@@ -282,28 +243,10 @@ const struct flash_driver nrf5_flash, nrf51_flash;
 static bool nrf5_bank_is_probed(const struct flash_bank *bank)
 {
 	struct nrf5_bank *nbank = bank->driver_priv;
-
 	assert(nbank);
 
 	return nbank->probed;
 }
-static int nrf5_probe(struct flash_bank *bank);
-
-static int nrf5_get_probed_chip_if_halted(struct flash_bank *bank, struct nrf5_info **chip)
-{
-	if (bank->target->state != TARGET_HALTED) {
-		LOG_ERROR("Target not halted");
-		return ERROR_TARGET_NOT_HALTED;
-	}
-
-	struct nrf5_bank *nbank = bank->driver_priv;
-	*chip = nbank->chip;
-
-	if (nrf5_bank_is_probed(bank))
-		return ERROR_OK;
-
-	return nrf5_probe(bank);
-}
 
 static int nrf5_wait_for_nvmc(struct nrf5_info *chip)
 {
@@ -431,9 +374,10 @@ static int nrf5_protect_check_clenr0(struct flash_bank *bank)
 {
 	int res;
 	uint32_t clenr0;
+
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
-
 	assert(chip);
 
 	res = target_read_u32(chip->target, NRF51_FICR_CLENR0,
@@ -462,8 +406,8 @@ static int nrf5_protect_check_clenr0(struct flash_bank *bank)
 static int nrf5_protect_check_bprot(struct flash_bank *bank)
 {
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
-
 	assert(chip);
 
 	static uint32_t nrf5_bprot_offsets[4] = { 0x600, 0x604, 0x610, 0x614 };
@@ -493,8 +437,8 @@ static int nrf5_protect_check(struct flash_bank *bank)
 		return ERROR_OK;
 
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
-
 	assert(chip);
 
 	if (chip->features & NRF5_FEATURE_BPROT)
@@ -512,8 +456,11 @@ static int nrf5_protect_clenr0(struct flash_bank *bank, int set, unsigned int fi
 {
 	int res;
 	uint32_t clenr0, ppfc;
+
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	if (first != 0) {
 		LOG_ERROR("Code region 0 must start at the beginning of the bank");
@@ -570,18 +517,21 @@ error:
 static int nrf5_protect(struct flash_bank *bank, int set, unsigned int first,
 		unsigned int last)
 {
-	int res;
-	struct nrf5_info *chip;
-
 	/* UICR cannot be write protected so just bail out early */
 	if (bank->base == NRF5_UICR_BASE) {
 		LOG_ERROR("UICR page does not support protection");
 		return ERROR_FLASH_OPER_UNSUPPORTED;
 	}
 
-	res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
+	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	if (chip->features & NRF5_FEATURE_SERIES_51)
 		return nrf5_protect_clenr0(bank, set, first, last);
@@ -607,9 +557,9 @@ static bool nrf5_info_variant_to_str(uint32_t variant, char *bf)
 
 static const char *nrf5_decode_info_package(uint32_t package)
 {
-	for (size_t i = 0; i < ARRAY_SIZE(nrf5_packages_table); i++) {
-		if (nrf5_packages_table[i].package == package)
-			return nrf5_packages_table[i].code;
+	for (size_t i = 0; i < ARRAY_SIZE(nrf52_packages_table); i++) {
+		if (nrf52_packages_table[i].package == package)
+			return nrf52_packages_table[i].code;
 	}
 	return "xx";
 }
@@ -642,7 +592,9 @@ static int get_nrf5_chip_type_str(const struct nrf5_info *chip, char *buf, unsig
 static int nrf5_info(struct flash_bank *bank, struct command_invocation *cmd)
 {
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	char chip_type_str[256];
 	if (get_nrf5_chip_type_str(chip, chip_type_str, sizeof(chip_type_str)) != ERROR_OK)
@@ -676,11 +628,15 @@ static int nrf5_read_ficr_info(struct nrf5_info *chip)
 		chip->features = NRF5_FEATURE_SERIES_52;
 
 		switch (chip->ficr_info.part) {
+		case 0x52805:
 		case 0x52810:
+		case 0x52811:
 		case 0x52832:
 			chip->features |= NRF5_FEATURE_BPROT;
 			break;
 
+		case 0x52820:
+		case 0x52833:
 		case 0x52840:
 			chip->features |= NRF5_FEATURE_ACL_PROT;
 			break;
@@ -755,8 +711,11 @@ static int nrf5_get_ram_size(struct target *target, uint32_t *ram_size)
 static int nrf5_probe(struct flash_bank *bank)
 {
 	int res;
+
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 	struct target *target = chip->target;
 
 	uint32_t configid;
@@ -1024,11 +983,17 @@ static int nrf5_ll_flash_write(struct nrf5_info *chip, uint32_t address, const u
 static int nrf5_write(struct flash_bank *bank, const uint8_t *buffer,
 					uint32_t offset, uint32_t count)
 {
-	struct nrf5_info *chip;
+	int res;
 
-	int res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
+	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	assert(offset % 4 == 0);
 	assert(count % 4 == 0);
@@ -1081,11 +1046,16 @@ static int nrf5_erase(struct flash_bank *bank, unsigned int first,
 		unsigned int last)
 {
 	int res;
-	struct nrf5_info *chip;
 
-	res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
+	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	/* UICR CLENR0 based protection used on nRF51 prevents erase
 	 * absolutely silently. NVMC has no flag to indicate the protection
@@ -1101,7 +1071,7 @@ static int nrf5_erase(struct flash_bank *bank, unsigned int first,
 	}
 
 	/* For each sector to be erased */
-	for (unsigned int s = first; s <= last && res == ERROR_OK; s++) {
+	for (unsigned int s = first; s <= last; s++) {
 
 		if (chip->features & NRF5_FEATURE_SERIES_51
 				&& bank->sectors[s].is_protected == 1) {
@@ -1122,6 +1092,7 @@ static int nrf5_erase(struct flash_bank *bank, unsigned int first,
 static void nrf5_free_driver_priv(struct flash_bank *bank)
 {
 	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
 	struct nrf5_info *chip = nbank->chip;
 	if (!chip)
 		return;
@@ -1160,6 +1131,9 @@ FLASH_BANK_COMMAND_HANDLER(nrf5_flash_bank_command)
 	struct nrf5_info *chip;
 	struct nrf5_bank *nbank = NULL;
 
+	if (bank->driver == &nrf51_flash)
+		LOG_WARNING("Flash driver 'nrf51' is deprecated! Use 'nrf5' instead.");
+
 	switch (bank->base) {
 	case NRF5_FLASH_BASE:
 	case NRF5_UICR_BASE:
@@ -1210,11 +1184,15 @@ COMMAND_HANDLER(nrf5_handle_mass_erase_command)
 
 	assert(bank);
 
-	struct nrf5_info *chip;
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
 
-	res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
+	struct nrf5_bank *nbank = bank->driver_priv;
+	assert(nbank);
+	struct nrf5_info *chip = nbank->chip;
+	assert(chip);
 
 	if (chip->features & NRF5_FEATURE_SERIES_51) {
 		uint32_t ppfc;
@@ -1245,137 +1223,6 @@ COMMAND_HANDLER(nrf5_handle_mass_erase_command)
 	return res;
 }
 
-COMMAND_HANDLER(nrf5_handle_info_command)
-{
-	int res;
-	struct flash_bank *bank = NULL;
-	struct target *target = get_current_target(CMD_CTX);
-
-	res = get_flash_bank_by_addr(target, NRF5_FLASH_BASE, true, &bank);
-	if (res != ERROR_OK)
-		return res;
-
-	assert(bank);
-
-	struct nrf5_info *chip;
-
-	res = nrf5_get_probed_chip_if_halted(bank, &chip);
-	if (res != ERROR_OK)
-		return res;
-
-	static struct {
-		const uint32_t address;
-		uint32_t value;
-	} ficr[] = {
-		{ .address = NRF5_FICR_CODEPAGESIZE	},
-		{ .address = NRF5_FICR_CODESIZE	},
-		{ .address = NRF51_FICR_CLENR0		},
-		{ .address = NRF51_FICR_PPFC		},
-		{ .address = NRF51_FICR_NUMRAMBLOCK	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK0	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK1	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK2	},
-		{ .address = NRF51_FICR_SIZERAMBLOCK3	},
-		{ .address = NRF5_FICR_CONFIGID	},
-		{ .address = NRF5_FICR_DEVICEID0	},
-		{ .address = NRF5_FICR_DEVICEID1	},
-		{ .address = NRF5_FICR_ER0		},
-		{ .address = NRF5_FICR_ER1		},
-		{ .address = NRF5_FICR_ER2		},
-		{ .address = NRF5_FICR_ER3		},
-		{ .address = NRF5_FICR_IR0		},
-		{ .address = NRF5_FICR_IR1		},
-		{ .address = NRF5_FICR_IR2		},
-		{ .address = NRF5_FICR_IR3		},
-		{ .address = NRF5_FICR_DEVICEADDRTYPE	},
-		{ .address = NRF5_FICR_DEVICEADDR0	},
-		{ .address = NRF5_FICR_DEVICEADDR1	},
-		{ .address = NRF51_FICR_OVERRIDEN	},
-		{ .address = NRF51_FICR_NRF_1MBIT0	},
-		{ .address = NRF51_FICR_NRF_1MBIT1	},
-		{ .address = NRF51_FICR_NRF_1MBIT2	},
-		{ .address = NRF51_FICR_NRF_1MBIT3	},
-		{ .address = NRF51_FICR_NRF_1MBIT4	},
-		{ .address = NRF51_FICR_BLE_1MBIT0	},
-		{ .address = NRF51_FICR_BLE_1MBIT1	},
-		{ .address = NRF51_FICR_BLE_1MBIT2	},
-		{ .address = NRF51_FICR_BLE_1MBIT3	},
-		{ .address = NRF51_FICR_BLE_1MBIT4	},
-	}, uicr[] = {
-		{ .address = NRF51_UICR_CLENR0,		},
-		{ .address = NRF51_UICR_RBPCONF		},
-		{ .address = NRF51_UICR_XTALFREQ	},
-		{ .address = NRF51_UICR_FWID		},
-	};
-
-	for (size_t i = 0; i < ARRAY_SIZE(ficr); i++) {
-		res = target_read_u32(chip->target, ficr[i].address,
-				      &ficr[i].value);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Couldn't read %" PRIx32, ficr[i].address);
-			return res;
-		}
-	}
-
-	for (size_t i = 0; i < ARRAY_SIZE(uicr); i++) {
-		res = target_read_u32(chip->target, uicr[i].address,
-				      &uicr[i].value);
-		if (res != ERROR_OK) {
-			LOG_ERROR("Couldn't read %" PRIx32, uicr[i].address);
-			return res;
-		}
-	}
-
-	command_print(CMD,
-		 "\n[factory information control block]\n\n"
-		 "code page size: %"PRIu32"B\n"
-		 "code memory size: %"PRIu32"kB\n"
-		 "code region 0 size: %"PRIu32"kB\n"
-		 "pre-programmed code: %s\n"
-		 "number of ram blocks: %"PRIu32"\n"
-		 "ram block 0 size: %"PRIu32"B\n"
-		 "ram block 1 size: %"PRIu32"B\n"
-		 "ram block 2 size: %"PRIu32"B\n"
-		 "ram block 3 size: %"PRIu32 "B\n"
-		 "config id: %" PRIx32 "\n"
-		 "device id: 0x%"PRIx32"%08"PRIx32"\n"
-		 "encryption root: 0x%08"PRIx32"%08"PRIx32"%08"PRIx32"%08"PRIx32"\n"
-		 "identity root: 0x%08"PRIx32"%08"PRIx32"%08"PRIx32"%08"PRIx32"\n"
-		 "device address type: 0x%"PRIx32"\n"
-		 "device address: 0x%"PRIx32"%08"PRIx32"\n"
-		 "override enable: %"PRIx32"\n"
-		 "NRF_1MBIT values: %"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32"\n"
-		 "BLE_1MBIT values: %"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32" %"PRIx32"\n"
-		 "\n[user information control block]\n\n"
-		 "code region 0 size: %"PRIu32"kB\n"
-		 "read back protection configuration: %"PRIx32"\n"
-		 "reset value for XTALFREQ: %"PRIx32"\n"
-		 "firmware id: 0x%04"PRIx32,
-		 ficr[0].value,
-		 (ficr[1].value * ficr[0].value) / 1024,
-		 (ficr[2].value == 0xFFFFFFFF) ? 0 : ficr[2].value / 1024,
-		 ((ficr[3].value & 0xFF) == 0x00) ? "present" : "not present",
-		 ficr[4].value,
-		 ficr[5].value,
-		 (ficr[6].value == 0xFFFFFFFF) ? 0 : ficr[6].value,
-		 (ficr[7].value == 0xFFFFFFFF) ? 0 : ficr[7].value,
-		 (ficr[8].value == 0xFFFFFFFF) ? 0 : ficr[8].value,
-		 ficr[9].value,
-		 ficr[10].value, ficr[11].value,
-		 ficr[12].value, ficr[13].value, ficr[14].value, ficr[15].value,
-		 ficr[16].value, ficr[17].value, ficr[18].value, ficr[19].value,
-		 ficr[20].value,
-		 ficr[21].value, ficr[22].value,
-		 ficr[23].value,
-		 ficr[24].value, ficr[25].value, ficr[26].value, ficr[27].value, ficr[28].value,
-		 ficr[29].value, ficr[30].value, ficr[31].value, ficr[32].value, ficr[33].value,
-		 (uicr[0].value == 0xFFFFFFFF) ? 0 : uicr[0].value / 1024,
-		 uicr[1].value & 0xFFFF,
-		 uicr[2].value & 0xFF,
-		 uicr[3].value & 0xFFFF);
-
-	return ERROR_OK;
-}
 
 static const struct command_registration nrf5_exec_command_handlers[] = {
 	{
@@ -1385,13 +1232,6 @@ static const struct command_registration nrf5_exec_command_handlers[] = {
 		.help		= "Erase all flash contents of the chip.",
 		.usage		= "",
 	},
-	{
-		.name		= "info",
-		.handler	= nrf5_handle_info_command,
-		.mode		= COMMAND_EXEC,
-		.help		= "Show FICR and UICR info.",
-		.usage		= "",
-	},
 	COMMAND_REGISTRATION_DONE
 };
 
diff --git a/src/helper/binarybuffer.h b/src/helper/binarybuffer.h
index a8a5ef8..3446296 100644
--- a/src/helper/binarybuffer.h
+++ b/src/helper/binarybuffer.h
@@ -11,7 +11,8 @@
 #ifndef OPENOCD_HELPER_BINARYBUFFER_H
 #define OPENOCD_HELPER_BINARYBUFFER_H
 
-#include "list.h"
+#include <helper/list.h>
+#include <helper/types.h>
 
 /** @file
  * Support functions to access arbitrary bits in a byte array
diff --git a/src/helper/list.h b/src/helper/list.h
index c9de056..47290c2 100644
--- a/src/helper/list.h
+++ b/src/helper/list.h
@@ -1,15 +1,31 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
+/* SPDX-License-Identifier: BSD-2-Clause */
 
 /*
- * The content of this file is mainly copied/inspired from Linux kernel
- * code in include/linux/list.h, include/linux/types.h
- * Last aligned with kernel v5.12:
- * - skip the functions hlist_unhashed_lockless() and __list_del_clearprev()
- *   that are relevant only in kernel;
- * - Remove non-standard GCC extension "omitted conditional operand" from
- *   list_prepare_entry;
- * - expand READ_ONCE, WRITE_ONCE, smp_load_acquire, smp_store_release;
- * - make comments compatible with doxygen.
+ * Copyright (c) 2010 Isilon Systems, Inc.
+ * Copyright (c) 2010 iX Systems, Inc.
+ * Copyright (c) 2010 Panasas, Inc.
+ * Copyright (c) 2013-2016 Mellanox Technologies, Ltd.
+ * Copyright (c) 2021-2024 by Antonio Borneo <borneo.antonio@gmail.com>
+ * All rights reserved.
+ */
+
+/*
+ * Circular doubly linked list implementation.
+ *
+ * The content of this file is mainly copied/inspired from FreeBSD code in:
+ * https://cgit.freebsd.org/src/tree/
+ * files:
+ * sys/compat/linuxkpi/common/include/linux/list.h
+ * sys/compat/linuxkpi/common/include/linux/types.h
+ *
+ * Last aligned with release/13.3.0:
+ *
+ * - Skip 'hlist_*' double linked lists with a single pointer list head.
+ * - Expand WRITE_ONCE().
+ * - Use TAB for indentation.
+ * - Put macro arguments within parenthesis.
+ * - Remove blank lines after an open brace '{'.
+ * - Remove multiple assignment.
  *
  * There is an example of using this file in contrib/list_example.c.
  */
@@ -17,155 +33,64 @@
 #ifndef OPENOCD_HELPER_LIST_H
 #define OPENOCD_HELPER_LIST_H
 
-/* begin local changes */
-#include <helper/types.h>
+/* begin OpenOCD changes */
 
-#define LIST_POISON1 NULL
-#define LIST_POISON2 NULL
+#include <stddef.h>
 
 struct list_head {
-	struct list_head *next, *prev;
+	struct list_head *next;
+	struct list_head *prev;
 };
 
-/* end local changes */
-
-/*
- * Circular doubly linked list implementation.
- *
- * Some of the internal functions ("__xxx") are useful when
- * manipulating whole lists rather than single entries, as
- * sometimes we already know the next/prev entries and we can
- * generate better code by using them directly rather than
- * using the generic single-entry routines.
- */
+/* end OpenOCD changes */
 
 #define LIST_HEAD_INIT(name) { &(name), &(name) }
 
 #define LIST_HEAD(name) \
 	struct list_head name = LIST_HEAD_INIT(name)
 
-/**
- * INIT_LIST_HEAD - Initialize a list_head structure
- * @param list list_head structure to be initialized.
- *
- * Initializes the list_head to point to itself.  If it is a list header,
- * the result is an empty list.
- */
-static inline void INIT_LIST_HEAD(struct list_head *list)
+static inline void
+INIT_LIST_HEAD(struct list_head *list)
 {
 	list->next = list;
 	list->prev = list;
 }
 
-#ifdef CONFIG_DEBUG_LIST
-extern bool __list_add_valid(struct list_head *new,
-			      struct list_head *prev,
-			      struct list_head *next);
-extern bool __list_del_entry_valid(struct list_head *entry);
-#else
-static inline bool __list_add_valid(struct list_head *new,
-				struct list_head *prev,
-				struct list_head *next)
+static inline int
+list_empty(const struct list_head *head)
 {
-	return true;
+	return (head->next == head);
 }
-static inline bool __list_del_entry_valid(struct list_head *entry)
-{
-	return true;
-}
-#endif
 
-/*
- * Insert a new entry between two known consecutive entries.
- *
- * This is only for internal list manipulation where we know
- * the prev/next entries already!
- */
-static inline void __list_add(struct list_head *new,
-			      struct list_head *prev,
-			      struct list_head *next)
-{
-	if (!__list_add_valid(new, prev, next))
-		return;
-
-	next->prev = new;
-	new->next = next;
-	new->prev = prev;
-	prev->next = new;
-}
-
-/**
- * list_add - add a new entry
- * @param new new entry to be added
- * @param head list head to add it after
- *
- * Insert a new entry after the specified head.
- * This is good for implementing stacks.
- */
-static inline void list_add(struct list_head *new, struct list_head *head)
+static inline int
+list_empty_careful(const struct list_head *head)
 {
-	__list_add(new, head, head->next);
-}
-
+	struct list_head *next = head->next;
 
-/**
- * list_add_tail - add a new entry
- * @param new new entry to be added
- * @param head list head to add it before
- *
- * Insert a new entry before the specified head.
- * This is useful for implementing queues.
- */
-static inline void list_add_tail(struct list_head *new, struct list_head *head)
-{
-	__list_add(new, head->prev, head);
+	return ((next == head) && (next == head->prev));
 }
 
-/*
- * Delete a list entry by making the prev/next entries
- * point to each other.
- *
- * This is only for internal list manipulation where we know
- * the prev/next entries already!
- */
-static inline void __list_del(struct list_head *prev, struct list_head *next)
+static inline void
+__list_del(struct list_head *prev, struct list_head *next)
 {
 	next->prev = prev;
 	prev->next = next;
 }
 
-/* Ignore kernel __list_del_clearprev() */
-
-static inline void __list_del_entry(struct list_head *entry)
+static inline void
+__list_del_entry(struct list_head *entry)
 {
-	if (!__list_del_entry_valid(entry))
-		return;
-
 	__list_del(entry->prev, entry->next);
 }
 
-/**
- * list_del - deletes entry from list.
- * @param entry the element to delete from the list.
- * Note: list_empty() on entry does not return true after this, the entry is
- * in an undefined state.
- */
-static inline void list_del(struct list_head *entry)
+static inline void
+list_del(struct list_head *entry)
 {
-	__list_del_entry(entry);
-	entry->next = LIST_POISON1;
-	entry->prev = LIST_POISON2;
+	__list_del(entry->prev, entry->next);
 }
 
-/**
- * list_replace - replace old entry by new one
- * @param old the element to be replaced
- * @param new the new element to insert
- *
- * If @a old was empty, it will be overwritten.
- */
-static inline void list_replace(struct list_head *old,
-				struct list_head *new)
+static inline void
+list_replace(struct list_head *old, struct list_head *new)
 {
 	new->next = old->next;
 	new->next->prev = new;
@@ -173,205 +98,189 @@ static inline void list_replace(struct list_head *old,
 	new->prev->next = new;
 }
 
-/**
- * list_replace_init - replace old entry by new one and initialize the old one
- * @param old the element to be replaced
- * @param new the new element to insert
- *
- * If @a old was empty, it will be overwritten.
- */
-static inline void list_replace_init(struct list_head *old,
-				     struct list_head *new)
+static inline void
+list_replace_init(struct list_head *old, struct list_head *new)
 {
 	list_replace(old, new);
 	INIT_LIST_HEAD(old);
 }
 
-/**
- * list_swap - replace entry1 with entry2 and re-add entry1 at entry2's position
- * @param entry1 the location to place entry2
- * @param entry2 the location to place entry1
- */
-static inline void list_swap(struct list_head *entry1,
-			     struct list_head *entry2)
+static inline void
+linux_list_add(struct list_head *new, struct list_head *prev,
+	struct list_head *next)
 {
-	struct list_head *pos = entry2->prev;
-
-	list_del(entry2);
-	list_replace(entry1, entry2);
-	if (pos == entry1)
-		pos = entry2;
-	list_add(entry1, pos);
+	next->prev = new;
+	new->next = next;
+	new->prev = prev;
+	prev->next = new;
 }
 
-/**
- * list_del_init - deletes entry from list and reinitialize it.
- * @param entry the element to delete from the list.
- */
-static inline void list_del_init(struct list_head *entry)
+static inline void
+list_del_init(struct list_head *entry)
 {
-	__list_del_entry(entry);
+	list_del(entry);
 	INIT_LIST_HEAD(entry);
 }
 
-/**
- * list_move - delete from one list and add as another's head
- * @param list the entry to move
- * @param head the head that will precede our entry
- */
-static inline void list_move(struct list_head *list, struct list_head *head)
+#define	list_entry(ptr, type, field)	container_of(ptr, type, field)
+
+#define	list_first_entry(ptr, type, member) \
+	list_entry((ptr)->next, type, member)
+
+#define	list_last_entry(ptr, type, member)	\
+	list_entry((ptr)->prev, type, member)
+
+#define	list_first_entry_or_null(ptr, type, member) \
+	(!list_empty(ptr) ? list_first_entry(ptr, type, member) : NULL)
+
+#define	list_next_entry(ptr, member)					\
+	list_entry(((ptr)->member.next), typeof(*(ptr)), member)
+
+#define	list_safe_reset_next(ptr, n, member) \
+	(n) = list_next_entry(ptr, member)
+
+#define	list_prev_entry(ptr, member)					\
+	list_entry(((ptr)->member.prev), typeof(*(ptr)), member)
+
+#define	list_for_each(p, head)						\
+	for (p = (head)->next; p != (head); p = (p)->next)
+
+#define	list_for_each_safe(p, n, head)					\
+	for (p = (head)->next, n = (p)->next; p != (head); p = n, n = (p)->next)
+
+#define list_for_each_entry(p, h, field)				\
+	for (p = list_entry((h)->next, typeof(*p), field); &(p)->field != (h); \
+	    p = list_entry((p)->field.next, typeof(*p), field))
+
+#define list_for_each_entry_safe(p, n, h, field)			\
+	for (p = list_entry((h)->next, typeof(*p), field),		\
+	    n = list_entry((p)->field.next, typeof(*p), field); &(p)->field != (h);\
+	    p = n, n = list_entry(n->field.next, typeof(*n), field))
+
+#define	list_for_each_entry_from(p, h, field) \
+	for ( ; &(p)->field != (h); \
+	    p = list_entry((p)->field.next, typeof(*p), field))
+
+#define	list_for_each_entry_continue(p, h, field)			\
+	for (p = list_next_entry((p), field); &(p)->field != (h);	\
+	    p = list_next_entry((p), field))
+
+#define	list_for_each_entry_safe_from(pos, n, head, member)			\
+	for (n = list_entry((pos)->member.next, typeof(*pos), member);		\
+	     &(pos)->member != (head);						\
+	     pos = n, n = list_entry(n->member.next, typeof(*n), member))
+
+#define	list_for_each_entry_reverse(p, h, field)			\
+	for (p = list_entry((h)->prev, typeof(*p), field); &(p)->field != (h); \
+	    p = list_entry((p)->field.prev, typeof(*p), field))
+
+#define	list_for_each_entry_safe_reverse(p, n, h, field)		\
+	for (p = list_entry((h)->prev, typeof(*p), field),		\
+	    n = list_entry((p)->field.prev, typeof(*p), field); &(p)->field != (h); \
+	    p = n, n = list_entry(n->field.prev, typeof(*n), field))
+
+#define	list_for_each_entry_continue_reverse(p, h, field) \
+	for (p = list_entry((p)->field.prev, typeof(*p), field); &(p)->field != (h); \
+	    p = list_entry((p)->field.prev, typeof(*p), field))
+
+#define	list_for_each_prev(p, h) for (p = (h)->prev; p != (h); p = (p)->prev)
+
+#define	list_for_each_entry_from_reverse(p, h, field)	\
+	for (; &p->field != (h);			\
+	     p = list_prev_entry(p, field))
+
+static inline void
+list_add(struct list_head *new, struct list_head *head)
 {
-	__list_del_entry(list);
-	list_add(list, head);
+	linux_list_add(new, head, head->next);
 }
 
-/**
- * list_move_tail - delete from one list and add as another's tail
- * @param list the entry to move
- * @param head the head that will follow our entry
- */
-static inline void list_move_tail(struct list_head *list,
-				  struct list_head *head)
+static inline void
+list_add_tail(struct list_head *new, struct list_head *head)
 {
-	__list_del_entry(list);
-	list_add_tail(list, head);
+	linux_list_add(new, head->prev, head);
 }
 
-/**
- * list_bulk_move_tail - move a subsection of a list to its tail
- * @param head  the head that will follow our entry
- * @param first the first entry to move
- * @param last  the last entry to move, can be the same as first
- *
- * Move all entries between @a first and including @a last before @a head.
- * All three entries must belong to the same linked list.
- */
-static inline void list_bulk_move_tail(struct list_head *head,
-				       struct list_head *first,
-				       struct list_head *last)
+static inline void
+list_move(struct list_head *list, struct list_head *head)
 {
-	first->prev->next = last->next;
-	last->next->prev = first->prev;
-
-	head->prev->next = first;
-	first->prev = head->prev;
-
-	last->next = head;
-	head->prev = last;
+	list_del(list);
+	list_add(list, head);
 }
 
-/**
- * list_is_first -- tests whether @a list is the first entry in list @a head
- * @param list the entry to test
- * @param head the head of the list
- */
-static inline int list_is_first(const struct list_head *list, const struct list_head *head)
+static inline void
+list_move_tail(struct list_head *entry, struct list_head *head)
 {
-	return list->prev == head;
+	list_del(entry);
+	list_add_tail(entry, head);
 }
 
-/**
- * list_is_last - tests whether @a list is the last entry in list @a head
- * @param list the entry to test
- * @param head the head of the list
- */
-static inline int list_is_last(const struct list_head *list, const struct list_head *head)
+static inline void
+list_rotate_to_front(struct list_head *entry, struct list_head *head)
 {
-	return list->next == head;
+	list_move_tail(entry, head);
 }
 
-/**
- * list_is_head - tests whether @a list is the list @a head
- * @param list the entry to test
- * @param head the head of the list
- */
-static inline int list_is_head(const struct list_head *list, const struct list_head *head)
+static inline void
+list_bulk_move_tail(struct list_head *head, struct list_head *first,
+	struct list_head *last)
 {
-	return list == head;
+	first->prev->next = last->next;
+	last->next->prev = first->prev;
+	head->prev->next = first;
+	first->prev = head->prev;
+	last->next = head;
+	head->prev = last;
 }
 
-/**
- * list_empty - tests whether a list is empty
- * @param head the list to test.
- */
-static inline int list_empty(const struct list_head *head)
+static inline void
+linux_list_splice(const struct list_head *list, struct list_head *prev,
+	struct list_head *next)
 {
-	return head->next == head;
+	struct list_head *first;
+	struct list_head *last;
+
+	if (list_empty(list))
+		return;
+	first = list->next;
+	last = list->prev;
+	first->prev = prev;
+	prev->next = first;
+	last->next = next;
+	next->prev = last;
 }
 
-/**
- * list_del_init_careful - deletes entry from list and reinitialize it.
- * @param entry the element to delete from the list.
- *
- * This is the same as list_del_init(), except designed to be used
- * together with list_empty_careful() in a way to guarantee ordering
- * of other memory operations.
- *
- * Any memory operations done before a list_del_init_careful() are
- * guaranteed to be visible after a list_empty_careful() test.
- */
-static inline void list_del_init_careful(struct list_head *entry)
+static inline void
+list_splice(const struct list_head *list, struct list_head *head)
 {
-	__list_del_entry(entry);
-	entry->prev = entry;
-	entry->next = entry;
+	linux_list_splice(list, head, head->next);
 }
 
-/**
- * list_empty_careful - tests whether a list is empty and not being modified
- * @param head the list to test
- *
- * Description:
- * tests whether a list is empty _and_ checks that no other CPU might be
- * in the process of modifying either member (next or prev)
- *
- * NOTE: using list_empty_careful() without synchronization
- * can only be safe if the only activity that can happen
- * to the list entry is list_del_init(). Eg. it cannot be used
- * if another CPU could re-list_add() it.
- */
-static inline int list_empty_careful(const struct list_head *head)
+static inline void
+list_splice_tail(struct list_head *list, struct list_head *head)
 {
-	struct list_head *next = head->next;
-	return (next == head) && (next == head->prev);
+	linux_list_splice(list, head->prev, head);
 }
 
-/**
- * list_rotate_left - rotate the list to the left
- * @param head the head of the list
- */
-static inline void list_rotate_left(struct list_head *head)
+static inline void
+list_splice_init(struct list_head *list, struct list_head *head)
 {
-	struct list_head *first;
-
-	if (!list_empty(head)) {
-		first = head->next;
-		list_move_tail(first, head);
-	}
+	linux_list_splice(list, head, head->next);
+	INIT_LIST_HEAD(list);
 }
 
-/**
- * list_rotate_to_front() - Rotate list to specific item.
- * @param list The desired new front of the list.
- * @param head The head of the list.
- *
- * Rotates list so that @a list becomes the new front of the list.
- */
-static inline void list_rotate_to_front(struct list_head *list,
-					struct list_head *head)
+static inline void
+list_splice_tail_init(struct list_head *list, struct list_head *head)
 {
-	/*
-	 * Deletes the list head from the list denoted by @a head and
-	 * places it as the tail of @a list, this effectively rotates the
-	 * list so that @a list is at the front.
-	 */
-	list_move_tail(head, list);
+	linux_list_splice(list, head->prev, head);
+	INIT_LIST_HEAD(list);
 }
 
-/**
- * list_is_singular - tests whether a list has just one entry.
- * @param head the list to test.
+/*
+ * Double linked lists with a single pointer list head.
+ * IGNORED
  */
+
 static inline int list_is_singular(const struct list_head *head)
 {
 	return !list_empty(head) && (head->next == head->prev);
@@ -389,472 +298,79 @@ static inline void __list_cut_position(struct list_head *list,
 	new_first->prev = head;
 }
 
-/**
- * list_cut_position - cut a list into two
- * @param list a new list to add all removed entries
- * @param head a list with entries
- * @param entry an entry within head, could be the head itself
- *	and if so we won't cut the list
- *
- * This helper moves the initial part of @a head, up to and
- * including @a entry, from @a head to @a list. You should
- * pass on @a entry an element you know is on @a head. @a list
- * should be an empty list or a list you do not care about
- * losing its data.
- *
- */
 static inline void list_cut_position(struct list_head *list,
 		struct list_head *head, struct list_head *entry)
 {
 	if (list_empty(head))
 		return;
-	if (list_is_singular(head) && !list_is_head(entry, head) && (entry != head->next))
+	if (list_is_singular(head) &&
+		(head->next != entry && head != entry))
 		return;
-	if (list_is_head(entry, head))
+	if (entry == head)
 		INIT_LIST_HEAD(list);
 	else
 		__list_cut_position(list, head, entry);
 }
 
-/**
- * list_cut_before - cut a list into two, before given entry
- * @param list  a new list to add all removed entries
- * @param head  a list with entries
- * @param entry an entry within head, could be the head itself
- *
- * This helper moves the initial part of @a head, up to but
- * excluding @a entry, from @a head to @a list.  You should pass
- * in @a entry an element you know is on @a head.  @a list should
- * be an empty list or a list you do not care about losing
- * its data.
- * If @a entry == @a head, all entries on @a head are moved to
- * @a list.
- */
-static inline void list_cut_before(struct list_head *list,
-				   struct list_head *head,
-				   struct list_head *entry)
+static inline int list_is_first(const struct list_head *list,
+				const struct list_head *head)
 {
-	if (head->next == entry) {
-		INIT_LIST_HEAD(list);
-		return;
-	}
-	list->next = head->next;
-	list->next->prev = list;
-	list->prev = entry->prev;
-	list->prev->next = list;
-	head->next = entry;
-	entry->prev = head;
+	return (list->prev == head);
 }
 
-static inline void __list_splice(const struct list_head *list,
-				 struct list_head *prev,
-				 struct list_head *next)
+static inline int list_is_last(const struct list_head *list,
+				const struct list_head *head)
 {
-	struct list_head *first = list->next;
-	struct list_head *last = list->prev;
-
-	first->prev = prev;
-	prev->next = first;
-
-	last->next = next;
-	next->prev = last;
-}
-
-/**
- * list_splice - join two lists, this is designed for stacks
- * @param list the new list to add.
- * @param head the place to add it in the first list.
- */
-static inline void list_splice(const struct list_head *list,
-				struct list_head *head)
-{
-	if (!list_empty(list))
-		__list_splice(list, head, head->next);
+	return list->next == head;
 }
 
-/**
- * list_splice_tail - join two lists, each list being a queue
- * @param list the new list to add.
- * @param head the place to add it in the first list.
- */
-static inline void list_splice_tail(struct list_head *list,
-				struct list_head *head)
+static inline size_t
+list_count_nodes(const struct list_head *list)
 {
-	if (!list_empty(list))
-		__list_splice(list, head->prev, head);
-}
+	const struct list_head *lh;
+	size_t count;
 
-/**
- * list_splice_init - join two lists and reinitialise the emptied list.
- * @param list the new list to add.
- * @param head the place to add it in the first list.
- *
- * The list at @a list is reinitialised
- */
-static inline void list_splice_init(struct list_head *list,
-				    struct list_head *head)
-{
-	if (!list_empty(list)) {
-		__list_splice(list, head, head->next);
-		INIT_LIST_HEAD(list);
+	count = 0;
+	list_for_each(lh, list) {
+		count++;
 	}
-}
 
-/**
- * list_splice_tail_init - join two lists and reinitialise the emptied list
- * @param list the new list to add.
- * @param head the place to add it in the first list.
- *
- * Each of the lists is a queue.
- * The list at @a list is reinitialised
- */
-static inline void list_splice_tail_init(struct list_head *list,
-					 struct list_head *head)
-{
-	if (!list_empty(list)) {
-		__list_splice(list, head->prev, head);
-		INIT_LIST_HEAD(list);
-	}
+	return (count);
 }
 
-/**
- * list_entry - get the struct for this entry
- * @param ptr    the &struct list_head pointer.
- * @param type   the type of the struct this is embedded in.
- * @param member the name of the list_head within the struct.
- */
-#define list_entry(ptr, type, member) \
-	container_of(ptr, type, member)
-
-/**
- * list_first_entry - get the first element from a list
- * @param ptr    the list head to take the element from.
- * @param type   the type of the struct this is embedded in.
- * @param member the name of the list_head within the struct.
- *
- * Note, that list is expected to be not empty.
- */
-#define list_first_entry(ptr, type, member) \
-	list_entry((ptr)->next, type, member)
-
-/**
- * list_last_entry - get the last element from a list
- * @param ptr    the list head to take the element from.
- * @param type   the type of the struct this is embedded in.
- * @param member the name of the list_head within the struct.
- *
- * Note, that list is expected to be not empty.
- */
-#define list_last_entry(ptr, type, member) \
-	list_entry((ptr)->prev, type, member)
-
-/**
- * list_first_entry_or_null - get the first element from a list
- * @param ptr    the list head to take the element from.
- * @param type   the type of the struct this is embedded in.
- * @param member the name of the list_head within the struct.
- *
- * Note that if the list is empty, it returns NULL.
- */
-#define list_first_entry_or_null(ptr, type, member) ({ \
-	struct list_head *head__ = (ptr); \
-	struct list_head *pos__ = head__->next; \
-	pos__ != head__ ? list_entry(pos__, type, member) : NULL; \
-})
-
-/**
- * list_next_entry - get the next element in list
- * @param pos    the type * to cursor
- * @param member the name of the list_head within the struct.
- */
-#define list_next_entry(pos, member) \
-	list_entry((pos)->member.next, typeof(*(pos)), member)
-
-/**
- * list_next_entry_circular - get the next element in list
- * @param pos    the type * to cursor.
- * @param head   the list head to take the element from.
- * @param member the name of the list_head within the struct.
- *
- * Wraparound if pos is the last element (return the first element).
- * Note, that list is expected to be not empty.
- */
-#define list_next_entry_circular(pos, head, member) \
-	(list_is_last(&(pos)->member, head) ? \
-	list_first_entry(head, typeof(*(pos)), member) : list_next_entry(pos, member))
-
-/**
- * list_prev_entry - get the prev element in list
- * @param pos    the type * to cursor
- * @param member the name of the list_head within the struct.
- */
-#define list_prev_entry(pos, member) \
-	list_entry((pos)->member.prev, typeof(*(pos)), member)
-
-/**
- * list_prev_entry_circular - get the prev element in list
- * @param pos    the type * to cursor.
- * @param head   the list head to take the element from.
- * @param member the name of the list_head within the struct.
- *
- * Wraparound if pos is the first element (return the last element).
- * Note, that list is expected to be not empty.
- */
-#define list_prev_entry_circular(pos, head, member) \
-	(list_is_first(&(pos)->member, head) ? \
-	list_last_entry(head, typeof(*(pos)), member) : list_prev_entry(pos, member))
-
-/**
- * list_for_each	-	iterate over a list
- * @param pos  the &struct list_head to use as a loop cursor.
- * @param head the head for your list.
- */
-#define list_for_each(pos, head) \
-	for (pos = (head)->next; !list_is_head(pos, (head)); pos = pos->next)
-
-/* Ignore kernel list_for_each_rcu() */
-
-/**
- * list_for_each_continue - continue iteration over a list
- * @param pos  the &struct list_head to use as a loop cursor.
- * @param head the head for your list.
- *
- * Continue to iterate over a list, continuing after the current position.
+/*
+ * Double linked lists with a single pointer list head.
+ * IGNORED
  */
-#define list_for_each_continue(pos, head) \
-	for (pos = pos->next; pos != (head); pos = pos->next)
 
-/**
- * list_for_each_prev	-	iterate over a list backwards
- * @param pos  the &struct list_head to use as a loop cursor.
- * @param head the head for your list.
- */
-#define list_for_each_prev(pos, head) \
-	for (pos = (head)->prev; pos != (head); pos = pos->prev)
+/* begin OpenOCD extensions */
 
 /**
- * list_for_each_safe - iterate over a list safe against removal of list entry
- * @param pos  the &struct list_head to use as a loop cursor.
- * @param n    another &struct list_head to use as temporary storage
- * @param head the head for your list.
- */
-#define list_for_each_safe(pos, n, head) \
-	for (pos = (head)->next, n = pos->next; pos != (head); \
-		pos = n, n = pos->next)
-
-/**
- * list_for_each_prev_safe - iterate over a list backwards safe against removal of list entry
- * @param pos  the &struct list_head to use as a loop cursor.
- * @param n    another &struct list_head to use as temporary storage
- * @param head the head for your list.
+ * list_for_each_entry_direction - iterate forward/backward over list of given type
+ * @param is_fwd the iterate direction, true for forward, false for backward.
+ * @param p      the type * to use as a loop cursor.
+ * @param h      the head of the list.
+ * @param field  the name of the list_head within the struct.
  */
-#define list_for_each_prev_safe(pos, n, head) \
-	for (pos = (head)->prev, n = pos->prev; \
-	     pos != (head); \
-	     pos = n, n = pos->prev)
+#define list_for_each_entry_direction(is_fwd, p, h, field)					\
+	for (p = list_entry(is_fwd ? (h)->next : (h)->prev, typeof(*p), field);	\
+		&(p)->field != (h);													\
+		p = list_entry(is_fwd ? (p)->field.next : (p)->field.prev, typeof(*p), field))
 
 /**
- * list_count_nodes - count nodes in the list
- * @param head	the head for your list.
+ * list_rotate_left - rotate the list to the left
+ * @param h the head of the list
  */
-static inline size_t list_count_nodes(struct list_head *head)
+static inline void list_rotate_left(struct list_head *h)
 {
-	struct list_head *pos;
-	size_t count = 0;
-
-	list_for_each(pos, head)
-		count++;
+	struct list_head *first;
 
-	return count;
+	if (!list_empty(h)) {
+		first = h->next;
+		list_move_tail(first, h);
+	}
 }
 
-/**
- * list_entry_is_head - test if the entry points to the head of the list
- * @param pos    the type * to cursor
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- */
-#define list_entry_is_head(pos, head, member)				\
-	(&pos->member == (head))
-
-/**
- * list_for_each_entry	-	iterate over list of given type
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- */
-#define list_for_each_entry(pos, head, member)				\
-	for (pos = list_first_entry(head, typeof(*pos), member);	\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = list_next_entry(pos, member))
-
-/**
- * list_for_each_entry_reverse - iterate backwards over list of given type.
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- */
-#define list_for_each_entry_reverse(pos, head, member)			\
-	for (pos = list_last_entry(head, typeof(*pos), member);		\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = list_prev_entry(pos, member))
-
-/**
- * list_for_each_entry_direction - iterate forward/backward over list of given type
- * @param forward the iterate direction, true for forward, false for backward.
- * @param pos     the type * to use as a loop cursor.
- * @param head    the head for your list.
- * @param member  the name of the list_head within the struct.
- */
-#define list_for_each_entry_direction(forward, pos, head, member)		\
-	for (pos = forward ? list_first_entry(head, typeof(*pos), member)	\
-					   : list_last_entry(head, typeof(*pos), member);	\
-		 !list_entry_is_head(pos, head, member);						\
-		 pos = forward ? list_next_entry(pos, member)					\
-					   : list_prev_entry(pos, member))
-
-/**
- * list_prepare_entry - prepare a pos entry for use in list_for_each_entry_continue()
- * @param pos    the type * to use as a start point
- * @param head   the head of the list
- * @param member the name of the list_head within the struct.
- *
- * Prepares a pos entry for use as a start point in list_for_each_entry_continue().
- */
-#define list_prepare_entry(pos, head, member) \
-	((pos) ? (pos) : list_entry(head, typeof(*pos), member))
-
-/**
- * list_for_each_entry_continue - continue iteration over list of given type
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Continue to iterate over list of given type, continuing after
- * the current position.
- */
-#define list_for_each_entry_continue(pos, head, member)		\
-	for (pos = list_next_entry(pos, member);			\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = list_next_entry(pos, member))
-
-/**
- * list_for_each_entry_continue_reverse - iterate backwards from the given point
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Start to iterate over list of given type backwards, continuing after
- * the current position.
- */
-#define list_for_each_entry_continue_reverse(pos, head, member)		\
-	for (pos = list_prev_entry(pos, member);			\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = list_prev_entry(pos, member))
-
-/**
- * list_for_each_entry_from - iterate over list of given type from the current point
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Iterate over list of given type, continuing from current position.
- */
-#define list_for_each_entry_from(pos, head, member)			\
-	for (; !list_entry_is_head(pos, head, member);			\
-	     pos = list_next_entry(pos, member))
-
-/**
- * list_for_each_entry_from_reverse - iterate backwards over list of given type
- *                                    from the current point
- * @param pos    the type * to use as a loop cursor.
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Iterate backwards over list of given type, continuing from current position.
- */
-#define list_for_each_entry_from_reverse(pos, head, member)		\
-	for (; !list_entry_is_head(pos, head, member);			\
-	     pos = list_prev_entry(pos, member))
-
-/**
- * list_for_each_entry_safe - iterate over list of given type safe against removal of list entry
- * @param pos    the type * to use as a loop cursor.
- * @param n      another type * to use as temporary storage
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- */
-#define list_for_each_entry_safe(pos, n, head, member)			\
-	for (pos = list_first_entry(head, typeof(*pos), member),	\
-		n = list_next_entry(pos, member);			\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = n, n = list_next_entry(n, member))
-
-/**
- * list_for_each_entry_safe_continue - continue list iteration safe against removal
- * @param pos    the type * to use as a loop cursor.
- * @param n      another type * to use as temporary storage
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Iterate over list of given type, continuing after current point,
- * safe against removal of list entry.
- */
-#define list_for_each_entry_safe_continue(pos, n, head, member)		\
-	for (pos = list_next_entry(pos, member),				\
-		n = list_next_entry(pos, member);				\
-	     !list_entry_is_head(pos, head, member);				\
-	     pos = n, n = list_next_entry(n, member))
-
-/**
- * list_for_each_entry_safe_from - iterate over list from current point safe against removal
- * @param pos    the type * to use as a loop cursor.
- * @param n      another type * to use as temporary storage
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Iterate over list of given type from current point, safe against
- * removal of list entry.
- */
-#define list_for_each_entry_safe_from(pos, n, head, member)			\
-	for (n = list_next_entry(pos, member);					\
-	     !list_entry_is_head(pos, head, member);				\
-	     pos = n, n = list_next_entry(n, member))
-
-/**
- * list_for_each_entry_safe_reverse - iterate backwards over list safe against removal
- * @param pos    the type * to use as a loop cursor.
- * @param n      another type * to use as temporary storage
- * @param head   the head for your list.
- * @param member the name of the list_head within the struct.
- *
- * Iterate backwards over list of given type, safe against removal
- * of list entry.
- */
-#define list_for_each_entry_safe_reverse(pos, n, head, member)		\
-	for (pos = list_last_entry(head, typeof(*pos), member),		\
-		n = list_prev_entry(pos, member);			\
-	     !list_entry_is_head(pos, head, member);			\
-	     pos = n, n = list_prev_entry(n, member))
-
-/**
- * list_safe_reset_next - reset a stale list_for_each_entry_safe loop
- * @param pos    the loop cursor used in the list_for_each_entry_safe loop
- * @param n      temporary storage used in list_for_each_entry_safe
- * @param member the name of the list_head within the struct.
- *
- * list_safe_reset_next is not safe to use in general if the list may be
- * modified concurrently (eg. the lock is dropped in the loop body). An
- * exception to this is if the cursor element (pos) is pinned in the list,
- * and list_safe_reset_next is called after re-taking the lock and before
- * completing the current iteration of the loop body.
- */
-#define list_safe_reset_next(pos, n, member)				\
-	n = list_next_entry(pos, member)
-
-/*
- * Double linked lists with a single pointer list head.
- * IGNORED
- */
+/* end OpenOCD extensions */
 
 #endif /* OPENOCD_HELPER_LIST_H */
diff --git a/src/helper/log.h b/src/helper/log.h
index ee71bf0..818716a 100644
--- a/src/helper/log.h
+++ b/src/helper/log.h
@@ -114,6 +114,15 @@ extern int debug_level;
 				expr); \
 	} while (0)
 
+#define LOG_CUSTOM_LEVEL(level, expr ...) \
+	do { \
+		enum log_levels _level = level; \
+		if (debug_level >= _level) \
+			log_printf_lf(_level, \
+				__FILE__, __LINE__, __func__, \
+				expr); \
+	} while (0)
+
 #define LOG_INFO(expr ...) \
 	log_printf_lf(LOG_LVL_INFO, __FILE__, __LINE__, __func__, expr)
 
diff --git a/src/helper/options.c b/src/helper/options.c
index 05cde67..61a1014 100644
--- a/src/helper/options.c
+++ b/src/helper/options.c
@@ -341,6 +341,10 @@ int parse_cmdline_args(struct command_context *cmd_ctx, int argc, char *argv[])
 		exit(0);
 	}
 
+	/* dump full command line */
+	for (int i = 0; i < argc; i++)
+		LOG_DEBUG("ARGV[%d] = \"%s\"", i, argv[i]);
+
 	/* paths specified on the command line take precedence over these
 	 * built-in paths
 	 */
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 8df5176..3d839e6 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -20,6 +20,11 @@
 #include <jtag/interface.h>
 #include <jtag/commands.h>
 
+#include <helper/time_support.h>
+
+/* Timeout for retrying on SWD WAIT in msec */
+#define SWD_WAIT_TIMEOUT 500
+
 /**
  * Function bitbang_stableclocks
  * issues a number of clock cycles while staying in a stable state.
@@ -474,7 +479,8 @@ static void bitbang_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay
 		return;
 	}
 
-	for (;;) {
+	int64_t timeout = timeval_ms() + SWD_WAIT_TIMEOUT;
+	for (unsigned int retry = 0;; retry++) {
 		uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)];
 
 		cmd |= SWD_CMD_START | SWD_CMD_PARK;
@@ -488,16 +494,25 @@ static void bitbang_swd_read_reg(uint8_t cmd, uint32_t *value, uint32_t ap_delay
 		uint32_t data = buf_get_u32(trn_ack_data_parity_trn, 1 + 3, 32);
 		int parity = buf_get_u32(trn_ack_data_parity_trn, 1 + 3 + 32, 1);
 
-		LOG_DEBUG_IO("%s %s read reg %X = %08" PRIx32,
-			  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
-			  cmd & SWD_CMD_APNDP ? "AP" : "DP",
-			  (cmd & SWD_CMD_A32) >> 1,
-			  data);
+		LOG_CUSTOM_LEVEL((ack != SWD_ACK_OK && (retry == 0 || ack != SWD_ACK_WAIT))
+				? LOG_LVL_DEBUG : LOG_LVL_DEBUG_IO,
+			"%s %s read reg %X = %08" PRIx32,
+			ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+			cmd & SWD_CMD_APNDP ? "AP" : "DP",
+			(cmd & SWD_CMD_A32) >> 1,
+			data);
 
-		if (ack == SWD_ACK_WAIT) {
+		if (ack == SWD_ACK_WAIT && timeval_ms() <= timeout) {
 			swd_clear_sticky_errors();
+			if (retry > 20)
+				alive_sleep(1);
+
 			continue;
-		} else if (ack != SWD_ACK_OK) {
+		}
+		if (retry > 1)
+			LOG_DEBUG("SWD WAIT: retried %u times", retry);
+
+		if (ack != SWD_ACK_OK) {
 			queued_retval = swd_ack_to_error_code(ack);
 			return;
 		}
@@ -524,12 +539,14 @@ static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay
 		return;
 	}
 
+	int64_t timeout = timeval_ms() + SWD_WAIT_TIMEOUT;
+
 	/* Devices do not reply to DP_TARGETSEL write cmd, ignore received ack */
 	bool check_ack = swd_cmd_returns_ack(cmd);
 
 	/* init the array to silence scan-build */
 	uint8_t trn_ack_data_parity_trn[DIV_ROUND_UP(4 + 3 + 32 + 1 + 4, 8)] = {0};
-	for (;;) {
+	for (unsigned int retry = 0;; retry++) {
 		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32, value);
 		buf_set_u32(trn_ack_data_parity_trn, 1 + 3 + 1 + 32, 1, parity_u32(value));
 
@@ -554,22 +571,29 @@ static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay
 		bitbang_swd_exchange(false, trn_ack_data_parity_trn, 1 + 3 + 1, 32 + 1);
 
 		int ack = buf_get_u32(trn_ack_data_parity_trn, 1, 3);
+		LOG_CUSTOM_LEVEL((check_ack && ack != SWD_ACK_OK && (retry == 0 || ack != SWD_ACK_WAIT))
+				? LOG_LVL_DEBUG : LOG_LVL_DEBUG_IO,
+			"%s%s %s write reg %X = %08" PRIx32,
+			check_ack ? "" : "ack ignored ",
+			ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
+			cmd & SWD_CMD_APNDP ? "AP" : "DP",
+			(cmd & SWD_CMD_A32) >> 1,
+			buf_get_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32));
+
+		if (check_ack && ack == SWD_ACK_WAIT && timeval_ms() <= timeout) {
+			swd_clear_sticky_errors();
+			if (retry > 20)
+				alive_sleep(1);
 
-		LOG_DEBUG_IO("%s%s %s write reg %X = %08" PRIx32,
-			  check_ack ? "" : "ack ignored ",
-			  ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
-			  cmd & SWD_CMD_APNDP ? "AP" : "DP",
-			  (cmd & SWD_CMD_A32) >> 1,
-			  buf_get_u32(trn_ack_data_parity_trn, 1 + 3 + 1, 32));
-
-		if (check_ack) {
-			if (ack == SWD_ACK_WAIT) {
-				swd_clear_sticky_errors();
-				continue;
-			} else if (ack != SWD_ACK_OK) {
-				queued_retval = swd_ack_to_error_code(ack);
-				return;
-			}
+			continue;
+		}
+
+		if (retry > 1)
+			LOG_DEBUG("SWD WAIT: retried %u times", retry);
+
+		if (check_ack && ack != SWD_ACK_OK) {
+			queued_retval = swd_ack_to_error_code(ack);
+			return;
 		}
 
 		if (cmd & SWD_CMD_APNDP)
diff --git a/src/jtag/drivers/cmsis_dap.c b/src/jtag/drivers/cmsis_dap.c
index 341d35c..d7367d8 100644
--- a/src/jtag/drivers/cmsis_dap.c
+++ b/src/jtag/drivers/cmsis_dap.c
@@ -944,7 +944,7 @@ static void cmsis_dap_swd_read_process(struct cmsis_dap *dap, enum cmsis_dap_blo
 	if (ack != SWD_ACK_OK) {
 		LOG_DEBUG("SWD ack not OK @ %d %s", transfer_count,
 			  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
-		queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+		queued_retval = swd_ack_to_error_code(ack);
 		/* TODO: use results of transfers completed before the error occurred? */
 		goto skip;
 	}
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 49762bc..a0b120b 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -1506,7 +1506,8 @@ static int ftdi_swd_run_queue(void)
 		/* Devices do not reply to DP_TARGETSEL write cmd, ignore received ack */
 		bool check_ack = swd_cmd_returns_ack(swd_cmd_queue[i].cmd);
 
-		LOG_DEBUG_IO("%s%s %s %s reg %X = %08"PRIx32,
+		LOG_CUSTOM_LEVEL((check_ack && ack != SWD_ACK_OK) ? LOG_LVL_DEBUG : LOG_LVL_DEBUG_IO,
+				"%s%s %s %s reg %X = %08" PRIx32,
 				check_ack ? "" : "ack ignored ",
 				ack == SWD_ACK_OK ? "OK" : ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK",
 				swd_cmd_queue[i].cmd & SWD_CMD_APNDP ? "AP" : "DP",
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index c0d2adc..98b0d16 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -782,7 +782,7 @@ static int kitprog_swd_run_queue(void)
 			if (ack != SWD_ACK_OK || (buffer[read_index] & 0x08)) {
 				LOG_DEBUG("SWD ack not OK: %d %s", i,
 					  ack == SWD_ACK_WAIT ? "WAIT" : ack == SWD_ACK_FAULT ? "FAULT" : "JUNK");
-				queued_retval = ack == SWD_ACK_WAIT ? ERROR_WAIT : ERROR_FAIL;
+				queued_retval = swd_ack_to_error_code(ack);
 				break;
 			}
 			read_index++;
diff --git a/src/jtag/startup.tcl b/src/jtag/startup.tcl
index 4eca677..41db38e 100644
--- a/src/jtag/startup.tcl
+++ b/src/jtag/startup.tcl
@@ -1146,4 +1146,132 @@ proc "pld device" {driver tap_name {opt 0}} {
 	}
 }
 
+lappend _telnet_autocomplete_skip "ipdbg -start"
+proc "ipdbg -start" {args} {
+	echo "DEPRECATED! use 'ipdbg create-hub' and 'chip.ipdbghub ipdbg start ...', not 'ipdbg -start ...'"
+	set tap_name ""
+	set pld_name ""
+	set tool_num "1"
+	set port_num "4242"
+	set idx 0
+	set num_args [llength $args]
+	while {$idx < $num_args} {
+		set arg [lindex $args $idx]
+		switch -- $arg {
+			"-tap" {
+				incr idx
+				if {$idx >= $num_args || [string index [lindex $args $idx] 0] == "-"} {
+					echo "no TAP name given"
+					return
+				}
+				set tap_name [lindex $args $idx]
+			}
+			"-pld" {
+				incr idx
+				if {$idx >= $num_args || [string index [lindex $args $idx] 0] == "-"} {
+					echo "no PLD name given"
+					return
+				}
+				set pld_name [lindex $args $idx]
+			}
+			"-tool" {
+				if {[expr {$idx + 1}] < $num_args && [string index [lindex $args [expr {$idx + 1}]] 0] != "-"} {
+					set tool_num [lindex $args [expr {$idx + 1}]]
+					set args [lreplace $args [expr {$idx + 1}] [expr {$idx + 1}]]
+					incr num_args -1
+				}
+				set args [lreplace $args $idx $idx]
+				incr num_args -1
+				incr idx -1
+			}
+			"-port" {
+				if {[expr {$idx + 1}] < $num_args && [string index [lindex $args [expr {$idx + 1}]] 0] != "-"} {
+					set port_num [lindex $args [expr {$idx + 1}]]
+					set args [lreplace $args [expr {$idx + 1}] [expr {$idx + 1}]]
+					incr num_args -1
+				}
+				set args [lreplace $args $idx $idx]
+				incr num_args -1
+				incr idx -1
+			}
+			"-hub" {
+				set args [lreplace $args $idx $idx "-ir" ]
+			}
+			default {
+#				don't touch remaining arguments
+			}
+		}
+		incr idx
+	}
+
+	set hub_name ""
+	if {$tap_name != ""} {
+		set hub_name [lindex [split $tap_name .] 0].ipdbghub
+	} elseif {$pld_name != ""} {
+		set hub_name [lindex [split $pld_name .] 0].ipdbghub
+	} else {
+		echo "parsing arguments failed: no tap and no pld given."
+		return
+	}
+
+	echo "name: $hub_name"
+	echo "ipdbg create-hub $hub_name $args"
+
+	catch {eval ipdbg create-hub $hub_name $args}
+
+	eval $hub_name ipdbg start -tool $tool_num -port $port_num
+}
+
+lappend _telnet_autocomplete_skip "ipdbg -stop"
+proc "ipdbg -stop" {args} {
+	echo "DEPRECATED! use 'chip.ipdbghub ipdbg stop ...', not 'ipdbg -stop ...'"
+	set tap_name ""
+	set pld_name ""
+	set tool_num "1"
+	set idx 0
+	set num_args [llength $args]
+	while {$idx < $num_args} {
+		set arg [lindex $args $idx]
+		switch -- $arg {
+			"-tap" {
+				incr idx
+				if {$idx >= $num_args || [string index [lindex $args $idx] 0] == "-"} {
+					echo "no TAP name given"
+					return
+				}
+				set tap_name [lindex $args $idx]
+			}
+			"-pld" {
+				incr idx
+				if {$idx >= $num_args || [string index [lindex $args $idx] 0] == "-"} {
+					echo "no PLD name given"
+					return
+				}
+				set pld_name [lindex $args $idx]
+			}
+			"-tool" {
+				if {[expr {$idx + 1}] < $num_args && [string index [lindex $args [expr {$idx + 1}]] 0] != "-"} {
+					set tool_num [lindex $args [expr {$idx + 1}]]
+				}
+			}
+			default {
+#				don't touch remaining arguments
+			}
+		}
+		incr idx
+	}
+
+	set hub_name ""
+	if {$tap_name != ""} {
+		set hub_name [lindex [split $tap_name .] 0].ipdbghub
+	} elseif {$pld_name != ""} {
+		set hub_name [lindex [split $pld_name .] 0].ipdbghub
+	} else {
+		echo "parsing arguments failed: no tap and no pld given."
+		return
+	}
+
+	eval $hub_name ipdbg stop -tool $tool_num
+}
+
 # END MIGRATION AIDS
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index d0c9245..a028789 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -53,6 +53,8 @@
 enum gdb_output_flag {
 	/* GDB doesn't accept 'O' packets */
 	GDB_OUTPUT_NO,
+	/* GDB doesn't accept 'O' packets but accepts notifications */
+	GDB_OUTPUT_NOTIF,
 	/* GDB accepts 'O' packets */
 	GDB_OUTPUT_ALL,
 };
@@ -71,6 +73,8 @@ struct gdb_connection {
 	enum target_state frontend_state;
 	struct image *vflash_image;
 	bool closed;
+	/* set to prevent re-entrance from log messages during gdb_get_packet()
+	 * and gdb_put_packet(). */
 	bool busy;
 	int noack_mode;
 	/* set flag to true if you want the next stepi to return immediately.
@@ -1537,9 +1541,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.
- */
 static int gdb_read_memory_packet(struct connection *connection,
 		char const *packet, int packet_size)
 {
@@ -3470,6 +3471,13 @@ static int gdb_v_packet(struct connection *connection,
 	if (strncmp(packet, "vFlashDone", 10) == 0) {
 		uint32_t written;
 
+		/* GDB command 'flash-erase' does not send a vFlashWrite,
+		 * so nothing to write here. */
+		if (!gdb_connection->vflash_image) {
+			gdb_put_packet(connection, "OK", 2);
+			return ERROR_OK;
+		}
+
 		/* process the flashing buffer. No need to erase as GDB
 		 * always issues a vFlashErase first. */
 		target_call_event_callbacks(target,
@@ -3564,7 +3572,7 @@ static void gdb_log_callback(void *priv, const char *file, unsigned line,
 	struct connection *connection = priv;
 	struct gdb_connection *gdb_con = connection->priv;
 
-	if (gdb_con->output_flag == GDB_OUTPUT_NO)
+	if (gdb_con->output_flag != GDB_OUTPUT_ALL)
 		/* No out allowed */
 		return;
 
@@ -3649,10 +3657,14 @@ static int gdb_input_inner(struct connection *connection)
 					retval = gdb_set_register_packet(connection, packet, packet_size);
 					break;
 				case 'm':
+					gdb_con->output_flag = GDB_OUTPUT_NOTIF;
 					retval = gdb_read_memory_packet(connection, packet, packet_size);
+					gdb_con->output_flag = GDB_OUTPUT_NO;
 					break;
 				case 'M':
+					gdb_con->output_flag = GDB_OUTPUT_NOTIF;
 					retval = gdb_write_memory_packet(connection, packet, packet_size);
+					gdb_con->output_flag = GDB_OUTPUT_NO;
 					break;
 				case 'z':
 				case 'Z':
@@ -3743,9 +3755,9 @@ static int gdb_input_inner(struct connection *connection)
 					retval = gdb_detach(connection);
 					break;
 				case 'X':
+					gdb_con->output_flag = GDB_OUTPUT_NOTIF;
 					retval = gdb_write_memory_binary_packet(connection, packet, packet_size);
-					if (retval != ERROR_OK)
-						return retval;
+					gdb_con->output_flag = GDB_OUTPUT_NO;
 					break;
 				case 'k':
 					if (gdb_con->extended_protocol) {
@@ -3843,19 +3855,51 @@ static int gdb_input(struct connection *connection)
 	return ERROR_OK;
 }
 
+/*
+ * Send custom notification packet as keep-alive during memory read/write.
+ *
+ * From gdb 7.0 (released 2009-10-06) an unknown notification received during
+ * memory read/write would be silently dropped.
+ * Before gdb 7.0 any character, with exclusion of "+-$", would be considered
+ * as junk and ignored.
+ * In both cases the reception will reset the timeout counter in gdb, thus
+ * working as a keep-alive.
+ * Check putpkt_binary() and getpkt_sane() in gdb commit
+ * 74531fed1f2d662debc2c209b8b3faddceb55960
+ *
+ * Enable remote debug in gdb with 'set debug remote 1' to either dump the junk
+ * characters in gdb pre-7.0 and the notification from gdb 7.0.
+ */
+static void gdb_async_notif(struct connection *connection)
+{
+	static unsigned char count;
+	unsigned char checksum = 0;
+	char buf[22];
+
+	int len = sprintf(buf, "%%oocd_keepalive:%2.2x", count++);
+	for (int i = 1; i < len; i++)
+		checksum += buf[i];
+	len += sprintf(buf + len, "#%2.2x", checksum);
+
+#ifdef _DEBUG_GDB_IO_
+	LOG_DEBUG("sending packet '%s'", buf);
+#endif
+
+	gdb_write(connection, buf, len);
+}
+
 static void gdb_keep_client_alive(struct connection *connection)
 {
 	struct gdb_connection *gdb_con = connection->priv;
 
-	if (gdb_con->busy) {
-		/* do not send packets, retry asap */
-		return;
-	}
-
 	switch (gdb_con->output_flag) {
 	case GDB_OUTPUT_NO:
 		/* no need for keep-alive */
 		break;
+	case GDB_OUTPUT_NOTIF:
+		/* send asynchronous notification */
+		gdb_async_notif(connection);
+		break;
 	case GDB_OUTPUT_ALL:
 		/* send an empty O packet */
 		gdb_output_con(connection, "");
diff --git a/src/server/ipdbg.c b/src/server/ipdbg.c
index 0733230..e7eb96e 100644
--- a/src/server/ipdbg.c
+++ b/src/server/ipdbg.c
@@ -15,22 +15,15 @@
 #include "ipdbg.h"
 
 #define IPDBG_BUFFER_SIZE 16384
-#define IPDBG_MIN_NUM_OF_OPTIONS 2
-#define IPDBG_MAX_NUM_OF_OPTIONS 14
+#define IPDBG_MIN_NUM_OF_CREATE_OPTIONS 3
+#define IPDBG_MAX_NUM_OF_CREATE_OPTIONS 10
+#define IPDBG_NUM_OF_START_OPTIONS 4
+#define IPDBG_NUM_OF_STOP_OPTIONS 2
+#define IPDBG_NUM_OF_QUEUE_OPTIONS 2
 #define IPDBG_MIN_DR_LENGTH 11
 #define IPDBG_MAX_DR_LENGTH 13
 #define IPDBG_TCP_PORT_STR_MAX_LENGTH 6
 #define IPDBG_SCRATCH_MEMORY_SIZE 1024
-#define IPDBG_EMPTY_DOWN_TRANSFERS 1024
-#define IPDBG_CONSECUTIVE_UP_TRANSFERS 1024
-
-#if IPDBG_SCRATCH_MEMORY_SIZE < IPDBG_EMPTY_DOWN_TRANSFERS
-#error "scratch Memory must be at least IPDBG_EMPTY_DOWN_TRANSFERS"
-#endif
-
-#if IPDBG_SCRATCH_MEMORY_SIZE < IPDBG_CONSECUTIVE_UP_TRANSFERS
-#error "scratch Memory must be at least IPDBG_CONSECUTIVE_UP_TRANSFERS"
-#endif
 
 /* private connection data for IPDBG */
 struct ipdbg_fifo {
@@ -75,6 +68,8 @@ struct ipdbg_hub {
 	uint32_t xoff_mask;
 	uint32_t tool_mask;
 	uint32_t last_dn_tool;
+	char *name;
+	size_t using_queue_size;
 	struct ipdbg_hub *next;
 	struct jtag_tap *tap;
 	struct connection **connections;
@@ -247,59 +242,27 @@ static void ipdbg_add_hub(struct ipdbg_hub *hub)
 		for (ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next)
 			;
 		ihub->next = hub;
-	} else
+	} else {
 		ipdbg_first_hub = hub;
+	}
 }
 
-static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uint8_t data_register_length,
-					  struct ipdbg_virtual_ir_info *virtual_ir, struct ipdbg_hub **hub)
+static int ipdbg_remove_hub(struct ipdbg_hub *hub)
 {
-	*hub = NULL;
-	struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
-	if (!new_hub)
-		goto mem_err_hub;
-
-	const size_t dreg_buffer_size = DIV_ROUND_UP(data_register_length, 8);
-	new_hub->max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
-
-	new_hub->scratch_memory.dr_out_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
-	new_hub->scratch_memory.dr_in_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
-	new_hub->scratch_memory.fields = calloc(IPDBG_SCRATCH_MEMORY_SIZE, sizeof(struct scan_field));
-	new_hub->connections = calloc(new_hub->max_tools, sizeof(struct connection *));
-
-	if (virtual_ir)
-		new_hub->scratch_memory.vir_out_val = calloc(1, DIV_ROUND_UP(virtual_ir->length, 8));
-
-	if (!new_hub->scratch_memory.dr_out_vals || !new_hub->scratch_memory.dr_in_vals ||
-		!new_hub->scratch_memory.fields || (virtual_ir && !new_hub->scratch_memory.vir_out_val) ||
-		!new_hub->connections)
-		goto mem_err2;
-
-	if (virtual_ir)
-		buf_set_u32(new_hub->scratch_memory.vir_out_val, 0, virtual_ir->length, virtual_ir->value);
-
-	new_hub->tap                  = tap;
-	new_hub->user_instruction     = user_instruction;
-	new_hub->data_register_length = data_register_length;
-	new_hub->valid_mask           = BIT(data_register_length - 1);
-	new_hub->xoff_mask            = BIT(data_register_length - 2);
-	new_hub->tool_mask            = (new_hub->xoff_mask - 1) >> 8;
-	new_hub->last_dn_tool         = new_hub->tool_mask;
-	new_hub->virtual_ir           = virtual_ir;
+	if (!ipdbg_first_hub)
+		return ERROR_FAIL;
+	if (hub == ipdbg_first_hub) {
+		ipdbg_first_hub = ipdbg_first_hub->next;
+		return ERROR_OK;
+	}
 
-	*hub = new_hub;
-	return ERROR_OK;
+	for (struct ipdbg_hub *ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next) {
+		if (hub == ihub->next) {
+			ihub->next = hub->next;
+			return ERROR_OK;
+		}
+	}
 
-mem_err2:
-	free(new_hub->scratch_memory.vir_out_val);
-	free(new_hub->connections);
-	free(new_hub->scratch_memory.fields);
-	free(new_hub->scratch_memory.dr_in_vals);
-	free(new_hub->scratch_memory.dr_out_vals);
-	free(new_hub);
-mem_err_hub:
-	free(virtual_ir);
-	LOG_ERROR("Out of memory");
 	return ERROR_FAIL;
 }
 
@@ -309,6 +272,7 @@ static void ipdbg_free_hub(struct ipdbg_hub *hub)
 		return;
 	free(hub->connections);
 	free(hub->virtual_ir);
+	free(hub->name);
 	free(hub->scratch_memory.dr_out_vals);
 	free(hub->scratch_memory.dr_in_vals);
 	free(hub->scratch_memory.fields);
@@ -316,23 +280,42 @@ static void ipdbg_free_hub(struct ipdbg_hub *hub)
 	free(hub);
 }
 
-static int ipdbg_remove_hub(struct ipdbg_hub *hub)
+static struct ipdbg_hub *ipdbg_allocate_hub(uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir,
+											const char *name)
 {
-	if (!ipdbg_first_hub)
-		return ERROR_FAIL;
-	if (hub == ipdbg_first_hub) {
-		ipdbg_first_hub = ipdbg_first_hub->next;
-		return ERROR_OK;
+	struct ipdbg_hub *new_hub = calloc(1, sizeof(struct ipdbg_hub));
+	if (!new_hub) {
+		LOG_ERROR("Out of memory");
+		return NULL;
 	}
 
-	for (struct ipdbg_hub *ihub = ipdbg_first_hub; ihub->next; ihub = ihub->next) {
-		if (hub == ihub->next) {
-			ihub->next = hub->next;
-			return ERROR_OK;
-		}
+	new_hub->name = strdup(name);
+	if (!new_hub->name) {
+		free(new_hub);
+		LOG_ERROR("Out of memory");
+		return NULL;
 	}
 
-	return ERROR_FAIL;
+	const size_t dreg_buffer_size = DIV_ROUND_UP(data_register_length, 8);
+	uint32_t max_tools = ipdbg_max_tools_from_data_register_length(data_register_length);
+
+	new_hub->scratch_memory.dr_out_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
+	new_hub->scratch_memory.dr_in_vals = calloc(IPDBG_SCRATCH_MEMORY_SIZE, dreg_buffer_size);
+	new_hub->scratch_memory.fields = calloc(IPDBG_SCRATCH_MEMORY_SIZE, sizeof(struct scan_field));
+	new_hub->connections = calloc(max_tools, sizeof(struct connection *));
+
+	if (virtual_ir)
+		new_hub->scratch_memory.vir_out_val = calloc(1, DIV_ROUND_UP(virtual_ir->length, 8));
+
+	if (!new_hub->scratch_memory.dr_out_vals || !new_hub->scratch_memory.dr_in_vals ||
+		!new_hub->scratch_memory.fields || (virtual_ir && !new_hub->scratch_memory.vir_out_val) ||
+		!new_hub->connections) {
+		ipdbg_free_hub(new_hub);
+		LOG_ERROR("Out of memory");
+		return NULL;
+	}
+
+	return new_hub;
 }
 
 static void ipdbg_init_scan_field(struct scan_field *fields, uint8_t *in_value, int num_bits, const uint8_t *out_value)
@@ -470,7 +453,7 @@ static int ipdbg_shift_empty_data(struct ipdbg_hub *hub)
 
 	const size_t dreg_buffer_size = DIV_ROUND_UP(hub->data_register_length, 8);
 	memset(hub->scratch_memory.dr_out_vals, 0, dreg_buffer_size);
-	for (size_t i = 0; i < IPDBG_EMPTY_DOWN_TRANSFERS; ++i) {
+	for (size_t i = 0; i < hub->using_queue_size; ++i) {
 		ipdbg_init_scan_field(hub->scratch_memory.fields + i,
 								hub->scratch_memory.dr_in_vals + i * dreg_buffer_size,
 								hub->data_register_length,
@@ -482,7 +465,7 @@ static int ipdbg_shift_empty_data(struct ipdbg_hub *hub)
 
 	if (retval == ERROR_OK) {
 		uint32_t up_data;
-		for (size_t i = 0; i < IPDBG_EMPTY_DOWN_TRANSFERS; ++i) {
+		for (size_t i = 0; i < hub->using_queue_size; ++i) {
 			up_data = buf_get_u32(hub->scratch_memory.dr_in_vals +
 									i * dreg_buffer_size, 0,
 									hub->data_register_length);
@@ -531,8 +514,8 @@ static int ipdbg_jtag_transfer_bytes(struct ipdbg_hub *hub,
 		return ERROR_FAIL;
 
 	const size_t dreg_buffer_size = DIV_ROUND_UP(hub->data_register_length, 8);
-	size_t num_tx = (connection->dn_fifo.count < IPDBG_CONSECUTIVE_UP_TRANSFERS) ?
-		connection->dn_fifo.count : IPDBG_CONSECUTIVE_UP_TRANSFERS;
+	size_t num_tx = (connection->dn_fifo.count < hub->using_queue_size) ?
+		connection->dn_fifo.count : hub->using_queue_size;
 
 	for (size_t i = 0; i < num_tx; ++i) {
 		uint32_t dn_data = hub->valid_mask | ((tool & hub->tool_mask) << 8) |
@@ -760,23 +743,62 @@ static const struct service_driver ipdbg_service_driver = {
 	.keep_client_alive_handler = NULL,
 };
 
-static int ipdbg_start(uint16_t port, struct jtag_tap *tap, uint32_t user_instruction,
-					uint8_t data_register_length, struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
+static struct ipdbg_hub *ipdbg_get_hub_by_name(const char *name)
 {
-	LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
+	struct ipdbg_hub *hub = NULL;
+	for (hub = ipdbg_first_hub; hub; hub = hub->next) {
+		if (strcmp(hub->name, name) == 0)
+			break;
+	}
+	return hub;
+};
 
-	struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
-	if (hub) {
-		free(virtual_ir);
-		if (hub->data_register_length != data_register_length) {
-			LOG_DEBUG("hub must have the same data_register_length for all tools");
-			return ERROR_FAIL;
+static int ipdbg_stop_service(struct ipdbg_service *service)
+{
+	int retval = ipdbg_remove_service(service);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("BUG: ipdbg_remove_service failed");
+		return retval;
+	}
+
+	char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
+	snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", service->port);
+	retval = remove_service("ipdbg", port_str_buffer);
+	/* The ipdbg_service structure is freed by server.c:remove_service().
+	   There the "priv" pointer is freed.*/
+	if (retval != ERROR_OK) {
+		LOG_ERROR("BUG: remove_service failed");
+		return retval;
+	}
+	return ERROR_OK;
+}
+
+int ipdbg_server_free(void)
+{
+	int retval = ERROR_OK;
+	for (struct ipdbg_hub *hub = ipdbg_first_hub; hub;) {
+		for (uint8_t tool = 0; tool < hub->max_tools; ++tool) {
+			struct ipdbg_service *service = ipdbg_find_service(hub, tool);
+			if (service) {
+				int new_retval = ipdbg_stop_service(service);
+				if (new_retval != ERROR_OK)
+					retval = new_retval;
+				hub->active_services--;
+			}
 		}
-	} else {
-		int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, &hub);
-		if (retval != ERROR_OK)
-			return retval;
+		struct ipdbg_hub *next_hub = hub->next;
+		int new_retval = ipdbg_remove_hub(hub);
+		if (new_retval != ERROR_OK)
+			retval = new_retval;
+		ipdbg_free_hub(hub);
+		hub = next_hub;
 	}
+	return retval;
+}
+
+static int ipdbg_start(struct ipdbg_hub *hub, uint16_t port, uint8_t tool)
+{
+	LOG_INFO("starting ipdbg service on port %d for tool %d", port, tool);
 
 	struct ipdbg_service *service = NULL;
 	int retval = ipdbg_create_service(hub, tool, &service, port);
@@ -790,82 +812,229 @@ static int ipdbg_start(uint16_t port, struct jtag_tap *tap, uint32_t user_instru
 	char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
 	snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", port);
 	retval = add_service(&ipdbg_service_driver, port_str_buffer, 1, service);
-	if (retval == ERROR_OK) {
-		ipdbg_add_service(service);
-		if (hub->active_services == 0 && hub->active_connections == 0)
-			ipdbg_add_hub(hub);
-		hub->active_services++;
-	} else {
-		if (hub->active_services == 0 && hub->active_connections == 0)
-			ipdbg_free_hub(hub);
+	if (retval != ERROR_OK) {
 		free(service);
+		return retval;
 	}
+	ipdbg_add_service(service);
+	hub->active_services++;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_ipdbg_start_command)
+{
+	struct ipdbg_hub *hub = CMD_DATA;
 
+	uint16_t port = 4242;
+	uint8_t tool = 1;
+
+	if (CMD_ARGC > IPDBG_NUM_OF_START_OPTIONS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned int i = 0; i < CMD_ARGC; ++i) {
+		if (strcmp(CMD_ARGV[i], "-port") == 0) {
+			COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
+		} else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
+			COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
+		} else {
+			command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
+			return ERROR_FAIL;
+		}
+	}
+
+	return ipdbg_start(hub, port, tool);
+}
+
+static int ipdbg_stop(struct ipdbg_hub *hub, uint8_t tool)
+{
+	struct ipdbg_service *service = ipdbg_find_service(hub, tool);
+	if (!service) {
+		LOG_ERROR("No service for hub '%s'/tool %d found", hub->name, tool);
+		return ERROR_FAIL;
+	}
+
+	int retval = ipdbg_stop_service(service);
+	hub->active_services--;
+
+	LOG_INFO("stopped ipdbg service for tool %d", tool);
 	return retval;
 }
 
-static int ipdbg_stop(struct jtag_tap *tap, uint32_t user_instruction,
-			struct ipdbg_virtual_ir_info *virtual_ir, uint8_t tool)
+COMMAND_HANDLER(handle_ipdbg_stop_command)
+{
+	struct ipdbg_hub *hub = CMD_DATA;
+
+	uint8_t tool = 1;
+
+	if (CMD_ARGC > IPDBG_NUM_OF_STOP_OPTIONS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned int i = 0; i < CMD_ARGC; ++i) {
+		if (strcmp(CMD_ARGV[i], "-tool") == 0) {
+			COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
+		} else {
+			command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
+			return ERROR_FAIL;
+		}
+	}
+
+	return ipdbg_stop(hub, tool);
+}
+
+static const struct command_registration ipdbg_hostserver_subcommand_handlers[] = {
+	{
+		.name = "start",
+		.mode = COMMAND_EXEC,
+		.handler = handle_ipdbg_start_command,
+		.help = "Starts a IPDBG Host server.",
+		.usage = "-tool number -port port"
+	}, {
+		.name = "stop",
+		.mode = COMMAND_EXEC,
+		.handler = handle_ipdbg_stop_command,
+		.help = "Stops a IPDBG Host server.",
+		.usage = "-tool number"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static COMMAND_HELPER(ipdbg_config_queuing, struct ipdbg_hub *hub, unsigned int size)
 {
-	struct ipdbg_hub *hub = ipdbg_find_hub(tap, user_instruction, virtual_ir);
-	free(virtual_ir);
 	if (!hub)
 		return ERROR_FAIL;
 
-	struct ipdbg_service *service = ipdbg_find_service(hub, tool);
-	if (!service)
+	if (hub->active_connections) {
+		command_print(CMD, "Configuration change not allowed when hub has active connections");
 		return ERROR_FAIL;
-
-	int retval = ipdbg_remove_service(service);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("BUG: ipdbg_remove_service failed");
-		return retval;
 	}
 
-	char port_str_buffer[IPDBG_TCP_PORT_STR_MAX_LENGTH];
-	snprintf(port_str_buffer, IPDBG_TCP_PORT_STR_MAX_LENGTH, "%u", service->port);
-	retval = remove_service("ipdbg", port_str_buffer);
-	/* The ipdbg_service structure is freed by server.c:remove_service().
-	   There the "priv" pointer is freed.*/
-	if (retval != ERROR_OK) {
-		LOG_ERROR("BUG: remove_service failed");
-		return retval;
+	if (size == 0 || size > IPDBG_SCRATCH_MEMORY_SIZE) {
+		command_print(CMD, "queuing size out of range! Must be 0 < size <= %d", IPDBG_SCRATCH_MEMORY_SIZE);
+		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
-	hub->active_services--;
-	if (hub->active_connections == 0 && hub->active_services == 0) {
-		retval = ipdbg_remove_hub(hub);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("BUG: ipdbg_remove_hub failed");
-			return retval;
+
+	hub->using_queue_size = size;
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(handle_ipdbg_cfg_queuing_command)
+{
+	struct ipdbg_hub *hub = CMD_DATA;
+
+	unsigned int size;
+
+	if (CMD_ARGC != IPDBG_NUM_OF_QUEUE_OPTIONS)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned int i = 0; i < CMD_ARGC; ++i) {
+		if (strcmp(CMD_ARGV[i], "-size") == 0) {
+			COMMAND_PARSE_ADDITIONAL_NUMBER(uint, i, size, "size");
+		} else {
+			command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
+			return ERROR_FAIL;
 		}
-		ipdbg_free_hub(hub);
 	}
+
+	return CALL_COMMAND_HANDLER(ipdbg_config_queuing, hub, size);
+}
+
+static const struct command_registration ipdbg_hub_subcommand_handlers[] = {
+	{
+		.name = "ipdbg",
+		.mode = COMMAND_EXEC,
+		.help = "IPDBG Hub commands.",
+		.usage = "",
+		.chain = ipdbg_hostserver_subcommand_handlers
+	},
+	{
+		.name = "queuing",
+		.handler = handle_ipdbg_cfg_queuing_command,
+		.mode = COMMAND_ANY,
+		.help = "configures queuing between IPDBG Host and Hub.",
+		.usage = "-size size",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static int ipdbg_register_hub_command(struct ipdbg_hub *hub, struct command_invocation *cmd)
+{
+	Jim_Interp *interp = CMD_CTX->interp;
+
+	/* does this command exist? */
+	Jim_Cmd *jcmd = Jim_GetCommand(interp, Jim_NewStringObj(interp, hub->name, -1), JIM_NONE);
+	if (jcmd) {
+		LOG_ERROR("cannot create Hub because a command with name '%s' already exists", hub->name);
+		return ERROR_FAIL;
+	}
+
+	const struct command_registration obj_commands[] = {
+		{
+			.name = hub->name,
+			.mode = COMMAND_EXEC,
+			.help = "IPDBG Hub command group.",
+			.usage = "",
+			.chain = ipdbg_hub_subcommand_handlers
+		},
+		COMMAND_REGISTRATION_DONE
+	};
+
+	return register_commands_with_data(CMD_CTX, NULL, obj_commands, hub);
+}
+
+static int ipdbg_create_hub(struct jtag_tap *tap, uint32_t user_instruction, uint8_t data_register_length,
+					  struct ipdbg_virtual_ir_info *virtual_ir, const char *name, struct command_invocation *cmd)
+{
+	struct ipdbg_hub *new_hub = ipdbg_allocate_hub(data_register_length, virtual_ir, name);
+	if (!new_hub)
+		return ERROR_FAIL;
+
+	if (virtual_ir)
+		buf_set_u32(new_hub->scratch_memory.vir_out_val, 0, virtual_ir->length, virtual_ir->value);
+	new_hub->tap                  = tap;
+	new_hub->user_instruction     = user_instruction;
+	new_hub->data_register_length = data_register_length;
+	new_hub->valid_mask           = BIT(data_register_length - 1);
+	new_hub->xoff_mask            = BIT(data_register_length - 2);
+	new_hub->tool_mask            = (new_hub->xoff_mask - 1) >> 8;
+	new_hub->last_dn_tool         = new_hub->tool_mask;
+	new_hub->virtual_ir           = virtual_ir;
+	new_hub->max_tools            = ipdbg_max_tools_from_data_register_length(data_register_length);
+	new_hub->using_queue_size     = IPDBG_SCRATCH_MEMORY_SIZE;
+
+	int retval = ipdbg_register_hub_command(new_hub, cmd);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Creating hub failed");
+		ipdbg_free_hub(new_hub);
+		return ERROR_FAIL;
+	}
+
+	ipdbg_add_hub(new_hub);
+
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(handle_ipdbg_command)
+COMMAND_HANDLER(handle_ipdbg_create_hub_command)
 {
 	struct jtag_tap *tap = NULL;
-	uint16_t port = 4242;
-	uint8_t tool = 1;
 	uint32_t user_instruction = 0x00;
 	uint8_t data_register_length = IPDBG_MAX_DR_LENGTH;
-	bool start = true;
-	bool hub_configured = false;
 	bool has_virtual_ir = false;
 	uint32_t virtual_ir_instruction = 0x00e;
 	uint32_t virtual_ir_length = 5;
 	uint32_t virtual_ir_value = 0x11;
 	struct ipdbg_virtual_ir_info *virtual_ir = NULL;
 	int user_num = 1;
+	bool hub_configured = false;
 
-	if ((CMD_ARGC < IPDBG_MIN_NUM_OF_OPTIONS) || (CMD_ARGC > IPDBG_MAX_NUM_OF_OPTIONS))
+	if (CMD_ARGC < IPDBG_MIN_NUM_OF_CREATE_OPTIONS || CMD_ARGC > IPDBG_MAX_NUM_OF_CREATE_OPTIONS)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	for (unsigned int i = 0; i < CMD_ARGC; ++i) {
+	const char *hub_name = CMD_ARGV[0];
+
+	for (unsigned int i = 1; i < CMD_ARGC; ++i) {
 		if (strcmp(CMD_ARGV[i], "-tap") == 0) {
 			if (i + 1 >= CMD_ARGC || CMD_ARGV[i + 1][0] == '-') {
-				command_print(CMD, "no TAP given");
+				command_print(CMD, "no TAP name given");
 				return ERROR_FAIL;
 			}
 			tap = jtag_tap_by_string(CMD_ARGV[i + 1]);
@@ -874,7 +1043,7 @@ COMMAND_HANDLER(handle_ipdbg_command)
 				return ERROR_FAIL;
 			}
 			++i;
-		} else if (strcmp(CMD_ARGV[i], "-hub") == 0) {
+		} else if (strcmp(CMD_ARGV[i], "-ir") == 0) {
 			COMMAND_PARSE_ADDITIONAL_NUMBER(u32, i, user_instruction, "ir_value to select hub");
 			hub_configured = true;
 			COMMAND_PARSE_OPTIONAL_NUMBER(u8, i, data_register_length);
@@ -917,20 +1086,11 @@ COMMAND_HANDLER(handle_ipdbg_command)
 			COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_length);
 			COMMAND_PARSE_OPTIONAL_NUMBER(u32, i, virtual_ir_instruction);
 			has_virtual_ir = true;
-		} else if (strcmp(CMD_ARGV[i], "-port") == 0) {
-			COMMAND_PARSE_ADDITIONAL_NUMBER(u16, i, port, "port number");
-		} else if (strcmp(CMD_ARGV[i], "-tool") == 0) {
-			COMMAND_PARSE_ADDITIONAL_NUMBER(u8, i, tool, "tool");
-		} else if (strcmp(CMD_ARGV[i], "-stop") == 0) {
-			start = false;
-		} else if (strcmp(CMD_ARGV[i], "-start") == 0) {
-			start = true;
 		} else {
 			command_print(CMD, "Unknown argument: %s", CMD_ARGV[i]);
 			return ERROR_FAIL;
 		}
 	}
-
 	if (!tap) {
 		command_print(CMD, "no valid tap selected");
 		return ERROR_FAIL;
@@ -941,8 +1101,8 @@ COMMAND_HANDLER(handle_ipdbg_command)
 		return ERROR_FAIL;
 	}
 
-	if (tool >= ipdbg_max_tools_from_data_register_length(data_register_length)) {
-		command_print(CMD, "Tool: %d is invalid", tool);
+	if (ipdbg_get_hub_by_name(hub_name)) {
+		LOG_ERROR("IPDBG hub with name '%s' already exists", hub_name);
 		return ERROR_FAIL;
 	}
 
@@ -957,20 +1117,38 @@ COMMAND_HANDLER(handle_ipdbg_command)
 		virtual_ir->value       = virtual_ir_value;
 	}
 
-	if (start)
-		return ipdbg_start(port, tap, user_instruction, data_register_length, virtual_ir, tool);
-	else
-		return ipdbg_stop(tap, user_instruction, virtual_ir, tool);
+	if (ipdbg_find_hub(tap, user_instruction, virtual_ir)) {
+		LOG_ERROR("IPDBG hub for given TAP and user-instruction already exists");
+		free(virtual_ir);
+		return ERROR_FAIL;
+	}
+
+	int retval = ipdbg_create_hub(tap, user_instruction, data_register_length, virtual_ir, hub_name, cmd);
+	if (retval != ERROR_OK)
+		free(virtual_ir);
+
+	return retval;
 }
 
+static const struct command_registration ipdbg_config_command_handlers[] = {
+	{
+		.name = "create-hub",
+		.mode = COMMAND_ANY,
+		.handler = handle_ipdbg_create_hub_command,
+		.help = "create a IPDBG Hub",
+		.usage = "name.ipdbghub (-tap device.tap -ir ir_value [dr_length] |"
+			" -pld name.pld [user]) [-vir [vir_value [length [instr_code]]]]",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
 static const struct command_registration ipdbg_command_handlers[] = {
 	{
 		.name = "ipdbg",
-		.handler = handle_ipdbg_command,
-		.mode = COMMAND_EXEC,
-		.help = "Starts or stops an IPDBG JTAG-Host server.",
-		.usage = "[-start|-stop] -tap device.tap -hub ir_value [dr_length]"
-				 " [-port number] [-tool number] [-vir [vir_value [length [instr_code]]]]",
+		.mode = COMMAND_ANY,
+		.help = "IPDBG Hub/Host commands.",
+		.usage = "",
+		.chain = ipdbg_config_command_handlers,
 	},
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/server/ipdbg.h b/src/server/ipdbg.h
index 6b70545..1f4156b 100644
--- a/src/server/ipdbg.h
+++ b/src/server/ipdbg.h
@@ -7,5 +7,6 @@
 #include <helper/command.h>
 
 int ipdbg_register_commands(struct command_context *cmd_ctx);
+int ipdbg_server_free(void);
 
 #endif /* OPENOCD_IPDBG_IPDBG_H */
diff --git a/src/server/server.c b/src/server/server.c
index 2be9045..0649ec9 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -23,6 +23,7 @@
 #include "openocd.h"
 #include "tcl_server.h"
 #include "telnet_server.h"
+#include "ipdbg.h"
 
 #include <signal.h>
 
@@ -714,6 +715,7 @@ void server_free(void)
 	tcl_service_free();
 	telnet_service_free();
 	jsp_service_free();
+	ipdbg_server_free();
 
 	free(bindto_name);
 }
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 1c056a0..2e4d0b5 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -93,6 +93,7 @@ static int aarch64_restore_system_control_reg(struct target *target)
 		case ARM_MODE_HYP:
 		case ARM_MODE_UND:
 		case ARM_MODE_SYS:
+		case ARM_MODE_MON:
 			instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 			break;
 
@@ -172,6 +173,7 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	case ARM_MODE_HYP:
 	case ARM_MODE_UND:
 	case ARM_MODE_SYS:
+	case ARM_MODE_MON:
 		instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
 		break;
 
@@ -1043,6 +1045,7 @@ static int aarch64_post_debug_entry(struct target *target)
 	case ARM_MODE_HYP:
 	case ARM_MODE_UND:
 	case ARM_MODE_SYS:
+	case ARM_MODE_MON:
 		instr = ARMV4_5_MRC(15, 0, 0, 1, 0, 0);
 		break;
 
@@ -2891,13 +2894,8 @@ static int aarch64_jim_configure(struct target *target, struct jim_getopt_info *
 	 * options, JIM_OK if it correctly parsed the topmost option
 	 * and JIM_ERR if an error occurred during parameter evaluation.
 	 * For JIM_CONTINUE, we check our own params.
-	 *
-	 * adiv5_jim_configure() assumes 'private_config' to point to
-	 * 'struct adiv5_private_config'. Override 'private_config'!
 	 */
-	target->private_config = &pc->adiv5_config;
-	e = adiv5_jim_configure(target, goi);
-	target->private_config = pc;
+	e = adiv5_jim_configure_ext(target, goi, &pc->adiv5_config, ADI_CONFIGURE_DAP_COMPULSORY);
 	if (e != JIM_CONTINUE)
 		return e;
 
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index 6d6f287..1231005 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -84,16 +84,8 @@ static void swd_clear_sticky_errors(struct adiv5_dap *dap)
 static int swd_run_inner(struct adiv5_dap *dap)
 {
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
-	int retval;
-
-	retval = swd->run();
-
-	if (retval != ERROR_OK) {
-		/* fault response */
-		dap->do_reconnect = true;
-	}
 
-	return retval;
+	return swd->run();
 }
 
 static inline int check_sync(struct adiv5_dap *dap)
@@ -105,14 +97,13 @@ static inline int check_sync(struct adiv5_dap *dap)
 static int swd_queue_dp_bankselect(struct adiv5_dap *dap, unsigned int reg)
 {
 	/* Only register address 0 (ADIv6 only) and 4 are banked. */
-	if ((reg & 0xf) > 4)
+	if (is_adiv6(dap) ? (reg & 0xf) > 4 : (reg & 0xf) != 4)
 		return ERROR_OK;
 
 	uint32_t sel = (reg >> 4) & DP_SELECT_DPBANK;
 
-	/* DP register 0 is not mapped according to ADIv5
-	 * whereas ADIv6 ensures DPBANKSEL = 0 after line reset */
-	if ((dap->select_valid || ((reg & 0xf) == 0 && dap->select_dpbanksel_valid))
+	/* ADIv6 ensures DPBANKSEL = 0 after line reset */
+	if ((dap->select_valid || (is_adiv6(dap) && dap->select_dpbanksel_valid))
 			&& (sel == (dap->select & DP_SELECT_DPBANK)))
 		return ERROR_OK;
 
@@ -146,7 +137,7 @@ static int swd_queue_dp_read_inner(struct adiv5_dap *dap, unsigned int reg,
 static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 		uint32_t data)
 {
-	int retval;
+	int retval = ERROR_OK;
 	const struct swd_driver *swd = adiv5_dap_swd_driver(dap);
 	assert(swd);
 
@@ -167,7 +158,11 @@ static int swd_queue_dp_write_inner(struct adiv5_dap *dap, unsigned int reg,
 	if (reg == DP_SELECT1)
 		dap->select = ((uint64_t)data << 32) | (dap->select & 0xffffffffull);
 
-	retval = swd_queue_dp_bankselect(dap, reg);
+	/* DP_ABORT write is not banked.
+	 * Prevent writing DP_SELECT before as it would fail on locked up DP */
+	if (reg != DP_ABORT)
+		retval = swd_queue_dp_bankselect(dap, reg);
+
 	if (retval == ERROR_OK) {
 		swd->write_reg(swd_cmd(false, false, reg), data, 0);
 
@@ -285,15 +280,15 @@ static int swd_multidrop_select(struct adiv5_dap *dap)
 		swd_multidrop_selected_dap = NULL;
 		if (retry > 3) {
 			LOG_ERROR("Failed to select multidrop %s", adiv5_dap_name(dap));
+			dap->do_reconnect = true;
 			return retval;
 		}
 
 		LOG_DEBUG("Failed to select multidrop %s, retrying...",
 				  adiv5_dap_name(dap));
-		/* we going to retry localy, do not ask for full reconnect */
-		dap->do_reconnect = false;
 	}
 
+	dap->do_reconnect = false;
 	return retval;
 }
 
@@ -632,7 +627,13 @@ static int swd_run(struct adiv5_dap *dap)
 
 	swd_finish_read(dap);
 
-	return swd_run_inner(dap);
+	retval = swd_run_inner(dap);
+	if (retval != ERROR_OK) {
+		/* fault response */
+		dap->do_reconnect = true;
+	}
+
+	return retval;
 }
 
 /** Put the SWJ-DP back to JTAG mode */
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index ff12658..9129ace 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -2424,23 +2424,26 @@ err_no_param:
 	return JIM_ERR;
 }
 
-int adiv5_jim_configure(struct target *target, struct jim_getopt_info *goi)
+int adiv5_jim_configure_ext(struct target *target, struct jim_getopt_info *goi,
+		struct adiv5_private_config *pc, enum adiv5_configure_dap_optional optional)
 {
-	struct adiv5_private_config *pc;
 	int e;
 
-	pc = (struct adiv5_private_config *)target->private_config;
 	if (!pc) {
-		pc = calloc(1, sizeof(struct adiv5_private_config));
+		pc = (struct adiv5_private_config *)target->private_config;
 		if (!pc) {
-			LOG_ERROR("Out of memory");
-			return JIM_ERR;
+			pc = calloc(1, sizeof(struct adiv5_private_config));
+			if (!pc) {
+				LOG_ERROR("Out of memory");
+				return JIM_ERR;
+			}
+			pc->ap_num = DP_APSEL_INVALID;
+			target->private_config = pc;
 		}
-		pc->ap_num = DP_APSEL_INVALID;
-		target->private_config = pc;
 	}
 
-	target->has_dap = true;
+	if (optional == ADI_CONFIGURE_DAP_COMPULSORY)
+		target->has_dap = true;
 
 	e = adiv5_jim_spot_configure(goi, &pc->dap, &pc->ap_num, NULL);
 	if (e != JIM_OK)
@@ -2455,11 +2458,17 @@ int adiv5_jim_configure(struct target *target, struct jim_getopt_info *goi)
 		}
 		target->tap = pc->dap->tap;
 		target->dap_configured = true;
+		target->has_dap = true;
 	}
 
 	return JIM_OK;
 }
 
+int adiv5_jim_configure(struct target *target, struct jim_getopt_info *goi)
+{
+	return adiv5_jim_configure_ext(target, goi, NULL, ADI_CONFIGURE_DAP_COMPULSORY);
+}
+
 int adiv5_verify_config(struct adiv5_private_config *pc)
 {
 	if (!pc)
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 60c161f..92c3dbc 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -788,6 +788,15 @@ struct adiv5_private_config {
 };
 
 extern int adiv5_verify_config(struct adiv5_private_config *pc);
+
+enum adiv5_configure_dap_optional {
+	ADI_CONFIGURE_DAP_COMPULSORY = false,
+	ADI_CONFIGURE_DAP_OPTIONAL = true
+};
+
+extern int adiv5_jim_configure_ext(struct target *target, struct jim_getopt_info *goi,
+								   struct adiv5_private_config *pc,
+								   enum adiv5_configure_dap_optional optional);
 extern int adiv5_jim_configure(struct target *target, struct jim_getopt_info *goi);
 
 struct adiv5_mem_ap_spot {
diff --git a/src/target/mem_ap.c b/src/target/mem_ap.c
index 5c81e3a..61a9475 100644
--- a/src/target/mem_ap.c
+++ b/src/target/mem_ap.c
@@ -194,11 +194,11 @@ static const char *mem_ap_get_gdb_arch(const struct target *target)
  * reg[24]:     32 bits, fps
  * reg[25]:     32 bits, cpsr
  *
- * Set 'exist' only to reg[0..15], so initial response to GDB is correct
+ * GDB requires only reg[0..15]
  */
 #define NUM_REGS     26
+#define NUM_GDB_REGS 16
 #define MAX_REG_SIZE 96
-#define REG_EXIST(n) ((n) < 16)
 #define REG_SIZE(n)  ((((n) >= 16) && ((n) < 24)) ? 96 : 32)
 
 struct mem_ap_alloc_reg_list {
@@ -218,14 +218,14 @@ static int mem_ap_get_gdb_reg_list(struct target *target, struct reg **reg_list[
 	}
 
 	*reg_list = mem_ap_alloc->reg_list;
-	*reg_list_size = NUM_REGS;
+	*reg_list_size = (reg_class == REG_CLASS_ALL) ? NUM_REGS : NUM_GDB_REGS;
 	struct reg *regs = mem_ap_alloc->regs;
 
 	for (int i = 0; i < NUM_REGS; i++) {
 		regs[i].number = i;
 		regs[i].value = mem_ap_alloc->regs_value;
 		regs[i].size = REG_SIZE(i);
-		regs[i].exist = REG_EXIST(i);
+		regs[i].exist = true;
 		regs[i].type = &mem_ap_reg_arch_type;
 		(*reg_list)[i] = &regs[i];
 	}
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 5b94e6c..6bbf71b 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -161,6 +161,67 @@ static const struct {
 #define MIPS32_NUM_REGS ARRAY_SIZE(mips32_regs)
 
 
+
+#define zero	0
+
+#define AT	1
+
+#define v0	2
+#define v1	3
+
+#define a0	4
+#define a1	5
+#define a2	6
+#define	a3	7
+#define t0	8
+#define t1	9
+#define t2	10
+#define t3	11
+#define t4	12
+#define t5	13
+#define t6	14
+#define t7	15
+#define ta0	12	/* alias for $t4 */
+#define ta1	13	/* alias for $t5 */
+#define ta2	14	/* alias for $t6 */
+#define ta3	15	/* alias for $t7 */
+
+#define s0	16
+#define s1	17
+#define s2	18
+#define s3	19
+#define s4	20
+#define s5	21
+#define s6	22
+#define s7	23
+#define s8	30		/* == fp */
+
+#define t8	24
+#define t9	25
+#define k0	26
+#define k1	27
+
+#define gp	28
+
+#define sp	29
+#define fp	30
+#define ra	31
+
+
+static const struct {
+	const char *name;
+} mips32_dsp_regs[MIPS32NUMDSPREGS] = {
+	{ "hi0"},
+	{ "hi1"},
+	{ "hi2"},
+	{ "hi3"},
+	{ "lo0"},
+	{ "lo1"},
+	{ "lo2"},
+	{ "lo3"},
+	{ "control"},
+};
+
 static int mips32_get_core_reg(struct reg *reg)
 {
 	int retval;
@@ -201,6 +262,61 @@ static int mips32_set_core_reg(struct reg *reg, uint8_t *buf)
 	return ERROR_OK;
 }
 
+/**
+ * mips32_set_all_fpr_width - Set the width of all floating-point registers
+ * @param[in] mips32: MIPS32 common structure
+ * @param[in] fp64: Flag indicating whether to set the width to 64 bits (double precision)
+ *
+ * @brief Sets the width of all floating-point registers based on the specified flag.
+ */
+static void mips32_set_all_fpr_width(struct mips32_common *mips32, bool fp64)
+{
+	struct reg_cache *cache = mips32->core_cache;
+	struct reg *reg_list = cache->reg_list;
+	int i;
+
+	for (i = MIPS32_REGLIST_FP_INDEX; i < (MIPS32_REGLIST_FP_INDEX + MIPS32_REG_FP_COUNT); i++) {
+		reg_list[i].size = fp64 ? 64 : 32;
+		reg_list[i].reg_data_type->type = fp64 ? REG_TYPE_IEEE_DOUBLE : REG_TYPE_IEEE_SINGLE;
+	}
+}
+
+/**
+ * mips32_detect_fpr_mode_change - Detect changes in floating-point register mode
+ * @param[in] mips32: MIPS32 common structure
+ * @param[in] cp0_status: Value of the CP0 status register
+ *
+ * @brief Detects changes in the floating-point register mode based on the CP0 status register.
+ * If changes are detected, it updates the internal state
+ * and logs a warning message indicating the mode change.
+ */
+static void mips32_detect_fpr_mode_change(struct mips32_common *mips32, uint32_t cp0_status)
+{
+	if (!mips32->fp_imp)
+		return;
+
+	/* CP0.Status.FR indicates the working mode of floating-point register.
+	 * When FP = 0, fpr can contain any 32bit data type,
+	 * 64bit data types are stored in even-odd register pairs.
+	 * When FP = 1, fpr can contain any data types.*/
+	bool fpu_in_64bit = ((cp0_status & BIT(MIPS32_CP0_STATUS_FR_SHIFT)) != 0);
+
+	/* CP0.Status.CU1 indicated whether CoProcessor1(which is FPU) is present. */
+	bool fp_enabled = ((cp0_status & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0);
+
+	if (mips32->fpu_in_64bit != fpu_in_64bit) {
+		mips32->fpu_in_64bit = fpu_in_64bit;
+		mips32_set_all_fpr_width(mips32, fpu_in_64bit);
+		LOG_WARNING("** FP mode changed to %sbit, you must reconnect GDB **", fpu_in_64bit ? "64" : "32");
+	}
+
+	if (mips32->fpu_enabled != fp_enabled) {
+		mips32->fpu_enabled = fp_enabled;
+		const char *s = fp_enabled ? "enabled" : "disabled";
+		LOG_WARNING("** FP is %s, register update %s **", s, s);
+	}
+}
+
 static int mips32_read_core_reg(struct target *target, unsigned int num)
 {
 	unsigned int cnum;
@@ -217,6 +333,8 @@ static int mips32_read_core_reg(struct target *target, unsigned int num)
 		cnum = num - MIPS32_REGLIST_C0_INDEX;
 		reg_value = mips32->core_regs.cp0[cnum];
 		buf_set_u32(mips32->core_cache->reg_list[num].value, 0, 32, reg_value);
+		if (cnum == MIPS32_REG_C0_STATUS_INDEX)
+			mips32_detect_fpr_mode_change(mips32, reg_value);
 	} else if (num >= MIPS32_REGLIST_FPC_INDEX) {
 		/* FPCR */
 		cnum = num - MIPS32_REGLIST_FPC_INDEX;
@@ -258,6 +376,8 @@ static int mips32_write_core_reg(struct target *target, unsigned int num)
 		cnum = num - MIPS32_REGLIST_C0_INDEX;
 		reg_value = buf_get_u32(mips32->core_cache->reg_list[num].value, 0, 32);
 		mips32->core_regs.cp0[cnum] = (uint32_t)reg_value;
+		if (cnum == MIPS32_REG_C0_STATUS_INDEX)
+			mips32_detect_fpr_mode_change(mips32, reg_value);
 	} else if (num >= MIPS32_REGLIST_FPC_INDEX) {
 		/* FPCR */
 		cnum = num - MIPS32_REGLIST_FPC_INDEX;
@@ -926,8 +1046,8 @@ static int mips32_read_config_fpu(struct mips32_common *mips32, struct mips_ejta
 		mips32->fp_imp = MIPS32_FP_IMP_NONE;
 		return ERROR_OK;
 	}
-	uint32_t status_value;
-	bool status_fr, status_cu1;
+	uint32_t fir_value, status_value;
+	bool fpu_in_64bit, fp_enabled;
 
 	retval = mips32_cp0_read(ejtag_info, &status_value, MIPS32_C0_STATUS, 0);
 	if (retval != ERROR_OK) {
@@ -935,20 +1055,34 @@ static int mips32_read_config_fpu(struct mips32_common *mips32, struct mips_ejta
 		return retval;
 	}
 
-	status_fr = (status_value >> MIPS32_CP0_STATUS_FR_SHIFT) & 0x1;
-	status_cu1 = (status_value >> MIPS32_CP0_STATUS_CU1_SHIFT) & 0x1;
-	if (status_cu1) {
-		/* TODO: read fpu(cp1) config register for current operating mode.
-		 * Now its set to 32 bits by default. */
-		snprintf(buf, sizeof(buf), "yes");
-		fp_imp = MIPS32_FP_IMP_32;
+	fpu_in_64bit = (status_value & BIT(MIPS32_CP0_STATUS_FR_SHIFT)) != 0;
+	fp_enabled = (status_value & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0;
+	if (fp_enabled) {
+		retval = mips32_cp1_control_read(ejtag_info, &fir_value, 0);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to read cp1 FIR register");
+			return retval;
+		}
+
+		if ((fir_value >> MIPS32_CP1_FIR_F64_SHIFT) & 0x1)
+			fp_imp++;
 	} else {
+		/* This is the only condition that writes to buf */
 		snprintf(buf, sizeof(buf), "yes, disabled");
 		fp_imp = MIPS32_FP_IMP_UNKNOWN;
 	}
 
-	mips32->fpu_in_64bit = status_fr;
-	mips32->fpu_enabled = status_cu1;
+	mips32->fpu_in_64bit = fpu_in_64bit;
+	mips32->fpu_enabled = fp_enabled;
+
+	mips32_set_all_fpr_width(mips32, fpu_in_64bit);
+
+	/* If fpu is not disabled, print out more information */
+	if (!buf[0])
+		snprintf(buf, sizeof(buf), "yes, %sbit (%s, working in %sbit)",
+			fp_imp == MIPS32_FP_IMP_64 ? "64" : "32",
+			fp_enabled ? "enabled" : "disabled",
+			fpu_in_64bit ? "64" : "32");
 
 	LOG_USER("FPU implemented: %s", buf);
 	mips32->fp_imp = fp_imp;
@@ -1545,6 +1679,204 @@ COMMAND_HANDLER(mips32_handle_cp0_command)
 }
 
 /**
+ * mips32_dsp_enable - Enable access to DSP registers
+ * @param[in] ctx: Context information for the pracc queue
+ * @param[in] isa: Instruction Set Architecture identifier
+ *
+ * @brief Enables access to DSP registers by modifying the status register.
+ *
+ * This function adds instructions to the context queue for enabling
+ * access to DSP registers by modifying the status register.
+ */
+static void mips32_dsp_enable(struct pracc_queue_info *ctx, int isa)
+{
+	/* Save Status Register */
+	/* move status to $9 (t1) 2*/
+	pracc_add(ctx, 0, MIPS32_MFC0(isa, 9, 12, 0));
+
+	/* Read it again in order to modify it */
+	/* move status to $0 (t0) 3*/
+	pracc_add(ctx, 0, MIPS32_MFC0(isa, 8, 12, 0));
+
+	/* Enable access to DSP registers by setting MX bit in status register */
+	/* $15 = MIPS32_PRACC_STACK 4/5/6*/
+	pracc_add(ctx, 0, MIPS32_LUI(isa, 15, UPPER16(MIPS32_DSP_ENABLE)));
+	pracc_add(ctx, 0, MIPS32_ORI(isa, 15, 15, LOWER16(MIPS32_DSP_ENABLE)));
+	pracc_add(ctx, 0, MIPS32_ISA_OR(8, 8, 15));
+	/* Enable DSP - update status registers 7*/
+	pracc_add(ctx, 0, MIPS32_MTC0(isa, 8, 12, 0));
+}
+
+/**
+ * mips32_dsp_restore - Restore DSP status registers to the previous setting
+ * @param[in] ctx: Context information pracc queue
+ * @param[in] isa: isa identifier
+ *
+ * @brief Restores the DSP status registers to their previous setting.
+ *
+ * This function adds instructions to the context queue for restoring the DSP
+ * status registers to their values before the operation.
+ */
+static void mips32_dsp_restore(struct pracc_queue_info *ctx, int isa)
+{
+	pracc_add(ctx, 0, MIPS32_MTC0(isa, 9, 12, 0)); /* Restore status registers to previous setting */
+	pracc_add(ctx, 0, MIPS32_NOP); /* nop */
+}
+
+/**
+ * mips32_pracc_read_dsp_reg - Read a value from a MIPS32 DSP register
+ * @param[in] ejtag_info: EJTAG information structure
+ * @param[out] val: Pointer to store the read value
+ * @param[in] reg: Index of the DSP register to read
+ *
+ * @brief Reads the value from the specified MIPS32 DSP register using EJTAG access.
+ *
+ * This function initiates a sequence of instructions to read the value from the
+ * specified DSP register. It will enable dsp module if its not enabled
+ * and restoring the status registers after the read operation.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_pracc_read_dsp_reg(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t reg)
+{
+	int isa = 0;
+
+	struct pracc_queue_info ctx = {
+		.max_code = 48,
+		.ejtag_info = ejtag_info
+	};
+
+	uint32_t dsp_read_code[] = {
+		MIPS32_MFHI(isa, t0),		/* mfhi t0 ($ac0) - OPCODE - 0x00004010 */
+		MIPS32_DSP_MFHI(t0, 1),		/* mfhi	t0,$ac1 - OPCODE - 0x00204010 */
+		MIPS32_DSP_MFHI(t0, 2),		/* mfhi	t0,$ac2 - OPCODE - 0x00404010 */
+		MIPS32_DSP_MFHI(t0, 3),		/* mfhi	t0,$ac3 - OPCODE - 0x00604010*/
+		MIPS32_MFLO(isa, t0),		/* mflo t0 ($ac0) - OPCODE - 0x00004012 */
+		MIPS32_DSP_MFLO(t0, 1),		/* mflo	t0,$ac1 - OPCODE - 0x00204012 */
+		MIPS32_DSP_MFLO(t0, 2),		/* mflo	t0,$ac2 - OPCODE - 0x00404012 */
+		MIPS32_DSP_MFLO(t0, 3),		/* mflo	t0,$ac3 - OPCODE - 0x00604012 */
+		MIPS32_DSP_RDDSP(t0, 0x3F),	/* rddsp t0, 0x3f (DSPCtl) - OPCODE - 0x7c3f44b8 */
+	};
+
+	/* Check status register to determine if dsp register access is enabled */
+	/* Get status register so it can be restored later */
+
+	ctx.pracc_list = NULL;
+
+	/* Init context queue */
+	pracc_queue_init(&ctx);
+
+	if (ctx.retval != ERROR_OK)
+		goto exit;
+
+	/* Enables DSP whether its already enabled or not */
+	mips32_dsp_enable(&ctx, isa);
+
+	/* move AC or Control to $8 (t0) 8*/
+	pracc_add(&ctx, 0, dsp_read_code[reg]);
+	/* Restore status registers to previous setting */
+	mips32_dsp_restore(&ctx, isa);
+
+	/* $15 = MIPS32_PRACC_BASE_ADDR 1*/
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 15, PRACC_UPPER_BASE_ADDR));
+	/* store $8 to pracc_out 10*/
+	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT, MIPS32_SW(isa, 8, PRACC_OUT_OFFSET, 15));
+	/* move COP0 DeSave to $15 11*/
+	pracc_add(&ctx, 0, MIPS32_MFC0(isa, 15, 31, 0));
+	/* restore upper 16 of $8 12*/
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 8, UPPER16(ejtag_info->reg8)));
+	/* restore lower 16 of $8 13*/
+	pracc_add(&ctx, 0, MIPS32_ORI(isa, 8, 8, LOWER16(ejtag_info->reg8)));
+	/* restore upper 16 of $9 14*/
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 9, UPPER16(ejtag_info->reg9)));
+	pracc_add(&ctx, 0, MIPS32_SYNC(isa));
+	/* jump to start 18*/
+	pracc_add(&ctx, 0, MIPS32_B(isa, NEG16(ctx.code_count + 1)));
+	/* restore lower 16 of $9 15*/
+	pracc_add(&ctx, 0, MIPS32_ORI(isa, 9, 9, LOWER16(ejtag_info->reg9)));
+
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1);
+exit:
+	pracc_queue_free(&ctx);
+	return ctx.retval;
+}
+
+/**
+ * mips32_pracc_write_dsp_reg - Write a value to a MIPS32 DSP register
+ * @param[in] ejtag_info: EJTAG information structure
+ * @param[in] val: Value to be written to the register
+ * @param[in] reg: Index of the DSP register to write
+ *
+ * @brief Writes the specified value to the specified MIPS32 DSP register.
+ *
+ * This function initiates a sequence of instructions to write the given value to the
+ * specified DSP register.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_pracc_write_dsp_reg(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t reg)
+{
+	int isa = 0;
+
+	struct pracc_queue_info ctx = {
+		.max_code = 48,
+		.ejtag_info = ejtag_info
+	};
+
+	uint32_t dsp_write_code[] = {
+		MIPS32_MTHI(isa, t0),		/* mthi t0 ($ac0) - OPCODE - 0x01000011 */
+		MIPS32_DSP_MTHI(t0, 1),		/* mthi t0, $ac1 - OPCODE - 0x01000811 */
+		MIPS32_DSP_MTHI(t0, 2),		/* mthi t0, $ac2 - OPCODE - 0x01001011 */
+		MIPS32_DSP_MTHI(t0, 3),		/* mthi t0, $ac3 - OPCODE - 0x01001811 */
+		MIPS32_MTLO(isa, t0),		/* mtlo t0 ($ac0) - OPCODE - 0x01000013 */
+		MIPS32_DSP_MTLO(t0, 1),		/* mtlo t0, $ac1 - OPCODE - 0x01000813 */
+		MIPS32_DSP_MTLO(t0, 2),		/* mtlo t0, $ac2 - OPCODE - 0x01001013 */
+		MIPS32_DSP_MTLO(t0, 3),		/* mtlo t0, $ac3 - OPCODE - 0x01001813 */
+		MIPS32_DSP_WRDSP(t0, 0x1F), /* wrdsp t0, 0x1f (DSPCtl) - OPCODE - 0x7d00fcf8*/
+	};
+
+	/* Init context queue */
+	pracc_queue_init(&ctx);
+	if (ctx.retval != ERROR_OK)
+		goto exit;
+
+	/* Enables DSP whether its already enabled or not */
+	mips32_dsp_enable(&ctx, isa);
+
+	/* Load val to $8 (t0) */
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 8, UPPER16(val)));
+	pracc_add(&ctx, 0, MIPS32_ORI(isa, 8, 8, LOWER16(val)));
+
+	/* move AC or Control to $8 (t0) */
+	pracc_add(&ctx, 0, dsp_write_code[reg]);
+
+	/* nop, delay in order to ensure write */
+	pracc_add(&ctx, 0, MIPS32_NOP);
+	/* Restore status registers to previous setting */
+	mips32_dsp_restore(&ctx, isa);
+
+	/* move COP0 DeSave to $15 */
+	pracc_add(&ctx, 0, MIPS32_MFC0(isa, 15, 31, 0));
+
+	/* restore $8 */
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 8, UPPER16(ejtag_info->reg8)));
+	pracc_add(&ctx, 0, MIPS32_ORI(isa, 8, 8, LOWER16(ejtag_info->reg8)));
+
+	/* restore upper 16 of $9 */
+	pracc_add(&ctx, 0, MIPS32_LUI(isa, 9, UPPER16(ejtag_info->reg9)));
+
+	/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_B(isa, NEG16(ctx.code_count + 1)));
+	/* restore lower 16 of $9  */
+	pracc_add(&ctx, 0, MIPS32_ORI(isa, 9, 9, LOWER16(ejtag_info->reg9)));
+
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
+exit:
+	pracc_queue_free(&ctx);
+	return ctx.retval;
+}
+
+/**
  * mips32_handle_cpuinfo_command - Handles the 'cpuinfo' command.
  * @param[in] cmd: Command invocation context.
  *
@@ -1747,6 +2079,167 @@ COMMAND_HANDLER(mips32_handle_cpuinfo_command)
 }
 
 /**
+ * mips32_dsp_find_register_by_name - Find DSP register index by name
+ * @param[in] reg_name: Name of the DSP register to find
+ *
+ * @brief Searches for a DSP register by name and returns its index.
+ * If no match is found, it returns MIPS32NUMDSPREGS.
+ *
+ * @return Index of the found register or MIPS32NUMDSPREGS if not found.
+ */
+static int mips32_dsp_find_register_by_name(const char *reg_name)
+{
+	if (reg_name)
+		for (int i = 0; i < MIPS32NUMDSPREGS; i++) {
+			if (strcmp(mips32_dsp_regs[i].name, reg_name) == 0)
+				return i;
+		}
+	return MIPS32NUMDSPREGS;
+}
+
+/**
+ * mips32_dsp_get_all_regs - Get values of all MIPS32 DSP registers
+ * @param[in] cmd: Command invocation context
+ * @param[in] ejtag_info: EJTAG information structure
+ *
+ * @brief This function iterates through all DSP registers, reads their values,
+ * and prints each register name along with its corresponding value.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_dsp_get_all_regs(struct command_invocation *cmd, struct mips_ejtag *ejtag_info)
+{
+	uint32_t value;
+	for (int i = 0; i < MIPS32NUMDSPREGS; i++) {
+		int retval = mips32_pracc_read_dsp_reg(ejtag_info, &value, i);
+		if (retval != ERROR_OK) {
+			command_print(CMD, "couldn't access reg %s", mips32_dsp_regs[i].name);
+			return retval;
+		}
+		command_print(CMD, "%*s: 0x%8.8x", 7, mips32_dsp_regs[i].name, value);
+	}
+	return ERROR_OK;
+}
+
+/**
+ * mips32_dsp_get_register - Get the value of a MIPS32 DSP register
+ * @param[in] cmd: Command invocation context
+ * @param[in] ejtag_info: EJTAG information structure
+ *
+ * @brief Retrieves the value of a specified MIPS32 DSP register.
+ * If the register is found, it reads the register value and prints the result.
+ * If the register is not found, it prints an error message.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_dsp_get_register(struct command_invocation *cmd, struct mips_ejtag *ejtag_info)
+{
+	uint32_t value;
+	int index = mips32_dsp_find_register_by_name(CMD_ARGV[0]);
+	if (index == MIPS32NUMDSPREGS) {
+		command_print(CMD, "ERROR: register '%s' not found", CMD_ARGV[0]);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	int retval = mips32_pracc_read_dsp_reg(ejtag_info, &value, index);
+	if (retval != ERROR_OK)
+		command_print(CMD, "ERROR: Could not access dsp register %s", CMD_ARGV[0]);
+	else
+		command_print(CMD, "0x%8.8x", value);
+
+	return retval;
+}
+
+/**
+ * mips32_dsp_set_register - Set the value of a MIPS32 DSP register
+ * @param[in] cmd: Command invocation context
+ * @param[in] ejtag_info: EJTAG information structure
+ *
+ * @brief Sets the value of a specified MIPS32 DSP register.
+ * If the register is found, it writes provided value to the register.
+ * If the register is not found or there is an error in writing the value,
+ * it prints an error message.
+ *
+ * @return ERROR_OK on success; error code on failure.
+ */
+static int mips32_dsp_set_register(struct command_invocation *cmd, struct mips_ejtag *ejtag_info)
+{
+	uint32_t value;
+	int index = mips32_dsp_find_register_by_name(CMD_ARGV[0]);
+	if (index == MIPS32NUMDSPREGS) {
+		command_print(CMD, "ERROR: register '%s' not found", CMD_ARGV[0]);
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+
+	int retval = mips32_pracc_write_dsp_reg(ejtag_info, value, index);
+	if (retval != ERROR_OK)
+		command_print(CMD, "Error: could not write to dsp register %s", CMD_ARGV[0]);
+
+	return retval;
+}
+
+/**
+ * mips32_handle_dsp_command - Handles mips dsp related command
+ * @param[in] cmd: Command invocation context
+ *
+ * @brief Reads or sets the content of each dsp register.
+ *
+ * @return ERROR_OK on success; error code on failure.
+*/
+COMMAND_HANDLER(mips32_handle_dsp_command)
+{
+	int retval, tmp;
+	struct target *target = get_current_target(CMD_CTX);
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	retval = mips32_verify_pointer(CMD, mips32);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->state != TARGET_HALTED) {
+		command_print(CMD, "target must be stopped for \"%s\" command", CMD_NAME);
+		return ERROR_OK;
+	}
+
+	/* Check for too many command args */
+	if (CMD_ARGC >= 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* Check if DSP access supported or not */
+	if (!mips32->dsp_imp) {
+		/* Issue Error Message */
+		command_print(CMD, "DSP not implemented by this processor");
+		return ERROR_OK;
+	}
+
+	switch (CMD_ARGC) {
+		case 0:
+			retval = mips32_dsp_get_all_regs(CMD, ejtag_info);
+			break;
+		case 1:
+			retval = mips32_dsp_get_register(CMD, ejtag_info);
+			break;
+		case 2:
+			tmp = *CMD_ARGV[0];
+			if (isdigit(tmp)) {
+				command_print(CMD, "Error: invalid dsp command format");
+				retval = ERROR_COMMAND_ARGUMENT_INVALID;
+			} else {
+				retval = mips32_dsp_set_register(CMD, ejtag_info);
+			}
+			break;
+		default:
+			command_print(CMD, "Error: invalid argument format, required 0-2, given %d", CMD_ARGC);
+			retval = ERROR_COMMAND_ARGUMENT_INVALID;
+			break;
+	}
+	return retval;
+}
+
+/**
  * mips32_handle_ejtag_reg_command - Handler commands related to EJTAG
  * @param[in] cmd: Command invocation context
  *
@@ -1848,6 +2341,14 @@ static const struct command_registration mips32_exec_command_handlers[] = {
 		.usage = "",
 	},
 	{
+		.name = "dsp",
+		.handler = mips32_handle_dsp_command,
+		.mode = COMMAND_EXEC,
+		.help = "display or set DSP register; "
+			"with no arguments, displays all registers and their values",
+		.usage = "[[register_name] [value]]",
+	},
+	{
 		.name = "scan_delay",
 		.handler = mips32_handle_scan_delay_command,
 		.mode = COMMAND_ANY,
diff --git a/src/target/mips32.h b/src/target/mips32.h
index 208c9da..a557f31 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -69,7 +69,7 @@
 
 #define MIPS32_SCAN_DELAY_LEGACY_MODE 2000000
 
-#define MIPS32_NUM_DSPREGS		9
+#define MIPS32NUMDSPREGS		9
 
 /* Bit Mask indicating CP0 register supported by this core */
 #define	MIPS_CP0_MK4		0x0001
@@ -459,10 +459,13 @@ struct mips32_algorithm {
 #define MIPS32_OP_AND	0x24u
 #define MIPS32_OP_CACHE	0x2Fu
 #define MIPS32_OP_COP0	0x10u
+#define MIPS32_OP_COP1	0x11u
 #define MIPS32_OP_J	0x02u
 #define MIPS32_OP_JR	0x08u
 #define MIPS32_OP_LUI	0x0Fu
 #define MIPS32_OP_LW	0x23u
+#define MIPS32_OP_LWC1	0x31u
+#define MIPS32_OP_LDC1	0x35u
 #define MIPS32_OP_LB	0x20u
 #define MIPS32_OP_LBU	0x24u
 #define MIPS32_OP_LHU	0x25u
@@ -470,6 +473,7 @@ struct mips32_algorithm {
 #define MIPS32_OP_MTHI	0x11u
 #define MIPS32_OP_MFLO	0x12u
 #define MIPS32_OP_MTLO	0x13u
+#define MIPS32_OP_MUL	0x02u
 #define MIPS32_OP_RDHWR	0x3Bu
 #define MIPS32_OP_SB	0x28u
 #define MIPS32_OP_SH	0x29u
@@ -485,6 +489,8 @@ struct mips32_algorithm {
 #define MIPS32_OP_SLLV	0x04u
 #define MIPS32_OP_SLTI	0x0Au
 #define MIPS32_OP_MOVN	0x0Bu
+#define MIPS32_OP_SWC1	0x39u
+#define MIPS32_OP_SDC1	0x3Du
 
 #define MIPS32_OP_REGIMM	0x01u
 #define MIPS32_OP_SDBBP	0x3Fu
@@ -517,6 +523,7 @@ struct mips32_algorithm {
 #define MIPS32_ISA_BGTZ(reg, off)		MIPS32_I_INST(MIPS32_OP_BGTZ, reg, 0, off)
 #define MIPS32_ISA_BNE(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BNE, src, tar, off)
 #define MIPS32_ISA_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
+#define MIPS32_ISA_CFC1(gpr, cpr)		MIPS32_R_INST(MIPS32_OP_COP1, MIPS32_COP_CF, gpr, cpr, 0, 0)
 #define MIPS32_ISA_J(tar)			MIPS32_J_INST(MIPS32_OP_J, (0x0FFFFFFFu & (tar)) >> 2)
 #define MIPS32_ISA_JR(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
 #define MIPS32_ISA_JRHB(reg)		MIPS32_R_INST(0, reg, 0, 0, 0x10, MIPS32_OP_JR)
@@ -526,9 +533,15 @@ struct mips32_algorithm {
 #define MIPS32_ISA_LHU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LHU, base, reg, off)
 #define MIPS32_ISA_LUI(reg, val)		MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
 #define MIPS32_ISA_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
+#define MIPS32_ISA_LWC1(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LWC1, base, reg, off)
+#define MIPS32_ISA_LDC1(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LDC1, base, reg, off)
 
 #define MIPS32_ISA_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP_MF, gpr, cpr, 0, sel)
 #define MIPS32_ISA_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP_MT, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MFC1(gpr, cpr)		MIPS32_R_INST(MIPS32_OP_COP1, MIPS32_COP_MF, gpr, cpr, 0, 0)
+#define MIPS32_ISA_MFHC1(gpr, cpr)		MIPS32_R_INST(MIPS32_OP_COP1, MIPS32_COP_MFH, gpr, cpr, 0, 0)
+#define MIPS32_ISA_MTC1(gpr, cpr)		MIPS32_R_INST(MIPS32_OP_COP1, MIPS32_COP_MT, gpr, cpr, 0, 0)
+#define MIPS32_ISA_MTHC1(gpr, cpr)		MIPS32_R_INST(MIPS32_OP_COP1, MIPS32_COP_MTH, gpr, cpr, 0, 0)
 #define MIPS32_ISA_MFLO(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
 #define MIPS32_ISA_MFHI(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
 #define MIPS32_ISA_MTLO(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
@@ -542,6 +555,8 @@ struct mips32_algorithm {
 #define MIPS32_ISA_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
 #define MIPS32_ISA_SH(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SH, base, reg, off)
 #define MIPS32_ISA_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
+#define MIPS32_ISA_SWC1(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SWC1, base, reg, off)
+#define MIPS32_ISA_SDC1(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SDC1, base, reg, off)
 
 #define MIPS32_ISA_SLL(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLL)
 #define MIPS32_ISA_SLLV(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLLV)
@@ -588,6 +603,7 @@ struct mips32_algorithm {
 #define MMIPS32_OP_BGTZ			0x06u
 #define MMIPS32_OP_BNE			0x2Du
 #define MMIPS32_OP_CACHE		0x06u
+#define MMIPS32_OP_CFC1			0x40u
 #define MMIPS32_OP_J			0x35u
 #define MMIPS32_OP_JALR			0x03Cu
 #define MMIPS32_OP_JALRHB		0x07Cu
@@ -596,8 +612,14 @@ struct mips32_algorithm {
 #define MMIPS32_OP_LHU			0x0Du
 #define MMIPS32_OP_LUI			0x0Du
 #define MMIPS32_OP_LW			0x3Fu
+#define MMIPS32_OP_LWC1			0x27u
+#define MMIPS32_OP_LDC1			0x2Fu
 #define MMIPS32_OP_MFC0			0x03u
+#define MMIPS32_OP_MFC1			0x80u
+#define MMIPS32_OP_MFHC1		0xC0u
 #define MMIPS32_OP_MTC0			0x0Bu
+#define MMIPS32_OP_MTC1			0xA0u
+#define MMIPS32_OP_MTHC1		0xE0u
 #define MMIPS32_OP_MFLO			0x075u
 #define MMIPS32_OP_MFHI			0x035u
 #define MMIPS32_OP_MTLO			0x0F5u
@@ -608,6 +630,8 @@ struct mips32_algorithm {
 #define MMIPS32_OP_SB			0x06u
 #define MMIPS32_OP_SH			0x0Eu
 #define MMIPS32_OP_SW			0x3Eu
+#define MMIPS32_OP_SWC1			0x26u
+#define MMIPS32_OP_SDC1			0x2Eu
 #define MMIPS32_OP_SLTU			0x390u
 #define MMIPS32_OP_SLL			0x000u
 #define MMIPS32_OP_SLTI			0x24u
@@ -627,6 +651,7 @@ struct mips32_algorithm {
 #define MMIPS32_BGTZ(reg, off)			MIPS32_I_INST(MMIPS32_POOL32I, MMIPS32_OP_BGTZ, reg, off)
 #define MMIPS32_BNE(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BNE, tar, src, off)
 #define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(MMIPS32_POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
+#define MMIPS32_CFC1(gpr, cpr)			MIPS32_R_INST(MMIPS32_POOL32F, gpr, cpr, 0, MMIPS32_OP_CFC1, MMIPS32_POOL32FXF)
 
 #define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, ((0x07FFFFFFu & ((tar) >> 1))))
 #define MMIPS32_JR(reg)				MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_JALR, MMIPS32_POOL32AXF)
@@ -636,13 +661,19 @@ struct mips32_algorithm {
 #define MMIPS32_LHU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LHU, reg, base, off)
 #define MMIPS32_LUI(reg, val)			MIPS32_I_INST(MMIPS32_POOL32I, MMIPS32_OP_LUI, reg, val)
 #define MMIPS32_LW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LW, reg, base, off)
+#define MMIPS32_LWC1(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LWC1, reg, base, off)
+#define MMIPS32_LDC1(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LDC1, reg, base, off)
 
 #define MMIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MMIPS32_POOL32A, gpr, cpr, sel,\
 														MMIPS32_OP_MFC0, MMIPS32_POOL32AXF)
+#define MMIPS32_MFC1(gpr, cpr)			MIPS32_R_INST(MMIPS32_POOL32F, gpr, cpr, 0, MMIPS32_OP_MFC1, MMIPS32_POOL32FXF)
+#define MMIPS32_MFHC1(gpr, cpr)			MIPS32_R_INST(MMIPS32_POOL32F, gpr, cpr, 0, MMIPS32_OP_MFHC1, MMIPS32_POOL32FXF)
 #define MMIPS32_MFLO(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MFLO, MMIPS32_POOL32AXF)
 #define MMIPS32_MFHI(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MFHI, MMIPS32_POOL32AXF)
 #define MMIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MMIPS32_POOL32A, gpr, cpr, sel,\
 														MMIPS32_OP_MTC0, MMIPS32_POOL32AXF)
+#define MMIPS32_MTC1(gpr, cpr)			MIPS32_R_INST(MMIPS32_POOL32F, gpr, cpr, 0, MMIPS32_OP_MTC1, MMIPS32_POOL32FXF)
+#define MMIPS32_MTHC1(gpr, cpr)			MIPS32_R_INST(MMIPS32_POOL32F, gpr, cpr, 0, MMIPS32_OP_MTHC1, MMIPS32_POOL32FXF)
 #define MMIPS32_MTLO(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MTLO, MMIPS32_POOL32AXF)
 #define MMIPS32_MTHI(reg)			MIPS32_R_INST(MMIPS32_POOL32A, 0, reg, 0, MMIPS32_OP_MTHI, MMIPS32_POOL32AXF)
 
@@ -653,6 +684,8 @@ struct mips32_algorithm {
 #define MMIPS32_SB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SB, reg, base, off)
 #define MMIPS32_SH(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SH, reg, base, off)
 #define MMIPS32_SW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SW, reg, base, off)
+#define MMIPS32_SWC1(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SWC1, reg, base, off)
+#define MMIPS32_SDC1(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SDC1, reg, base, off)
 
 #define MMIPS32_SRL(reg, src, off)		MIPS32_R_INST(MMIPS32_POOL32A, reg, src, off, 0, MMIPS32_OP_SRL)
 #define MMIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(MMIPS32_POOL32A, tar, src, dst, 0, MMIPS32_OP_SLTU)
@@ -686,6 +719,7 @@ struct mips32_algorithm {
 #define MIPS32_BGTZ(isa, reg, off)		(isa ? MMIPS32_BGTZ(reg, off) : MIPS32_ISA_BGTZ(reg, off))
 #define MIPS32_BNE(isa, src, tar, off)		(isa ? MMIPS32_BNE(src, tar, off) : MIPS32_ISA_BNE(src, tar, off))
 #define MIPS32_CACHE(isa, op, off, base)	(isa ? MMIPS32_CACHE(op, off, base) : MIPS32_ISA_CACHE(op, off, base))
+#define MIPS32_CFC1(isa, gpr, cpr)		(isa ? MMIPS32_CFC1(gpr, cpr) : MIPS32_ISA_CFC1(gpr, cpr))
 
 #define MIPS32_J(isa, tar)			(isa ? MMIPS32_J(tar) : MIPS32_ISA_J(tar))
 #define MIPS32_JR(isa, reg)			(isa ? MMIPS32_JR(reg) : MIPS32_ISA_JR(reg))
@@ -694,10 +728,15 @@ struct mips32_algorithm {
 #define MIPS32_LBU(isa, reg, off, base)		(isa ? MMIPS32_LBU(reg, off, base) : MIPS32_ISA_LBU(reg, off, base))
 #define MIPS32_LHU(isa, reg, off, base)		(isa ? MMIPS32_LHU(reg, off, base) : MIPS32_ISA_LHU(reg, off, base))
 #define MIPS32_LW(isa, reg, off, base)		(isa ? MMIPS32_LW(reg, off, base) : MIPS32_ISA_LW(reg, off, base))
+#define MIPS32_LWC1(isa, reg, off, base)	(isa ? MMIPS32_LWC1(reg, off, base) : MIPS32_ISA_LWC1(reg, off, base))
 #define MIPS32_LUI(isa, reg, val)		(isa ? MMIPS32_LUI(reg, val) : MIPS32_ISA_LUI(reg, val))
 
 #define MIPS32_MFC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MFC0(gpr, cpr, sel) : MIPS32_ISA_MFC0(gpr, cpr, sel))
 #define MIPS32_MTC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MTC0(gpr, cpr, sel) : MIPS32_ISA_MTC0(gpr, cpr, sel))
+#define MIPS32_MFC1(isa, gpr, cpr)		(isa ? MMIPS32_MFC1(gpr, cpr) : MIPS32_ISA_MFC1(gpr, cpr))
+#define MIPS32_MFHC1(isa, gpr, cpr)		(isa ? MMIPS32_MFHC1(gpr, cpr) : MIPS32_ISA_MFHC1(gpr, cpr))
+#define MIPS32_MTC1(isa, gpr, cpr)		(isa ? MMIPS32_MTC1(gpr, cpr) : MIPS32_ISA_MTC1(gpr, cpr))
+#define MIPS32_MTHC1(isa, gpr, cpr)		(isa ? MMIPS32_MTHC1(gpr, cpr) : MIPS32_ISA_MTHC1(gpr, cpr))
 #define MIPS32_MFLO(isa, reg)			(isa ? MMIPS32_MFLO(reg) : MIPS32_ISA_MFLO(reg))
 #define MIPS32_MFHI(isa, reg)			(isa ? MMIPS32_MFHI(reg) : MIPS32_ISA_MFHI(reg))
 #define MIPS32_MTLO(isa, reg)			(isa ? MMIPS32_MTLO(reg) : MIPS32_ISA_MTLO(reg))
@@ -710,6 +749,8 @@ struct mips32_algorithm {
 #define MIPS32_SB(isa, reg, off, base)		(isa ? MMIPS32_SB(reg, off, base) : MIPS32_ISA_SB(reg, off, base))
 #define MIPS32_SH(isa, reg, off, base)		(isa ? MMIPS32_SH(reg, off, base) : MIPS32_ISA_SH(reg, off, base))
 #define MIPS32_SW(isa, reg, off, base)		(isa ? MMIPS32_SW(reg, off, base) : MIPS32_ISA_SW(reg, off, base))
+#define MIPS32_SWC1(isa, reg, off, base)	(isa ? MMIPS32_SWC1(reg, off, base) : MIPS32_ISA_SWC1(reg, off, base))
+#define MIPS32_SDC1(isa, reg, off, base)	(isa ? MMIPS32_SDC1(reg, off, base) : MIPS32_ISA_SDC1(reg, off, base))
 
 #define MIPS32_SLL(isa, dst, src, sa)		(isa ? MMIPS32_SLL(dst, src, sa) : MIPS32_ISA_SLL(dst, src, sa))
 #define MIPS32_EHB(isa)		                (isa ? MMIPS32_SLL(0, 0, 3) : MIPS32_ISA_SLL(0, 0, 3))
@@ -734,6 +775,24 @@ struct mips32_algorithm {
 /* ejtag specific instructions */
 #define MICRO_MIPS32_SDBBP			0x000046C0
 #define MICRO_MIPS_SDBBP			0x46C0
+#define MIPS32_DSP_ENABLE			0x1000000
+
+#define MIPS32_S_INST(rs, rac, opcode)			\
+	(((rs) << 21) | ((rac) << 11) | (opcode))
+
+#define MIPS32_DSP_R_INST(rt, immd, opcode, extrw) \
+	((0x1F << 26) | ((immd) << 16) | ((rt) << 11) | ((opcode) << 6) | (extrw))
+#define MIPS32_DSP_W_INST(rs, immd, opcode, extrw) \
+	((0x1F << 26) | ((rs) << 21) | ((immd) << 11) | ((opcode) << 6) | (extrw))
+
+#define MIPS32_DSP_MFHI(reg, ac)		MIPS32_R_INST(0, ac, 0, reg, 0, MIPS32_OP_MFHI)
+#define MIPS32_DSP_MFLO(reg, ac)	MIPS32_R_INST(0, ac, 0, reg, 0, MIPS32_OP_MFLO)
+#define MIPS32_DSP_MTLO(reg, ac)	MIPS32_S_INST(reg, ac, MIPS32_OP_MTLO)
+#define MIPS32_DSP_MTHI(reg, ac)	MIPS32_S_INST(reg, ac, MIPS32_OP_MTHI)
+#define MIPS32_DSP_RDDSP(rt, mask)	MIPS32_DSP_R_INST(rt, mask, 0x12, 0x38)
+#define MIPS32_DSP_WRDSP(rs, mask)	MIPS32_DSP_W_INST(rs, mask, 0x13, 0x38)
+
+
 /*
  * MIPS32 Config1 Register (CP0 Register 16, Select 1)
  */
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 22edf6a..aaf3875 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -588,6 +588,26 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	return ctx.retval;
 }
 
+int mips32_cp1_control_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp1_c_reg)
+{
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
+	pracc_queue_init(&ctx);
+
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
+	pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, cp1_c_reg));			/* move cp1c reg to $8 */
+	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* store $8 to pracc_out */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* restore $15 from DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8)));	/* restore upper 16 bits  of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */
+
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1);
+	pracc_queue_free(&ctx);
+	return ctx.retval;
+}
+
 /**
  * \b mips32_pracc_sync_cache
  *
@@ -856,6 +876,9 @@ int mips32_pracc_write_regs(struct mips32_common *mips32)
 	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	uint32_t *gprs = mips32->core_regs.gpr;
 	uint32_t *c0rs = mips32->core_regs.cp0;
+	bool fpu_in_64bit = ((c0rs[0] & BIT(MIPS32_CP0_STATUS_FR_SHIFT)) != 0);
+	bool fp_enabled = ((c0rs[0] & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0);
+	uint32_t rel = (ejtag_info->config[0] & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT;
 
 	pracc_queue_init(&ctx);
 
@@ -895,6 +918,31 @@ int mips32_pracc_write_regs(struct mips32_common *mips32)
 
 	if (mips32_cpu_support_hazard_barrier(ejtag_info))
 		pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
+
+	/* store FPRs */
+	if (mips32->fp_imp && fp_enabled) {
+		uint64_t *fprs = mips32->core_regs.fpr;
+		if (fpu_in_64bit) {
+			for (int i = 0; i != MIPS32_REG_FP_COUNT; i++) {
+				uint32_t fp_lo = fprs[i] & 0xffffffff;
+				uint32_t fp_hi = (fprs[i] >> 32) & 0xffffffff;
+				pracc_add_li32(&ctx, 2, fp_lo, 0);
+				pracc_add_li32(&ctx, 3, fp_hi, 0);
+				pracc_add(&ctx, 0, MIPS32_MTC1(ctx.isa, 2, i));
+				pracc_add(&ctx, 0, MIPS32_MTHC1(ctx.isa, 3, i));
+			}
+		} else {
+			for (int i = 0; i != MIPS32_REG_FP_COUNT; i++) {
+				uint32_t fp_lo = fprs[i] & 0xffffffff;
+				pracc_add_li32(&ctx, 2, fp_lo, 0);
+				pracc_add(&ctx, 0, MIPS32_MTC1(ctx.isa, 2, i));
+			}
+		}
+
+		if (rel > MIPS32_RELEASE_1)
+			pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa));
+	}
+
 	/* load registers 2 to 31 with li32, optimize */
 	for (int i = 2; i < 32; i++)
 		pracc_add_li32(&ctx, i, gprs[i], 1);
@@ -1014,6 +1062,9 @@ int mips32_pracc_read_regs(struct mips32_common *mips32)
 	struct mips32_core_regs *core_regs = &mips32->core_regs;
 	unsigned int offset_gpr = ((uint8_t *)&core_regs->gpr[0]) - (uint8_t *)core_regs;
 	unsigned int offset_cp0 = ((uint8_t *)&core_regs->cp0[0]) - (uint8_t *)core_regs;
+	unsigned int offset_fpr = ((uint8_t *)&core_regs->fpr[0]) - (uint8_t *)core_regs;
+	unsigned int offset_fpcr = ((uint8_t *)&core_regs->fpcr[0]) - (uint8_t *)core_regs;
+	bool fp_enabled;
 
 	/*
 	 * This procedure has to be in 2 distinctive steps, because we can
@@ -1040,11 +1091,64 @@ int mips32_pracc_read_regs(struct mips32_common *mips32)
 	ejtag_info->reg8 = mips32->core_regs.gpr[8];
 	ejtag_info->reg9 = mips32->core_regs.gpr[9];
 
+	if (ctx.retval != ERROR_OK)
+		return ctx.retval;
+
 	/* we only care if FP is actually impl'd and if cp1 is enabled */
 	/* since we already read cp0 in the prev step */
 	/* now we know what's in cp0.status */
-	/* TODO: Read FPRs */
+	fp_enabled = (mips32->core_regs.cp0[0] & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0;
+	if (mips32->fp_imp && fp_enabled) {
+		pracc_queue_init(&ctx);
 
+		mips32_pracc_store_regs_set_base_addr(&ctx);
+
+		/* FCSR */
+		pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, 31));
+		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset_fpcr,
+				  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset_fpcr, 1));
+
+		/* FIR */
+		pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, 0));
+		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset_fpcr + 4,
+				  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset_fpcr + 4, 1));
+
+		/* f0 to f31 */
+		if (mips32->fpu_in_64bit) {
+			for (int i = 0; i != 32; i++) {
+				size_t offset = offset_fpr + (i * 8);
+				/* current pracc implementation (or EJTAG itself) only supports 32b access */
+				/* so there is no way to use SDC1 */
+
+				/* lower half */
+				pracc_add(&ctx, 0, MIPS32_MFC1(ctx.isa, 8, i));
+				pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset,
+						  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset, 1));
+
+				/* upper half */
+				pracc_add(&ctx, 0, MIPS32_MFHC1(ctx.isa, 8, i));
+				pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset + 4,
+						  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset + 4, 1));
+			}
+		} else {
+			for (int i = 0; i != 32; i++) {
+				size_t offset = offset_fpr + (i * 8);
+				pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset,
+						  MIPS32_SWC1(ctx.isa, i, PRACC_OUT_OFFSET + offset, 1));
+			}
+		}
+
+		mips32_pracc_store_regs_restore(&ctx);
+
+		/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));
+		/* load $15 in DeSave */
+		pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0));
+
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, (uint32_t *)&mips32->core_regs, 1);
+
+		pracc_queue_free(&ctx);
+	}
 	return ctx.retval;
 }
 
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 78d0872..f78f891 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -103,6 +103,21 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info,
 int mips32_cp0_write(struct mips_ejtag *ejtag_info,
 		uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel);
 
+/**
+ * mips32_cp1_control_read
+ *
+ * @brief Simulates cfc1 ASM instruction (Move Control Word From Floating Point),
+ * i.e. implements copro C1 Control Register read.
+ *
+ * @param[in] ejtag_info
+ * @param[in] val Storage to hold read value
+ * @param[in] cp1_c_reg Number of copro C1 control register we want to read
+ *
+ * @return ERROR_OK on Success, ERROR_FAIL otherwise
+ */
+int mips32_cp1_control_read(struct mips_ejtag *ejtag_info,
+		uint32_t *val, uint32_t cp1_c_reg);
+
 static inline void pracc_swap16_array(struct mips_ejtag *ejtag_info, uint32_t *buf, int count)
 {
 	if (ejtag_info->isa && ejtag_info->endianness)
diff --git a/src/target/target.h b/src/target/target.h
index 303d5e6..c74b8c2 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -23,6 +23,7 @@
 #include <helper/list.h>
 #include "helper/replacements.h"
 #include "helper/system.h"
+#include <helper/types.h>
 #include <jim.h>
 
 struct reg;
diff --git a/src/target/xtensa/xtensa_chip.c b/src/target/xtensa/xtensa_chip.c
index ac758ed..ac4a49c 100644
--- a/src/target/xtensa/xtensa_chip.c
+++ b/src/target/xtensa/xtensa_chip.c
@@ -144,17 +144,7 @@ static int xtensa_chip_examine(struct target *target)
 
 static int xtensa_chip_jim_configure(struct target *target, struct jim_getopt_info *goi)
 {
-	static bool dap_configured;
-	int ret = adiv5_jim_configure(target, goi);
-	if (ret == JIM_OK) {
-		LOG_DEBUG("xtensa '-dap' target option found");
-		dap_configured = true;
-	}
-	if (!dap_configured) {
-		LOG_DEBUG("xtensa '-dap' target option not yet found, assuming JTAG...");
-		target->has_dap = false;
-	}
-	return ret;
+	return adiv5_jim_configure_ext(target, goi, NULL, ADI_CONFIGURE_DAP_OPTIONAL);
 }
 
 /** Methods for generic example of Xtensa-based chip-level targets. */
diff --git a/tcl/board/bemicro_cycloneiii.cfg b/tcl/board/bemicro_cycloneiii.cfg
index bd1459a..3c92b50 100644
--- a/tcl/board/bemicro_cycloneiii.cfg
+++ b/tcl/board/bemicro_cycloneiii.cfg
@@ -17,7 +17,8 @@ source [find fpga/altera-cycloneiii.cfg]
 #quartus_cpf --option=bitstream_compression=off -c output_files\cycloneiii_blinker.sof cycloneiii_blinker.rbf
 
 #openocd -f board/bemicro_cycloneiii.cfg -c "init" -c "pld load cycloneiii.pld cycloneiii_blinker.rbf"
-# "ipdbg -start -tap cycloneiii.tap -hub 0x00e -tool 0 -port 5555"
+# "ipdbg create-hub cycloneiii.ipdbghub -tap cycloneiii.tap -ir 0x00e"
+# "cycloneiii.ipdbghub ipdbg start -tool 0 -port 5555"
 
 
 set JTAGSPI_CHAIN_ID cycloneiii.pld
diff --git a/tcl/board/digilent_cmod_s7.cfg b/tcl/board/digilent_cmod_s7.cfg
index c52ee95..4fa45a1 100644
--- a/tcl/board/digilent_cmod_s7.cfg
+++ b/tcl/board/digilent_cmod_s7.cfg
@@ -15,7 +15,8 @@ adapter speed 10000
 
 source [find cpld/xilinx-xc7.cfg]
 
-# "ipdbg -start -tap xc7.tap -hub 0x02 -tool 0 -port 5555"
+# "ipdbg create-hub xc7.ipdbghub -tap xc7.tap -ir 0x02"
+# "xc7.ipdbghub ipdbg start -tool 0 -port 5555"
 #openocd -f board/digilent_cmod_s7.cfg -c "init" -c "pld load xc7.pld shared_folder/cmod_s7_fast.bit"
 
 set JTAGSPI_CHAIN_ID xc7.pld
diff --git a/tcl/board/ecp5_evaluation.cfg b/tcl/board/ecp5_evaluation.cfg
index dd663f7..71769f6 100644
--- a/tcl/board/ecp5_evaluation.cfg
+++ b/tcl/board/ecp5_evaluation.cfg
@@ -16,7 +16,8 @@ adapter speed 6000
 source [find fpga/lattice_ecp5.cfg]
 
 #openocd -f board/ecp5_evaluation.cfg -c "init" -c "pld load ecp5.pld shared_folder/ecp5_blinker_impl1.bit"
-#ipdbg -start -tap ecp5.tap -hub 0x32 -port 5555 -tool 0
+#ipdbg create-hub ecp5.ipdbghub -tap ecp5.tap -ir 0x32
+#ecp5.ipdbghub ipdbg start -tool 0 -port 5555
 
 set JTAGSPI_CHAIN_ID ecp5.pld
 source [find cpld/jtagspi.cfg]
diff --git a/tcl/board/gowin_runber.cfg b/tcl/board/gowin_runber.cfg
index 9496c6f..6cb0736 100644
--- a/tcl/board/gowin_runber.cfg
+++ b/tcl/board/gowin_runber.cfg
@@ -16,4 +16,5 @@ source [find fpga/gowin_gw1n.cfg]
 
 
 #openocd -f board/gowin_runber.cfg  -c "init" -c "pld load 0 impl/pnr/gw1n_blinker.fs"
-#ipdbg -start -tap gw1n.tap -hub 0x42 -port 5555 -tool 0
+#ipdbg create-hub gw1n.ipdbghub -tap gw1n.tap -ir 0x42
+#gw1n.ipdbghubipdbg start  -tool 0 -port 5555
diff --git a/tcl/board/trion_t20_bga256.cfg b/tcl/board/trion_t20_bga256.cfg
index dc76d39..ca44f0b 100644
--- a/tcl/board/trion_t20_bga256.cfg
+++ b/tcl/board/trion_t20_bga256.cfg
@@ -20,7 +20,8 @@ adapter speed 6000
 source [find fpga/efinix_trion.cfg]
 
 #openocd -f board/trion_t20_bga256.cfg -c "init" -c "pld load trion.pld outflow/trion_blinker.bit"
-#ipdbg -start -tap trion.tap -hub 0x8 -port 5555 -tool 0
+#ipdbg create-hub trion.ipdbghub -tap trion.tap -ir 0x8
+#trion.ipdbghub ipdbg start -tool 0 -port 5555
 
 set JTAGSPI_CHAIN_ID trion.pld
 source [find cpld/jtagspi.cfg]
diff --git a/tcl/target/nrf51.cfg b/tcl/target/nrf51.cfg
index 48c2715..3781ecc 100644
--- a/tcl/target/nrf51.cfg
+++ b/tcl/target/nrf51.cfg
@@ -45,13 +45,11 @@ if {![using_hla]} {
    cortex_m reset_config sysresetreq
 }
 
-flash bank $_CHIPNAME.flash nrf51 0x00000000 0 1 1 $_TARGETNAME
-flash bank $_CHIPNAME.uicr nrf51 0x10001000 0 1 1 $_TARGETNAME
+flash bank $_CHIPNAME.flash nrf5 0x00000000 0 0 0 $_TARGETNAME
+flash bank $_CHIPNAME.uicr nrf5 0x10001000 0 0 0 $_TARGETNAME
 
-#
 #  The chip should start up from internal 16Mhz RC, so setting adapter
 #  clock to 1Mhz should be OK
-#
 adapter speed 1000
 
 proc enable_all_ram {} {
@@ -60,4 +58,4 @@ proc enable_all_ram {} {
 	# resetting we enable all banks via the RAMON register
 	mww 0x40000524 0xF
 }
-$_TARGETNAME configure -event reset-end {  enable_all_ram }
+$_TARGETNAME configure -event reset-init {  enable_all_ram }