diff --git a/.travis.yml b/.travis.yml
index c7dc4247..2270229f 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,42 +1,11 @@
-dist: trusty
+dist: bionic
 sudo: required
 
 before_install:
   - sudo add-apt-repository -y ppa:team-gcc-arm-embedded/ppa
   - sudo apt-get update -qq
   - pip install --user intelhex
-  - gpg --recv-keys 3CEA9B8868BC3852618EB5B4707F91A424F006F5
-  - wget http://www.intra2net.com/en/developer/libftdi/download/libftdi1-1.2.tar.bz2
-  - wget http://www.intra2net.com/en/developer/libftdi/download/libftdi1-1.2.tar.bz2.sig
-  - gpg --trust-model always --verify libftdi1-1.2.tar.bz2.sig
-  - tar -xjf libftdi1-1.2.tar.bz2
-  - sudo apt-get install -y build-essential libboost-all-dev gcc-arm-embedded libusb-1.0-0-dev libhidapi-dev
-
-install:
-  - cd libftdi1-1.2
-  - if [ "$TRAVIS_OS_NAME" = "linux" ];
-    then
-      sudo apt-get update -qq;
-      if [ "$ARCH" = "x86_64" ];
-      then
-        sudo apt-get install -qq libusb-1.0-0-dev;
-      elif [ "$ARCH" = "i386" ];
-      then
-        sudo apt-get install -qq gcc-multilib libusb-1.0-0-dev:i386 pkg-config:i386;
-        export CFLAGS="-m32";
-      fi
-    fi
-  - if [ "$TRAVIS_OS_NAME" = "osx" ];
-    then
-      brew update;
-      brew install libusb;
-    fi
-  - mkdir build
-  - cd build
-  - cmake ../
-  - make
-  - sudo make install
-  - cd ../../
+  - sudo apt-get install -y build-essential libboost-all-dev gcc-arm-embedded libusb-1.0-0-dev libhidapi-dev libftdi1 libftdi1-dev
 
 script:
   - make -C libopencm3 lib
diff --git a/src/Makefile b/src/Makefile
index d2269b45..aa9adafc 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -62,7 +62,7 @@ SRC =			\
 
 include $(PLATFORM_DIR)/Makefile.inc
 
-OPT_FLAGS ?= -Og
+OPT_FLAGS ?= -Os
 CFLAGS += $(OPT_FLAGS)
 LDFLAGS += $(OPT_FLAGS)
 
diff --git a/src/platforms/f4discovery/platform.h b/src/platforms/f4discovery/platform.h
index 8f059094..a1a06f88 100644
--- a/src/platforms/f4discovery/platform.h
+++ b/src/platforms/f4discovery/platform.h
@@ -91,47 +91,54 @@
 #define SWDIO_MODE_DRIVE() \
 	gpio_mode_setup(SWDIO_PORT, GPIO_MODE_OUTPUT, \
 	                GPIO_PUPD_NONE, SWDIO_PIN);
-
+#define UART_PIN_SETUP() do { \
+	gpio_mode_setup(USBUSART_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
+	                USBUSART_TX_PIN); \
+	gpio_set_output_options(USBUSART_PORT, GPIO_OTYPE_PP, \
+					GPIO_OSPEED_100MHZ, USBUSART_TX_PIN); \
+	gpio_set_af(USBUSART_PORT, GPIO_AF7, USBUSART_TX_PIN); \
+	gpio_mode_setup(USBUSART_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, \
+	                USBUSART_RX_PIN); \
+	gpio_set_output_options(USBUSART_PORT, GPIO_OTYPE_OD, \
+					GPIO_OSPEED_100MHZ, USBUSART_RX_PIN); \
+	gpio_set_af(USBUSART_PORT, GPIO_AF7, USBUSART_RX_PIN); \
+} while(0)
 
 #define USB_DRIVER      stm32f107_usb_driver
 #define USB_IRQ         NVIC_OTG_FS_IRQ
-#define USB_ISR         otg_fs_isr
+#define USB_ISR(x)      otg_fs_isr(x)
 /* Interrupt priorities.  Low numbers are high priority.
- * For now USART1 preempts USB which may spin while buffer is drained.
  * TIM3 is used for traceswo capture and must be highest priority.
  */
-#define IRQ_PRI_USB		(2 << 4)
-#define IRQ_PRI_USBUSART	(1 << 4)
-#define IRQ_PRI_USBUSART_TIM	(3 << 4)
+#define IRQ_PRI_USB		(1 << 4)
+#define IRQ_PRI_USBUSART	(2 << 4)
+#define IRQ_PRI_USBUSART_DMA 	(2 << 4)
 #define IRQ_PRI_TRACE		(0 << 4)
 
 #define USBUSART USART3
 #define USBUSART_CR1 USART3_CR1
+#define USBUSART_DR USART3_DR
 #define USBUSART_IRQ NVIC_USART3_IRQ
 #define USBUSART_CLK RCC_USART3
-#define USBUSART_TX_PORT GPIOD
-#define USBUSART_TX_PIN  GPIO8
-#define USBUSART_RX_PORT GPIOD
-#define USBUSART_RX_PIN  GPIO9
-#define USBUSART_ISR usart3_isr
-#define USBUSART_TIM TIM4
-#define USBUSART_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM4)
-#define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
-#define USBUSART_TIM_ISR tim4_isr
-
-#define UART_PIN_SETUP() do { \
-	gpio_mode_setup(USBUSART_TX_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
-	                USBUSART_TX_PIN); \
-	gpio_mode_setup(USBUSART_RX_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
-	                USBUSART_RX_PIN); \
-	gpio_set_af(USBUSART_TX_PORT, GPIO_AF7, USBUSART_TX_PIN); \
-	gpio_set_af(USBUSART_RX_PORT, GPIO_AF7, USBUSART_RX_PIN); \
-    } while(0)
+#define USBUSART_PORT GPIOD
+#define USBUSART_TX_PIN GPIO8
+#define USBUSART_RX_PIN GPIO9
+#define USBUSART_ISR(x) usart3_isr(x)
+#define USBUSART_DMA_BUS DMA1
+#define USBUSART_DMA_CLK RCC_DMA1
+#define USBUSART_DMA_TX_CHAN DMA_STREAM3
+#define USBUSART_DMA_TX_IRQ NVIC_DMA1_STREAM3_IRQ
+#define USBUSART_DMA_TX_ISR(x) dma1_stream3_isr(x)
+#define USBUSART_DMA_RX_CHAN DMA_STREAM1
+#define USBUSART_DMA_RX_IRQ NVIC_DMA1_STREAM1_IRQ
+#define USBUSART_DMA_RX_ISR(x) dma1_stream1_isr(x)
+/* For STM32F4 DMA trigger source must be specified */
+#define USBUSART_DMA_TRG DMA_SxCR_CHSEL_4
 
 #define TRACE_TIM TIM3
 #define TRACE_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM3)
 #define TRACE_IRQ   NVIC_TIM3_IRQ
-#define TRACE_ISR   tim3_isr
+#define TRACE_ISR(x) tim3_isr(x)
 
 #define gpio_set_val(port, pin, val) do {	\
 	if(val)					\
diff --git a/src/platforms/hosted/bmp_serial.c b/src/platforms/hosted/bmp_serial.c
index b1050bd2..a7baa77c 100644
--- a/src/platforms/hosted/bmp_serial.c
+++ b/src/platforms/hosted/bmp_serial.c
@@ -178,6 +178,7 @@ static int scan_linux_id(char *name, char *type, char *version, char  *serial)
 		return -1;
 	}
 	strncpy(type, name, p - name);
+	type[p - name] = 0;
 	name = p;
 	while (*name != 'v')
 		name++;
@@ -191,6 +192,7 @@ static int scan_linux_id(char *name, char *type, char *version, char  *serial)
 		return -1;
 	}
 	strncpy(version, name, p - name);
+	version[p - name] = 0;
 	name = p;
 	while (*name == '_')
 		name++;
@@ -204,6 +206,7 @@ static int scan_linux_id(char *name, char *type, char *version, char  *serial)
 		return -1;
 	}
 	strncpy(serial, name, p - name);
+	serial[p - name] = 0;
 	return 0;
 }
 
@@ -245,8 +248,8 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 	if (found_bmps < 1) {
 		DEBUG_WARN("No BMP probe found\n");
 		return -1;
-	} else if (found_bmps > 1) {
-		DEBUG_INFO("Available Probes:\n");
+	} else if ((found_bmps > 1) || cl_opts->opt_list_only) {
+		DEBUG_WARN("Available Probes:\n");
 	}
 	dir = opendir(DEVICE_BY_ID);
 	i = 0;
@@ -258,7 +261,7 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 			if (scan_linux_id(dp->d_name, type, version, serial)) {
 				DEBUG_WARN("Unexpected device name found \"%s\"\n",
 						   dp->d_name);
-			} else if (found_bmps == 1) {
+			} else if ((found_bmps == 1) && (!cl_opts->opt_list_only)) {
 				strncpy(info->serial, serial, sizeof(info->serial));
 				found_bmps = 1;
 				strncpy(info->serial, serial, sizeof(info->serial));
@@ -266,13 +269,13 @@ int find_debuggers(BMP_CL_OPTIONS_t *cl_opts, bmp_info_t *info)
 				strncpy(info->product, type, sizeof(info->product));
 				strncpy(info->version, version, sizeof(info->version));
 				break;
-			} else if (found_bmps > 1) {
+			} else if (found_bmps > 0) {
 				DEBUG_WARN("%2d: %s, Black Sphere Technologies, Black Magic "
 						   "Probe (%s), %s\n", i, serial, type, version);
 			}
 		}
 	}
 	closedir(dir);
-	return (found_bmps == 1) ? 0 : 1;
+	return (found_bmps == 1 && !cl_opts->opt_list_only) ? 0 : 1;
 }
 #endif
diff --git a/src/platforms/hosted/ftdi_bmp.c b/src/platforms/hosted/ftdi_bmp.c
index 0d15113b..77cb369a 100644
--- a/src/platforms/hosted/ftdi_bmp.c
+++ b/src/platforms/hosted/ftdi_bmp.c
@@ -28,7 +28,7 @@
 #include <sys/time.h>
 
 #include "ftdi_bmp.h"
-#include <libftdi1/ftdi.h>
+#include <ftdi.h>
 
 struct ftdi_context *ftdic;
 
diff --git a/src/platforms/hosted/ftdi_bmp.h b/src/platforms/hosted/ftdi_bmp.h
index 51368194..355a1739 100644
--- a/src/platforms/hosted/ftdi_bmp.h
+++ b/src/platforms/hosted/ftdi_bmp.h
@@ -117,7 +117,7 @@ void libftdi_max_frequency_set(uint32_t freq) {};
 uint32_t libftdi_max_frequency_get(void) {return 0;};
 # pragma GCC diagnostic pop
 #else
-#include <libftdi1/ftdi.h>
+#include <ftdi.h>
 extern cable_desc_t cable_desc[];
 extern cable_desc_t *active_cable;
 extern struct ftdi_context *ftdic;
diff --git a/src/platforms/hosted/libftdi_jtagtap.c b/src/platforms/hosted/libftdi_jtagtap.c
index 043467be..905faaf9 100644
--- a/src/platforms/hosted/libftdi_jtagtap.c
+++ b/src/platforms/hosted/libftdi_jtagtap.c
@@ -26,7 +26,7 @@
 #include <unistd.h>
 #include <string.h>
 #include <assert.h>
-#include <libftdi1/ftdi.h>
+#include <ftdi.h>
 #include "platform.h"
 #include "ftdi_bmp.h"
 
diff --git a/src/platforms/hosted/libftdi_swdptap.c b/src/platforms/hosted/libftdi_swdptap.c
index cd2cf50c..b76cb06f 100644
--- a/src/platforms/hosted/libftdi_swdptap.c
+++ b/src/platforms/hosted/libftdi_swdptap.c
@@ -26,7 +26,7 @@
 #include <assert.h>
 
 #include "general.h"
-#include <libftdi1/ftdi.h>
+#include <ftdi.h>
 #include "platform.h"
 #include "ftdi_bmp.h"
 
diff --git a/src/platforms/hydrabus/platform.h b/src/platforms/hydrabus/platform.h
index c2c85333..93ccc75d 100644
--- a/src/platforms/hydrabus/platform.h
+++ b/src/platforms/hydrabus/platform.h
@@ -90,47 +90,54 @@
 #define SWDIO_MODE_DRIVE() \
 	gpio_mode_setup(SWDIO_PORT, GPIO_MODE_OUTPUT, \
 	                GPIO_PUPD_NONE, SWDIO_PIN);
-
+#define UART_PIN_SETUP() do { \
+	gpio_mode_setup(USBUSART_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
+	                USBUSART_TX_PIN); \
+	gpio_set_output_options(USBUSART_PORT, GPIO_OTYPE_PP, \
+					GPIO_OSPEED_100MHZ, USBUSART_TX_PIN); \
+	gpio_set_af(USBUSART_PORT, GPIO_AF7, USBUSART_TX_PIN); \
+	gpio_mode_setup(USBUSART_PORT, GPIO_MODE_AF, GPIO_PUPD_PULLUP, \
+	                USBUSART_RX_PIN); \
+	gpio_set_output_options(USBUSART_PORT, GPIO_OTYPE_OD, \
+					GPIO_OSPEED_100MHZ, USBUSART_RX_PIN); \
+	gpio_set_af(USBUSART_PORT, GPIO_AF7, USBUSART_RX_PIN); \
+} while(0)
 
 #define USB_DRIVER      stm32f107_usb_driver
 #define USB_IRQ         NVIC_OTG_FS_IRQ
-#define USB_ISR         otg_fs_isr
+#define USB_ISR(x)      otg_fs_isr(x)
 /* Interrupt priorities.  Low numbers are high priority.
- * For now USART1 preempts USB which may spin while buffer is drained.
  * TIM3 is used for traceswo capture and must be highest priority.
  */
-#define IRQ_PRI_USB		(2 << 4)
-#define IRQ_PRI_USBUSART	(1 << 4)
-#define IRQ_PRI_USBUSART_TIM	(3 << 4)
+#define IRQ_PRI_USB		(1 << 4)
+#define IRQ_PRI_USBUSART	(2 << 4)
+#define IRQ_PRI_USBUSART_DMA 	(2 << 4)
 #define IRQ_PRI_TRACE		(0 << 4)
 
 #define USBUSART USART1
 #define USBUSART_CR1 USART1_CR1
+#define USBUSART_DR USART1_DR
 #define USBUSART_IRQ NVIC_USART1_IRQ
 #define USBUSART_CLK RCC_USART1
-#define USBUSART_TX_PORT GPIOA
-#define USBUSART_TX_PIN  GPIO9
-#define USBUSART_RX_PORT GPIOA
-#define USBUSART_RX_PIN  GPIO10
-#define USBUSART_ISR usart1_isr
-#define USBUSART_TIM TIM4
-#define USBUSART_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM4)
-#define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
-#define USBUSART_TIM_ISR tim4_isr
-
-#define UART_PIN_SETUP() do { \
-	gpio_mode_setup(USBUSART_TX_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
-	                USBUSART_TX_PIN); \
-	gpio_mode_setup(USBUSART_RX_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, \
-	                USBUSART_RX_PIN); \
-	gpio_set_af(USBUSART_TX_PORT, GPIO_AF7, USBUSART_TX_PIN); \
-	gpio_set_af(USBUSART_RX_PORT, GPIO_AF7, USBUSART_RX_PIN); \
-    } while(0)
+#define USBUSART_PORT GPIOA
+#define USBUSART_TX_PIN GPIO9
+#define USBUSART_RX_PIN GPIO10
+#define USBUSART_ISR(x) usart1_isr(x)
+#define USBUSART_DMA_BUS DMA2
+#define USBUSART_DMA_CLK RCC_DMA2
+#define USBUSART_DMA_TX_CHAN DMA_STREAM7
+#define USBUSART_DMA_TX_IRQ NVIC_DMA2_STREAM7_IRQ
+#define USBUSART_DMA_TX_ISR(x) dma2_stream7_isr(x)
+#define USBUSART_DMA_RX_CHAN DMA_STREAM5
+#define USBUSART_DMA_RX_IRQ NVIC_DMA2_STREAM5_IRQ
+#define USBUSART_DMA_RX_ISR(x) dma2_stream5_isr(x)
+/* For STM32F4 DMA trigger source must be specified */
+#define USBUSART_DMA_TRG DMA_SxCR_CHSEL_4
 
 #define TRACE_TIM TIM3
 #define TRACE_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM3)
 #define TRACE_IRQ   NVIC_TIM3_IRQ
-#define TRACE_ISR   tim3_isr
+#define TRACE_ISR(x) tim3_isr(x)
 
 #define gpio_set_val(port, pin, val) do {	\
 	if(val)					\
@@ -182,4 +189,3 @@ static inline int platform_hwversion(void)
 #endif
 
 #endif
-
diff --git a/src/platforms/native/platform.h b/src/platforms/native/platform.h
index 0ef4178f..87ce5ea8 100644
--- a/src/platforms/native/platform.h
+++ b/src/platforms/native/platform.h
@@ -132,39 +132,47 @@ int usbuart_debug_write(const char *buf, size_t len);
 	SWD_CR = cr; \
 } while(0)
 #define UART_PIN_SETUP() do { \
-	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_50_MHZ, \
 	              GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN); \
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_INPUT, \
+				  GPIO_CNF_INPUT_PULL_UPDOWN, USBUSART_RX_PIN); \
+	gpio_set(USBUSART_PORT, USBUSART_RX_PIN); \
 } while(0)
 
 #define USB_DRIVER st_usbfs_v1_usb_driver
 #define USB_IRQ    NVIC_USB_LP_CAN_RX0_IRQ
-#define USB_ISR    usb_lp_can_rx0_isr
+#define USB_ISR(x) usb_lp_can_rx0_isr(x)
 /* Interrupt priorities.  Low numbers are high priority.
- * For now USART1 preempts USB which may spin while buffer is drained.
  * TIM3 is used for traceswo capture and must be highest priority.
  */
-#define IRQ_PRI_USB             (2 << 4)
-#define IRQ_PRI_USBUSART        (1 << 4)
-#define IRQ_PRI_USBUSART_TIM    (3 << 4)
+#define IRQ_PRI_USB             (1 << 4)
+#define IRQ_PRI_USBUSART        (2 << 4)
+#define IRQ_PRI_USBUSART_DMA 	(2 << 4)
 #define IRQ_PRI_USB_VBUS        (14 << 4)
 #define IRQ_PRI_TRACE           (0 << 4)
 
 #define USBUSART USART1
 #define USBUSART_CR1 USART1_CR1
+#define USBUSART_DR USART1_DR
 #define USBUSART_IRQ NVIC_USART1_IRQ
 #define USBUSART_CLK RCC_USART1
 #define USBUSART_PORT GPIOA
 #define USBUSART_TX_PIN GPIO9
-#define USBUSART_ISR usart1_isr
-#define USBUSART_TIM TIM4
-#define USBUSART_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM4)
-#define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
-#define USBUSART_TIM_ISR tim4_isr
+#define USBUSART_RX_PIN GPIO10
+#define USBUSART_ISR(x) usart1_isr(x)
+#define USBUSART_DMA_BUS DMA1
+#define USBUSART_DMA_CLK RCC_DMA1
+#define USBUSART_DMA_TX_CHAN DMA_CHANNEL4
+#define USBUSART_DMA_TX_IRQ NVIC_DMA1_CHANNEL4_IRQ
+#define USBUSART_DMA_TX_ISR(x) dma1_channel4_isr(x)
+#define USBUSART_DMA_RX_CHAN DMA_CHANNEL5
+#define USBUSART_DMA_RX_IRQ NVIC_DMA1_CHANNEL5_IRQ
+#define USBUSART_DMA_RX_ISR(x) dma1_channel5_isr(x)
 
 #define TRACE_TIM TIM3
 #define TRACE_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM3)
 #define TRACE_IRQ   NVIC_TIM3_IRQ
-#define TRACE_ISR   tim3_isr
+#define TRACE_ISR(x)  tim3_isr(x)
 
 #define SET_RUN_STATE(state)	{running_status = (state);}
 #define SET_IDLE_STATE(state)	{gpio_set_val(LED_PORT, LED_IDLE_RUN, state);}
diff --git a/src/platforms/pc/cl_utils.c b/src/platforms/pc/cl_utils.c
index bca5acde..b67bd9bd 100644
--- a/src/platforms/pc/cl_utils.c
+++ b/src/platforms/pc/cl_utils.c
@@ -183,7 +183,7 @@ void cl_init(BMP_CL_OPTIONS_t *opt, int argc, char **argv)
 {
 	int c;
 	opt->opt_target_dev = 1;
-	opt->opt_flash_size = 16 * 1024 *1024;
+	opt->opt_flash_size = 0xffffffff;
 	opt->opt_flash_start = 0xffffffff;
 	opt->opt_max_swj_frequency = 4000000;
 	while((c = getopt(argc, argv, "eEhHv:d:f:s:I:c:Cln:m:M:wVtTa:S:jpP:rR")) != -1) {
@@ -364,6 +364,10 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 		num_targets = platform_jtag_scan(NULL);
 	} else {
 		num_targets = platform_adiv5_swdp_scan(opt->opt_targetid);
+		if (!num_targets) {
+			DEBUG_INFO("Scan SWD failed, trying JTAG!\n");
+			num_targets = platform_jtag_scan(NULL);
+		}
 	}
 	if (!num_targets) {
 		DEBUG_WARN("No target found\n");
@@ -395,7 +399,8 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 	}
 	/* Always scan memory map to find lowest flash */
 	/* List each defined Flash */
-	uint32_t flash_start = 0xffffffff;
+	uint32_t lowest_flash_start = 0xffffffff;
+	uint32_t lowest_flash_size = 0;
 	int n_flash = 0;
 	for (struct target_flash *f = t->flash; f; f = f->next)
 		n_flash++;
@@ -406,12 +411,16 @@ int cl_execute(BMP_CL_OPTIONS_t *opt)
 				DEBUG_INFO("Flash Start: 0x%08" PRIx32 " length = 0x%" PRIx32
 						   " blocksize 0x%" PRIx32 "\n",
 						   f->start, (uint32_t)f->length, (uint32_t)f->blocksize);
-				if (f->start < flash_start)
-					flash_start = f->start;
+				if (f->start < lowest_flash_start) {
+					lowest_flash_start = f->start;
+					lowest_flash_size = f->length;
+				}
 			}
 	}
 	if (opt->opt_flash_start == 0xffffffff)
-		opt->opt_flash_start = flash_start;
+		opt->opt_flash_start = lowest_flash_start;
+	if (opt->opt_flash_size == 0xffffffff)
+		opt->opt_flash_size = lowest_flash_size;
 	if (opt->opt_mode == BMP_MODE_SWJ_TEST) {
 		switch (t->core[0]) {
 		case 'M':
diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index a1d74f5f..7d79dd1b 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -3,7 +3,6 @@ ST_BOOTLOADER ?=
 CC = $(CROSS_COMPILE)gcc
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
-OPT_FLAGS = -Os
 CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DDISCOVERY_STLINK -I../libopencm3/include \
 	-I platforms/stm32
diff --git a/src/platforms/stlink/platform.h b/src/platforms/stlink/platform.h
index 12c498d9..f848ae79 100644
--- a/src/platforms/stlink/platform.h
+++ b/src/platforms/stlink/platform.h
@@ -87,33 +87,41 @@ int usbuart_debug_write(const char *buf, size_t len);
 	cr  |=  (0x1 * SWD_CR_MULT); \
 	SWD_CR = cr; \
 } while(0)
-#define UART_PIN_SETUP() \
-	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \
-	              GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN);
+#define UART_PIN_SETUP() do { \
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_50_MHZ, \
+	              GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN); \
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_INPUT, \
+				  GPIO_CNF_INPUT_PULL_UPDOWN, USBUSART_RX_PIN); \
+	gpio_set(USBUSART_PORT, USBUSART_RX_PIN); \
+} while(0)
 
 #define USB_DRIVER      st_usbfs_v1_usb_driver
 #define USB_IRQ	        NVIC_USB_LP_CAN_RX0_IRQ
-#define USB_ISR	        usb_lp_can_rx0_isr
-/* Interrupt priorities.  Low numbers are high priority.
- * For now USART2 preempts USB which may spin while buffer is drained.
- */
-#define IRQ_PRI_USB		(2 << 4)
-#define IRQ_PRI_USBUSART	(1 << 4)
-#define IRQ_PRI_USBUSART_TIM	(3 << 4)
+#define USB_ISR(x)      usb_lp_can_rx0_isr(x)
+/* Interrupt priorities.  Low numbers are high priority. */
+#define IRQ_PRI_USB		(1 << 4)
+#define IRQ_PRI_USBUSART	(2 << 4)
+#define IRQ_PRI_USBUSART_DMA 	(2 << 4)
 #define IRQ_PRI_USB_VBUS	(14 << 4)
-#define IRQ_PRI_SWO_DMA			(1 << 4)
+#define IRQ_PRI_SWO_DMA			(0 << 4)
 
 #define USBUSART USART2
 #define USBUSART_CR1 USART2_CR1
+#define USBUSART_DR USART2_DR
 #define USBUSART_IRQ NVIC_USART2_IRQ
 #define USBUSART_CLK RCC_USART2
 #define USBUSART_PORT GPIOA
 #define USBUSART_TX_PIN GPIO2
-#define USBUSART_ISR usart2_isr
-#define USBUSART_TIM TIM4
-#define USBUSART_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM4)
-#define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
-#define USBUSART_TIM_ISR tim4_isr
+#define USBUSART_RX_PIN GPIO3
+#define USBUSART_ISR(x) usart2_isr(x)
+#define USBUSART_DMA_BUS DMA1
+#define USBUSART_DMA_CLK RCC_DMA1
+#define USBUSART_DMA_TX_CHAN DMA_CHANNEL7
+#define USBUSART_DMA_TX_IRQ NVIC_DMA1_CHANNEL7_IRQ
+#define USBUSART_DMA_TX_ISR(x) dma1_channel7_isr(x)
+#define USBUSART_DMA_RX_CHAN DMA_CHANNEL6
+#define USBUSART_DMA_RX_IRQ NVIC_DMA1_CHANNEL6_IRQ
+#define USBUSART_DMA_RX_ISR(x) dma1_channel6_isr(x)
 
 /* On F103, only USART1 is on AHB2 and can reach 4.5 MBaud at 72 MHz.*/
 #define SWO_UART				USART1
@@ -172,4 +180,3 @@ extern uint32_t detect_rev(void);
 
 
 #endif
-
diff --git a/src/platforms/stm32/usbuart.c b/src/platforms/stm32/usbuart.c
index d2dc215e..05b57df6 100644
--- a/src/platforms/stm32/usbuart.c
+++ b/src/platforms/stm32/usbuart.c
@@ -21,7 +21,8 @@
 #include <libopencm3/stm32/rcc.h>
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/stm32/usart.h>
-#include <libopencm3/stm32/timer.h>
+#include <libopencm3/stm32/dma.h>
+#include <libopencm3/cm3/cortex.h>
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/cm3/scs.h>
 #include <libopencm3/usb/usbd.h>
@@ -30,102 +31,143 @@
 #include "general.h"
 #include "cdcacm.h"
 
-#define USBUART_TIMER_FREQ_HZ 1000000U /* 1us per tick */
-#define USBUART_RUN_FREQ_HZ 5000U /* 200us (or 100 characters at 2Mbps) */
+#ifdef STM32F4
+#define dma_channel_reset(dma, channel) dma_stream_reset(dma, channel)
+#define dma_enable_channel(dma, channel) dma_enable_stream(dma, channel)
+#define dma_disable_channel(dma, channel) dma_disable_stream(dma, channel)
 
-#define FIFO_SIZE 128
+#define DMA_PSIZE_8BIT DMA_SxCR_PSIZE_8BIT
+#define DMA_MSIZE_8BIT DMA_SxCR_MSIZE_8BIT
+#define DMA_PL_HIGH	DMA_SxCR_PL_HIGH
+#define DMA_CGIF DMA_ISR_FLAGS
+#else
+#define DMA_PSIZE_8BIT DMA_CCR_PSIZE_8BIT
+#define DMA_MSIZE_8BIT DMA_CCR_MSIZE_8BIT
+#define DMA_PL_HIGH	DMA_CCR_PL_HIGH
+#define DMA_CGIF DMA_IFCR_CGIF_BIT
+#endif
 
-/* RX Fifo buffer */
-static uint8_t buf_rx[FIFO_SIZE];
-/* Fifo in pointer, writes assumed to be atomic, should be only incremented within RX ISR */
-static uint8_t buf_rx_in;
-/* Fifo out pointer, writes assumed to be atomic, should be only incremented outside RX ISR */
+#define TX_LED_ACT (1 << 0)
+#define RX_LED_ACT (1 << 1)
+
+#define RX_FIFO_SIZE (128)
+#define TX_BUF_SIZE (128)
+
+/* TX double buffer */
+static uint8_t buf_tx[TX_BUF_SIZE * 2];
+/* Active buffer part idx */
+static uint8_t buf_tx_act_idx;
+/* Active buffer part used capacity */
+static uint8_t buf_tx_act_sz;
+/* TX transfer complete */
+static bool tx_trfr_cplt = true;
+/* RX Fifo buffer with space for copy fn overrun */
+static uint8_t buf_rx[RX_FIFO_SIZE + sizeof(uint64_t)];
+/* RX Fifo out pointer, writes assumed to be atomic */
 static uint8_t buf_rx_out;
+/* RX usb transfer complete */
+static bool rx_usb_trfr_cplt = true;
+
+#ifdef USBUART_DEBUG
+/* Debug Fifo buffer with space for copy fn overrun */
+static uint8_t usb_dbg_buf[RX_FIFO_SIZE + sizeof(uint64_t)];
+/* Debug Fifo in pointer */
+static uint8_t usb_dbg_in;
+/* Debug Fifo out pointer */
+static uint8_t usb_dbg_out;
+#endif
 
 static void usbuart_run(void);
 
+/*
+ * Update led state atomically respecting RX anb TX states.
+ */
+static void usbuart_set_led_state(uint8_t ledn, bool state)
+{
+	CM_ATOMIC_CONTEXT();
+
+	static uint8_t led_state = 0;
+
+	if (state)
+	{
+		led_state |= ledn;
+		gpio_set(LED_PORT_UART, LED_UART);
+	}
+	else
+	{
+		led_state &= ~ledn;
+		if (!led_state)
+			gpio_clear(LED_PORT_UART, LED_UART);
+	}
+}
+
 void usbuart_init(void)
 {
+	/* Enable clocks */
 	rcc_periph_clock_enable(USBUSART_CLK);
+	rcc_periph_clock_enable(USBUSART_DMA_CLK);
 
+	/* Setup UART parameters */
 	UART_PIN_SETUP();
-
-	/* Setup UART parameters. */
 	usart_set_baudrate(USBUSART, 38400);
 	usart_set_databits(USBUSART, 8);
 	usart_set_stopbits(USBUSART, USART_STOPBITS_1);
 	usart_set_mode(USBUSART, USART_MODE_TX_RX);
 	usart_set_parity(USBUSART, USART_PARITY_NONE);
 	usart_set_flow_control(USBUSART, USART_FLOWCONTROL_NONE);
+	USBUSART_CR1 |= USART_CR1_IDLEIE;
 
-	/* Finally enable the USART. */
-	usart_enable(USBUSART);
+	/* Setup USART TX DMA */
+	dma_channel_reset(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+	dma_set_peripheral_address(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, (uint32_t)&USBUSART_DR);
+	dma_enable_memory_increment_mode(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+	dma_set_peripheral_size(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_PSIZE_8BIT);
+	dma_set_memory_size(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_MSIZE_8BIT);
+	dma_set_priority(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_PL_HIGH);
+	dma_enable_transfer_complete_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+#ifdef STM32F4
+	dma_set_transfer_mode(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_SxCR_DIR_MEM_TO_PERIPHERAL);
+	dma_channel_select(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, USBUSART_DMA_TRG);
+	dma_set_dma_flow_control(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+	dma_enable_direct_mode(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+#else
+	dma_set_read_from_memory(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+#endif
+
+	/* Setup USART RX DMA */
+	dma_channel_reset(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+	dma_set_peripheral_address(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, (uint32_t)&USBUSART_DR);
+	dma_set_memory_address(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, (uint32_t)buf_rx);
+	dma_set_number_of_data(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, RX_FIFO_SIZE);
+	dma_enable_memory_increment_mode(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+	dma_enable_circular_mode(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+	dma_set_peripheral_size(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_PSIZE_8BIT);
+	dma_set_memory_size(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_MSIZE_8BIT);
+	dma_set_priority(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_PL_HIGH);
+	dma_enable_half_transfer_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+	dma_enable_transfer_complete_interrupt(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+#ifdef STM32F4
+	dma_set_transfer_mode(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_SxCR_DIR_PERIPHERAL_TO_MEM);
+	dma_channel_select(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, USBUSART_DMA_TRG);
+	dma_set_dma_flow_control(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+	dma_enable_direct_mode(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+#else
+	dma_set_read_from_peripheral(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
+#endif
+	dma_enable_channel(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN);
 
 	/* Enable interrupts */
-	USBUSART_CR1 |= USART_CR1_RXNEIE;
 	nvic_set_priority(USBUSART_IRQ, IRQ_PRI_USBUSART);
+	nvic_set_priority(USBUSART_DMA_TX_IRQ, IRQ_PRI_USBUSART_DMA);
+	nvic_set_priority(USBUSART_DMA_RX_IRQ, IRQ_PRI_USBUSART_DMA);
 	nvic_enable_irq(USBUSART_IRQ);
+	nvic_enable_irq(USBUSART_DMA_TX_IRQ);
+	nvic_enable_irq(USBUSART_DMA_RX_IRQ);
 
-	/* Setup timer for running deferred FIFO processing */
-	USBUSART_TIM_CLK_EN();
-	timer_set_mode(USBUSART_TIM, TIM_CR1_CKD_CK_INT,
-			TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
-	timer_set_prescaler(USBUSART_TIM,
-			rcc_apb2_frequency / USBUART_TIMER_FREQ_HZ * 2 - 1);
-	timer_set_period(USBUSART_TIM,
-			USBUART_TIMER_FREQ_HZ / USBUART_RUN_FREQ_HZ - 1);
-
-	/* Setup update interrupt in NVIC */
-	nvic_set_priority(USBUSART_TIM_IRQ, IRQ_PRI_USBUSART_TIM);
-	nvic_enable_irq(USBUSART_TIM_IRQ);
-
-	/* turn the timer on */
-	timer_enable_counter(USBUSART_TIM);
-}
-
-/*
- * Runs deferred processing for usb uart rx, draining RX FIFO by sending
- * characters to host PC via CDCACM.  Allowed to read from FIFO in pointer,
- * but not write to it. Allowed to write to FIFO out pointer.
- */
-static void usbuart_run(void)
-{
-	/* forcibly empty fifo if no USB endpoint */
-	if (cdcacm_get_config() != 1)
-	{
-		buf_rx_out = buf_rx_in;
-	}
-
-	/* if fifo empty, nothing further to do */
-	if (buf_rx_in == buf_rx_out) {
-		/* turn off LED, disable IRQ */
-		timer_disable_irq(USBUSART_TIM, TIM_DIER_UIE);
-		gpio_clear(LED_PORT_UART, LED_UART);
-	}
-	else
-	{
-		uint8_t packet_buf[CDCACM_PACKET_SIZE];
-		uint8_t packet_size = 0;
-		uint8_t buf_out = buf_rx_out;
-
-		/* copy from uart FIFO into local usb packet buffer */
-		while (buf_rx_in != buf_out && packet_size < CDCACM_PACKET_SIZE)
-		{
-			packet_buf[packet_size++] = buf_rx[buf_out++];
-
-			/* wrap out pointer */
-			if (buf_out >= FIFO_SIZE)
-			{
-				buf_out = 0;
-			}
-
-		}
-
-		/* advance fifo out pointer by amount written */
-		buf_rx_out += usbd_ep_write_packet(usbdev,
-				CDCACM_UART_ENDPOINT, packet_buf, packet_size);
-		buf_rx_out %= FIFO_SIZE;
-	}
+	/* Finally enable the USART */
+	usart_enable(USBUSART);
+	usart_enable_tx_dma(USBUSART);
+	usart_enable_rx_dma(USBUSART);
 }
 
 void usbuart_set_line_coding(struct usb_cdc_line_coding *coding)
@@ -133,9 +175,9 @@ void usbuart_set_line_coding(struct usb_cdc_line_coding *coding)
 	usart_set_baudrate(USBUSART, coding->dwDTERate);
 
 	if (coding->bParityType)
-		usart_set_databits(USBUSART, coding->bDataBits + 1);
+		usart_set_databits(USBUSART, (coding->bDataBits + 1 <= 8 ? 8 : 9));
 	else
-		usart_set_databits(USBUSART, coding->bDataBits);
+		usart_set_databits(USBUSART, (coding->bDataBits <= 8 ? 8 : 9));
 
 	switch(coding->bCharFormat) {
 	case 0:
@@ -145,6 +187,7 @@ void usbuart_set_line_coding(struct usb_cdc_line_coding *coding)
 		usart_set_stopbits(USBUSART, USART_STOPBITS_1_5);
 		break;
 	case 2:
+	default:
 		usart_set_stopbits(USBUSART, USART_STOPBITS_2);
 		break;
 	}
@@ -157,100 +200,240 @@ void usbuart_set_line_coding(struct usb_cdc_line_coding *coding)
 		usart_set_parity(USBUSART, USART_PARITY_ODD);
 		break;
 	case 2:
+	default:
 		usart_set_parity(USBUSART, USART_PARITY_EVEN);
 		break;
 	}
 }
 
+/*
+ * Copy data from fifo into continuous buffer. Return copied length.
+ */
+static uint32_t copy_from_fifo(uint8_t *dst, const uint8_t *src, uint32_t start, uint32_t end, uint32_t len, uint32_t fifo_sz)
+{
+	uint32_t out_len = 0;
+	for (uint32_t buf_out = start; buf_out != end && out_len < len; buf_out %= fifo_sz)
+		dst[out_len++] = src[buf_out++];
+
+	return out_len;
+}
+
+/*
+ * Changes USBUSART TX buffer in which data is accumulated from USB.
+ * Filled buffer is submitted to DMA for transfer.
+ */
+static void usbuart_change_dma_tx_buf(void)
+{
+	/* Select buffer for transmission */
+	uint8_t *const tx_buf_ptr = &buf_tx[buf_tx_act_idx * TX_BUF_SIZE];
+
+	/* Configure DMA */
+	dma_set_memory_address(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, (uint32_t)tx_buf_ptr);
+	dma_set_number_of_data(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, buf_tx_act_sz);
+	dma_enable_channel(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+
+	/* Change active buffer */
+	buf_tx_act_sz = 0;
+	buf_tx_act_idx ^= 1;
+}
+
 void usbuart_usb_out_cb(usbd_device *dev, uint8_t ep)
 {
 	(void)ep;
 
-	char buf[CDCACM_PACKET_SIZE];
-	int len = usbd_ep_read_packet(dev, CDCACM_UART_ENDPOINT,
-					buf, CDCACM_PACKET_SIZE);
+	usbd_ep_nak_set(dev, CDCACM_UART_ENDPOINT, 1);
+
+	/* Read new packet directly into TX buffer */
+	uint8_t *const tx_buf_ptr = &buf_tx[buf_tx_act_idx * TX_BUF_SIZE];
+	const uint16_t len = usbd_ep_read_packet(dev, CDCACM_UART_ENDPOINT,
+						tx_buf_ptr + buf_tx_act_sz, CDCACM_PACKET_SIZE);
 
 #if defined(BLACKMAGIC)
 	/* Don't bother if uart is disabled.
 	 * This will be the case on mini while we're being debugged.
 	 */
 	if(!(RCC_APB2ENR & RCC_APB2ENR_USART1EN))
+	{
+		usbd_ep_nak_set(dev, CDCACM_UART_ENDPOINT, 0);
 		return;
+	}
 #endif
 
-	gpio_set(LED_PORT_UART, LED_UART);
-	for(int i = 0; i < len; i++)
-		usart_send_blocking(USBUSART, buf[i]);
-	gpio_clear(LED_PORT_UART, LED_UART);
+	if (len)
+	{
+		buf_tx_act_sz += len;
+
+		/* If DMA is idle, schedule new transfer */
+		if (tx_trfr_cplt)
+		{
+			tx_trfr_cplt = false;
+			usbuart_change_dma_tx_buf();
+
+			/* Enable LED */
+			usbuart_set_led_state(TX_LED_ACT, true);
+		}
+	}
+
+	/* Enable USBUART TX packet reception if buffer has enough space */
+	if (TX_BUF_SIZE - buf_tx_act_sz >= CDCACM_PACKET_SIZE)
+		usbd_ep_nak_set(dev, CDCACM_UART_ENDPOINT, 0);
 }
 
 #ifdef USBUART_DEBUG
 int usbuart_debug_write(const char *buf, size_t len)
 {
-	for (size_t i = 0; i < len; i++) {
-		if (buf[i] == '\n') {
-			buf_rx[buf_rx_in++] = '\r';
-			buf_rx_in %= FIFO_SIZE;
+	if (nvic_get_active_irq(USB_IRQ) || nvic_get_active_irq(USBUSART_IRQ) || nvic_get_active_irq(USBUSART_DMA_RX_IRQ))
+		return 0;
+
+	CM_ATOMIC_CONTEXT();
+
+	for (size_t i = 0; i < len && (usb_dbg_in + 1) % RX_FIFO_SIZE != usb_dbg_out; i++)
+	{
+		if (buf[i] == '\n')
+		{
+			usb_dbg_buf[usb_dbg_in++] = '\r';
+			usb_dbg_in %= RX_FIFO_SIZE;
+
+			if ((usb_dbg_in + 1) % RX_FIFO_SIZE == usb_dbg_out)
+				break;
 		}
-		buf_rx[buf_rx_in++] = buf[i];
-		buf_rx_in %= FIFO_SIZE;
+		usb_dbg_buf[usb_dbg_in++] = buf[i];
+		usb_dbg_in %= RX_FIFO_SIZE;
 	}
-	/* enable deferred processing if we put data in the FIFO */
-	timer_enable_irq(USBUSART_TIM, TIM_DIER_UIE);
+
+	usbuart_run();
+
 	return len;
 }
 #endif
 
-void usbuart_usb_in_cb(usbd_device *dev, uint8_t ep)
-{
-	(void) dev;
-	(void) ep;
-}
-
 /*
- * Read a character from the UART RX and stuff it in a software FIFO.
- * Allowed to read from FIFO out pointer, but not write to it.
- * Allowed to write to FIFO in pointer.
+ * Runs deferred processing for USBUSART RX, draining RX FIFO by sending
+ * characters to host PC via CDCACM. Allowed to write to FIFO OUT pointer.
  */
-void USBUSART_ISR(void)
+static void usbuart_send_rx_packet(void)
 {
-	uint32_t err = USART_SR(USBUSART);
-	char c = usart_recv(USBUSART);
-#if !defined(USART_SR_NE) && defined(USART_ISR_NF)
-# define USART_SR_NE USART_ISR_NF
+	rx_usb_trfr_cplt = false;
+	/* Calculate writing position in the FIFO */
+	const uint32_t buf_rx_in = (RX_FIFO_SIZE - dma_get_number_of_data(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN)) % RX_FIFO_SIZE;
+
+	/* Forcibly empty fifo if no USB endpoint.
+	 * If fifo empty, nothing further to do. */
+	if (cdcacm_get_config() != 1 || (buf_rx_in == buf_rx_out
+#ifdef USBUART_DEBUG
+		&& usb_dbg_in == usb_dbg_out
 #endif
-	if (err & (USART_FLAG_ORE | USART_FLAG_FE | USART_SR_NE))
-		return;
-
-	/* Turn on LED */
-	gpio_set(LED_PORT_UART, LED_UART);
-
-	/* If the next increment of rx_in would put it at the same point
-	* as rx_out, the FIFO is considered full.
-	*/
-	if (((buf_rx_in + 1) % FIFO_SIZE) != buf_rx_out)
+	))
 	{
-		/* insert into FIFO */
-		buf_rx[buf_rx_in++] = c;
+#ifdef USBUART_DEBUG
+		usb_dbg_out = usb_dbg_in;
+#endif
+		buf_rx_out = buf_rx_in;
+		/* Turn off LED */
+		usbuart_set_led_state(RX_LED_ACT, false);
+		rx_usb_trfr_cplt = true;
+	}
+	else
+	{
+		/* To avoid the need of sending ZLP don't transmit full packet.
+		 * Also reserve space for copy function overrun.
+		 */
+		uint8_t packet_buf[CDCACM_PACKET_SIZE - 1 + sizeof(uint64_t)];
+		uint32_t packet_size;
 
-		/* wrap out pointer */
-		if (buf_rx_in >= FIFO_SIZE)
+#ifdef USBUART_DEBUG
+		/* Copy data from DEBUG FIFO into local usb packet buffer */
+		packet_size = copy_from_fifo(packet_buf, usb_dbg_buf, usb_dbg_out, usb_dbg_in, CDCACM_PACKET_SIZE - 1, RX_FIFO_SIZE);
+		/* Send if buffer not empty */
+		if (packet_size)
 		{
-			buf_rx_in = 0;
+			const uint16_t written = usbd_ep_write_packet(usbdev, CDCACM_UART_ENDPOINT, packet_buf, packet_size);
+			usb_dbg_out = (usb_dbg_out + written) % RX_FIFO_SIZE;
+			return;
 		}
+#endif
 
-		/* enable deferred processing if we put data in the FIFO */
-		timer_enable_irq(USBUSART_TIM, TIM_DIER_UIE);
+		/* Copy data from uart RX FIFO into local usb packet buffer */
+		packet_size = copy_from_fifo(packet_buf, buf_rx, buf_rx_out, buf_rx_in, CDCACM_PACKET_SIZE - 1, RX_FIFO_SIZE);
+
+		/* Advance fifo out pointer by amount written */
+		const uint16_t written = usbd_ep_write_packet(usbdev, CDCACM_UART_ENDPOINT, packet_buf, packet_size);
+		buf_rx_out = (buf_rx_out + written) % RX_FIFO_SIZE;
 	}
 }
 
-void USBUSART_TIM_ISR(void)
+void usbuart_usb_in_cb(usbd_device *dev, uint8_t ep)
 {
-	/* need to clear timer update event */
-	timer_clear_flag(USBUSART_TIM, TIM_SR_UIF);
+	(void) ep;
+	(void) dev;
 
-	/* process FIFO */
+	usbuart_send_rx_packet();
+}
+
+static void usbuart_run(void)
+{
+	nvic_disable_irq(USB_IRQ);
+
+	/* Enable LED */
+	usbuart_set_led_state(RX_LED_ACT, true);
+
+	/* Try to send a packet if usb is idle */
+	if (rx_usb_trfr_cplt)
+		usbuart_send_rx_packet();
+
+	nvic_enable_irq(USB_IRQ);
+}
+
+void USBUSART_ISR(void)
+{
+	nvic_disable_irq(USBUSART_DMA_RX_IRQ);
+
+	/* Get IDLE flag and reset interrupt flags */
+	const bool isIdle = usart_get_flag(USBUSART, USART_FLAG_IDLE);
+	usart_recv(USBUSART);
+
+	/* If line is now idle, then transmit a packet */
+	if (isIdle)
+		usbuart_run();
+
+	nvic_enable_irq(USBUSART_DMA_RX_IRQ);
+}
+
+void USBUSART_DMA_TX_ISR(void)
+{
+	nvic_disable_irq(USB_IRQ);
+
+	/* Stop DMA */
+	dma_disable_channel(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN);
+	dma_clear_interrupt_flags(USBUSART_DMA_BUS, USBUSART_DMA_TX_CHAN, DMA_CGIF);
+
+	/* If new buffer is ready, continue transmission.
+	 * Otherwise report transfer completion.
+	 */
+	if (buf_tx_act_sz)
+	{
+		usbuart_change_dma_tx_buf();
+		usbd_ep_nak_set(usbdev, CDCACM_UART_ENDPOINT, 0);
+	}
+	else
+	{
+		usbuart_set_led_state(TX_LED_ACT, false);
+		tx_trfr_cplt = true;
+	}
+
+	nvic_enable_irq(USB_IRQ);
+}
+
+void USBUSART_DMA_RX_ISR(void)
+{
+	nvic_disable_irq(USBUSART_IRQ);
+
+	/* Clear flags */
+	dma_clear_interrupt_flags(USBUSART_DMA_BUS, USBUSART_DMA_RX_CHAN, DMA_CGIF);
+	/* Transmit a packet */
 	usbuart_run();
+
+	nvic_enable_irq(USBUSART_IRQ);
 }
 
 #ifdef ENABLE_DEBUG
diff --git a/src/platforms/swlink/Makefile.inc b/src/platforms/swlink/Makefile.inc
index 5cad72ef..18841fb2 100644
--- a/src/platforms/swlink/Makefile.inc
+++ b/src/platforms/swlink/Makefile.inc
@@ -2,7 +2,6 @@ CROSS_COMPILE ?= arm-none-eabi-
 CC = $(CROSS_COMPILE)gcc
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
-OPT_FLAGS = -Os
 CFLAGS += -mcpu=cortex-m3 -mthumb \
 	-DSTM32F1 -DDISCOVERY_SWLINK -I../libopencm3/include \
 	-I platforms/stm32
diff --git a/src/platforms/swlink/platform.h b/src/platforms/swlink/platform.h
index 0eb7f4d4..36cab71c 100644
--- a/src/platforms/swlink/platform.h
+++ b/src/platforms/swlink/platform.h
@@ -83,39 +83,47 @@ int usbuart_debug_write(const char *buf, size_t len);
 } while(0)
 #define UART_PIN_SETUP() do { \
 	AFIO_MAPR |= AFIO_MAPR_USART1_REMAP; \
-	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_2_MHZ, \
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_OUTPUT_50_MHZ, \
 	              GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, USBUSART_TX_PIN); \
-} while (0)
+	gpio_set_mode(USBUSART_PORT, GPIO_MODE_INPUT, \
+				  GPIO_CNF_INPUT_PULL_UPDOWN, USBUSART_RX_PIN); \
+	gpio_set(USBUSART_PORT, USBUSART_RX_PIN); \
+} while(0)
 
 #define USB_DRIVER      st_usbfs_v1_usb_driver
 #define USB_IRQ         NVIC_USB_LP_CAN_RX0_IRQ
-#define USB_ISR         usb_lp_can_rx0_isr
+#define USB_ISR(x)      usb_lp_can_rx0_isr(x)
 /* Interrupt priorities.  Low numbers are high priority.
- * For now USART1 preempts USB which may spin while buffer is drained.
  * TIM2 is used for traceswo capture and must be highest priority.
  */
-#define IRQ_PRI_USB		(2 << 4)
-#define IRQ_PRI_USBUSART	(1 << 4)
-#define IRQ_PRI_USBUSART_TIM	(3 << 4)
+#define IRQ_PRI_USB		(1 << 4)
+#define IRQ_PRI_USBUSART	(2 << 4)
+#define IRQ_PRI_USBUSART_DMA 	(2 << 4)
 #define IRQ_PRI_USB_VBUS	(14 << 4)
 #define IRQ_PRI_SWO_DMA		(0 << 4)
 
 #define USBUSART USART1
 #define USBUSART_CR1 USART1_CR1
+#define USBUSART_DR USART1_DR
 #define USBUSART_IRQ NVIC_USART1_IRQ
 #define USBUSART_CLK RCC_USART1
 #define USBUSART_PORT GPIOB
 #define USBUSART_TX_PIN GPIO6
-#define USBUSART_ISR usart1_isr
-#define USBUSART_TIM TIM4
-#define USBUSART_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM4)
-#define USBUSART_TIM_IRQ NVIC_TIM4_IRQ
-#define USBUSART_TIM_ISR tim4_isr
+#define USBUSART_RX_PIN GPIO7
+#define USBUSART_ISR(x) usart1_isr(x)
+#define USBUSART_DMA_BUS DMA1
+#define USBUSART_DMA_CLK RCC_DMA1
+#define USBUSART_DMA_TX_CHAN DMA_CHANNEL4
+#define USBUSART_DMA_TX_IRQ NVIC_DMA1_CHANNEL4_IRQ
+#define USBUSART_DMA_TX_ISR(x) dma1_channel4_isr(x)
+#define USBUSART_DMA_RX_CHAN DMA_CHANNEL5
+#define USBUSART_DMA_RX_IRQ NVIC_DMA1_CHANNEL5_IRQ
+#define USBUSART_DMA_RX_ISR(x) dma1_channel5_isr(x)
 
 #define TRACE_TIM TIM2
 #define TRACE_TIM_CLK_EN() rcc_periph_clock_enable(RCC_TIM2)
 #define TRACE_IRQ   NVIC_TIM2_IRQ
-#define TRACE_ISR   tim2_isr
+#define TRACE_ISR(x)   tim2_isr(x)
 #define TRACE_IC_IN TIM_IC_IN_TI2
 #define TRACE_TRIG_IN TIM_SMCR_TS_IT1FP2
 
@@ -177,4 +185,3 @@ extern uint8_t detect_rev(void);
 #endif
 
 #endif
-
diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index 57a64560..50a42664 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -413,7 +413,17 @@ static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr, int recursion,
 	addr &= 0xfffff000; /* Mask out base address */
 	if (addr == 0) /* No rom table on this AP */
 		return;
-	uint32_t cidr = adiv5_ap_read_id(ap, addr + CIDR0_OFFSET);
+	volatile uint32_t cidr;
+	volatile struct exception e;
+	TRY_CATCH (e, EXCEPTION_TIMEOUT) {
+		cidr = adiv5_ap_read_id(ap, addr + CIDR0_OFFSET);
+	}
+	if (e.type) {
+		DEBUG_WARN("CIDR read timeout on AP%d, aborting.\n", num_entry);
+		adiv5_dp_abort(ap->dp, ADIV5_DP_ABORT_DAPABORT);
+		return;
+	}
+
 	if ((cidr & ~CID_CLASS_MASK) != CID_PREAMBLE)
 				return;
 #if defined(ENABLE_DEBUG)
@@ -603,7 +613,7 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	uint32_t cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
 	DEBUG_INFO("AP %3d: IDR=%08"PRIx32" CFG=%08"PRIx32" BASE=%08" PRIx32
 			   " CSW=%08"PRIx32, apsel, ap->idr, cfg, ap->base, ap->csw);
-	DEBUG_INFO(" (AHB-AP var%x rev%x)\n",
+	DEBUG_INFO(" (AHB-AP var%" PRIx32 " rev%" PRIx32 "\n",
 			   (ap->idr >> 4) & 0xf, ap->idr >> 28);
 #endif
 	adiv5_ap_ref(ap);
@@ -620,7 +630,8 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		free(dp);
 		return;
 	}
-	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%d %srev%d)\n", dp->idcode,
+	DEBUG_INFO("DPIDR 0x%08" PRIx32 " (v%" PRId32 " %srev%" PRId32 ")\n",
+			   dp->idcode,
 			   (dp->idcode >> 12) & 0xf,
 			   (dp->idcode & 0x10000) ? "MINDP " : "", dp->idcode >> 28);
 	volatile uint32_t ctrlstat = 0;
diff --git a/src/target/cortexm.c b/src/target/cortexm.c
index e3e2daf4..5c76eead 100644
--- a/src/target/cortexm.c
+++ b/src/target/cortexm.c
@@ -86,7 +86,7 @@ static int cortexm_breakwatch_clear(target *t, struct breakwatch *);
 static target_addr cortexm_check_watch(target *t);
 
 #define CORTEXM_MAX_WATCHPOINTS	4	/* architecture says up to 15, no implementation has > 4 */
-#define CORTEXM_MAX_BREAKPOINTS	6	/* architecture says up to 127, no implementation has > 6 */
+#define CORTEXM_MAX_BREAKPOINTS	8	/* architecture says up to 127, no implementation has > 8 */
 
 static int cortexm_hostio_request(target *t);
 
@@ -327,9 +327,11 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 		t->core = "M0";
 		break;
 	default:
-		DEBUG_WARN("Unexpected CortexM CPUID partno %04x\n", cpuid_partno);
+		DEBUG_WARN("Unexpected CortexM CPUID partno %04" PRIx32 "\n",
+				   cpuid_partno);
 	}
-	DEBUG_INFO("CPUID 0x%08" PRIx32 " (%s var %x rev %x)\n", t->cpuid,
+	DEBUG_INFO("CPUID 0x%08" PRIx32 " (%s var %" PRIx32 " rev %" PRIx32 ")\n",
+			   t->cpuid,
 			   t->core, (t->cpuid & CPUID_REVISION_MASK) >> 20,
 			   t->cpuid & CPUID_PATCH_MASK);
 
@@ -400,10 +402,6 @@ bool cortexm_probe(ADIv5_AP_t *ap)
 		PROBE(stm32l0_probe);
 		PROBE(stm32l4_probe);
 		PROBE(stm32g0_probe);
-		if (ap->ap_partno == 0x472) {
-			t->driver = "STM32L552(no flash)";
-			target_halt_resume(t, 0);
-		}
 		break;
 	case AP_DESIGNER_CYPRESS:
 		DEBUG_WARN("Unhandled Cypress device\n");
diff --git a/src/target/stm32f4.c b/src/target/stm32f4.c
index f51ce44b..148497c1 100644
--- a/src/target/stm32f4.c
+++ b/src/target/stm32f4.c
@@ -415,7 +415,7 @@ static int stm32f4_flash_erase(struct target_flash *f, target_addr addr,
 	/* Check for error */
 	sr = target_mem_read32(t, FLASH_SR);
 	if(sr & SR_ERROR_MASK) {
-		DEBUG_WARN("stm32f4 flash erase: sr error: 0x%" PRIu32 "\n", sr);
+		DEBUG_WARN("stm32f4 flash erase: sr error: 0x%" PRIx32 "\n", sr);
 		return -1;
 	}
 	return 0;
diff --git a/src/target/stm32l4.c b/src/target/stm32l4.c
index d63376ec..3d0e8a27 100644
--- a/src/target/stm32l4.c
+++ b/src/target/stm32l4.c
@@ -1,7 +1,7 @@
 /*
  * This file is part of the Black Magic Debug project.
  *
- * Copyright (C) 2015, 2017 - 2020  Uwe Bonnes
+ * Copyright (C) 2015, 2017 - 2021  Uwe Bonnes
  *                             <bon@elektron.ikp.physik.tu-darmstadt.de>
  *
  * This program is free software: you can redistribute it and/or modify
@@ -28,7 +28,7 @@
  * RM0394 STM32L43xxx STM32L44xxx STM32L45xxx STM32L46xxxx advanced
  *  ARM®-based 32-bit MCUs Rev.3
  * RM0432 STM32L4Rxxx and STM32L4Sxxx advanced Arm®-based 32-bit MCU. Rev 1
- * RM0440 STM32G4 Series advanced Arm®-based 32-bit MCU. Rev 1
+ * RM0440 STM32G4 Series advanced Arm®-based 32-bit MCU. Rev 6
  *
  *
  */
@@ -51,20 +51,16 @@ const struct command_s stm32l4_cmd_list[] = {
 	{NULL, NULL, NULL}
 };
 
-
 static int stm32l4_flash_erase(struct target_flash *f, target_addr addr, size_t len);
 static int stm32l4_flash_write(struct target_flash *f,
                                target_addr dest, const void *src, size_t len);
 
 /* Flash Program ad Erase Controller Register Map */
-#define FPEC_BASE			0x40022000
-#define FLASH_ACR			(FPEC_BASE+0x00)
-#define FLASH_KEYR			(FPEC_BASE+0x08)
-#define FLASH_OPTKEYR		(FPEC_BASE+0x0c)
-#define FLASH_SR			(FPEC_BASE+0x10)
-#define FLASH_CR			(FPEC_BASE+0x14)
-#define FLASH_OPTR			(FPEC_BASE+0x20)
-//#define FLASH_OPTCR		(FPEC_BASE+0x14)
+#define L4_FPEC_BASE			0x40022000
+#define L5_FPEC_BASE			0x40022000
+#define WL_FPEC_BASE			0x58004000
+
+#define L5_FLASH_OPTR_TZEN	(1 << 31)
 
 #define FLASH_CR_PG			(1 << 0)
 #define FLASH_CR_PER		(1 << 1)
@@ -95,6 +91,8 @@ static int stm32l4_flash_write(struct target_flash *f,
 #define FLASH_SR_ERROR_MASK	0xC3FA
 #define FLASH_SR_BSY		(1 << 16)
 
+#define FLASH_SIZE_MAX_G4_CAT4  (512U * 1024U)   // 512 kiB
+
 #define KEY1 0x45670123
 #define KEY2 0xCDEF89AB
 
@@ -107,7 +105,7 @@ static int stm32l4_flash_write(struct target_flash *f,
 #define OR_DUALBANK		(1 << 21)
 /* Used in STM32L47R*/
 #define OR_DB1M 		(1 << 21)
-/* Used in STM32L47R and STM32G47 */
+/* Used in STM32L47R, STM32G47 and STM32L55*/
 #define OR_DBANK 		(1 << 22)
 
 #define DBGMCU_CR(dbgmcureg)	(dbgmcureg + 0x04)
@@ -117,8 +115,10 @@ static int stm32l4_flash_write(struct target_flash *f,
 
 enum {
         STM32L4_DBGMCU_IDCODE_PHYS = 0xe0042000,
+        STM32L5_DBGMCU_IDCODE_PHYS = 0xe0044000,
 };
-#define FLASH_SIZE_REG  0x1FFF75E0
+#define L4_FLASH_SIZE_REG  0x1FFF75E0
+#define L5_FLASH_SIZE_REG  0x0bfa05e0
 
 struct stm32l4_flash {
 	struct target_flash f;
@@ -138,6 +138,9 @@ enum ID_STM32L4 {
 	ID_STM32L4R  = 0x470u, /* RM0432, Rev.5 */
 	ID_STM32G43  = 0x468u, /* RM0440, Rev.1 */
 	ID_STM32G47  = 0x469u, /* RM0440, Rev.1 */
+	ID_STM32G49  = 0x479u, /* RM0440, Rev.6 */
+	ID_STM32L55  = 0x472u, /* RM0438, Rev.4 */
+	ID_STM32WLXX = 0x497u, /* RM0461, Rev.3, RM453, Rev.1 */
 };
 
 enum FAM_STM32L4 {
@@ -145,11 +148,50 @@ enum FAM_STM32L4 {
 	FAM_STM32L4Rx = 2,
 	FAM_STM32WBxx = 4,
 	FAM_STM32G4xx = 5,
+	FAM_STM32L55x = 6,
+	FAM_STM32WLxx = 7,
 };
 
 #define DUAL_BANK	0x80u
 #define RAM_COUNT_MSK	0x07u
 
+enum stm32l4_flash_regs {
+	FLASH_KEYR,
+	FLASH_OPTKEYR,
+	FLASH_SR,
+	FLASH_CR,
+	FLASH_OPTR,
+	FLASHSIZE,
+	FLASH_REGS_COUNT
+};
+
+static const uint32_t stm32l4_flash_regs_map[FLASH_REGS_COUNT] = {
+	L4_FPEC_BASE + 0x08, /* KEYR */
+	L4_FPEC_BASE + 0x0c, /* OPTKEYR */
+	L4_FPEC_BASE + 0x10, /* SR */
+	L4_FPEC_BASE + 0x14, /* CR */
+	L4_FPEC_BASE + 0x20, /* OPTR */
+	L4_FLASH_SIZE_REG,   /* FLASHSIZE */
+};
+
+static const uint32_t stm32l5_flash_regs_map[FLASH_REGS_COUNT] = {
+	L5_FPEC_BASE + 0x08, /* KEYR */
+	L5_FPEC_BASE + 0x10, /* OPTKEYR */
+	L5_FPEC_BASE + 0x20, /* SR */
+	L5_FPEC_BASE + 0x28, /* CR */
+	L5_FPEC_BASE + 0x40, /* OPTR */
+	L5_FLASH_SIZE_REG,   /* FLASHSIZE */
+};
+
+static const uint32_t stm32wl_flash_regs_map[FLASH_REGS_COUNT] = {
+	WL_FPEC_BASE + 0x08, /* KEYR */
+	WL_FPEC_BASE + 0x0c, /* OPTKEYR */
+	WL_FPEC_BASE + 0x10, /* SR */
+	WL_FPEC_BASE + 0x14, /* CR */
+	WL_FPEC_BASE + 0x20, /* OPTR */
+	L4_FLASH_SIZE_REG,   /* FLASHSIZE */
+};
+
 struct stm32l4_info {
 	char designator[10];
 	uint16_t sram1; /* Normal SRAM mapped at 0x20000000*/
@@ -158,9 +200,10 @@ struct stm32l4_info {
 	enum ID_STM32L4 idcode;
 	enum FAM_STM32L4 family;
 	uint8_t flags;          /* Only DUAL_BANK is evaluated for now.*/
+	const uint32_t *flash_regs_map;
 };
 
-struct stm32l4_info const L4info[] = {
+static struct stm32l4_info const L4info[] = {
 	{
 		.idcode = ID_STM32L41,
 		.family = FAM_STM32L4xx,
@@ -168,6 +211,7 @@ struct stm32l4_info const L4info[] = {
 		.sram1 = 32,
 		.sram2 = 8,
 		.flags = 2,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32L43,
@@ -176,6 +220,7 @@ struct stm32l4_info const L4info[] = {
 		.sram1 = 48,
 		.sram2 = 16,
 		.flags = 2,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32L45,
@@ -184,6 +229,7 @@ struct stm32l4_info const L4info[] = {
 		.sram1 = 128,
 		.sram2 = 32,
 		.flags = 2,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32L47,
@@ -192,6 +238,7 @@ struct stm32l4_info const L4info[] = {
 		.sram1 = 96,
 		.sram2 = 32,
 		.flags = 2 | DUAL_BANK,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32L49,
@@ -200,6 +247,7 @@ struct stm32l4_info const L4info[] = {
 		.sram1 = 256,
 		.sram2 = 64,
 		.flags = 2 | DUAL_BANK,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32L4R,
@@ -209,6 +257,7 @@ struct stm32l4_info const L4info[] = {
 		.sram2 = 64,
 		.sram3 = 384,
 		.flags = 3 | DUAL_BANK,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32G43,
@@ -216,14 +265,43 @@ struct stm32l4_info const L4info[] = {
 		.designator = "STM32G43",
 		.sram1 = 22,
 		.sram2 = 10,
+		.flash_regs_map = stm32l4_flash_regs_map,
 	},
 	{
 		.idcode = ID_STM32G47,
 		.family = FAM_STM32G4xx,
 		.designator = "STM32G47",
-		.sram1 = 96, /* SRAM1 and SRAM2 are mapped contigiously */
+		.sram1 = 96, /* SRAM1 and SRAM2 are mapped continuous */
 		.sram2 = 32, /* CCM SRAM is mapped as per SRAM2 on G4 */
 		.flags = 2,
+		.flash_regs_map = stm32l4_flash_regs_map,
+	},
+	{
+		.idcode = ID_STM32G49,
+		.family = FAM_STM32G4xx,
+		.designator = "STM32G49",
+		.sram1 = 96, /* SRAM1 and SRAM2 are mapped continuously */
+		.sram2 = 16, /* CCM SRAM is mapped as per SRAM2 on G4 */
+		.flags = 2,
+		.flash_regs_map = stm32l4_flash_regs_map,
+	},
+	{
+		.idcode = ID_STM32L55,
+		.family = FAM_STM32L55x,
+		.designator = "STM32L55",
+		.sram1 = 192, /* SRAM1 and SRAM2 are mapped continuous */
+		.sram2 =  64,
+		.flags = 2,
+		.flash_regs_map = stm32l5_flash_regs_map,
+	},
+	{
+		.idcode = ID_STM32WLXX,
+		.family = FAM_STM32WLxx,
+		.designator = "STM32WLxx",
+		.sram1 = 64,
+		.sram2 = 32,
+		.flags = 2,
+		.flash_regs_map = stm32wl_flash_regs_map,
 	},
 	{
 		/* Terminator */
@@ -240,6 +318,27 @@ static struct stm32l4_info const * stm32l4_get_chip_info(uint32_t idcode) {
 	return p;
 }
 
+static uint32_t stm32l4_flash_read16(target *t, enum stm32l4_flash_regs reg)
+{
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
+	uint32_t addr = chip->flash_regs_map[reg];
+	return target_mem_read16(t, addr);
+}
+
+static uint32_t stm32l4_flash_read32(target *t, enum stm32l4_flash_regs reg)
+{
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
+	uint32_t addr = chip->flash_regs_map[reg];
+	return target_mem_read32(t, addr);
+}
+
+static void stm32l4_flash_write32(target *t, enum stm32l4_flash_regs reg, uint32_t value)
+{
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
+	uint32_t addr = chip->flash_regs_map[reg];
+	target_mem_write32(t, addr, value);
+}
+
 static void stm32l4_add_flash(target *t,
                               uint32_t addr, size_t length, size_t blocksize,
                               uint32_t bank1_start)
@@ -263,6 +362,17 @@ static void stm32l4_add_flash(target *t,
 	sf->bank1_start = bank1_start;
 	target_add_flash(t, f);
 }
+#define L5_RCC_APB1ENR1        0x50021058
+#define L5_RCC_APB1ENR1_PWREN (1 << 28)
+#define L5_PWR_CR1             0x50007000
+#define L5_PWR_CR1_VOS        (3 << 9)
+/* For flash programming, L5 needs to be in VOS 0 or 1 while reset set 2 (or even 3?) */
+static void stm32l5_flash_enable(target *t)
+{
+	target_mem_write32(t, L5_RCC_APB1ENR1, L5_RCC_APB1ENR1_PWREN);
+	uint32_t pwr_cr1 = target_mem_read32(t, L5_PWR_CR1) & ~L5_PWR_CR1_VOS;
+	target_mem_write32(t, L5_PWR_CR1, pwr_cr1);
+}
 
 static bool stm32l4_attach(target *t)
 {
@@ -273,9 +383,16 @@ static bool stm32l4_attach(target *t)
 	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
 
 
-	uint32_t idcodereg = STM32L4_DBGMCU_IDCODE_PHYS;
-
-
+	uint32_t idcodereg;
+	switch(chip->family) {
+	case FAM_STM32L55x:
+		idcodereg = STM32L5_DBGMCU_IDCODE_PHYS;
+		stm32l5_flash_enable(t);
+		break;
+	default:
+		idcodereg = STM32L4_DBGMCU_IDCODE_PHYS;
+		break;
+	}
 	/* Save DBGMCU_CR to restore it when detaching*/
 	struct stm32l4_priv_s *priv_storage = calloc(1, sizeof(*priv_storage));
 	priv_storage->dbgmcu_cr = target_mem_read32(t, DBGMCU_CR(idcodereg));
@@ -284,20 +401,22 @@ static bool stm32l4_attach(target *t)
 	/* Enable debugging during all low power modes*/
 	target_mem_write32(t, DBGMCU_CR(idcodereg), DBGMCU_CR_DBG_SLEEP | DBGMCU_CR_DBG_STANDBY | DBGMCU_CR_DBG_STOP);
 
-
 	/* Free previously loaded memory map */
 	target_mem_map_free(t);
 
-	/* Add RAM to memory map */
-	target_add_ram(t, 0x10000000, chip->sram2 << 10);
+	/* Add Code RAM to memory map */
+	if (chip->family == FAM_STM32L55x)
+		target_add_ram(t, 0x0A000000, (chip->sram1 + chip->sram2) << 10);
+	else
+		target_add_ram(t, 0x10000000, chip->sram2 << 10);
 	/* All L4 beside L47 alias SRAM2 after SRAM1.*/
 	uint32_t ramsize = (t->idcode == ID_STM32L47)?
 		chip->sram1 : (chip->sram1 + chip->sram2 + chip->sram3);
 	target_add_ram(t, 0x20000000, ramsize << 10);
 
+	uint32_t size = stm32l4_flash_read16(t, FLASHSIZE);
 	/* Add the flash to memory map. */
-	uint32_t size = target_mem_read16(t, FLASH_SIZE_REG);
-	uint32_t options =  target_mem_read32(t, FLASH_OPTR);
+	uint32_t options = stm32l4_flash_read32(t, FLASH_OPTR);
 
 	if (chip->family == FAM_STM32L4Rx) {
 		/* rm0432 Rev. 2 does not mention 1 MB devices or explain DB1M.*/
@@ -306,14 +425,26 @@ static bool stm32l4_attach(target *t)
 			stm32l4_add_flash(t, 0x08100000, 0x00100000, 0x1000, 0x08100000);
 		} else
 			stm32l4_add_flash(t, 0x08000000, 0x00200000, 0x2000, -1);
+	} else if (chip->family == FAM_STM32L55x) {
+		/* FIXME: Test behaviour on 256 k devices */
+		if (options & OR_DBANK) {
+			stm32l4_add_flash(t, 0x08000000, 0x00040000, 0x0800, 0x08040000);
+			stm32l4_add_flash(t, 0x08040000, 0x00040000, 0x0800, 0x08040000);
+		} else
+			stm32l4_add_flash(t, 0x08000000, 0x00080000, 0x0800, -1);
 	} else if (chip->family == FAM_STM32G4xx) {
-		// RM0440 describes G43x as Category 2, G47x/G48x as Category 3 devices
+		// RM0440 describes G43x/G44x as Category 2, G47x/G48x as Category 3 and G49x/G4Ax as Category 4 devices
 		// Cat 2 is always 128k with 2k pages, single bank
 		// Cat 3 is dual bank with an option bit to choose single 512k bank with 4k pages or dual bank as 2x256k with 2k pages
+		// Cat 4 is single bank with up to 512k, 2k pages
 		if (chip->idcode == ID_STM32G43) {
 			uint32_t banksize = size << 10;
 			stm32l4_add_flash(t, 0x08000000, banksize, 0x0800, -1);
 		}
+		else if (chip->idcode == ID_STM32G49) {
+			// Announce maximum possible flash size on this chip
+			stm32l4_add_flash(t, 0x08000000, FLASH_SIZE_MAX_G4_CAT4, 0x0800, -1);
+		}
 		else {
 			if (options & OR_DBANK) {
 				uint32_t banksize = size << 9;
@@ -337,7 +468,7 @@ static bool stm32l4_attach(target *t)
 		stm32l4_add_flash(t, 0x08000000, size << 10, 0x800, -1);
 
 	/* Clear all errors in the status register. */
-	target_mem_write32(t, FLASH_SR, target_mem_read32(t, FLASH_SR));
+	stm32l4_flash_write32(t, FLASH_SR, stm32l4_flash_read32(t, FLASH_SR));
 
 	return true;
 }
@@ -354,7 +485,18 @@ static void stm32l4_detach(target *t)
 
 bool stm32l4_probe(target *t)
 {
-	struct stm32l4_info const *chip = stm32l4_get_chip_info(t->idcode);
+	uint32_t idcode_reg = STM32L4_DBGMCU_IDCODE_PHYS;
+	ADIv5_AP_t *ap = cortexm_ap(t);
+	if (ap->dp->idcode == 0x0Be12477) {
+		idcode_reg = STM32L5_DBGMCU_IDCODE_PHYS;
+		if ((stm32l4_flash_read32(t, FLASH_OPTR)) & L5_FLASH_OPTR_TZEN) {
+			DEBUG_WARN("STM32L5 Trust Zone enabled\n");
+		}
+	}
+	uint32_t idcode = target_mem_read32(t, idcode_reg) & 0xfff;
+	DEBUG_INFO("Read %" PRIx32 ": %" PRIx32 "\n", idcode_reg, idcode);
+
+	struct stm32l4_info const *chip = stm32l4_get_chip_info(idcode);
 
 	if( !chip->idcode )	/* Not found */
 		return false;
@@ -368,10 +510,10 @@ bool stm32l4_probe(target *t)
 
 static void stm32l4_flash_unlock(target *t)
 {
-	if (target_mem_read32(t, FLASH_CR) & FLASH_CR_LOCK) {
+	if ((stm32l4_flash_read32(t, FLASH_CR)) & FLASH_CR_LOCK) {
 		/* Enable FPEC controller access */
-		target_mem_write32(t, FLASH_KEYR, KEY1);
-		target_mem_write32(t, FLASH_KEYR, KEY2);
+		stm32l4_flash_write32(t, FLASH_KEYR, KEY1);
+		stm32l4_flash_write32(t, FLASH_KEYR, KEY2);
 	}
 }
 
@@ -386,11 +528,11 @@ static int stm32l4_flash_erase(struct target_flash *f, target_addr addr, size_t
 	stm32l4_flash_unlock(t);
 
 	/* Read FLASH_SR to poll for BSY bit */
-	while(target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
+	while(stm32l4_flash_read32(t, FLASH_SR) & FLASH_SR_BSY)
 		if(target_check_error(t))
 			return -1;
 	/* Fixme: OPTVER always set after reset! Wrong option defaults?*/
-	target_mem_write32(t, FLASH_SR, target_mem_read32(t, FLASH_SR));
+	stm32l4_flash_write32(t, FLASH_SR, stm32l4_flash_read32(t, FLASH_SR));
 	page = (addr - 0x08000000) / blocksize;
 	while(len) {
 		uint32_t cr;
@@ -399,13 +541,13 @@ static int stm32l4_flash_erase(struct target_flash *f, target_addr addr, size_t
 		if (addr >= bank1_start)
 			cr |= FLASH_CR_BKER;
 		/* Flash page erase instruction */
-		target_mem_write32(t, FLASH_CR, cr);
+		stm32l4_flash_write32(t, FLASH_CR, cr);
 		/* write address to FMA */
 		cr |= FLASH_CR_STRT;
-		target_mem_write32(t, FLASH_CR, cr);
+		stm32l4_flash_write32(t, FLASH_CR, cr);
 
 		/* Read FLASH_SR to poll for BSY bit */
-		while(target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
+		while(stm32l4_flash_read32(t, FLASH_SR) & FLASH_SR_BSY)
 			if(target_check_error(t))
 				return -1;
 		if (len > blocksize)
@@ -417,7 +559,7 @@ static int stm32l4_flash_erase(struct target_flash *f, target_addr addr, size_t
 	}
 
 	/* Check for error */
-	sr = target_mem_read32(t, FLASH_SR);
+	sr = stm32l4_flash_read32(t, FLASH_SR);
 	if(sr & FLASH_SR_ERROR_MASK)
 		return -1;
 
@@ -428,12 +570,12 @@ static int stm32l4_flash_write(struct target_flash *f,
                                target_addr dest, const void *src, size_t len)
 {
 	target *t = f->t;
-	target_mem_write32(t, FLASH_CR, FLASH_CR_PG);
+	stm32l4_flash_write32(t, FLASH_CR, FLASH_CR_PG);
 	target_mem_write(t, dest, src, len);
 	/* Wait for completion or an error */
 	uint32_t sr;
 	do {
-		sr = target_mem_read32(t, FLASH_SR);
+		sr = stm32l4_flash_read32(t, FLASH_SR);
 		if (target_check_error(t)) {
 			DEBUG_WARN("stm32l4 flash write: comm error\n");
 			return -1;
@@ -441,7 +583,7 @@ static int stm32l4_flash_write(struct target_flash *f,
 	} while (sr & FLASH_SR_BSY);
 
 	if(sr & FLASH_SR_ERROR_MASK) {
-		DEBUG_WARN("stm32l4 flash write error: sr 0x%" PRIu32 "\n", sr);
+		DEBUG_WARN("stm32l4 flash write error: sr 0x%" PRIx32 "\n", sr);
 		return -1;
 	}
 	return 0;
@@ -452,18 +594,18 @@ static bool stm32l4_cmd_erase(target *t, uint32_t action)
 	stm32l4_flash_unlock(t);
 	/* Erase time is 25 ms. No need for a spinner.*/
 	/* Flash erase action start instruction */
-	target_mem_write32(t, FLASH_CR, action);
-	target_mem_write32(t, FLASH_CR, action | FLASH_CR_STRT);
+	stm32l4_flash_write32(t, FLASH_CR, action);
+	stm32l4_flash_write32(t, FLASH_CR, action | FLASH_CR_STRT);
 
 	/* Read FLASH_SR to poll for BSY bit */
-	while (target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY) {
+	while (stm32l4_flash_read32(t, FLASH_SR) & FLASH_SR_BSY) {
 		if(target_check_error(t)) {
 			return false;
 		}
 	}
 
 	/* Check for error */
-	uint16_t sr = target_mem_read32(t, FLASH_SR);
+	uint16_t sr = stm32l4_flash_read32(t, FLASH_SR);
 	if (sr & FLASH_SR_ERROR_MASK)
 		return false;
 	return true;
@@ -503,22 +645,22 @@ static bool stm32l4_option_write(
 {
 	tc_printf(t, "Device will lose connection. Rescan!\n");
 	stm32l4_flash_unlock(t);
-	target_mem_write32(t, FLASH_OPTKEYR, OPTKEY1);
-	target_mem_write32(t, FLASH_OPTKEYR, OPTKEY2);
-	while (target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
+	stm32l4_flash_write32(t, FLASH_OPTKEYR, OPTKEY1);
+	stm32l4_flash_write32(t, FLASH_OPTKEYR, OPTKEY2);
+	while (stm32l4_flash_read32(t, FLASH_SR) & FLASH_SR_BSY)
 		if(target_check_error(t))
 			return true;
 	for (int i = 0; i < len; i++)
-		target_mem_write32(t, FPEC_BASE + i2offset[i], values[i]);
-	target_mem_write32(t, FLASH_CR, FLASH_CR_OPTSTRT);
-	while (target_mem_read32(t, FLASH_SR) & FLASH_SR_BSY)
+		target_mem_write32(t, L4_FPEC_BASE + i2offset[i], values[i]);
+	stm32l4_flash_write32(t, FLASH_CR, FLASH_CR_OPTSTRT);
+	while (stm32l4_flash_read32(t, FLASH_SR) & FLASH_SR_BSY)
 		if(target_check_error(t))
 			return true;
-	target_mem_write32(t, FLASH_CR, FLASH_CR_OBL_LAUNCH);
-	while (target_mem_read32(t, FLASH_CR) & FLASH_CR_OBL_LAUNCH)
+	stm32l4_flash_write32(t, FLASH_CR, FLASH_CR_OBL_LAUNCH);
+	while (stm32l4_flash_read32(t, FLASH_CR) & FLASH_CR_OBL_LAUNCH)
 		if(target_check_error(t))
 			return true;
-	target_mem_write32(t, FLASH_CR, FLASH_CR_LOCK);
+	stm32l4_flash_write32(t, FLASH_CR, FLASH_CR_LOCK);
 	return false;
 }
 
@@ -540,8 +682,19 @@ static bool stm32l4_option_write(
 
 static bool stm32l4_cmd_option(target *t, int argc, char *argv[])
 {
+	if (t->idcode == ID_STM32L55) {
+		tc_printf(t, "STM32L5 options not implemented!\n");
+		return false;
+	}
+	if (t->idcode == ID_STM32WLXX) {
+		tc_printf(t, "STM32WLxx options not implemented!\n");
+		return false;
+	}
 	static const uint32_t g4_values[11] = {
-		0xFFEFF8AA, 0xFFFFFFFF, 0x00FF0000, 0xFF00FFFF, 0xFF00FFFF, 0xFF00FF00,
+		/* SEC_SIZE1 occupies 9 bits on G49/G4A (cat 4),
+		 * 8 bits on cat 3 and 7 bits on cat 2.
+		 * It is safe to write 0xFF00FE00 (cat 4 value) in FLASH_SEC1R */
+		0xFFEFF8AA, 0xFFFFFFFF, 0x00FF0000, 0xFF00FFFF, 0xFF00FFFF, 0xFF00FE00,
 		0xFFFFFFFF, 0xFFFFFFFF, 0xFF00FFFF, 0xFF00FFFF, 0xFF00FF00
 	};
 
@@ -559,6 +712,12 @@ static bool stm32l4_cmd_option(target *t, int argc, char *argv[])
 		len = 11;
 		for (int i = 0; i < len; i++)
 			values[i] = g4_values[i];
+	} else if ((t->idcode == ID_STM32G43) || (t->idcode == ID_STM32G49)) {
+		/* G4 cat 2 and 4 (single bank) */
+		i2offset = g4_i2offset;
+		len = 6;
+		for (int i = 0; i < len; i++)
+			values[i] = g4_values[i];
 	} else {
 		len = 9;
 	}
@@ -569,7 +728,7 @@ static bool stm32l4_cmd_option(target *t, int argc, char *argv[])
 		for (i = 2; i < argc; i++)
 			values[i - 2] = strtoul(argv[i], NULL, 0);
 		for (i = i - 2; i < len; i++) {
-			uint32_t addr = FPEC_BASE + i2offset[i];
+			uint32_t addr = L4_FPEC_BASE + i2offset[i];
 			values[i] = target_mem_read32(t, addr);
 		}
 		if ((values[0] & 0xff) == 0xCC) {
@@ -586,8 +745,8 @@ static bool stm32l4_cmd_option(target *t, int argc, char *argv[])
 		return false;
 	}
 	for (int i = 0; i < len; i ++) {
-		uint32_t addr = FPEC_BASE + i2offset[i];
-		val = target_mem_read32(t, FPEC_BASE + i2offset[i]);
+		uint32_t addr = L4_FPEC_BASE + i2offset[i];
+		val = target_mem_read32(t, L4_FPEC_BASE + i2offset[i]);
 		tc_printf(t, "0x%08X: 0x%08X\n", addr, val);
 	}
 	return true;