diff --git a/src/platforms/stlink/Makefile.inc b/src/platforms/stlink/Makefile.inc
index 9e72e56..7cacb0e 100644
--- a/src/platforms/stlink/Makefile.inc
+++ b/src/platforms/stlink/Makefile.inc
@@ -3,11 +3,12 @@ CC = $(CROSS_COMPILE)gcc
 OBJCOPY = $(CROSS_COMPILE)objcopy
 
 CFLAGS += -mcpu=cortex-m3 -mthumb \
-	-DSTM32F1 -I../libopencm3/include -Iplatforms/stlink -Iplatforms/stm32
-LDFLAGS = -lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
+	-DSTM32F1 -DDISCOVERY_STLINK -I../libopencm3/include -Iplatforms/stlink -Iplatforms/stm32
+LDFLAGS_BOOT = -lopencm3_stm32f1 -Wl,--defsym,_stack=0x20005000 \
 	-Wl,-T,platforms/stm32/stlink.ld -nostartfiles -lc -lnosys \
 	-Wl,-Map=mapfile -mthumb -mcpu=cortex-m3 -Wl,-gc-sections \
 	-L../libopencm3/lib
+LDFLAGS = $(LDFLAGS_BOOT) -Wl,-Ttext=0x8002000
 
 VPATH += platforms/stm32
 
@@ -15,11 +16,20 @@ SRC += 	cdcacm.c	\
 	platform.c	\
 	platforms/native/usbuart.c \
 
-all:	blackmagic.bin
+all:	blackmagic.bin blackmagic_dfu.bin blackmagic_dfu.hex
 
 blackmagic.bin:	blackmagic
 	$(OBJCOPY) -O binary $^ $@
 
-host_clean:
-	-rm blackmagic.bin
+blackmagic_dfu: usbdfu.o
+	$(CC) $^ -o $@ $(LDFLAGS_BOOT)
+
+blackmagic_dfu.bin:	blackmagic_dfu
+	$(OBJCOPY) -O binary $^ $@
+
+blackmagic_dfu.hex:	blackmagic_dfu
+	$(OBJCOPY) -O ihex $^ $@
+
+host_clean:
+	-rm blackmagic.bin blackmagic_dfu blackmagic_dfu.bin blackmagic_dfu.hex
 
diff --git a/src/platforms/stlink/cdcacm.c b/src/platforms/stlink/cdcacm.c
index 1cad4d7..b051e62 100644
--- a/src/platforms/stlink/cdcacm.c
+++ b/src/platforms/stlink/cdcacm.c
@@ -39,6 +39,8 @@
 #include "platform.h"
 #include <usbuart.h>
 
+#define DFU_IF_NO 4
+
 usbd_device * usbdev;
 
 static char *get_dev_unique_id(char *serial_no);
@@ -46,6 +48,7 @@ static char *get_dev_unique_id(char *serial_no);
 static int configured;
 static int cdcacm_gdb_dtr = 1;
 
+
 static const struct usb_device_descriptor dev = {
         .bLength = USB_DT_DEVICE_SIZE,
         .bDescriptorType = USB_DT_DEVICE,
@@ -271,6 +274,41 @@ static const struct usb_iface_assoc_descriptor uart_assoc = {
 	.iFunction = 0,
 };
 
+const struct usb_dfu_descriptor dfu_function = {
+	.bLength = sizeof(struct usb_dfu_descriptor),
+	.bDescriptorType = DFU_FUNCTIONAL,
+	.bmAttributes = USB_DFU_CAN_DOWNLOAD | USB_DFU_WILL_DETACH,
+	.wDetachTimeout = 255,
+	.wTransferSize = 1024,
+	.bcdDFUVersion = 0x011A,
+};
+
+const struct usb_interface_descriptor dfu_iface = {
+	.bLength = USB_DT_INTERFACE_SIZE,
+	.bDescriptorType = USB_DT_INTERFACE,
+	.bInterfaceNumber = DFU_IF_NO,
+	.bAlternateSetting = 0,
+	.bNumEndpoints = 0,
+	.bInterfaceClass = 0xFE,
+	.bInterfaceSubClass = 1,
+	.bInterfaceProtocol = 1,
+	.iInterface = 6,
+
+	.extra = &dfu_function,
+	.extralen = sizeof(dfu_function),
+};
+
+static const struct usb_iface_assoc_descriptor dfu_assoc = {
+	.bLength = USB_DT_INTERFACE_ASSOCIATION_SIZE,
+	.bDescriptorType = USB_DT_INTERFACE_ASSOCIATION,
+	.bFirstInterface = 4,
+	.bInterfaceCount = 1,
+	.bFunctionClass = 0xFE,
+	.bFunctionSubClass = 1,
+	.bFunctionProtocol = 1,
+	.iFunction = 6,
+};
+
 static const struct usb_interface ifaces[] = {{
 	.num_altsetting = 1,
 	.iface_assoc = &gdb_assoc,
@@ -285,13 +323,17 @@ static const struct usb_interface ifaces[] = {{
 }, {
 	.num_altsetting = 1,
 	.altsetting = uart_data_iface,
+}, {
+	.num_altsetting = 1,
+	.iface_assoc = &dfu_assoc,
+	.altsetting = &dfu_iface,
 }};
 
 static const struct usb_config_descriptor config = {
 	.bLength = USB_DT_CONFIGURATION_SIZE,
 	.bDescriptorType = USB_DT_CONFIGURATION,
 	.wTotalLength = 0,
-	.bNumInterfaces = 4,
+	.bNumInterfaces = 5,
 	.bConfigurationValue = 1,
 	.iConfiguration = 0,
 	.bmAttributes = 0x80,
@@ -308,8 +350,33 @@ static const char *usb_strings[] = {
 	serial_no,
 	"Black Magic GDB Server",
 	"Black Magic UART Port",
+	"Black Magic Firmware Upgrade",
 };
 
+static void dfu_detach_complete(usbd_device *dev, struct usb_setup_data *req)
+{
+	(void)dev;
+	(void)req;
+
+	/* Disconnect USB cable by resetting USB Device and pulling USB_DP low*/
+	rcc_peripheral_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_clear_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
+	gpio_clear(GPIOA, GPIO12);
+	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
+
+	/* Assert boot-request pin */
+	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPCEN);
+	gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_2_MHZ,
+			GPIO_CNF_OUTPUT_PUSHPULL, GPIO13);
+	gpio_set(GPIOC, GPIO13);
+
+	/* Reset core to enter bootloader */
+	scb_reset_core();
+}
+
 static int cdcacm_control_request(usbd_device *dev,
     struct usb_setup_data *req, uint8_t **buf, uint16_t *len,
     void (**complete)(usbd_device *dev, struct usb_setup_data *req))
@@ -340,6 +407,24 @@ static int cdcacm_control_request(usbd_device *dev,
 		default:
 			return 0;
 		}
+	case DFU_GETSTATUS:
+		if(req->wIndex == DFU_IF_NO) {
+			(*buf)[0] = DFU_STATUS_OK;
+			(*buf)[1] = 0;
+			(*buf)[2] = 0;
+			(*buf)[3] = 0;
+			(*buf)[4] = STATE_APP_IDLE;
+			(*buf)[5] = 0;	/* iString not used here */
+			*len = 6;
+
+			return 1;
+		}
+	case DFU_DETACH:
+		if(req->wIndex == DFU_IF_NO) {
+			*complete = dfu_detach_complete;
+			return 1;
+		}
+		return 0;
 	}
 	return 0;
 }
@@ -405,7 +490,7 @@ void cdcacm_init(void)
 	get_dev_unique_id(serial_no);
 
 	usbdev = usbd_init(&stm32f103_usb_driver,
-					&dev, &config, usb_strings, 5);
+					&dev, &config, usb_strings, 6);
 	usbd_set_control_buffer_size(usbdev, sizeof(usbd_control_buffer));
 	usbd_register_set_config_callback(usbdev, cdcacm_set_config);
 
diff --git a/src/platforms/stm32/usbdfu.c b/src/platforms/stm32/usbdfu.c
index d7034fb..6aea11a 100644
--- a/src/platforms/stm32/usbdfu.c
+++ b/src/platforms/stm32/usbdfu.c
@@ -41,6 +41,10 @@ usbd_device *usbdev;
 /* We need a special large control buffer for this device: */
 u8 usbd_control_buffer[1024];
 
+#if defined(DISCOVERY_STLINK)
+u32 led2_state = 0;
+#endif
+
 static enum dfu_state usbdfu_state = STATE_DFU_IDLE;
 
 static char *get_dev_unique_id(char *serial_no);
@@ -118,7 +122,11 @@ static char serial_no[9];
 
 static const char *usb_strings[] = {
 	"Black Sphere Technologies",
+#if defined(DISCOVERY_STLINK)
+	"Black Magic (Upgrade) for STLink/Discovery",
+#else
 	"Black Magic Probe (Upgrade)",
+#endif
 	serial_no,
 	/* This string is used by ST Microelectronics' DfuSe utility */
 	"@Internal Flash   /0x08000000/8*001Ka,120*001Kg"
@@ -181,7 +189,19 @@ usbdfu_getstatus_complete(usbd_device *dev, struct usb_setup_data *req)
 		return;
 
 	case STATE_DFU_MANIFEST:
-		/* USB device must detach, we just reset... */
+#if defined (DISCOVERY_STLINK)
+		/* Disconnect USB cable by resetting USB Device
+                   and pulling USB_DP low*/
+		rcc_peripheral_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+		rcc_peripheral_clear_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+		rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USBEN);
+		rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
+		gpio_clear(GPIOA, GPIO12);
+		gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+			GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
+#else
+        /* USB device must detach, we just reset... */
+#endif
 		scb_reset_system();
 		return; /* Will never return */
 	default:
@@ -250,8 +270,13 @@ static int usbdfu_control_request(usbd_device *dev,
 
 int main(void)
 {
+#if defined (DISCOVERY_STLINK)
+	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPCEN);
+	if(!gpio_get(GPIOC, GPIO13)) {
+#else
 	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPBEN);
 	if(gpio_get(GPIOB, GPIO12)) {
+#endif
 		/* Boot the application if it's valid */
 		if((*(volatile u32*)APP_ADDRESS & 0x2FFE0000) == 0x20000000) {
 			/* Set vector table base address */
@@ -272,15 +297,31 @@ int main(void)
 		flash_program_option_bytes(FLASH_OBP_WRP10, 0x03FC);
 	}
 
+#if defined (DISCOVERY_STLINK)
+	/* Just in case: Disconnect USB cable by resetting USB Device
+	and pulling USB_DP low*/
+	rcc_peripheral_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_clear_reset(&RCC_APB1RSTR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USBEN);
+	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
+	gpio_clear(GPIOA, GPIO12);
+	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+		GPIO_CNF_OUTPUT_OPENDRAIN, GPIO12);
+#endif
 	rcc_clock_setup_in_hse_8mhz_out_72mhz();
 
 	rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_USBEN);
 	rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_IOPAEN);
 
+#if defined(DISCOVERY_STLINK)
+	gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
+			GPIO_CNF_INPUT_ANALOG, GPIO0);
+#else
 	gpio_set_mode(GPIOA, GPIO_MODE_INPUT, 0, GPIO8);
 
 	gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_2_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, GPIO11);
+#endif
 	systick_set_clocksource(STK_CTRL_CLKSOURCE_AHB_DIV8);
 	systick_set_reload(900000);
 	systick_interrupt_enable();
@@ -298,9 +339,11 @@ int main(void)
 				USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT,
 				usbdfu_control_request);
 
+#if defined(BLACKMAGIG)
 	gpio_set(GPIOA, GPIO8);
 	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
 			GPIO_CNF_OUTPUT_PUSHPULL, GPIO8);
+#endif
 
 	while (1)
 		usbd_poll(usbdev);
@@ -328,5 +371,16 @@ static char *get_dev_unique_id(char *s)
 
 void sys_tick_handler()
 {
+#if defined(DISCOVERY_STLINK)
+	if (led2_state & 1)
+		gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_2_MHZ,
+			GPIO_CNF_OUTPUT_PUSHPULL, GPIO9);
+	else
+		gpio_set_mode(GPIOA, GPIO_MODE_INPUT,
+		GPIO_CNF_INPUT_ANALOG, GPIO9);
+	led2_state++;
+#else
 	gpio_toggle(GPIOB, GPIO11); /* LED2 on/off */
+#endif
 }
+