add daplink component

10 files changed, 5430 insertions, 0 deletions

diff --git a/demo/cmsis-dap_v2.1_tempate.c b/demo/cmsis-dap_v2.1_tempate.c
new file mode 100644
index 00000000..6e51a432
--- /dev/null
+++ b/demo/cmsis-dap_v2.1_tempate.c
@@ -0,0 +1,282 @@
+#include "usbd_core.h"
+#include "usbd_cdc.h"
+#include "DAP_config.h"
+#include "DAP.h"
+
+#define DAP_IN_EP  0x81
+#define DAP_OUT_EP 0x02
+
+#define CDC_IN_EP  0x83
+#define CDC_OUT_EP 0x04
+#define CDC_INT_EP 0x85
+
+#define USBD_VID           0xd6e7
+#define USBD_PID           0x3507
+#define USBD_MAX_POWER     500
+#define USBD_LANGID_STRING 1033
+
+#define CMSIS_DAP_INTERFACE_SIZE (9 + 7 + 7)
+#define USB_CONFIG_SIZE          (9 + CMSIS_DAP_INTERFACE_SIZE + CDC_ACM_DESCRIPTOR_LEN)
+
+#ifdef CONFIG_USB_HS
+#define CDC_MAX_MPS 512
+#else
+#define CDC_MAX_MPS 64
+#endif
+
+#define WCID_VENDOR_CODE 0x01
+
+__ALIGN_BEGIN const uint8_t WCID_StringDescriptor_MSOS[18] __ALIGN_END = {
+    ///////////////////////////////////////
+    /// MS OS string descriptor
+    ///////////////////////////////////////
+    0x12,                       /* bLength */
+    USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
+    /* MSFT100 */
+    'M', 0x00, 'S', 0x00, 'F', 0x00, 'T', 0x00, /* wcChar_7 */
+    '1', 0x00, '0', 0x00, '0', 0x00,            /* wcChar_7 */
+    WCID_VENDOR_CODE,                           /* bVendorCode */
+    0x00,                                       /* bReserved */
+};
+
+__ALIGN_BEGIN const uint8_t WINUSB_WCIDDescriptor[40] __ALIGN_END = {
+    ///////////////////////////////////////
+    /// WCID descriptor
+    ///////////////////////////////////////
+    0x28, 0x00, 0x00, 0x00,                   /* dwLength */
+    0x00, 0x01,                               /* bcdVersion */
+    0x04, 0x00,                               /* wIndex */
+    0x01,                                     /* bCount */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* bReserved_7 */
+
+    ///////////////////////////////////////
+    /// WCID function descriptor
+    ///////////////////////////////////////
+    0x00, /* bFirstInterfaceNumber */
+    0x01, /* bReserved */
+    /* WINUSB */
+    'W', 'I', 'N', 'U', 'S', 'B', 0x00, 0x00, /* cCID_8 */
+    /*  */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* cSubCID_8 */
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,             /* bReserved_6 */
+};
+
+__ALIGN_BEGIN const uint8_t WINUSB_IF0_WCIDProperties[142] __ALIGN_END = {
+    ///////////////////////////////////////
+    /// WCID property descriptor
+    ///////////////////////////////////////
+    0x8e, 0x00, 0x00, 0x00, /* dwLength */
+    0x00, 0x01,             /* bcdVersion */
+    0x05, 0x00,             /* wIndex */
+    0x01, 0x00,             /* wCount */
+
+    ///////////////////////////////////////
+    /// registry propter descriptor
+    ///////////////////////////////////////
+    0x84, 0x00, 0x00, 0x00, /* dwSize */
+    0x01, 0x00, 0x00, 0x00, /* dwPropertyDataType */
+    0x28, 0x00,             /* wPropertyNameLength */
+    /* DeviceInterfaceGUID */
+    'D', 0x00, 'e', 0x00, 'v', 0x00, 'i', 0x00,  /* wcName_20 */
+    'c', 0x00, 'e', 0x00, 'I', 0x00, 'n', 0x00,  /* wcName_20 */
+    't', 0x00, 'e', 0x00, 'r', 0x00, 'f', 0x00,  /* wcName_20 */
+    'a', 0x00, 'c', 0x00, 'e', 0x00, 'G', 0x00,  /* wcName_20 */
+    'U', 0x00, 'I', 0x00, 'D', 0x00, 0x00, 0x00, /* wcName_20 */
+    0x4e, 0x00, 0x00, 0x00,                      /* dwPropertyDataLength */
+    /* {CDB3B5AD-293B-4663-AA36-1AAE46463776} */
+    '{', 0x00, 'C', 0x00, 'D', 0x00, 'B', 0x00, /* wcData_39 */
+    '3', 0x00, 'B', 0x00, '5', 0x00, 'A', 0x00, /* wcData_39 */
+    'D', 0x00, '-', 0x00, '2', 0x00, '9', 0x00, /* wcData_39 */
+    '3', 0x00, 'B', 0x00, '-', 0x00, '4', 0x00, /* wcData_39 */
+    '6', 0x00, '6', 0x00, '3', 0x00, '-', 0x00, /* wcData_39 */
+    'A', 0x00, 'A', 0x00, '3', 0x00, '6', 0x00, /* wcData_39 */
+    '-', 0x00, '1', 0x00, 'A', 0x00, 'A', 0x00, /* wcData_39 */
+    'E', 0x00, '4', 0x00, '6', 0x00, '4', 0x00, /* wcData_39 */
+    '6', 0x00, '3', 0x00, '7', 0x00, '7', 0x00, /* wcData_39 */
+    '6', 0x00, '}', 0x00, 0x00, 0x00,           /* wcData_39 */
+};
+
+struct usb_msosv1_descriptor msosv1_desc = {
+    .string = (uint8_t *)WCID_StringDescriptor_MSOS,
+    .string_len = 18,
+    .vendor_code = WCID_VENDOR_CODE,
+    .compat_id = (uint8_t *)WINUSB_WCIDDescriptor,
+    .compat_id_len = sizeof(WINUSB_WCIDDescriptor),
+    .comp_id_property = (uint8_t *)WINUSB_IF0_WCIDProperties,
+    .comp_id_property_len = sizeof(WINUSB_IF0_WCIDProperties),
+};
+
+const uint8_t daplink_descriptor[] = {
+    USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01),
+    /* Configuration 0 */
+    USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x03, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
+    /* Interface 0 */
+    USB_INTERFACE_DESCRIPTOR_INIT(0x00, 0x00, 0x02, 0xFF, 0x00, 0x00, 0x02),
+    /* Endpoint OUT 1 */
+    USB_ENDPOINT_DESCRIPTOR_INIT(DAP_IN_EP, USB_ENDPOINT_TYPE_BULK, CDC_MAX_MPS, 0x00),
+    /* Endpoint IN 2 */
+    USB_ENDPOINT_DESCRIPTOR_INIT(DAP_OUT_EP, USB_ENDPOINT_TYPE_BULK, CDC_MAX_MPS, 0x00),
+    CDC_ACM_DESCRIPTOR_INIT(0x01, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, 0x00),
+    /* String 0 (LANGID) */
+    USB_LANGID_INIT(USBD_LANGID_STRING),
+    /* String 1 (Manufacturer) */
+    0x14,                       /* bLength */
+    USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
+    'C', 0x00,                  /* wcChar0 */
+    'h', 0x00,                  /* wcChar1 */
+    'e', 0x00,                  /* wcChar2 */
+    'r', 0x00,                  /* wcChar3 */
+    'r', 0x00,                  /* wcChar4 */
+    'y', 0x00,                  /* wcChar5 */
+    'U', 0x00,                  /* wcChar6 */
+    'S', 0x00,                  /* wcChar7 */
+    'B', 0x00,                  /* wcChar8 */
+    /* String 2 (Product) */
+    0x28,                       // bLength
+    USB_DESCRIPTOR_TYPE_STRING, // bDescriptorType
+    'C', 0x00,                  // wcChar0
+    'h', 0x00,                  // wcChar1
+    'e', 0x00,                  // wcChar2
+    'r', 0x00,                  // wcChar3
+    'r', 0x00,                  // wcChar4
+    'y', 0x00,                  // wcChar5
+    'U', 0x00,                  // wcChar6
+    'S', 0x00,                  // wcChar7
+    'B', 0x00,                  // wcChar8
+    ' ', 0x00,                  // wcChar9
+    'C', 0x00,                  // wcChar10
+    'M', 0x00,                  // wcChar11
+    'S', 0x00,                  // wcChar12
+    'I', 0x00,                  // wcChar13
+    'S', 0x00,                  // wcChar14
+    '-', 0x00,                  // wcChar15
+    'D', 0x00,                  // wcChar16
+    'A', 0x00,                  // wcChar17
+    'P', 0x00,                  // wcChar18
+    /* String 3 (Serial Number) */
+    0x1A,                       // bLength
+    USB_DESCRIPTOR_TYPE_STRING, // bDescriptorType
+    '0', 0,                     // wcChar0
+    '1', 0,                     // wcChar1
+    '2', 0,                     // wcChar2
+    '3', 0,                     // wcChar3
+    '4', 0,                     // wcChar4
+    '5', 0,                     // wcChar5
+    'A', 0,                     // wcChar6
+    'B', 0,                     // wcChar7
+    'C', 0,                     // wcChar8
+    'D', 0,                     // wcChar9
+    'E', 0,                     // wcChar10
+    'F', 0,                     // wcChar11
+#ifdef CONFIG_USB_HS
+    /* Device Qualifier */
+    0x0a,
+    USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
+    0x10,
+    0x02,
+    0x00,
+    0x00,
+    0x00,
+    0x40,
+    0x01,
+    0x00,
+#endif
+    /* End */
+    0x00
+};
+
+USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t cdc_read_buffer[CDC_MAX_MPS];
+
+USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t USB_Request[DAP_PACKET_SIZE];
+USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t USB_Response[DAP_PACKET_SIZE];
+
+volatile bool ep_tx_busy_flag = false;
+
+void usbd_configure_done_callback(void)
+{
+    /* setup first out ep read transfer */
+    usbd_ep_start_read(CDC_OUT_EP, cdc_read_buffer, CDC_MAX_MPS);
+    usbd_ep_start_read(DAP_OUT_EP, USB_Request, DAP_PACKET_SIZE);
+}
+
+void usbd_cdc_acm_bulk_out(uint8_t ep, uint32_t nbytes)
+{
+    /* setup next out ep read transfer */
+    if (1) {
+        usbd_ep_start_read(CDC_OUT_EP, cdc_read_buffer, CDC_MAX_MPS);
+    }
+}
+
+void usbd_cdc_acm_bulk_in(uint8_t ep, uint32_t nbytes)
+{
+    if ((nbytes % CDC_MAX_MPS) == 0 && nbytes) {
+        /* send zlp */
+        usbd_ep_start_write(CDC_IN_EP, NULL, 0);
+    } else {
+        ep_tx_busy_flag = false;
+    }
+}
+
+void usb_dap_recv_callback(uint8_t ep, uint32_t nbytes)
+{
+    uint32_t actual_len;
+    if (USB_Request[0] == ID_DAP_TransferAbort) {
+        usbd_ep_start_read(ep, USB_Request, DAP_PACKET_SIZE);
+        DAP_TransferAbort = 1;
+        return;
+    }
+    actual_len = DAP_ProcessCommand(USB_Request, USB_Response);
+
+    usbd_ep_start_write(DAP_IN_EP, USB_Response, actual_len);
+    usbd_ep_start_read(ep, USB_Request, DAP_PACKET_SIZE);
+}
+
+void usb_dap_send_callback(uint8_t ep, uint32_t nbytes)
+{
+    if ((nbytes % CDC_MAX_MPS) == 0 && nbytes) {
+        /* send zlp */
+        usbd_ep_start_write(DAP_IN_EP, NULL, 0);
+    } else {
+        ep_tx_busy_flag = false;
+    }
+}
+
+static struct usbd_endpoint cdc_out_ep = {
+    .ep_addr = CDC_OUT_EP,
+    .ep_cb = usbd_cdc_acm_bulk_out
+};
+
+static struct usbd_endpoint cdc_in_ep = {
+    .ep_addr = CDC_IN_EP,
+    .ep_cb = usbd_cdc_acm_bulk_in
+};
+
+static struct usbd_endpoint dap_out_ep = {
+    .ep_addr = DAP_OUT_EP,
+    .ep_cb = usb_dap_recv_callback
+};
+static struct usbd_endpoint dap_in_ep = {
+    .ep_addr = DAP_IN_EP,
+    .ep_cb = usb_dap_send_callback
+};
+
+struct usbd_interface dap_interface;
+
+void daplink_init(void)
+{
+    usbd_desc_register(daplink_descriptor);
+    usbd_msosv1_desc_register(&msosv1_desc);
+
+    /*!< winusb */
+    usbd_add_interface(&dap_interface);
+    usbd_add_endpoint(&dap_out_ep);
+    usbd_add_endpoint(&dap_in_ep);
+
+    /*!< cdc acm */
+    usbd_add_interface(usbd_cdc_acm_alloc_intf());
+    usbd_add_interface(usbd_cdc_acm_alloc_intf());
+    usbd_add_endpoint(&cdc_out_ep);
+    usbd_add_endpoint(&cdc_in_ep);
+
+    usbd_initialize();
+}
\ No newline at end of file
diff --git a/third_party/CMSIS-DAP-2.1.1/Config/DAP_config.h b/third_party/CMSIS-DAP-2.1.1/Config/DAP_config.h
new file mode 100644
index 00000000..f04915b0
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Config/DAP_config.h
@@ -0,0 +1,562 @@
+/*
+ * Copyright (c) 2013-2021 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        16. June 2021
+ * $Revision:    V2.1.0
+ *
+ * Project:      CMSIS-DAP Configuration
+ * Title:        DAP_config.h CMSIS-DAP Configuration File (Template)
+ *
+ *---------------------------------------------------------------------------*/
+
+#ifndef __DAP_CONFIG_H__
+#define __DAP_CONFIG_H__
+
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_Debug_gr CMSIS-DAP Debug Unit Information
+\ingroup DAP_ConfigIO_gr
+@{
+Provides definitions about the hardware and configuration of the Debug Unit.
+
+This information includes:
+ - Definition of Cortex-M processor parameters used in CMSIS-DAP Debug Unit.
+ - Debug Unit Identification strings (Vendor, Product, Serial Number).
+ - Debug Unit communication packet size.
+ - Debug Access Port supported modes and settings (JTAG/SWD and SWO).
+ - Optional information about a connected Target Device (for Evaluation Boards).
+*/
+
+#include <stdint.h>
+
+#ifndef __STATIC_INLINE
+#define __STATIC_INLINE static inline
+#endif
+#ifndef __STATIC_FORCEINLINE
+#define __STATIC_FORCEINLINE                   __STATIC_INLINE
+#endif
+/// Processor Clock of the Cortex-M MCU used in the Debug Unit.
+/// This value is used to calculate the SWD/JTAG clock speed.
+#define CPU_CLOCK               100000000U      ///< Specifies the CPU Clock in Hz.
+
+/// Number of processor cycles for I/O Port write operations.
+/// This value is used to calculate the SWD/JTAG clock speed that is generated with I/O
+/// Port write operations in the Debug Unit by a Cortex-M MCU. Most Cortex-M processors
+/// require 2 processor cycles for a I/O Port Write operation.  If the Debug Unit uses
+/// a Cortex-M0+ processor with high-speed peripheral I/O only 1 processor cycle might be
+/// required.
+#define IO_PORT_WRITE_CYCLES    2U              ///< I/O Cycles: 2=default, 1=Cortex-M0+ fast I/0.
+
+/// Indicate that Serial Wire Debug (SWD) communication mode is available at the Debug Access Port.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define DAP_SWD                 1               ///< SWD Mode:  1 = available, 0 = not available.
+
+/// Indicate that JTAG communication mode is available at the Debug Port.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define DAP_JTAG                1               ///< JTAG Mode: 1 = available, 0 = not available.
+
+/// Configure maximum number of JTAG devices on the scan chain connected to the Debug Access Port.
+/// This setting impacts the RAM requirements of the Debug Unit. Valid range is 1 .. 255.
+#define DAP_JTAG_DEV_CNT        8U              ///< Maximum number of JTAG devices on scan chain.
+
+/// Default communication mode on the Debug Access Port.
+/// Used for the command \ref DAP_Connect when Port Default mode is selected.
+#define DAP_DEFAULT_PORT        1U              ///< Default JTAG/SWJ Port Mode: 1 = SWD, 2 = JTAG.
+
+/// Default communication speed on the Debug Access Port for SWD and JTAG mode.
+/// Used to initialize the default SWD/JTAG clock frequency.
+/// The command \ref DAP_SWJ_Clock can be used to overwrite this default setting.
+#define DAP_DEFAULT_SWJ_CLOCK   1000000U        ///< Default SWD/JTAG clock frequency in Hz.
+
+/// Maximum Package Size for Command and Response data.
+/// This configuration settings is used to optimize the communication performance with the
+/// debugger and depends on the USB peripheral. Typical vales are 64 for Full-speed USB HID or WinUSB,
+/// 1024 for High-speed USB HID and 512 for High-speed USB WinUSB.
+#define DAP_PACKET_SIZE         512U            ///< Specifies Packet Size in bytes.
+
+/// Maximum Package Buffers for Command and Response data.
+/// This configuration settings is used to optimize the communication performance with the
+/// debugger and depends on the USB peripheral. For devices with limited RAM or USB buffer the
+/// setting can be reduced (valid range is 1 .. 255).
+#define DAP_PACKET_COUNT        8U              ///< Specifies number of packets buffered.
+
+/// Indicate that UART Serial Wire Output (SWO) trace is available.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define SWO_UART                0               ///< SWO UART:  1 = available, 0 = not available.
+
+/// USART Driver instance number for the UART SWO.
+#define SWO_UART_DRIVER         0               ///< USART Driver instance number (Driver_USART#).
+
+/// Maximum SWO UART Baudrate.
+#define SWO_UART_MAX_BAUDRATE   10000000U       ///< SWO UART Maximum Baudrate in Hz.
+
+/// Indicate that Manchester Serial Wire Output (SWO) trace is available.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define SWO_MANCHESTER          0               ///< SWO Manchester:  1 = available, 0 = not available.
+
+/// SWO Trace Buffer Size.
+#define SWO_BUFFER_SIZE         4096U           ///< SWO Trace Buffer Size in bytes (must be 2^n).
+
+/// SWO Streaming Trace.
+#define SWO_STREAM              0               ///< SWO Streaming Trace: 1 = available, 0 = not available.
+
+/// Clock frequency of the Test Domain Timer. Timer value is returned with \ref TIMESTAMP_GET.
+#define TIMESTAMP_CLOCK         100000000U      ///< Timestamp clock in Hz (0 = timestamps not supported).
+
+/// Indicate that UART Communication Port is available.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define DAP_UART                0               ///< DAP UART:  1 = available, 0 = not available.
+
+/// USART Driver instance number for the UART Communication Port.
+#define DAP_UART_DRIVER         0               ///< USART Driver instance number (Driver_USART#).
+
+/// UART Receive Buffer Size.
+#define DAP_UART_RX_BUFFER_SIZE 1024U           ///< Uart Receive Buffer Size in bytes (must be 2^n).
+
+/// UART Transmit Buffer Size.
+#define DAP_UART_TX_BUFFER_SIZE 1024U           ///< Uart Transmit Buffer Size in bytes (must be 2^n).
+
+/// Indicate that UART Communication via USB COM Port is available.
+/// This information is returned by the command \ref DAP_Info as part of <b>Capabilities</b>.
+#define DAP_UART_USB_COM_PORT   0               ///< USB COM Port:  1 = available, 0 = not available.
+
+/// Debug Unit is connected to fixed Target Device.
+/// The Debug Unit may be part of an evaluation board and always connected to a fixed
+/// known device. In this case a Device Vendor, Device Name, Board Vendor and Board Name strings
+/// are stored and may be used by the debugger or IDE to configure device parameters.
+#define TARGET_FIXED            0               ///< Target: 1 = known, 0 = unknown;
+
+#define TARGET_DEVICE_VENDOR    "Arm"           ///< String indicating the Silicon Vendor
+#define TARGET_DEVICE_NAME      "Cortex-M"      ///< String indicating the Target Device
+#define TARGET_BOARD_VENDOR     "Arm"           ///< String indicating the Board Vendor
+#define TARGET_BOARD_NAME       "Arm board"     ///< String indicating the Board Name
+
+#if TARGET_FIXED != 0
+#include <string.h>
+static const char TargetDeviceVendor [] = TARGET_DEVICE_VENDOR;
+static const char TargetDeviceName   [] = TARGET_DEVICE_NAME;
+static const char TargetBoardVendor  [] = TARGET_BOARD_VENDOR;
+static const char TargetBoardName    [] = TARGET_BOARD_NAME;
+#endif
+
+/** Get Vendor Name string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetVendorString (char *str) {
+  (void)str;
+  return (0U);
+}
+
+/** Get Product Name string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetProductString (char *str) {
+  (void)str;
+  return (0U);
+}
+
+/** Get Serial Number string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetSerNumString (char *str) {
+  (void)str;
+  return (0U);
+}
+
+/** Get Target Device Vendor string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetTargetDeviceVendorString (char *str) {
+#if TARGET_FIXED != 0
+  uint8_t len;
+
+  strcpy(str, TargetDeviceVendor);
+  len = (uint8_t)(strlen(TargetDeviceVendor) + 1U);
+  return (len);
+#else
+  (void)str;
+  return (0U);
+#endif
+}
+
+/** Get Target Device Name string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetTargetDeviceNameString (char *str) {
+#if TARGET_FIXED != 0
+  uint8_t len;
+
+  strcpy(str, TargetDeviceName);
+  len = (uint8_t)(strlen(TargetDeviceName) + 1U);
+  return (len);
+#else
+  (void)str;
+  return (0U);
+#endif
+}
+
+/** Get Target Board Vendor string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetTargetBoardVendorString (char *str) {
+#if TARGET_FIXED != 0
+  uint8_t len;
+
+  strcpy(str, TargetBoardVendor);
+  len = (uint8_t)(strlen(TargetBoardVendor) + 1U);
+  return (len);
+#else
+  (void)str;
+  return (0U);
+#endif
+}
+
+/** Get Target Board Name string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetTargetBoardNameString (char *str) {
+#if TARGET_FIXED != 0
+  uint8_t len;
+
+  strcpy(str, TargetBoardName);
+  len = (uint8_t)(strlen(TargetBoardName) + 1U);
+  return (len);
+#else
+  (void)str;
+  return (0U);
+#endif
+}
+
+/** Get Product Firmware Version string.
+\param str Pointer to buffer to store the string (max 60 characters).
+\return String length (including terminating NULL character) or 0 (no string).
+*/
+__STATIC_INLINE uint8_t DAP_GetProductFirmwareVersionString (char *str) {
+  (void)str;
+  return (0U);
+}
+
+///@}
+
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_PortIO_gr CMSIS-DAP Hardware I/O Pin Access
+\ingroup DAP_ConfigIO_gr
+@{
+
+Standard I/O Pins of the CMSIS-DAP Hardware Debug Port support standard JTAG mode
+and Serial Wire Debug (SWD) mode. In SWD mode only 2 pins are required to implement the debug
+interface of a device. The following I/O Pins are provided:
+
+JTAG I/O Pin                 | SWD I/O Pin          | CMSIS-DAP Hardware pin mode
+---------------------------- | -------------------- | ---------------------------------------------
+TCK: Test Clock              | SWCLK: Clock         | Output Push/Pull
+TMS: Test Mode Select        | SWDIO: Data I/O      | Output Push/Pull; Input (for receiving data)
+TDI: Test Data Input         |                      | Output Push/Pull
+TDO: Test Data Output        |                      | Input
+nTRST: Test Reset (optional) |                      | Output Open Drain with pull-up resistor
+nRESET: Device Reset         | nRESET: Device Reset | Output Open Drain with pull-up resistor
+
+
+DAP Hardware I/O Pin Access Functions
+-------------------------------------
+The various I/O Pins are accessed by functions that implement the Read, Write, Set, or Clear to
+these I/O Pins.
+
+For the SWDIO I/O Pin there are additional functions that are called in SWD I/O mode only.
+This functions are provided to achieve faster I/O that is possible with some advanced GPIO
+peripherals that can independently write/read a single I/O pin without affecting any other pins
+of the same I/O port. The following SWDIO I/O Pin functions are provided:
+ - \ref PIN_SWDIO_OUT_ENABLE to enable the output mode from the DAP hardware.
+ - \ref PIN_SWDIO_OUT_DISABLE to enable the input mode to the DAP hardware.
+ - \ref PIN_SWDIO_IN to read from the SWDIO I/O pin with utmost possible speed.
+ - \ref PIN_SWDIO_OUT to write to the SWDIO I/O pin with utmost possible speed.
+*/
+
+
+// Configure DAP I/O pins ------------------------------
+
+/** Setup JTAG I/O pins: TCK, TMS, TDI, TDO, nTRST, and nRESET.
+Configures the DAP Hardware I/O pins for JTAG mode:
+ - TCK, TMS, TDI, nTRST, nRESET to output mode and set to high level.
+ - TDO to input mode.
+*/
+__STATIC_INLINE void PORT_JTAG_SETUP (void) {
+  ;
+}
+
+/** Setup SWD I/O pins: SWCLK, SWDIO, and nRESET.
+Configures the DAP Hardware I/O pins for Serial Wire Debug (SWD) mode:
+ - SWCLK, SWDIO, nRESET to output mode and set to default high level.
+ - TDI, nTRST to HighZ mode (pins are unused in SWD mode).
+*/
+__STATIC_INLINE void PORT_SWD_SETUP (void) {
+  ;
+}
+
+/** Disable JTAG/SWD I/O Pins.
+Disables the DAP Hardware I/O pins which configures:
+ - TCK/SWCLK, TMS/SWDIO, TDI, TDO, nTRST, nRESET to High-Z mode.
+*/
+__STATIC_INLINE void PORT_OFF (void) {
+  ;
+}
+
+
+// SWCLK/TCK I/O pin -------------------------------------
+
+/** SWCLK/TCK I/O pin: Get Input.
+\return Current status of the SWCLK/TCK DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_SWCLK_TCK_IN  (void) {
+  return (0U);
+}
+
+/** SWCLK/TCK I/O pin: Set Output to High.
+Set the SWCLK/TCK DAP hardware I/O pin to high level.
+*/
+__STATIC_FORCEINLINE void     PIN_SWCLK_TCK_SET (void) {
+  ;
+}
+
+/** SWCLK/TCK I/O pin: Set Output to Low.
+Set the SWCLK/TCK DAP hardware I/O pin to low level.
+*/
+__STATIC_FORCEINLINE void     PIN_SWCLK_TCK_CLR (void) {
+  ;
+}
+
+
+// SWDIO/TMS Pin I/O --------------------------------------
+
+/** SWDIO/TMS I/O pin: Get Input.
+\return Current status of the SWDIO/TMS DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_SWDIO_TMS_IN  (void) {
+  return (0U);
+}
+
+/** SWDIO/TMS I/O pin: Set Output to High.
+Set the SWDIO/TMS DAP hardware I/O pin to high level.
+*/
+__STATIC_FORCEINLINE void     PIN_SWDIO_TMS_SET (void) {
+  ;
+}
+
+/** SWDIO/TMS I/O pin: Set Output to Low.
+Set the SWDIO/TMS DAP hardware I/O pin to low level.
+*/
+__STATIC_FORCEINLINE void     PIN_SWDIO_TMS_CLR (void) {
+  ;
+}
+
+/** SWDIO I/O pin: Get Input (used in SWD mode only).
+\return Current status of the SWDIO DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_SWDIO_IN      (void) {
+  return (0U);
+}
+
+/** SWDIO I/O pin: Set Output (used in SWD mode only).
+\param bit Output value for the SWDIO DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE void     PIN_SWDIO_OUT     (uint32_t bit) {
+  ;
+}
+
+/** SWDIO I/O pin: Switch to Output mode (used in SWD mode only).
+Configure the SWDIO DAP hardware I/O pin to output mode. This function is
+called prior \ref PIN_SWDIO_OUT function calls.
+*/
+__STATIC_FORCEINLINE void     PIN_SWDIO_OUT_ENABLE  (void) {
+  ;
+}
+
+/** SWDIO I/O pin: Switch to Input mode (used in SWD mode only).
+Configure the SWDIO DAP hardware I/O pin to input mode. This function is
+called prior \ref PIN_SWDIO_IN function calls.
+*/
+__STATIC_FORCEINLINE void     PIN_SWDIO_OUT_DISABLE (void) {
+  ;
+}
+
+
+// TDI Pin I/O ---------------------------------------------
+
+/** TDI I/O pin: Get Input.
+\return Current status of the TDI DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_TDI_IN  (void) {
+  return (0U);
+}
+
+/** TDI I/O pin: Set Output.
+\param bit Output value for the TDI DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE void     PIN_TDI_OUT (uint32_t bit) {
+  ;
+}
+
+
+// TDO Pin I/O ---------------------------------------------
+
+/** TDO I/O pin: Get Input.
+\return Current status of the TDO DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_TDO_IN  (void) {
+  return (0U);
+}
+
+
+// nTRST Pin I/O -------------------------------------------
+
+/** nTRST I/O pin: Get Input.
+\return Current status of the nTRST DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_nTRST_IN   (void) {
+  return (0U);
+}
+
+/** nTRST I/O pin: Set Output.
+\param bit JTAG TRST Test Reset pin status:
+           - 0: issue a JTAG TRST Test Reset.
+           - 1: release JTAG TRST Test Reset.
+*/
+__STATIC_FORCEINLINE void     PIN_nTRST_OUT  (uint32_t bit) {
+  ;
+}
+
+// nRESET Pin I/O------------------------------------------
+
+/** nRESET I/O pin: Get Input.
+\return Current status of the nRESET DAP hardware I/O pin.
+*/
+__STATIC_FORCEINLINE uint32_t PIN_nRESET_IN  (void) {
+  return (0U);
+}
+
+/** nRESET I/O pin: Set Output.
+\param bit target device hardware reset pin status:
+           - 0: issue a device hardware reset.
+           - 1: release device hardware reset.
+*/
+__STATIC_FORCEINLINE void     PIN_nRESET_OUT (uint32_t bit) {
+  ;
+}
+
+///@}
+
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_LEDs_gr CMSIS-DAP Hardware Status LEDs
+\ingroup DAP_ConfigIO_gr
+@{
+
+CMSIS-DAP Hardware may provide LEDs that indicate the status of the CMSIS-DAP Debug Unit.
+
+It is recommended to provide the following LEDs for status indication:
+ - Connect LED: is active when the DAP hardware is connected to a debugger.
+ - Running LED: is active when the debugger has put the target device into running state.
+*/
+
+/** Debug Unit: Set status of Connected LED.
+\param bit status of the Connect LED.
+           - 1: Connect LED ON: debugger is connected to CMSIS-DAP Debug Unit.
+           - 0: Connect LED OFF: debugger is not connected to CMSIS-DAP Debug Unit.
+*/
+__STATIC_INLINE void LED_CONNECTED_OUT (uint32_t bit) {}
+
+/** Debug Unit: Set status Target Running LED.
+\param bit status of the Target Running LED.
+           - 1: Target Running LED ON: program execution in target started.
+           - 0: Target Running LED OFF: program execution in target stopped.
+*/
+__STATIC_INLINE void LED_RUNNING_OUT (uint32_t bit) {}
+
+///@}
+
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_Timestamp_gr CMSIS-DAP Timestamp
+\ingroup DAP_ConfigIO_gr
+@{
+Access function for Test Domain Timer.
+
+The value of the Test Domain Timer in the Debug Unit is returned by the function \ref TIMESTAMP_GET. By
+default, the DWT timer is used.  The frequency of this timer is configured with \ref TIMESTAMP_CLOCK.
+
+*/
+
+/** Get timestamp of Test Domain Timer.
+\return Current timestamp value.
+*/
+__STATIC_INLINE uint32_t TIMESTAMP_GET (void) {
+  return (0);
+}
+
+///@}
+
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Config_Initialization_gr CMSIS-DAP Initialization
+\ingroup DAP_ConfigIO_gr
+@{
+
+CMSIS-DAP Hardware I/O and LED Pins are initialized with the function \ref DAP_SETUP.
+*/
+
+/** Setup of the Debug Unit I/O pins and LEDs (called when Debug Unit is initialized).
+This function performs the initialization of the CMSIS-DAP Hardware I/O Pins and the
+Status LEDs. In detail the operation of Hardware I/O and LED pins are enabled and set:
+ - I/O clock system enabled.
+ - all I/O pins: input buffer enabled, output pins are set to HighZ mode.
+ - for nTRST, nRESET a weak pull-up (if available) is enabled.
+ - LED output pins are enabled and LEDs are turned off.
+*/
+__STATIC_INLINE void DAP_SETUP (void) {
+  ;
+}
+
+/** Reset Target Device with custom specific I/O pin or command sequence.
+This function allows the optional implementation of a device specific reset sequence.
+It is called when the command \ref DAP_ResetTarget and is for example required
+when a device needs a time-critical unlock sequence that enables the debug port.
+\return 0 = no device specific reset sequence is implemented.\n
+        1 = a device specific reset sequence is implemented.
+*/
+__STATIC_INLINE uint8_t RESET_TARGET (void) {
+  return (0U);             // change to '1' when a device reset sequence is implemented
+}
+
+///@}
+
+
+#endif /* __DAP_CONFIG_H__ */
diff --git a/third_party/CMSIS-DAP-2.1.1/Include/DAP.h b/third_party/CMSIS-DAP-2.1.1/Include/DAP.h
new file mode 100644
index 00000000..55cf686e
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Include/DAP.h
@@ -0,0 +1,367 @@
+/*
+ * Copyright (c) 2013-2022 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        26. April 2022
+ * $Revision:    V2.1.1
+ *
+ * Project:      CMSIS-DAP Include
+ * Title:        DAP.h Definitions
+ *
+ *---------------------------------------------------------------------------*/
+
+#ifndef __DAP_H__
+#define __DAP_H__
+
+
+// DAP Firmware Version
+#ifdef  DAP_FW_V1
+#define DAP_FW_VER                      "1.3.0"
+#else
+#define DAP_FW_VER                      "2.1.1"
+#endif
+
+// DAP Command IDs
+#define ID_DAP_Info                     0x00U
+#define ID_DAP_HostStatus               0x01U
+#define ID_DAP_Connect                  0x02U
+#define ID_DAP_Disconnect               0x03U
+#define ID_DAP_TransferConfigure        0x04U
+#define ID_DAP_Transfer                 0x05U
+#define ID_DAP_TransferBlock            0x06U
+#define ID_DAP_TransferAbort            0x07U
+#define ID_DAP_WriteABORT               0x08U
+#define ID_DAP_Delay                    0x09U
+#define ID_DAP_ResetTarget              0x0AU
+#define ID_DAP_SWJ_Pins                 0x10U
+#define ID_DAP_SWJ_Clock                0x11U
+#define ID_DAP_SWJ_Sequence             0x12U
+#define ID_DAP_SWD_Configure            0x13U
+#define ID_DAP_SWD_Sequence             0x1DU
+#define ID_DAP_JTAG_Sequence            0x14U
+#define ID_DAP_JTAG_Configure           0x15U
+#define ID_DAP_JTAG_IDCODE              0x16U
+#define ID_DAP_SWO_Transport            0x17U
+#define ID_DAP_SWO_Mode                 0x18U
+#define ID_DAP_SWO_Baudrate             0x19U
+#define ID_DAP_SWO_Control              0x1AU
+#define ID_DAP_SWO_Status               0x1BU
+#define ID_DAP_SWO_ExtendedStatus       0x1EU
+#define ID_DAP_SWO_Data                 0x1CU
+#define ID_DAP_UART_Transport           0x1FU
+#define ID_DAP_UART_Configure           0x20U
+#define ID_DAP_UART_Control             0x22U
+#define ID_DAP_UART_Status              0x23U
+#define ID_DAP_UART_Transfer            0x21U
+
+#define ID_DAP_QueueCommands            0x7EU
+#define ID_DAP_ExecuteCommands          0x7FU
+
+// DAP Vendor Command IDs
+#define ID_DAP_Vendor0                  0x80U
+#define ID_DAP_Vendor1                  0x81U
+#define ID_DAP_Vendor2                  0x82U
+#define ID_DAP_Vendor3                  0x83U
+#define ID_DAP_Vendor4                  0x84U
+#define ID_DAP_Vendor5                  0x85U
+#define ID_DAP_Vendor6                  0x86U
+#define ID_DAP_Vendor7                  0x87U
+#define ID_DAP_Vendor8                  0x88U
+#define ID_DAP_Vendor9                  0x89U
+#define ID_DAP_Vendor10                 0x8AU
+#define ID_DAP_Vendor11                 0x8BU
+#define ID_DAP_Vendor12                 0x8CU
+#define ID_DAP_Vendor13                 0x8DU
+#define ID_DAP_Vendor14                 0x8EU
+#define ID_DAP_Vendor15                 0x8FU
+#define ID_DAP_Vendor16                 0x90U
+#define ID_DAP_Vendor17                 0x91U
+#define ID_DAP_Vendor18                 0x92U
+#define ID_DAP_Vendor19                 0x93U
+#define ID_DAP_Vendor20                 0x94U
+#define ID_DAP_Vendor21                 0x95U
+#define ID_DAP_Vendor22                 0x96U
+#define ID_DAP_Vendor23                 0x97U
+#define ID_DAP_Vendor24                 0x98U
+#define ID_DAP_Vendor25                 0x99U
+#define ID_DAP_Vendor26                 0x9AU
+#define ID_DAP_Vendor27                 0x9BU
+#define ID_DAP_Vendor28                 0x9CU
+#define ID_DAP_Vendor29                 0x9DU
+#define ID_DAP_Vendor30                 0x9EU
+#define ID_DAP_Vendor31                 0x9FU
+
+#define ID_DAP_Invalid                  0xFFU
+
+// DAP Status Code
+#define DAP_OK                          0U
+#define DAP_ERROR                       0xFFU
+
+// DAP ID
+#define DAP_ID_VENDOR                   1U
+#define DAP_ID_PRODUCT                  2U
+#define DAP_ID_SER_NUM                  3U
+#define DAP_ID_DAP_FW_VER               4U
+#define DAP_ID_DEVICE_VENDOR            5U
+#define DAP_ID_DEVICE_NAME              6U
+#define DAP_ID_BOARD_VENDOR             7U
+#define DAP_ID_BOARD_NAME               8U
+#define DAP_ID_PRODUCT_FW_VER           9U
+#define DAP_ID_CAPABILITIES             0xF0U
+#define DAP_ID_TIMESTAMP_CLOCK          0xF1U
+#define DAP_ID_UART_RX_BUFFER_SIZE      0xFBU
+#define DAP_ID_UART_TX_BUFFER_SIZE      0xFCU
+#define DAP_ID_SWO_BUFFER_SIZE          0xFDU
+#define DAP_ID_PACKET_COUNT             0xFEU
+#define DAP_ID_PACKET_SIZE              0xFFU
+
+// DAP Host Status
+#define DAP_DEBUGGER_CONNECTED          0U
+#define DAP_TARGET_RUNNING              1U
+
+// DAP Port
+#define DAP_PORT_AUTODETECT             0U      // Autodetect Port
+#define DAP_PORT_DISABLED               0U      // Port Disabled (I/O pins in High-Z)
+#define DAP_PORT_SWD                    1U      // SWD Port (SWCLK, SWDIO) + nRESET
+#define DAP_PORT_JTAG                   2U      // JTAG Port (TCK, TMS, TDI, TDO, nTRST) + nRESET
+
+// DAP SWJ Pins
+#define DAP_SWJ_SWCLK_TCK               0       // SWCLK/TCK
+#define DAP_SWJ_SWDIO_TMS               1       // SWDIO/TMS
+#define DAP_SWJ_TDI                     2       // TDI
+#define DAP_SWJ_TDO                     3       // TDO
+#define DAP_SWJ_nTRST                   5       // nTRST
+#define DAP_SWJ_nRESET                  7       // nRESET
+
+// DAP Transfer Request
+#define DAP_TRANSFER_APnDP              (1U<<0)
+#define DAP_TRANSFER_RnW                (1U<<1)
+#define DAP_TRANSFER_A2                 (1U<<2)
+#define DAP_TRANSFER_A3                 (1U<<3)
+#define DAP_TRANSFER_MATCH_VALUE        (1U<<4)
+#define DAP_TRANSFER_MATCH_MASK         (1U<<5)
+#define DAP_TRANSFER_TIMESTAMP          (1U<<7)
+
+// DAP Transfer Response
+#define DAP_TRANSFER_OK                 (1U<<0)
+#define DAP_TRANSFER_WAIT               (1U<<1)
+#define DAP_TRANSFER_FAULT              (1U<<2)
+#define DAP_TRANSFER_ERROR              (1U<<3)
+#define DAP_TRANSFER_MISMATCH           (1U<<4)
+
+// DAP SWO Trace Mode
+#define DAP_SWO_OFF                     0U
+#define DAP_SWO_UART                    1U
+#define DAP_SWO_MANCHESTER              2U
+
+// DAP SWO Trace Status
+#define DAP_SWO_CAPTURE_ACTIVE          (1U<<0)
+#define DAP_SWO_CAPTURE_PAUSED          (1U<<1)
+#define DAP_SWO_STREAM_ERROR            (1U<<6)
+#define DAP_SWO_BUFFER_OVERRUN          (1U<<7)
+
+// DAP UART Transport
+#define DAP_UART_TRANSPORT_NONE         0U
+#define DAP_UART_TRANSPORT_USB_COM_PORT 1U
+#define DAP_UART_TRANSPORT_DAP_COMMAND  2U
+
+// DAP UART Control
+#define DAP_UART_CONTROL_RX_ENABLE      (1U<<0)
+#define DAP_UART_CONTROL_RX_DISABLE     (1U<<1)
+#define DAP_UART_CONTROL_RX_BUF_FLUSH   (1U<<2)
+#define DAP_UART_CONTROL_TX_ENABLE      (1U<<4)
+#define DAP_UART_CONTROL_TX_DISABLE     (1U<<5)
+#define DAP_UART_CONTROL_TX_BUF_FLUSH   (1U<<6)
+
+// DAP UART Status
+#define DAP_UART_STATUS_RX_ENABLED      (1U<<0)
+#define DAP_UART_STATUS_RX_DATA_LOST    (1U<<1)
+#define DAP_UART_STATUS_FRAMING_ERROR   (1U<<2)
+#define DAP_UART_STATUS_PARITY_ERROR    (1U<<3)
+#define DAP_UART_STATUS_TX_ENABLED      (1U<<4)
+
+// DAP UART Configure Error
+#define DAP_UART_CFG_ERROR_DATA_BITS    (1U<<0)
+#define DAP_UART_CFG_ERROR_PARITY       (1U<<1)
+#define DAP_UART_CFG_ERROR_STOP_BITS    (1U<<2)
+
+// Debug Port Register Addresses
+#define DP_IDCODE                       0x00U   // IDCODE Register (SW Read only)
+#define DP_ABORT                        0x00U   // Abort Register (SW Write only)
+#define DP_CTRL_STAT                    0x04U   // Control & Status
+#define DP_WCR                          0x04U   // Wire Control Register (SW Only)
+#define DP_SELECT                       0x08U   // Select Register (JTAG R/W & SW W)
+#define DP_RESEND                       0x08U   // Resend (SW Read Only)
+#define DP_RDBUFF                       0x0CU   // Read Buffer (Read Only)
+
+// JTAG IR Codes
+#define JTAG_ABORT                      0x08U
+#define JTAG_DPACC                      0x0AU
+#define JTAG_APACC                      0x0BU
+#define JTAG_IDCODE                     0x0EU
+#define JTAG_BYPASS                     0x0FU
+
+// JTAG Sequence Info
+#define JTAG_SEQUENCE_TCK               0x3FU   // TCK count
+#define JTAG_SEQUENCE_TMS               0x40U   // TMS value
+#define JTAG_SEQUENCE_TDO               0x80U   // TDO capture
+
+// SWD Sequence Info
+#define SWD_SEQUENCE_CLK                0x3FU   // SWCLK count
+#define SWD_SEQUENCE_DIN                0x80U   // SWDIO capture
+
+
+#include <stddef.h>
+#include <stdint.h>
+#include "cmsis_compiler.h"
+
+// DAP Data structure
+typedef struct {
+  uint8_t     debug_port;                       // Debug Port
+  uint8_t     fast_clock;                       // Fast Clock Flag
+  uint8_t     padding[2];
+  uint32_t   clock_delay;                       // Clock Delay
+  uint32_t     timestamp;                       // Last captured Timestamp
+  struct {                                      // Transfer Configuration
+    uint8_t   idle_cycles;                      // Idle cycles after transfer
+    uint8_t    padding[3];
+    uint16_t  retry_count;                      // Number of retries after WAIT response
+    uint16_t  match_retry;                      // Number of retries if read value does not match
+    uint32_t  match_mask;                       // Match Mask
+  } transfer;
+#if (DAP_SWD != 0)
+  struct {                                      // SWD Configuration
+    uint8_t    turnaround;                      // Turnaround period
+    uint8_t    data_phase;                      // Always generate Data Phase
+  } swd_conf;
+#endif
+#if (DAP_JTAG != 0)
+  struct {                                      // JTAG Device Chain
+    uint8_t   count;                            // Number of devices
+    uint8_t   index;                            // Device index (device at TDO has index 0)
+#if (DAP_JTAG_DEV_CNT != 0)
+    uint8_t   ir_length[DAP_JTAG_DEV_CNT];      // IR Length in bits
+    uint16_t  ir_before[DAP_JTAG_DEV_CNT];      // Bits before IR
+    uint16_t  ir_after [DAP_JTAG_DEV_CNT];      // Bits after IR
+#endif
+  } jtag_dev;
+#endif
+} DAP_Data_t;
+
+extern          DAP_Data_t DAP_Data;            // DAP Data
+extern volatile uint8_t    DAP_TransferAbort;   // Transfer Abort Flag
+
+
+#ifdef  __cplusplus
+extern "C"
+{
+#endif
+
+// Functions
+extern void     SWJ_Sequence    (uint32_t count, const uint8_t *data);
+extern void     SWD_Sequence    (uint32_t info,  const uint8_t *swdo, uint8_t *swdi);
+extern void     JTAG_Sequence   (uint32_t info,  const uint8_t *tdi,  uint8_t *tdo);
+extern void     JTAG_IR         (uint32_t ir);
+extern uint32_t JTAG_ReadIDCode (void);
+extern void     JTAG_WriteAbort (uint32_t data);
+extern uint8_t  JTAG_Transfer   (uint32_t request, uint32_t *data);
+extern uint8_t  SWD_Transfer    (uint32_t request, uint32_t *data);
+
+extern void     Delayms         (uint32_t delay);
+
+extern uint32_t SWO_Transport      (const uint8_t *request, uint8_t *response);
+extern uint32_t SWO_Mode           (const uint8_t *request, uint8_t *response);
+extern uint32_t SWO_Baudrate       (const uint8_t *request, uint8_t *response);
+extern uint32_t SWO_Control        (const uint8_t *request, uint8_t *response);
+extern uint32_t SWO_Status                                 (uint8_t *response);
+extern uint32_t SWO_ExtendedStatus (const uint8_t *request, uint8_t *response);
+extern uint32_t SWO_Data           (const uint8_t *request, uint8_t *response);
+
+extern void     SWO_QueueTransfer    (uint8_t *buf, uint32_t num);
+extern void     SWO_AbortTransfer    (void);
+extern void     SWO_TransferComplete (void);
+
+extern uint32_t SWO_Mode_UART     (uint32_t enable);
+extern uint32_t SWO_Baudrate_UART (uint32_t baudrate);
+extern uint32_t SWO_Control_UART  (uint32_t active);
+extern void     SWO_Capture_UART  (uint8_t *buf, uint32_t num);
+extern uint32_t SWO_GetCount_UART (void);
+
+extern uint32_t SWO_Mode_Manchester     (uint32_t enable);
+extern uint32_t SWO_Baudrate_Manchester (uint32_t baudrate);
+extern uint32_t SWO_Control_Manchester  (uint32_t active);
+extern void     SWO_Capture_Manchester  (uint8_t *buf, uint32_t num);
+extern uint32_t SWO_GetCount_Manchester (void);
+
+extern uint32_t UART_Transport (const uint8_t *request, uint8_t *response);
+extern uint32_t UART_Configure (const uint8_t *request, uint8_t *response);
+extern uint32_t UART_Control   (const uint8_t *request, uint8_t *response);
+extern uint32_t UART_Status                            (uint8_t *response);
+extern uint32_t UART_Transfer  (const uint8_t *request, uint8_t *response);
+
+extern uint8_t  USB_COM_PORT_Activate (uint32_t cmd);
+
+extern uint32_t DAP_ProcessVendorCommand (const uint8_t *request, uint8_t *response);
+extern uint32_t DAP_ProcessCommand       (const uint8_t *request, uint8_t *response);
+extern uint32_t DAP_ExecuteCommand       (const uint8_t *request, uint8_t *response);
+
+extern void     DAP_Setup (void);
+
+// Configurable delay for clock generation
+#ifndef DELAY_SLOW_CYCLES
+#define DELAY_SLOW_CYCLES       3U      // Number of cycles for one iteration
+#endif
+#if defined(__CC_ARM)
+__STATIC_FORCEINLINE void PIN_DELAY_SLOW (uint32_t delay) {
+  uint32_t count = delay;
+  while (--count);
+}
+#else
+__STATIC_FORCEINLINE void PIN_DELAY_SLOW (uint32_t delay) {
+  __ASM volatile (
+  ".syntax unified\n"
+  "0:\n\t"
+    "subs %0,%0,#1\n\t"
+    "bne  0b\n"
+  : "+l" (delay) : : "cc"
+  );
+}
+#endif
+
+// Fixed delay for fast clock generation
+#ifndef DELAY_FAST_CYCLES
+#define DELAY_FAST_CYCLES       0U      // Number of cycles: 0..3
+#endif
+__STATIC_FORCEINLINE void PIN_DELAY_FAST (void) {
+#if (DELAY_FAST_CYCLES >= 1U)
+  __NOP();
+#endif
+#if (DELAY_FAST_CYCLES >= 2U)
+  __NOP();
+#endif
+#if (DELAY_FAST_CYCLES >= 3U)
+  __NOP();
+#endif
+}
+
+#ifdef  __cplusplus
+}
+#endif
+
+
+#endif  /* __DAP_H__ */
diff --git a/third_party/CMSIS-DAP-2.1.1/LICENSE.txt b/third_party/CMSIS-DAP-2.1.1/LICENSE.txt
new file mode 100644
index 00000000..8dada3ed
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/LICENSE.txt
@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "{}"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright {yyyy} {name of copyright owner}
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/DAP.c b/third_party/CMSIS-DAP-2.1.1/Source/DAP.c
new file mode 100644
index 00000000..606917ea
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/DAP.c
@@ -0,0 +1,1812 @@
+/*
+ * Copyright (c) 2013-2022 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        26. April 2022
+ * $Revision:    V2.1.1
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        DAP.c CMSIS-DAP Commands
+ *
+ *---------------------------------------------------------------------------*/
+
+#include <string.h>
+#include "DAP_config.h"
+#include "DAP.h"
+
+
+#if (DAP_PACKET_SIZE < 64U)
+#error "Minimum Packet Size is 64!"
+#endif
+#if (DAP_PACKET_SIZE > 32768U)
+#error "Maximum Packet Size is 32768!"
+#endif
+#if (DAP_PACKET_COUNT < 1U)
+#error "Minimum Packet Count is 1!"
+#endif
+#if (DAP_PACKET_COUNT > 255U)
+#error "Maximum Packet Count is 255!"
+#endif
+
+
+// Clock Macros
+#define MAX_SWJ_CLOCK(delay_cycles) \
+  ((CPU_CLOCK/2U) / (IO_PORT_WRITE_CYCLES + delay_cycles))
+
+
+         DAP_Data_t DAP_Data;           // DAP Data
+volatile uint8_t    DAP_TransferAbort;  // Transfer Abort Flag
+
+
+static const char DAP_FW_Ver [] = DAP_FW_VER;
+
+
+// Common clock delay calculation routine
+//   clock:    requested SWJ frequency in Hertz
+static void Set_Clock_Delay(uint32_t clock) {
+  uint32_t delay;
+
+  if (clock >= MAX_SWJ_CLOCK(DELAY_FAST_CYCLES)) {
+    DAP_Data.fast_clock  = 1U;
+    DAP_Data.clock_delay = 1U;
+  } else {
+    DAP_Data.fast_clock  = 0U;
+
+    delay = ((CPU_CLOCK/2U) + (clock - 1U)) / clock;
+    if (delay > IO_PORT_WRITE_CYCLES) {
+      delay -= IO_PORT_WRITE_CYCLES;
+      delay  = (delay + (DELAY_SLOW_CYCLES - 1U)) / DELAY_SLOW_CYCLES;
+    } else {
+      delay  = 1U;
+    }
+
+    DAP_Data.clock_delay = delay;
+  }
+}
+
+
+// Get DAP Information
+//   id:      info identifier
+//   info:    pointer to info data
+//   return:  number of bytes in info data
+static uint8_t DAP_Info(uint8_t id, uint8_t *info) {
+  uint8_t length = 0U;
+
+  switch (id) {
+    case DAP_ID_VENDOR:
+      length = DAP_GetVendorString((char *)info);
+      break;
+    case DAP_ID_PRODUCT:
+      length = DAP_GetProductString((char *)info);
+      break;
+    case DAP_ID_SER_NUM:
+      length = DAP_GetSerNumString((char *)info);
+      break;
+    case DAP_ID_DAP_FW_VER:
+      length = (uint8_t)sizeof(DAP_FW_Ver);
+      memcpy(info, DAP_FW_Ver, length);
+      break;
+    case DAP_ID_DEVICE_VENDOR:
+      length = DAP_GetTargetDeviceVendorString((char *)info);
+      break;
+    case DAP_ID_DEVICE_NAME:
+      length = DAP_GetTargetDeviceNameString((char *)info);
+      break;
+    case DAP_ID_BOARD_VENDOR:
+      length = DAP_GetTargetBoardVendorString((char *)info);
+      break;
+    case DAP_ID_BOARD_NAME:
+      length = DAP_GetTargetBoardNameString((char *)info);
+      break;
+    case DAP_ID_PRODUCT_FW_VER:
+      length = DAP_GetProductFirmwareVersionString((char *)info);
+      break;
+    case DAP_ID_CAPABILITIES:
+      info[0] = ((DAP_SWD  != 0)         ? (1U << 0) : 0U) |
+                ((DAP_JTAG != 0)         ? (1U << 1) : 0U) |
+                ((SWO_UART != 0)         ? (1U << 2) : 0U) |
+                ((SWO_MANCHESTER != 0)   ? (1U << 3) : 0U) |
+                /* Atomic Commands  */     (1U << 4)       |
+                ((TIMESTAMP_CLOCK != 0U) ? (1U << 5) : 0U) |
+                ((SWO_STREAM != 0U)      ? (1U << 6) : 0U) |
+                ((DAP_UART != 0U)        ? (1U << 7) : 0U);
+
+      info[1] = ((DAP_UART_USB_COM_PORT != 0) ? (1U << 0) : 0U);
+      length = 2U;
+      break;
+    case DAP_ID_TIMESTAMP_CLOCK:
+#if (TIMESTAMP_CLOCK != 0U)
+      info[0] = (uint8_t)(TIMESTAMP_CLOCK >>  0);
+      info[1] = (uint8_t)(TIMESTAMP_CLOCK >>  8);
+      info[2] = (uint8_t)(TIMESTAMP_CLOCK >> 16);
+      info[3] = (uint8_t)(TIMESTAMP_CLOCK >> 24);
+      length = 4U;
+#endif
+      break;
+    case DAP_ID_UART_RX_BUFFER_SIZE:
+#if (DAP_UART != 0)
+      info[0] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >>  0);
+      info[1] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >>  8);
+      info[2] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 16);
+      info[3] = (uint8_t)(DAP_UART_RX_BUFFER_SIZE >> 24);
+      length = 4U;
+#endif
+      break;
+    case DAP_ID_UART_TX_BUFFER_SIZE:
+#if (DAP_UART != 0)
+      info[0] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >>  0);
+      info[1] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >>  8);
+      info[2] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 16);
+      info[3] = (uint8_t)(DAP_UART_TX_BUFFER_SIZE >> 24);
+      length = 4U;
+#endif
+      break;
+    case DAP_ID_SWO_BUFFER_SIZE:
+#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
+      info[0] = (uint8_t)(SWO_BUFFER_SIZE >>  0);
+      info[1] = (uint8_t)(SWO_BUFFER_SIZE >>  8);
+      info[2] = (uint8_t)(SWO_BUFFER_SIZE >> 16);
+      info[3] = (uint8_t)(SWO_BUFFER_SIZE >> 24);
+      length = 4U;
+#endif
+      break;
+    case DAP_ID_PACKET_SIZE:
+      info[0] = (uint8_t)(DAP_PACKET_SIZE >> 0);
+      info[1] = (uint8_t)(DAP_PACKET_SIZE >> 8);
+      length = 2U;
+      break;
+    case DAP_ID_PACKET_COUNT:
+      info[0] = DAP_PACKET_COUNT;
+      length = 1U;
+      break;
+    default:
+      break;
+  }
+
+  return (length);
+}
+
+
+// Delay for specified time
+//    delay:  delay time in ms
+void Delayms(uint32_t delay) {
+  delay *= ((CPU_CLOCK/1000U) + (DELAY_SLOW_CYCLES-1U)) / DELAY_SLOW_CYCLES;
+  PIN_DELAY_SLOW(delay);
+}
+
+
+// Process Delay command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_Delay(const uint8_t *request, uint8_t *response) {
+  uint32_t delay;
+
+  delay  = (uint32_t)(*(request+0)) |
+           (uint32_t)(*(request+1) << 8);
+  delay *= ((CPU_CLOCK/1000000U) + (DELAY_SLOW_CYCLES-1U)) / DELAY_SLOW_CYCLES;
+
+  PIN_DELAY_SLOW(delay);
+
+  *response = DAP_OK;
+  return ((2U << 16) | 1U);
+}
+
+
+// Process Host Status command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_HostStatus(const uint8_t *request, uint8_t *response) {
+
+  switch (*request) {
+    case DAP_DEBUGGER_CONNECTED:
+      LED_CONNECTED_OUT((*(request+1) & 1U));
+      break;
+    case DAP_TARGET_RUNNING:
+      LED_RUNNING_OUT((*(request+1) & 1U));
+      break;
+    default:
+      *response = DAP_ERROR;
+      return ((2U << 16) | 1U);
+  }
+
+  *response = DAP_OK;
+  return ((2U << 16) | 1U);
+}
+
+
+// Process Connect command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_Connect(const uint8_t *request, uint8_t *response) {
+  uint32_t port;
+
+  if (*request == DAP_PORT_AUTODETECT) {
+    port = DAP_DEFAULT_PORT;
+  } else {
+    port = *request;
+  }
+
+  switch (port) {
+#if (DAP_SWD != 0)
+    case DAP_PORT_SWD:
+      DAP_Data.debug_port = DAP_PORT_SWD;
+      PORT_SWD_SETUP();
+      break;
+#endif
+#if (DAP_JTAG != 0)
+    case DAP_PORT_JTAG:
+      DAP_Data.debug_port = DAP_PORT_JTAG;
+      PORT_JTAG_SETUP();
+      break;
+#endif
+    default:
+      port = DAP_PORT_DISABLED;
+      break;
+  }
+
+  *response = (uint8_t)port;
+  return ((1U << 16) | 1U);
+}
+
+
+// Process Disconnect command and prepare response
+//   response: pointer to response data
+//   return:   number of bytes in response
+static uint32_t DAP_Disconnect(uint8_t *response) {
+
+  DAP_Data.debug_port = DAP_PORT_DISABLED;
+  PORT_OFF();
+
+  *response = DAP_OK;
+  return (1U);
+}
+
+
+// Process Reset Target command and prepare response
+//   response: pointer to response data
+//   return:   number of bytes in response
+static uint32_t DAP_ResetTarget(uint8_t *response) {
+
+  *(response+1) = RESET_TARGET();
+  *(response+0) = DAP_OK;
+  return (2U);
+}
+
+
+// Process SWJ Pins command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_SWJ_Pins(const uint8_t *request, uint8_t *response) {
+#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
+  uint32_t value;
+  uint32_t select;
+  uint32_t wait;
+  uint32_t timestamp;
+
+  value  = (uint32_t) *(request+0);
+  select = (uint32_t) *(request+1);
+  wait   = (uint32_t)(*(request+2) <<  0) |
+           (uint32_t)(*(request+3) <<  8) |
+           (uint32_t)(*(request+4) << 16) |
+           (uint32_t)(*(request+5) << 24);
+
+  if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
+    if ((value & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
+      PIN_SWCLK_TCK_SET();
+    } else {
+      PIN_SWCLK_TCK_CLR();
+    }
+  }
+  if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
+    if ((value & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
+      PIN_SWDIO_TMS_SET();
+    } else {
+      PIN_SWDIO_TMS_CLR();
+    }
+  }
+  if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
+    PIN_TDI_OUT(value >> DAP_SWJ_TDI);
+  }
+  if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
+    PIN_nTRST_OUT(value >> DAP_SWJ_nTRST);
+  }
+  if ((select & (1U << DAP_SWJ_nRESET)) != 0U){
+    PIN_nRESET_OUT(value >> DAP_SWJ_nRESET);
+  }
+
+  if (wait != 0U) {
+#if (TIMESTAMP_CLOCK != 0U)
+    if (wait > 3000000U) {
+      wait = 3000000U;
+    }
+#if (TIMESTAMP_CLOCK >= 1000000U)
+    wait *= TIMESTAMP_CLOCK / 1000000U;
+#else
+    wait /= 1000000U / TIMESTAMP_CLOCK;
+#endif
+#else
+    wait  = 1U;
+#endif
+    timestamp = TIMESTAMP_GET();
+    do {
+      if ((select & (1U << DAP_SWJ_SWCLK_TCK)) != 0U) {
+        if ((value >> DAP_SWJ_SWCLK_TCK) ^ PIN_SWCLK_TCK_IN()) {
+          continue;
+        }
+      }
+      if ((select & (1U << DAP_SWJ_SWDIO_TMS)) != 0U) {
+        if ((value >> DAP_SWJ_SWDIO_TMS) ^ PIN_SWDIO_TMS_IN()) {
+          continue;
+        }
+      }
+      if ((select & (1U << DAP_SWJ_TDI)) != 0U) {
+        if ((value >> DAP_SWJ_TDI) ^ PIN_TDI_IN()) {
+          continue;
+        }
+      }
+      if ((select & (1U << DAP_SWJ_nTRST)) != 0U) {
+        if ((value >> DAP_SWJ_nTRST) ^ PIN_nTRST_IN()) {
+          continue;
+        }
+      }
+      if ((select & (1U << DAP_SWJ_nRESET)) != 0U) {
+        if ((value >> DAP_SWJ_nRESET) ^ PIN_nRESET_IN()) {
+          continue;
+        }
+      }
+      break;
+    } while ((TIMESTAMP_GET() - timestamp) < wait);
+  }
+
+  value = (PIN_SWCLK_TCK_IN() << DAP_SWJ_SWCLK_TCK) |
+          (PIN_SWDIO_TMS_IN() << DAP_SWJ_SWDIO_TMS) |
+          (PIN_TDI_IN()       << DAP_SWJ_TDI)       |
+          (PIN_TDO_IN()       << DAP_SWJ_TDO)       |
+          (PIN_nTRST_IN()     << DAP_SWJ_nTRST)     |
+          (PIN_nRESET_IN()    << DAP_SWJ_nRESET);
+
+  *response = (uint8_t)value;
+#else
+  *response = 0U;
+#endif
+
+  return ((6U << 16) | 1U);
+}
+
+
+// Process SWJ Clock command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_SWJ_Clock(const uint8_t *request, uint8_t *response) {
+#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
+  uint32_t clock;
+  uint32_t delay;
+
+  clock = (uint32_t)(*(request+0) <<  0) |
+          (uint32_t)(*(request+1) <<  8) |
+          (uint32_t)(*(request+2) << 16) |
+          (uint32_t)(*(request+3) << 24);
+
+  if (clock == 0U) {
+    *response = DAP_ERROR;
+    return ((4U << 16) | 1U);
+  }
+
+  Set_Clock_Delay(clock);
+
+  *response = DAP_OK;
+#else
+  *response = DAP_ERROR;
+#endif
+
+  return ((4U << 16) | 1U);
+}
+
+
+// Process SWJ Sequence command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_SWJ_Sequence(const uint8_t *request, uint8_t *response) {
+  uint32_t count;
+
+  count = *request++;
+  if (count == 0U) {
+    count = 256U;
+  }
+
+#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
+  SWJ_Sequence(count, request);
+  *response = DAP_OK;
+#else
+  *response = DAP_ERROR;
+#endif
+
+  count = (count + 7U) >> 3;
+
+  return (((count + 1U) << 16) | 1U);
+}
+
+
+// Process SWD Configure command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_SWD_Configure(const uint8_t *request, uint8_t *response) {
+#if (DAP_SWD != 0)
+  uint8_t value;
+
+  value = *request;
+  DAP_Data.swd_conf.turnaround = (value & 0x03U) + 1U;
+  DAP_Data.swd_conf.data_phase = (value & 0x04U) ? 1U : 0U;
+
+  *response = DAP_OK;
+#else
+  *response = DAP_ERROR;
+#endif
+
+  return ((1U << 16) | 1U);
+}
+
+
+// Process SWD Sequence command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_SWD_Sequence(const uint8_t *request, uint8_t *response) {
+  uint32_t sequence_info;
+  uint32_t sequence_count;
+  uint32_t request_count;
+  uint32_t response_count;
+  uint32_t count;
+
+#if (DAP_SWD != 0)
+  *response++ = DAP_OK;
+#else
+  *response++ = DAP_ERROR;
+#endif
+  request_count  = 1U;
+  response_count = 1U;
+
+  sequence_count = *request++;
+  while (sequence_count--) {
+    sequence_info = *request++;
+    count = sequence_info & SWD_SEQUENCE_CLK;
+    if (count == 0U) {
+      count = 64U;
+    }
+    count = (count + 7U) / 8U;
+#if (DAP_SWD != 0)
+    if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
+      PIN_SWDIO_OUT_DISABLE();
+    } else {
+      PIN_SWDIO_OUT_ENABLE();
+    }
+    SWD_Sequence(sequence_info, request, response);
+    if (sequence_count == 0U) {
+      PIN_SWDIO_OUT_ENABLE();
+    }
+#endif
+    if ((sequence_info & SWD_SEQUENCE_DIN) != 0U) {
+      request_count++;
+#if (DAP_SWD != 0)
+      response += count;
+      response_count += count;
+#endif
+    } else {
+      request += count;
+      request_count += count + 1U;
+    }
+  }
+
+  return ((request_count << 16) | response_count);
+}
+
+
+// Process JTAG Sequence command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_JTAG_Sequence(const uint8_t *request, uint8_t *response) {
+  uint32_t sequence_info;
+  uint32_t sequence_count;
+  uint32_t request_count;
+  uint32_t response_count;
+  uint32_t count;
+
+#if (DAP_JTAG != 0)
+  *response++ = DAP_OK;
+#else
+  *response++ = DAP_ERROR;
+#endif
+  request_count  = 1U;
+  response_count = 1U;
+
+  sequence_count = *request++;
+  while (sequence_count--) {
+    sequence_info = *request++;
+    count = sequence_info & JTAG_SEQUENCE_TCK;
+    if (count == 0U) {
+      count = 64U;
+    }
+    count = (count + 7U) / 8U;
+#if (DAP_JTAG != 0)
+    JTAG_Sequence(sequence_info, request, response);
+#endif
+    request += count;
+    request_count += count + 1U;
+#if (DAP_JTAG != 0)
+    if ((sequence_info & JTAG_SEQUENCE_TDO) != 0U) {
+      response += count;
+      response_count += count;
+    }
+#endif
+  }
+
+  return ((request_count << 16) | response_count);
+}
+
+
+// Process JTAG Configure command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_JTAG_Configure(const uint8_t *request, uint8_t *response) {
+  uint32_t count;
+#if (DAP_JTAG != 0)
+  uint32_t length;
+  uint32_t bits;
+  uint32_t n;
+
+  count = *request++;
+  DAP_Data.jtag_dev.count = (uint8_t)count;
+
+  bits = 0U;
+  for (n = 0U; n < count; n++) {
+    length = *request++;
+    DAP_Data.jtag_dev.ir_length[n] =  (uint8_t)length;
+    DAP_Data.jtag_dev.ir_before[n] = (uint16_t)bits;
+    bits += length;
+  }
+  for (n = 0U; n < count; n++) {
+    bits -= DAP_Data.jtag_dev.ir_length[n];
+    DAP_Data.jtag_dev.ir_after[n] = (uint16_t)bits;
+  }
+
+  *response = DAP_OK;
+#else
+  count = *request;
+  *response = DAP_ERROR;
+#endif
+
+  return (((count + 1U) << 16) | 1U);
+}
+
+
+// Process JTAG IDCODE command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_JTAG_IDCode(const uint8_t *request, uint8_t *response) {
+#if (DAP_JTAG != 0)
+  uint32_t data;
+
+  if (DAP_Data.debug_port != DAP_PORT_JTAG) {
+    goto id_error;
+  }
+
+  // Device index (JTAP TAP)
+  DAP_Data.jtag_dev.index = *request;
+  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
+    goto id_error;
+  }
+
+  // Select JTAG chain
+  JTAG_IR(JTAG_IDCODE);
+
+  // Read IDCODE register
+  data = JTAG_ReadIDCode();
+
+  // Store Data
+  *(response+0) =  DAP_OK;
+  *(response+1) = (uint8_t)(data >>  0);
+  *(response+2) = (uint8_t)(data >>  8);
+  *(response+3) = (uint8_t)(data >> 16);
+  *(response+4) = (uint8_t)(data >> 24);
+
+  return ((1U << 16) | 5U);
+
+id_error:
+#endif
+  *response = DAP_ERROR;
+  return ((1U << 16) | 1U);
+}
+
+
+// Process Transfer Configure command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_TransferConfigure(const uint8_t *request, uint8_t *response) {
+
+  DAP_Data.transfer.idle_cycles =            *(request+0);
+  DAP_Data.transfer.retry_count = (uint16_t) *(request+1) |
+                                  (uint16_t)(*(request+2) << 8);
+  DAP_Data.transfer.match_retry = (uint16_t) *(request+3) |
+                                  (uint16_t)(*(request+4) << 8);
+
+  *response = DAP_OK;
+  return ((5U << 16) | 1U);
+}
+
+
+// Process SWD Transfer command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+#if (DAP_SWD != 0)
+static uint32_t DAP_SWD_Transfer(const uint8_t *request, uint8_t *response) {
+  const
+  uint8_t  *request_head;
+  uint32_t  request_count;
+  uint32_t  request_value;
+  uint8_t  *response_head;
+  uint32_t  response_count;
+  uint32_t  response_value;
+  uint32_t  post_read;
+  uint32_t  check_write;
+  uint32_t  match_value;
+  uint32_t  match_retry;
+  uint32_t  retry;
+  uint32_t  data;
+#if (TIMESTAMP_CLOCK != 0U)
+  uint32_t  timestamp;
+#endif
+
+  request_head   = request;
+
+  response_count = 0U;
+  response_value = 0U;
+  response_head  = response;
+  response      += 2;
+
+  DAP_TransferAbort = 0U;
+
+  post_read   = 0U;
+  check_write = 0U;
+
+  request++;            // Ignore DAP index
+
+  request_count = *request++;
+
+  for (; request_count != 0U; request_count--) {
+    request_value = *request++;
+    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+      // Read register
+      if (post_read) {
+        // Read was posted before
+        retry = DAP_Data.transfer.retry_count;
+        if ((request_value & (DAP_TRANSFER_APnDP | DAP_TRANSFER_MATCH_VALUE)) == DAP_TRANSFER_APnDP) {
+          // Read previous AP data and post next AP read
+          do {
+            response_value = SWD_Transfer(request_value, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        } else {
+          // Read previous AP data
+          do {
+            response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          post_read = 0U;
+        }
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+        // Store previous AP data
+        *response++ = (uint8_t) data;
+        *response++ = (uint8_t)(data >>  8);
+        *response++ = (uint8_t)(data >> 16);
+        *response++ = (uint8_t)(data >> 24);
+#if (TIMESTAMP_CLOCK != 0U)
+        if (post_read) {
+          // Store Timestamp of next AP read
+          if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+            timestamp = DAP_Data.timestamp;
+            *response++ = (uint8_t) timestamp;
+            *response++ = (uint8_t)(timestamp >>  8);
+            *response++ = (uint8_t)(timestamp >> 16);
+            *response++ = (uint8_t)(timestamp >> 24);
+          }
+        }
+#endif
+      }
+      if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
+        // Read with value match
+        match_value = (uint32_t)(*(request+0) <<  0) |
+                      (uint32_t)(*(request+1) <<  8) |
+                      (uint32_t)(*(request+2) << 16) |
+                      (uint32_t)(*(request+3) << 24);
+        request += 4;
+        match_retry = DAP_Data.transfer.match_retry;
+        if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
+          // Post AP read
+          retry = DAP_Data.transfer.retry_count;
+          do {
+            response_value = SWD_Transfer(request_value, NULL);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          if (response_value != DAP_TRANSFER_OK) {
+            break;
+          }
+        }
+        do {
+          // Read register until its value matches or retry counter expires
+          retry = DAP_Data.transfer.retry_count;
+          do {
+            response_value = SWD_Transfer(request_value, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          if (response_value != DAP_TRANSFER_OK) {
+            break;
+          }
+        } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
+        if ((data & DAP_Data.transfer.match_mask) != match_value) {
+          response_value |= DAP_TRANSFER_MISMATCH;
+        }
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+      } else {
+        // Normal read
+        retry = DAP_Data.transfer.retry_count;
+        if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
+          // Read AP register
+          if (post_read == 0U) {
+            // Post AP read
+            do {
+              response_value = SWD_Transfer(request_value, NULL);
+            } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+            if (response_value != DAP_TRANSFER_OK) {
+              break;
+            }
+#if (TIMESTAMP_CLOCK != 0U)
+            // Store Timestamp
+            if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+              timestamp = DAP_Data.timestamp;
+              *response++ = (uint8_t) timestamp;
+              *response++ = (uint8_t)(timestamp >>  8);
+              *response++ = (uint8_t)(timestamp >> 16);
+              *response++ = (uint8_t)(timestamp >> 24);
+            }
+#endif
+            post_read = 1U;
+          }
+        } else {
+          // Read DP register
+          do {
+            response_value = SWD_Transfer(request_value, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          if (response_value != DAP_TRANSFER_OK) {
+            break;
+          }
+#if (TIMESTAMP_CLOCK != 0U)
+          // Store Timestamp
+          if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+            timestamp = DAP_Data.timestamp;
+            *response++ = (uint8_t) timestamp;
+            *response++ = (uint8_t)(timestamp >>  8);
+            *response++ = (uint8_t)(timestamp >> 16);
+            *response++ = (uint8_t)(timestamp >> 24);
+          }
+#endif
+          // Store data
+          *response++ = (uint8_t) data;
+          *response++ = (uint8_t)(data >>  8);
+          *response++ = (uint8_t)(data >> 16);
+          *response++ = (uint8_t)(data >> 24);
+        }
+      }
+      check_write = 0U;
+    } else {
+      // Write register
+      if (post_read) {
+        // Read previous data
+        retry = DAP_Data.transfer.retry_count;
+        do {
+          response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+        // Store previous data
+        *response++ = (uint8_t) data;
+        *response++ = (uint8_t)(data >>  8);
+        *response++ = (uint8_t)(data >> 16);
+        *response++ = (uint8_t)(data >> 24);
+        post_read = 0U;
+      }
+      // Load data
+      data = (uint32_t)(*(request+0) <<  0) |
+             (uint32_t)(*(request+1) <<  8) |
+             (uint32_t)(*(request+2) << 16) |
+             (uint32_t)(*(request+3) << 24);
+      request += 4;
+      if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
+        // Write match mask
+        DAP_Data.transfer.match_mask = data;
+        response_value = DAP_TRANSFER_OK;
+      } else {
+        // Write DP/AP register
+        retry = DAP_Data.transfer.retry_count;
+        do {
+          response_value = SWD_Transfer(request_value, &data);
+        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+#if (TIMESTAMP_CLOCK != 0U)
+        // Store Timestamp
+        if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+          timestamp = DAP_Data.timestamp;
+          *response++ = (uint8_t) timestamp;
+          *response++ = (uint8_t)(timestamp >>  8);
+          *response++ = (uint8_t)(timestamp >> 16);
+          *response++ = (uint8_t)(timestamp >> 24);
+        }
+#endif
+        check_write = 1U;
+      }
+    }
+    response_count++;
+    if (DAP_TransferAbort) {
+      break;
+    }
+  }
+
+  for (; request_count != 0U; request_count--) {
+    // Process canceled requests
+    request_value = *request++;
+    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+      // Read register
+      if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
+        // Read with value match
+        request += 4;
+      }
+    } else {
+      // Write register
+      request += 4;
+    }
+  }
+
+  if (response_value == DAP_TRANSFER_OK) {
+    if (post_read) {
+      // Read previous data
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      // Store previous data
+      *response++ = (uint8_t) data;
+      *response++ = (uint8_t)(data >>  8);
+      *response++ = (uint8_t)(data >> 16);
+      *response++ = (uint8_t)(data >> 24);
+    } else if (check_write) {
+      // Check last write
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+    }
+  }
+
+end:
+  *(response_head+0) = (uint8_t)response_count;
+  *(response_head+1) = (uint8_t)response_value;
+
+  return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
+}
+#endif
+
+
+// Process JTAG Transfer command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+#if (DAP_JTAG != 0)
+static uint32_t DAP_JTAG_Transfer(const uint8_t *request, uint8_t *response) {
+  const
+  uint8_t  *request_head;
+  uint32_t  request_count;
+  uint32_t  request_value;
+  uint32_t  request_ir;
+  uint8_t  *response_head;
+  uint32_t  response_count;
+  uint32_t  response_value;
+  uint32_t  post_read;
+  uint32_t  match_value;
+  uint32_t  match_retry;
+  uint32_t  retry;
+  uint32_t  data;
+  uint32_t  ir;
+#if (TIMESTAMP_CLOCK != 0U)
+  uint32_t  timestamp;
+#endif
+
+  request_head   = request;
+
+  response_count = 0U;
+  response_value = 0U;
+  response_head  = response;
+  response      += 2;
+
+  DAP_TransferAbort = 0U;
+
+  ir        = 0U;
+  post_read = 0U;
+
+  // Device index (JTAP TAP)
+  DAP_Data.jtag_dev.index = *request++;
+  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
+    goto end;
+  }
+
+  request_count = *request++;
+
+  for (; request_count != 0U; request_count--) {
+    request_value = *request++;
+    request_ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
+    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+      // Read register
+      if (post_read) {
+        // Read was posted before
+        retry = DAP_Data.transfer.retry_count;
+        if ((ir == request_ir) && ((request_value & DAP_TRANSFER_MATCH_VALUE) == 0U)) {
+          // Read previous data and post next read
+          do {
+            response_value = JTAG_Transfer(request_value, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        } else {
+          // Select JTAG chain
+          if (ir != JTAG_DPACC) {
+            ir = JTAG_DPACC;
+            JTAG_IR(ir);
+          }
+          // Read previous data
+          do {
+            response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          post_read = 0U;
+        }
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+        // Store previous data
+        *response++ = (uint8_t) data;
+        *response++ = (uint8_t)(data >>  8);
+        *response++ = (uint8_t)(data >> 16);
+        *response++ = (uint8_t)(data >> 24);
+#if (TIMESTAMP_CLOCK != 0U)
+        if (post_read) {
+          // Store Timestamp of next AP read
+          if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+            timestamp = DAP_Data.timestamp;
+            *response++ = (uint8_t) timestamp;
+            *response++ = (uint8_t)(timestamp >>  8);
+            *response++ = (uint8_t)(timestamp >> 16);
+            *response++ = (uint8_t)(timestamp >> 24);
+          }
+        }
+#endif
+      }
+      if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
+        // Read with value match
+        match_value = (uint32_t)(*(request+0) <<  0) |
+                      (uint32_t)(*(request+1) <<  8) |
+                      (uint32_t)(*(request+2) << 16) |
+                      (uint32_t)(*(request+3) << 24);
+        request += 4;
+        match_retry  = DAP_Data.transfer.match_retry;
+        // Select JTAG chain
+        if (ir != request_ir) {
+          ir = request_ir;
+          JTAG_IR(ir);
+        }
+        // Post DP/AP read
+        retry = DAP_Data.transfer.retry_count;
+        do {
+          response_value = JTAG_Transfer(request_value, NULL);
+        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+        do {
+          // Read register until its value matches or retry counter expires
+          retry = DAP_Data.transfer.retry_count;
+          do {
+            response_value = JTAG_Transfer(request_value, &data);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          if (response_value != DAP_TRANSFER_OK) {
+            break;
+          }
+        } while (((data & DAP_Data.transfer.match_mask) != match_value) && match_retry-- && !DAP_TransferAbort);
+        if ((data & DAP_Data.transfer.match_mask) != match_value) {
+          response_value |= DAP_TRANSFER_MISMATCH;
+        }
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+      } else {
+        // Normal read
+        if (post_read == 0U) {
+          // Select JTAG chain
+          if (ir != request_ir) {
+            ir = request_ir;
+            JTAG_IR(ir);
+          }
+          // Post DP/AP read
+          retry = DAP_Data.transfer.retry_count;
+          do {
+            response_value = JTAG_Transfer(request_value, NULL);
+          } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+          if (response_value != DAP_TRANSFER_OK) {
+            break;
+          }
+#if (TIMESTAMP_CLOCK != 0U)
+          // Store Timestamp
+          if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+            timestamp = DAP_Data.timestamp;
+            *response++ = (uint8_t) timestamp;
+            *response++ = (uint8_t)(timestamp >>  8);
+            *response++ = (uint8_t)(timestamp >> 16);
+            *response++ = (uint8_t)(timestamp >> 24);
+          }
+#endif
+          post_read = 1U;
+        }
+      }
+    } else {
+      // Write register
+      if (post_read) {
+        // Select JTAG chain
+        if (ir != JTAG_DPACC) {
+          ir = JTAG_DPACC;
+          JTAG_IR(ir);
+        }
+        // Read previous data
+        retry = DAP_Data.transfer.retry_count;
+        do {
+          response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+        // Store previous data
+        *response++ = (uint8_t) data;
+        *response++ = (uint8_t)(data >>  8);
+        *response++ = (uint8_t)(data >> 16);
+        *response++ = (uint8_t)(data >> 24);
+        post_read = 0U;
+      }
+      // Load data
+      data = (uint32_t)(*(request+0) <<  0) |
+             (uint32_t)(*(request+1) <<  8) |
+             (uint32_t)(*(request+2) << 16) |
+             (uint32_t)(*(request+3) << 24);
+      request += 4;
+      if ((request_value & DAP_TRANSFER_MATCH_MASK) != 0U) {
+        // Write match mask
+        DAP_Data.transfer.match_mask = data;
+        response_value = DAP_TRANSFER_OK;
+      } else {
+        // Select JTAG chain
+        if (ir != request_ir) {
+          ir = request_ir;
+          JTAG_IR(ir);
+        }
+        // Write DP/AP register
+        retry = DAP_Data.transfer.retry_count;
+        do {
+          response_value = JTAG_Transfer(request_value, &data);
+        } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+        if (response_value != DAP_TRANSFER_OK) {
+          break;
+        }
+#if (TIMESTAMP_CLOCK != 0U)
+        // Store Timestamp
+        if ((request_value & DAP_TRANSFER_TIMESTAMP) != 0U) {
+          timestamp = DAP_Data.timestamp;
+          *response++ = (uint8_t) timestamp;
+          *response++ = (uint8_t)(timestamp >>  8);
+          *response++ = (uint8_t)(timestamp >> 16);
+          *response++ = (uint8_t)(timestamp >> 24);
+        }
+#endif
+      }
+    }
+    response_count++;
+    if (DAP_TransferAbort) {
+      break;
+    }
+  }
+
+  for (; request_count != 0U; request_count--) {
+    // Process canceled requests
+    request_value = *request++;
+    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+      // Read register
+      if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
+        // Read with value match
+        request += 4;
+      }
+    } else {
+      // Write register
+      request += 4;
+    }
+  }
+
+  if (response_value == DAP_TRANSFER_OK) {
+    // Select JTAG chain
+    if (ir != JTAG_DPACC) {
+      ir = JTAG_DPACC;
+      JTAG_IR(ir);
+    }
+    if (post_read) {
+      // Read previous data
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      // Store previous data
+      *response++ = (uint8_t) data;
+      *response++ = (uint8_t)(data >>  8);
+      *response++ = (uint8_t)(data >> 16);
+      *response++ = (uint8_t)(data >> 24);
+    } else {
+      // Check last write
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+    }
+  }
+
+end:
+  *(response_head+0) = (uint8_t)response_count;
+  *(response_head+1) = (uint8_t)response_value;
+
+  return (((uint32_t)(request - request_head) << 16) | (uint32_t)(response - response_head));
+}
+#endif
+
+
+// Process Dummy Transfer command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_Dummy_Transfer(const uint8_t *request, uint8_t *response) {
+  const
+  uint8_t  *request_head;
+  uint32_t  request_count;
+  uint32_t  request_value;
+
+  request_head  =  request;
+
+  request++;            // Ignore DAP index
+
+  request_count = *request++;
+
+  for (; request_count != 0U; request_count--) {
+    // Process dummy requests
+    request_value = *request++;
+    if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+      // Read register
+      if ((request_value & DAP_TRANSFER_MATCH_VALUE) != 0U) {
+        // Read with value match
+        request += 4;
+      }
+    } else {
+      // Write register
+      request += 4;
+    }
+  }
+
+  *(response+0) = 0U;   // Response count
+  *(response+1) = 0U;   // Response value
+
+  return (((uint32_t)(request - request_head) << 16) | 2U);
+}
+
+
+// Process Transfer command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_Transfer(const uint8_t *request, uint8_t *response) {
+  uint32_t num;
+
+  switch (DAP_Data.debug_port) {
+#if (DAP_SWD != 0)
+    case DAP_PORT_SWD:
+      num = DAP_SWD_Transfer(request, response);
+      break;
+#endif
+#if (DAP_JTAG != 0)
+    case DAP_PORT_JTAG:
+      num = DAP_JTAG_Transfer(request, response);
+      break;
+#endif
+    default:
+      num = DAP_Dummy_Transfer(request, response);
+      break;
+  }
+
+  return (num);
+}
+
+
+// Process SWD Transfer Block command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response
+#if (DAP_SWD != 0)
+static uint32_t DAP_SWD_TransferBlock(const uint8_t *request, uint8_t *response) {
+  uint32_t  request_count;
+  uint32_t  request_value;
+  uint32_t  response_count;
+  uint32_t  response_value;
+  uint8_t  *response_head;
+  uint32_t  retry;
+  uint32_t  data;
+
+  response_count = 0U;
+  response_value = 0U;
+  response_head  = response;
+  response      += 3;
+
+  DAP_TransferAbort = 0U;
+
+  request++;            // Ignore DAP index
+
+  request_count = (uint32_t)(*(request+0) << 0) |
+                  (uint32_t)(*(request+1) << 8);
+  request += 2;
+  if (request_count == 0U) {
+    goto end;
+  }
+
+  request_value = *request++;
+  if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+    // Read register block
+    if ((request_value & DAP_TRANSFER_APnDP) != 0U) {
+      // Post AP read
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = SWD_Transfer(request_value, NULL);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+    }
+    while (request_count--) {
+      // Read DP/AP register
+      if ((request_count == 0U) && ((request_value & DAP_TRANSFER_APnDP) != 0U)) {
+        // Last AP read
+        request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
+      }
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = SWD_Transfer(request_value, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      // Store data
+      *response++ = (uint8_t) data;
+      *response++ = (uint8_t)(data >>  8);
+      *response++ = (uint8_t)(data >> 16);
+      *response++ = (uint8_t)(data >> 24);
+      response_count++;
+    }
+  } else {
+    // Write register block
+    while (request_count--) {
+      // Load data
+      data = (uint32_t)(*(request+0) <<  0) |
+             (uint32_t)(*(request+1) <<  8) |
+             (uint32_t)(*(request+2) << 16) |
+             (uint32_t)(*(request+3) << 24);
+      request += 4;
+      // Write DP/AP register
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = SWD_Transfer(request_value, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      response_count++;
+    }
+    // Check last write
+    retry = DAP_Data.transfer.retry_count;
+    do {
+      response_value = SWD_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
+    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+  }
+
+end:
+  *(response_head+0) = (uint8_t)(response_count >> 0);
+  *(response_head+1) = (uint8_t)(response_count >> 8);
+  *(response_head+2) = (uint8_t) response_value;
+
+  return ((uint32_t)(response - response_head));
+}
+#endif
+
+
+// Process JTAG Transfer Block command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response
+#if (DAP_JTAG != 0)
+static uint32_t DAP_JTAG_TransferBlock(const uint8_t *request, uint8_t *response) {
+  uint32_t  request_count;
+  uint32_t  request_value;
+  uint32_t  response_count;
+  uint32_t  response_value;
+  uint8_t  *response_head;
+  uint32_t  retry;
+  uint32_t  data;
+  uint32_t  ir;
+
+  response_count = 0U;
+  response_value = 0U;
+  response_head  = response;
+  response      += 3;
+
+  DAP_TransferAbort = 0U;
+
+  // Device index (JTAP TAP)
+  DAP_Data.jtag_dev.index = *request++;
+  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
+    goto end;
+  }
+
+  request_count = (uint32_t)(*(request+0) << 0) |
+                  (uint32_t)(*(request+1) << 8);
+  request += 2;
+  if (request_count == 0U) {
+    goto end;
+  }
+
+  request_value = *request++;
+
+  // Select JTAG chain
+  ir = (request_value & DAP_TRANSFER_APnDP) ? JTAG_APACC : JTAG_DPACC;
+  JTAG_IR(ir);
+
+  if ((request_value & DAP_TRANSFER_RnW) != 0U) {
+    // Post read
+    retry = DAP_Data.transfer.retry_count;
+    do {
+      response_value = JTAG_Transfer(request_value, NULL);
+    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+    if (response_value != DAP_TRANSFER_OK) {
+      goto end;
+    }
+    // Read register block
+    while (request_count--) {
+      // Read DP/AP register
+      if (request_count == 0U) {
+        // Last read
+        if (ir != JTAG_DPACC) {
+          JTAG_IR(JTAG_DPACC);
+        }
+        request_value = DP_RDBUFF | DAP_TRANSFER_RnW;
+      }
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = JTAG_Transfer(request_value, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      // Store data
+      *response++ = (uint8_t) data;
+      *response++ = (uint8_t)(data >>  8);
+      *response++ = (uint8_t)(data >> 16);
+      *response++ = (uint8_t)(data >> 24);
+      response_count++;
+    }
+  } else {
+    // Write register block
+    while (request_count--) {
+      // Load data
+      data = (uint32_t)(*(request+0) <<  0) |
+             (uint32_t)(*(request+1) <<  8) |
+             (uint32_t)(*(request+2) << 16) |
+             (uint32_t)(*(request+3) << 24);
+      request += 4;
+      // Write DP/AP register
+      retry = DAP_Data.transfer.retry_count;
+      do {
+        response_value = JTAG_Transfer(request_value, &data);
+      } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+      if (response_value != DAP_TRANSFER_OK) {
+        goto end;
+      }
+      response_count++;
+    }
+    // Check last write
+    if (ir != JTAG_DPACC) {
+      JTAG_IR(JTAG_DPACC);
+    }
+    retry = DAP_Data.transfer.retry_count;
+    do {
+      response_value = JTAG_Transfer(DP_RDBUFF | DAP_TRANSFER_RnW, NULL);
+    } while ((response_value == DAP_TRANSFER_WAIT) && retry-- && !DAP_TransferAbort);
+  }
+
+end:
+  *(response_head+0) = (uint8_t)(response_count >> 0);
+  *(response_head+1) = (uint8_t)(response_count >> 8);
+  *(response_head+2) = (uint8_t) response_value;
+
+  return ((uint32_t)(response - response_head));
+}
+#endif
+
+
+// Process Transfer Block command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_TransferBlock(const uint8_t *request, uint8_t *response) {
+  uint32_t num;
+
+  switch (DAP_Data.debug_port) {
+#if (DAP_SWD != 0)
+    case DAP_PORT_SWD:
+      num = DAP_SWD_TransferBlock (request, response);
+      break;
+#endif
+#if (DAP_JTAG != 0)
+    case DAP_PORT_JTAG:
+      num = DAP_JTAG_TransferBlock(request, response);
+      break;
+#endif
+    default:
+      *(response+0) = 0U;       // Response count [7:0]
+      *(response+1) = 0U;       // Response count[15:8]
+      *(response+2) = 0U;       // Response value
+      num = 3U;
+      break;
+  }
+
+  if ((*(request+3) & DAP_TRANSFER_RnW) != 0U) {
+    // Read register block
+    num |=  4U << 16;
+  } else {
+    // Write register block
+    num |= (4U + (((uint32_t)(*(request+1)) | (uint32_t)(*(request+2) << 8)) * 4)) << 16;
+  }
+
+  return (num);
+}
+
+
+// Process SWD Write ABORT command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response
+#if (DAP_SWD != 0)
+static uint32_t DAP_SWD_WriteAbort(const uint8_t *request, uint8_t *response) {
+  uint32_t data;
+
+  // Load data (Ignore DAP index)
+  data = (uint32_t)(*(request+1) <<  0) |
+         (uint32_t)(*(request+2) <<  8) |
+         (uint32_t)(*(request+3) << 16) |
+         (uint32_t)(*(request+4) << 24);
+
+  // Write Abort register
+  SWD_Transfer(DP_ABORT, &data);
+
+  *response = DAP_OK;
+  return (1U);
+}
+#endif
+
+
+// Process JTAG Write ABORT command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response
+#if (DAP_JTAG != 0)
+static uint32_t DAP_JTAG_WriteAbort(const uint8_t *request, uint8_t *response) {
+  uint32_t data;
+
+  // Device index (JTAP TAP)
+  DAP_Data.jtag_dev.index = *request;
+  if (DAP_Data.jtag_dev.index >= DAP_Data.jtag_dev.count) {
+    *response = DAP_ERROR;
+    return (1U);
+  }
+
+  // Select JTAG chain
+  JTAG_IR(JTAG_ABORT);
+
+  // Load data
+  data = (uint32_t)(*(request+1) <<  0) |
+         (uint32_t)(*(request+2) <<  8) |
+         (uint32_t)(*(request+3) << 16) |
+         (uint32_t)(*(request+4) << 24);
+
+  // Write Abort register
+  JTAG_WriteAbort(data);
+
+  *response = DAP_OK;
+  return (1U);
+}
+#endif
+
+
+// Process Write ABORT command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+static uint32_t DAP_WriteAbort(const uint8_t *request, uint8_t *response) {
+  uint32_t num;
+
+  switch (DAP_Data.debug_port) {
+#if (DAP_SWD != 0)
+    case DAP_PORT_SWD:
+      num = DAP_SWD_WriteAbort (request, response);
+      break;
+#endif
+#if (DAP_JTAG != 0)
+    case DAP_PORT_JTAG:
+      num = DAP_JTAG_WriteAbort(request, response);
+      break;
+#endif
+    default:
+      *response = DAP_ERROR;
+      num = 1U;
+      break;
+  }
+  return ((5U << 16) | num);
+}
+
+
+// Process DAP Vendor command request and prepare response
+// Default function (can be overridden)
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+__WEAK uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response) {
+  (void)request;
+  *response = ID_DAP_Invalid;
+  return ((1U << 16) | 1U);
+}
+
+
+// Process DAP command request and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t DAP_ProcessCommand(const uint8_t *request, uint8_t *response) {
+  uint32_t num;
+
+  if ((*request >= ID_DAP_Vendor0) && (*request <= ID_DAP_Vendor31)) {
+    return DAP_ProcessVendorCommand(request, response);
+  }
+
+  *response++ = *request;
+
+  switch (*request++) {
+    case ID_DAP_Info:
+      num = DAP_Info(*request, response+1);
+      *response = (uint8_t)num;
+      return ((2U << 16) + 2U + num);
+
+    case ID_DAP_HostStatus:
+      num = DAP_HostStatus(request, response);
+      break;
+
+    case ID_DAP_Connect:
+      num = DAP_Connect(request, response);
+      break;
+    case ID_DAP_Disconnect:
+      num = DAP_Disconnect(response);
+      break;
+
+    case ID_DAP_Delay:
+      num = DAP_Delay(request, response);
+      break;
+
+    case ID_DAP_ResetTarget:
+      num = DAP_ResetTarget(response);
+      break;
+
+    case ID_DAP_SWJ_Pins:
+      num = DAP_SWJ_Pins(request, response);
+      break;
+    case ID_DAP_SWJ_Clock:
+      num = DAP_SWJ_Clock(request, response);
+      break;
+    case ID_DAP_SWJ_Sequence:
+      num = DAP_SWJ_Sequence(request, response);
+      break;
+
+    case ID_DAP_SWD_Configure:
+      num = DAP_SWD_Configure(request, response);
+      break;
+    case ID_DAP_SWD_Sequence:
+      num = DAP_SWD_Sequence(request, response);
+      break;
+
+    case ID_DAP_JTAG_Sequence:
+      num = DAP_JTAG_Sequence(request, response);
+      break;
+    case ID_DAP_JTAG_Configure:
+      num = DAP_JTAG_Configure(request, response);
+      break;
+    case ID_DAP_JTAG_IDCODE:
+      num = DAP_JTAG_IDCode(request, response);
+      break;
+
+    case ID_DAP_TransferConfigure:
+      num = DAP_TransferConfigure(request, response);
+      break;
+    case ID_DAP_Transfer:
+      num = DAP_Transfer(request, response);
+      break;
+    case ID_DAP_TransferBlock:
+      num = DAP_TransferBlock(request, response);
+      break;
+
+    case ID_DAP_WriteABORT:
+      num = DAP_WriteAbort(request, response);
+      break;
+
+#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
+    case ID_DAP_SWO_Transport:
+      num = SWO_Transport(request, response);
+      break;
+    case ID_DAP_SWO_Mode:
+      num = SWO_Mode(request, response);
+      break;
+    case ID_DAP_SWO_Baudrate:
+      num = SWO_Baudrate(request, response);
+      break;
+    case ID_DAP_SWO_Control:
+      num = SWO_Control(request, response);
+      break;
+    case ID_DAP_SWO_Status:
+      num = SWO_Status(response);
+      break;
+    case ID_DAP_SWO_ExtendedStatus:
+      num = SWO_ExtendedStatus(request, response);
+      break;
+    case ID_DAP_SWO_Data:
+      num = SWO_Data(request, response);
+      break;
+#endif
+
+#if (DAP_UART != 0)
+    case ID_DAP_UART_Transport:
+      num = UART_Transport(request, response);
+      break;
+    case ID_DAP_UART_Configure:
+      num = UART_Configure(request, response);
+      break;
+    case ID_DAP_UART_Control:
+      num = UART_Control(request, response);
+      break;
+    case ID_DAP_UART_Status:
+      num = UART_Status(response);
+      break;
+    case ID_DAP_UART_Transfer:
+      num = UART_Transfer(request, response);
+      break;
+#endif
+
+    default:
+      *(response-1) = ID_DAP_Invalid;
+      return ((1U << 16) | 1U);
+  }
+
+  return ((1U << 16) + 1U + num);
+}
+
+
+// Execute DAP command (process request and prepare response)
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t DAP_ExecuteCommand(const uint8_t *request, uint8_t *response) {
+  uint32_t cnt, num, n;
+
+  if (*request == ID_DAP_ExecuteCommands) {
+    *response++ = *request++;
+    cnt = *request++;
+    *response++ = (uint8_t)cnt;
+    num = (2U << 16) | 2U;
+    while (cnt--) {
+      n = DAP_ProcessCommand(request, response);
+      num += n;
+      request  += (uint16_t)(n >> 16);
+      response += (uint16_t) n;
+    }
+    return (num);
+  }
+
+  return DAP_ProcessCommand(request, response);
+}
+
+
+// Setup DAP
+void DAP_Setup(void) {
+
+  // Default settings
+  DAP_Data.debug_port  = 0U;
+  DAP_Data.transfer.idle_cycles = 0U;
+  DAP_Data.transfer.retry_count = 100U;
+  DAP_Data.transfer.match_retry = 0U;
+  DAP_Data.transfer.match_mask  = 0x00000000U;
+#if (DAP_SWD != 0)
+  DAP_Data.swd_conf.turnaround  = 1U;
+  DAP_Data.swd_conf.data_phase  = 0U;
+#endif
+#if (DAP_JTAG != 0)
+  DAP_Data.jtag_dev.count = 0U;
+#endif
+
+  // Sets DAP_Data.fast_clock and DAP_Data.clock_delay.
+  Set_Clock_Delay(DAP_DEFAULT_SWJ_CLOCK);
+
+  DAP_SETUP();  // Device specific setup
+}
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/DAP_vendor.c b/third_party/CMSIS-DAP-2.1.1/Source/DAP_vendor.c
new file mode 100644
index 00000000..4f2477aa
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/DAP_vendor.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2013-2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        1. December 2017
+ * $Revision:    V2.0.0
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        DAP_vendor.c CMSIS-DAP Vendor Commands
+ *
+ *---------------------------------------------------------------------------*/
+
+#include "DAP_config.h"
+#include "DAP.h"
+
+//**************************************************************************************************
+/**
+\defgroup DAP_Vendor_Adapt_gr Adapt Vendor Commands
+\ingroup DAP_Vendor_gr
+@{
+
+The file DAP_vendor.c provides template source code for extension of a Debug Unit with
+Vendor Commands. Copy this file to the project folder of the Debug Unit and add the
+file to the MDK-ARM project under the file group Configuration.
+*/
+
+/** Process DAP Vendor Command and prepare Response Data
+\param request   pointer to request data
+\param response  pointer to response data
+\return          number of bytes in response (lower 16 bits)
+                 number of bytes in request (upper 16 bits)
+*/
+uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response) {
+  uint32_t num = (1U << 16) | 1U;
+
+  *response++ = *request;        // copy Command ID
+
+  switch (*request++) {          // first byte in request is Command ID
+    case ID_DAP_Vendor0:
+#if 0                            // example user command
+      num += 1U << 16;           // increment request count
+      if (*request == 1U) {      // when first command data byte is 1
+        *response++ = 'X';       // send 'X' as response
+        num++;                   // increment response count
+      }
+#endif
+      break;
+
+    case ID_DAP_Vendor1:  break;
+    case ID_DAP_Vendor2:  break;
+    case ID_DAP_Vendor3:  break;
+    case ID_DAP_Vendor4:  break;
+    case ID_DAP_Vendor5:  break;
+    case ID_DAP_Vendor6:  break;
+    case ID_DAP_Vendor7:  break;
+    case ID_DAP_Vendor8:  break;
+    case ID_DAP_Vendor9:  break;
+    case ID_DAP_Vendor10: break;
+    case ID_DAP_Vendor11: break;
+    case ID_DAP_Vendor12: break;
+    case ID_DAP_Vendor13: break;
+    case ID_DAP_Vendor14: break;
+    case ID_DAP_Vendor15: break;
+    case ID_DAP_Vendor16: break;
+    case ID_DAP_Vendor17: break;
+    case ID_DAP_Vendor18: break;
+    case ID_DAP_Vendor19: break;
+    case ID_DAP_Vendor20: break;
+    case ID_DAP_Vendor21: break;
+    case ID_DAP_Vendor22: break;
+    case ID_DAP_Vendor23: break;
+    case ID_DAP_Vendor24: break;
+    case ID_DAP_Vendor25: break;
+    case ID_DAP_Vendor26: break;
+    case ID_DAP_Vendor27: break;
+    case ID_DAP_Vendor28: break;
+    case ID_DAP_Vendor29: break;
+    case ID_DAP_Vendor30: break;
+    case ID_DAP_Vendor31: break;
+  }
+
+  return (num);
+}
+
+///@}
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/JTAG_DP.c b/third_party/CMSIS-DAP-2.1.1/Source/JTAG_DP.c
new file mode 100644
index 00000000..24b1f3f9
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/JTAG_DP.c
@@ -0,0 +1,370 @@
+/*
+ * Copyright (c) 2013-2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        1. December 2017
+ * $Revision:    V2.0.0
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        JTAG_DP.c CMSIS-DAP JTAG DP I/O
+ *
+ *---------------------------------------------------------------------------*/
+
+#include "DAP_config.h"
+#include "DAP.h"
+
+
+// JTAG Macros
+
+#define PIN_TCK_SET PIN_SWCLK_TCK_SET
+#define PIN_TCK_CLR PIN_SWCLK_TCK_CLR
+#define PIN_TMS_SET PIN_SWDIO_TMS_SET
+#define PIN_TMS_CLR PIN_SWDIO_TMS_CLR
+
+#define JTAG_CYCLE_TCK()                \
+  PIN_TCK_CLR();                        \
+  PIN_DELAY();                          \
+  PIN_TCK_SET();                        \
+  PIN_DELAY()
+
+#define JTAG_CYCLE_TDI(tdi)             \
+  PIN_TDI_OUT(tdi);                     \
+  PIN_TCK_CLR();                        \
+  PIN_DELAY();                          \
+  PIN_TCK_SET();                        \
+  PIN_DELAY()
+
+#define JTAG_CYCLE_TDO(tdo)             \
+  PIN_TCK_CLR();                        \
+  PIN_DELAY();                          \
+  tdo = PIN_TDO_IN();                   \
+  PIN_TCK_SET();                        \
+  PIN_DELAY()
+
+#define JTAG_CYCLE_TDIO(tdi,tdo)        \
+  PIN_TDI_OUT(tdi);                     \
+  PIN_TCK_CLR();                        \
+  PIN_DELAY();                          \
+  tdo = PIN_TDO_IN();                   \
+  PIN_TCK_SET();                        \
+  PIN_DELAY()
+
+#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
+
+
+#if (DAP_JTAG != 0)
+
+
+// Generate JTAG Sequence
+//   info:   sequence information
+//   tdi:    pointer to TDI generated data
+//   tdo:    pointer to TDO captured data
+//   return: none
+void JTAG_Sequence (uint32_t info, const uint8_t *tdi, uint8_t *tdo) {
+  uint32_t i_val;
+  uint32_t o_val;
+  uint32_t bit;
+  uint32_t n, k;
+
+  n = info & JTAG_SEQUENCE_TCK;
+  if (n == 0U) {
+    n = 64U;
+  }
+
+  if (info & JTAG_SEQUENCE_TMS) {
+    PIN_TMS_SET();
+  } else {
+    PIN_TMS_CLR();
+  }
+
+  while (n) {
+    i_val = *tdi++;
+    o_val = 0U;
+    for (k = 8U; k && n; k--, n--) {
+      JTAG_CYCLE_TDIO(i_val, bit);
+      i_val >>= 1;
+      o_val >>= 1;
+      o_val  |= bit << 7;
+    }
+    o_val >>= k;
+    if (info & JTAG_SEQUENCE_TDO) {
+      *tdo++ = (uint8_t)o_val;
+    }
+  }
+}
+
+
+// JTAG Set IR
+//   ir:     IR value
+//   return: none
+#define JTAG_IR_Function(speed) /**/                                            \
+static void JTAG_IR_##speed (uint32_t ir) {                                     \
+  uint32_t n;                                                                   \
+                                                                                \
+  PIN_TMS_SET();                                                                \
+  JTAG_CYCLE_TCK();                         /* Select-DR-Scan */                \
+  JTAG_CYCLE_TCK();                         /* Select-IR-Scan */                \
+  PIN_TMS_CLR();                                                                \
+  JTAG_CYCLE_TCK();                         /* Capture-IR */                    \
+  JTAG_CYCLE_TCK();                         /* Shift-IR */                      \
+                                                                                \
+  PIN_TDI_OUT(1U);                                                              \
+  for (n = DAP_Data.jtag_dev.ir_before[DAP_Data.jtag_dev.index]; n; n--) {      \
+    JTAG_CYCLE_TCK();                       /* Bypass before data */            \
+  }                                                                             \
+  for (n = DAP_Data.jtag_dev.ir_length[DAP_Data.jtag_dev.index] - 1U; n; n--) { \
+    JTAG_CYCLE_TDI(ir);                     /* Set IR bits (except last) */     \
+    ir >>= 1;                                                                   \
+  }                                                                             \
+  n = DAP_Data.jtag_dev.ir_after[DAP_Data.jtag_dev.index];                      \
+  if (n) {                                                                      \
+    JTAG_CYCLE_TDI(ir);                     /* Set last IR bit */               \
+    PIN_TDI_OUT(1U);                                                            \
+    for (--n; n; n--) {                                                         \
+      JTAG_CYCLE_TCK();                     /* Bypass after data */             \
+    }                                                                           \
+    PIN_TMS_SET();                                                              \
+    JTAG_CYCLE_TCK();                       /* Bypass & Exit1-IR */             \
+  } else {                                                                      \
+    PIN_TMS_SET();                                                              \
+    JTAG_CYCLE_TDI(ir);                     /* Set last IR bit & Exit1-IR */    \
+  }                                                                             \
+                                                                                \
+  JTAG_CYCLE_TCK();                         /* Update-IR */                     \
+  PIN_TMS_CLR();                                                                \
+  JTAG_CYCLE_TCK();                         /* Idle */                          \
+  PIN_TDI_OUT(1U);                                                              \
+}
+
+
+// JTAG Transfer I/O
+//   request: A[3:2] RnW APnDP
+//   data:    DATA[31:0]
+//   return:  ACK[2:0]
+#define JTAG_TransferFunction(speed)        /**/                                \
+static uint8_t JTAG_Transfer##speed (uint32_t request, uint32_t *data) {        \
+  uint32_t ack;                                                                 \
+  uint32_t bit;                                                                 \
+  uint32_t val;                                                                 \
+  uint32_t n;                                                                   \
+                                                                                \
+  PIN_TMS_SET();                                                                \
+  JTAG_CYCLE_TCK();                         /* Select-DR-Scan */                \
+  PIN_TMS_CLR();                                                                \
+  JTAG_CYCLE_TCK();                         /* Capture-DR */                    \
+  JTAG_CYCLE_TCK();                         /* Shift-DR */                      \
+                                                                                \
+  for (n = DAP_Data.jtag_dev.index; n; n--) {                                   \
+    JTAG_CYCLE_TCK();                       /* Bypass before data */            \
+  }                                                                             \
+                                                                                \
+  JTAG_CYCLE_TDIO(request >> 1, bit);       /* Set RnW, Get ACK.0 */            \
+  ack  = bit << 1;                                                              \
+  JTAG_CYCLE_TDIO(request >> 2, bit);       /* Set A2,  Get ACK.1 */            \
+  ack |= bit << 0;                                                              \
+  JTAG_CYCLE_TDIO(request >> 3, bit);       /* Set A3,  Get ACK.2 */            \
+  ack |= bit << 2;                                                              \
+                                                                                \
+  if (ack != DAP_TRANSFER_OK) {                                                 \
+    /* Exit on error */                                                         \
+    PIN_TMS_SET();                                                              \
+    JTAG_CYCLE_TCK();                       /* Exit1-DR */                      \
+    goto exit;                                                                  \
+  }                                                                             \
+                                                                                \
+  if (request & DAP_TRANSFER_RnW) {                                             \
+    /* Read Transfer */                                                         \
+    val = 0U;                                                                   \
+    for (n = 31U; n; n--) {                                                     \
+      JTAG_CYCLE_TDO(bit);                  /* Get D0..D30 */                   \
+      val  |= bit << 31;                                                        \
+      val >>= 1;                                                                \
+    }                                                                           \
+    n = DAP_Data.jtag_dev.count - DAP_Data.jtag_dev.index - 1U;                 \
+    if (n) {                                                                    \
+      JTAG_CYCLE_TDO(bit);                  /* Get D31 */                       \
+      for (--n; n; n--) {                                                       \
+        JTAG_CYCLE_TCK();                   /* Bypass after data */             \
+      }                                                                         \
+      PIN_TMS_SET();                                                            \
+      JTAG_CYCLE_TCK();                     /* Bypass & Exit1-DR */             \
+    } else {                                                                    \
+      PIN_TMS_SET();                                                            \
+      JTAG_CYCLE_TDO(bit);                  /* Get D31 & Exit1-DR */            \
+    }                                                                           \
+    val |= bit << 31;                                                           \
+    if (data) { *data = val; }                                                  \
+  } else {                                                                      \
+    /* Write Transfer */                                                        \
+    val = *data;                                                                \
+    for (n = 31U; n; n--) {                                                     \
+      JTAG_CYCLE_TDI(val);                  /* Set D0..D30 */                   \
+      val >>= 1;                                                                \
+    }                                                                           \
+    n = DAP_Data.jtag_dev.count - DAP_Data.jtag_dev.index - 1U;                 \
+    if (n) {                                                                    \
+      JTAG_CYCLE_TDI(val);                  /* Set D31 */                       \
+      for (--n; n; n--) {                                                       \
+        JTAG_CYCLE_TCK();                   /* Bypass after data */             \
+      }                                                                         \
+      PIN_TMS_SET();                                                            \
+      JTAG_CYCLE_TCK();                     /* Bypass & Exit1-DR */             \
+    } else {                                                                    \
+      PIN_TMS_SET();                                                            \
+      JTAG_CYCLE_TDI(val);                  /* Set D31 & Exit1-DR */            \
+    }                                                                           \
+  }                                                                             \
+                                                                                \
+exit:                                                                           \
+  JTAG_CYCLE_TCK();                         /* Update-DR */                     \
+  PIN_TMS_CLR();                                                                \
+  JTAG_CYCLE_TCK();                         /* Idle */                          \
+  PIN_TDI_OUT(1U);                                                              \
+                                                                                \
+  /* Capture Timestamp */                                                       \
+  if (request & DAP_TRANSFER_TIMESTAMP) {                                       \
+    DAP_Data.timestamp = TIMESTAMP_GET();                                       \
+  }                                                                             \
+                                                                                \
+  /* Idle cycles */                                                             \
+  n = DAP_Data.transfer.idle_cycles;                                            \
+  while (n--) {                                                                 \
+    JTAG_CYCLE_TCK();                       /* Idle */                          \
+  }                                                                             \
+                                                                                \
+  return ((uint8_t)ack);                                                        \
+}
+
+
+#undef  PIN_DELAY
+#define PIN_DELAY() PIN_DELAY_FAST()
+JTAG_IR_Function(Fast)
+JTAG_TransferFunction(Fast)
+
+#undef  PIN_DELAY
+#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
+JTAG_IR_Function(Slow)
+JTAG_TransferFunction(Slow)
+
+
+// JTAG Read IDCODE register
+//   return: value read
+uint32_t JTAG_ReadIDCode (void) {
+  uint32_t bit;
+  uint32_t val;
+  uint32_t n;
+
+  PIN_TMS_SET();
+  JTAG_CYCLE_TCK();                         /* Select-DR-Scan */
+  PIN_TMS_CLR();
+  JTAG_CYCLE_TCK();                         /* Capture-DR */
+  JTAG_CYCLE_TCK();                         /* Shift-DR */
+
+  for (n = DAP_Data.jtag_dev.index; n; n--) {
+    JTAG_CYCLE_TCK();                       /* Bypass before data */
+  }
+
+  val = 0U;
+  for (n = 31U; n; n--) {
+    JTAG_CYCLE_TDO(bit);                    /* Get D0..D30 */
+    val  |= bit << 31;
+    val >>= 1;
+  }
+  PIN_TMS_SET();
+  JTAG_CYCLE_TDO(bit);                      /* Get D31 & Exit1-DR */
+  val |= bit << 31;
+
+  JTAG_CYCLE_TCK();                         /* Update-DR */
+  PIN_TMS_CLR();
+  JTAG_CYCLE_TCK();                         /* Idle */
+
+  return (val);
+}
+
+
+// JTAG Write ABORT register
+//   data:   value to write
+//   return: none
+void JTAG_WriteAbort (uint32_t data) {
+  uint32_t n;
+
+  PIN_TMS_SET();
+  JTAG_CYCLE_TCK();                         /* Select-DR-Scan */
+  PIN_TMS_CLR();
+  JTAG_CYCLE_TCK();                         /* Capture-DR */
+  JTAG_CYCLE_TCK();                         /* Shift-DR */
+
+  for (n = DAP_Data.jtag_dev.index; n; n--) {
+    JTAG_CYCLE_TCK();                       /* Bypass before data */
+  }
+
+  PIN_TDI_OUT(0U);
+  JTAG_CYCLE_TCK();                         /* Set RnW=0 (Write) */
+  JTAG_CYCLE_TCK();                         /* Set A2=0 */
+  JTAG_CYCLE_TCK();                         /* Set A3=0 */
+
+  for (n = 31U; n; n--) {
+    JTAG_CYCLE_TDI(data);                   /* Set D0..D30 */
+    data >>= 1;
+  }
+  n = DAP_Data.jtag_dev.count - DAP_Data.jtag_dev.index - 1U;
+  if (n) {
+    JTAG_CYCLE_TDI(data);                   /* Set D31 */
+    for (--n; n; n--) {
+      JTAG_CYCLE_TCK();                     /* Bypass after data */
+    }
+    PIN_TMS_SET();
+    JTAG_CYCLE_TCK();                       /* Bypass & Exit1-DR */
+  } else {
+    PIN_TMS_SET();
+    JTAG_CYCLE_TDI(data);                   /* Set D31 & Exit1-DR */
+  }
+
+  JTAG_CYCLE_TCK();                         /* Update-DR */
+  PIN_TMS_CLR();
+  JTAG_CYCLE_TCK();                         /* Idle */
+  PIN_TDI_OUT(1U);
+}
+
+
+// JTAG Set IR
+//   ir:     IR value
+//   return: none
+void JTAG_IR (uint32_t ir) {
+  if (DAP_Data.fast_clock) {
+    JTAG_IR_Fast(ir);
+  } else {
+    JTAG_IR_Slow(ir);
+  }
+}
+
+
+// JTAG Transfer I/O
+//   request: A[3:2] RnW APnDP
+//   data:    DATA[31:0]
+//   return:  ACK[2:0]
+uint8_t  JTAG_Transfer(uint32_t request, uint32_t *data) {
+  if (DAP_Data.fast_clock) {
+    return JTAG_TransferFast(request, data);
+  } else {
+    return JTAG_TransferSlow(request, data);
+  }
+}
+
+
+#endif  /* (DAP_JTAG != 0) */
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/SWO.c b/third_party/CMSIS-DAP-2.1.1/Source/SWO.c
new file mode 100644
index 00000000..4a850cf8
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/SWO.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2013-2021 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        29. March 2021
+ * $Revision:    V2.0.1
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        SWO.c CMSIS-DAP SWO I/O
+ *
+ *---------------------------------------------------------------------------*/
+
+#include "DAP_config.h"
+#include "DAP.h"
+#if (SWO_UART != 0)
+#include "Driver_USART.h"
+#endif
+#if (SWO_STREAM != 0)
+#include "cmsis_os2.h"
+#define   osObjectsExternal
+#include "osObjects.h"
+#endif
+
+#if (SWO_STREAM != 0)
+#ifdef DAP_FW_V1
+#error "SWO Streaming Trace not supported in DAP V1!"
+#endif
+#endif
+
+#if (SWO_UART != 0)
+
+// USART Driver
+#define _USART_Driver_(n)  Driver_USART##n
+#define  USART_Driver_(n) _USART_Driver_(n)
+extern ARM_DRIVER_USART    USART_Driver_(SWO_UART_DRIVER);
+#define pUSART           (&USART_Driver_(SWO_UART_DRIVER))
+
+static uint8_t USART_Ready = 0U;
+
+#endif  /* (SWO_UART != 0) */
+
+
+#if ((SWO_UART != 0) || (SWO_MANCHESTER != 0))
+
+
+#define SWO_STREAM_TIMEOUT      50U     /* Stream timeout in ms */
+
+#define USB_BLOCK_SIZE          512U    /* USB Block Size */
+#define TRACE_BLOCK_SIZE        64U     /* Trace Block Size (2^n: 32...512) */
+
+// Trace State
+static uint8_t  TraceTransport =  0U;       /* Trace Transport */
+static uint8_t  TraceMode      =  0U;       /* Trace Mode */
+static uint8_t  TraceStatus    =  0U;       /* Trace Status without Errors */
+static uint8_t  TraceError[2]  = {0U, 0U};  /* Trace Error flags (banked) */
+static uint8_t  TraceError_n   =  0U;       /* Active Trace Error bank */
+
+// Trace Buffer
+static uint8_t  TraceBuf[SWO_BUFFER_SIZE];  /* Trace Buffer (must be 2^n) */
+static volatile uint32_t TraceIndexI  = 0U; /* Incoming Trace Index */
+static volatile uint32_t TraceIndexO  = 0U; /* Outgoing Trace Index */
+static volatile uint8_t  TraceUpdate;       /* Trace Update Flag */
+static          uint32_t TraceBlockSize;    /* Current Trace Block Size */
+
+#if (TIMESTAMP_CLOCK != 0U)
+// Trace Timestamp
+static volatile struct {
+  uint32_t index;
+  uint32_t tick;
+} TraceTimestamp;
+#endif
+
+// Trace Helper functions
+static void     ClearTrace     (void);
+static void     ResumeTrace    (void);
+static uint32_t GetTraceCount  (void);
+static uint8_t  GetTraceStatus (void);
+static void     SetTraceError  (uint8_t flag);
+
+#if (SWO_STREAM != 0)
+extern osThreadId_t      SWO_ThreadId;
+static volatile uint8_t  TransferBusy = 0U; /* Transfer Busy Flag */
+static          uint32_t TransferSize;      /* Current Transfer Size */
+#endif
+
+
+#if (SWO_UART != 0)
+
+// USART Driver Callback function
+//   event: event mask
+static void USART_Callback (uint32_t event) {
+  uint32_t index_i;
+  uint32_t index_o;
+  uint32_t count;
+  uint32_t num;
+
+  if (event &  ARM_USART_EVENT_RECEIVE_COMPLETE) {
+#if (TIMESTAMP_CLOCK != 0U)
+    TraceTimestamp.tick = TIMESTAMP_GET();
+#endif
+    index_o  = TraceIndexO;
+    index_i  = TraceIndexI;
+    index_i += TraceBlockSize;
+    TraceIndexI = index_i;
+#if (TIMESTAMP_CLOCK != 0U)
+    TraceTimestamp.index = index_i;
+#endif
+    num   = TRACE_BLOCK_SIZE - (index_i & (TRACE_BLOCK_SIZE - 1U));
+    count = index_i - index_o;
+    if (count <= (SWO_BUFFER_SIZE - num)) {
+      index_i &= SWO_BUFFER_SIZE - 1U;
+      TraceBlockSize = num;
+      pUSART->Receive(&TraceBuf[index_i], num);
+    } else {
+      TraceStatus = DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED;
+    }
+    TraceUpdate = 1U;
+#if (SWO_STREAM != 0)
+    if (TraceTransport == 2U) {
+      if (count >= (USB_BLOCK_SIZE - (index_o & (USB_BLOCK_SIZE - 1U)))) {
+        osThreadFlagsSet(SWO_ThreadId, 1U);
+      }
+    }
+#endif
+  }
+  if (event &  ARM_USART_EVENT_RX_OVERFLOW) {
+    SetTraceError(DAP_SWO_BUFFER_OVERRUN);
+  }
+  if (event & (ARM_USART_EVENT_RX_BREAK         |
+               ARM_USART_EVENT_RX_FRAMING_ERROR |
+               ARM_USART_EVENT_RX_PARITY_ERROR)) {
+    SetTraceError(DAP_SWO_STREAM_ERROR);
+  }
+}
+
+// Enable or disable SWO Mode (UART)
+//   enable: enable flag
+//   return: 1 - Success, 0 - Error
+__WEAK uint32_t SWO_Mode_UART (uint32_t enable) {
+  int32_t status;
+
+  USART_Ready = 0U;
+
+  if (enable != 0U) {
+    status = pUSART->Initialize(USART_Callback);
+    if (status != ARM_DRIVER_OK) {
+      return (0U);
+    }
+    status = pUSART->PowerControl(ARM_POWER_FULL);
+    if (status != ARM_DRIVER_OK) {
+      pUSART->Uninitialize();
+      return (0U);
+    }
+  } else {
+    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
+    pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
+    pUSART->PowerControl(ARM_POWER_OFF);
+    pUSART->Uninitialize();
+  }
+  return (1U);
+}
+
+// Configure SWO Baudrate (UART)
+//   baudrate: requested baudrate
+//   return:   actual baudrate or 0 when not configured
+__WEAK uint32_t SWO_Baudrate_UART (uint32_t baudrate) {
+  int32_t  status;
+  uint32_t index;
+  uint32_t num;
+
+  if (baudrate > SWO_UART_MAX_BAUDRATE) {
+    baudrate = SWO_UART_MAX_BAUDRATE;
+  }
+
+  if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
+    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
+    if (pUSART->GetStatus().rx_busy) {
+      TraceIndexI += pUSART->GetRxCount();
+      pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
+    }
+  }
+
+  status = pUSART->Control(ARM_USART_MODE_ASYNCHRONOUS |
+                           ARM_USART_DATA_BITS_8       |
+                           ARM_USART_PARITY_NONE       |
+                           ARM_USART_STOP_BITS_1,
+                           baudrate);
+
+  if (status == ARM_DRIVER_OK) {
+    USART_Ready = 1U;
+  } else {
+    USART_Ready = 0U;
+    return (0U);
+  }
+
+  if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
+    if ((TraceStatus & DAP_SWO_CAPTURE_PAUSED) == 0U) {
+      index = TraceIndexI & (SWO_BUFFER_SIZE - 1U);
+      num = TRACE_BLOCK_SIZE - (index & (TRACE_BLOCK_SIZE - 1U));
+      TraceBlockSize = num;
+      pUSART->Receive(&TraceBuf[index], num);
+    }
+    pUSART->Control(ARM_USART_CONTROL_RX, 1U);
+  }
+
+  return (baudrate);
+}
+
+// Control SWO Capture (UART)
+//   active: active flag
+//   return: 1 - Success, 0 - Error
+__WEAK uint32_t SWO_Control_UART (uint32_t active) {
+  int32_t status;
+
+  if (active) {
+    if (!USART_Ready) {
+      return (0U);
+    }
+    TraceBlockSize = 1U;
+    status = pUSART->Receive(&TraceBuf[0], 1U);
+    if (status != ARM_DRIVER_OK) {
+      return (0U);
+    }
+    status = pUSART->Control(ARM_USART_CONTROL_RX, 1U);
+    if (status != ARM_DRIVER_OK) {
+      return (0U);
+    }
+  } else {
+    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
+    if (pUSART->GetStatus().rx_busy) {
+      TraceIndexI += pUSART->GetRxCount();
+      pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
+    }
+  }
+  return (1U);
+}
+
+// Start SWO Capture (UART)
+//   buf: pointer to buffer for capturing
+//   num: number of bytes to capture
+__WEAK void SWO_Capture_UART (uint8_t *buf, uint32_t num) {
+  TraceBlockSize = num;
+  pUSART->Receive(buf, num);
+}
+
+// Get SWO Pending Trace Count (UART)
+//   return: number of pending trace data bytes
+__WEAK uint32_t SWO_GetCount_UART (void) {
+  uint32_t count;
+
+  if (pUSART->GetStatus().rx_busy) {
+    count = pUSART->GetRxCount();
+  } else {
+    count = 0U;
+  }
+  return (count);
+}
+
+#endif  /* (SWO_UART != 0) */
+
+
+#if (SWO_MANCHESTER != 0)
+
+// Enable or disable SWO Mode (Manchester)
+//   enable: enable flag
+//   return: 1 - Success, 0 - Error
+__WEAK uint32_t SWO_Mode_Manchester (uint32_t enable) {
+  return (0U);
+}
+
+// Configure SWO Baudrate (Manchester)
+//   baudrate: requested baudrate
+//   return:   actual baudrate or 0 when not configured
+__WEAK uint32_t SWO_Baudrate_Manchester (uint32_t baudrate) {
+  return (0U);
+}
+
+// Control SWO Capture (Manchester)
+//   active: active flag
+//   return: 1 - Success, 0 - Error
+__WEAK uint32_t SWO_Control_Manchester (uint32_t active) {
+  return (0U);
+}
+
+// Start SWO Capture (Manchester)
+//   buf: pointer to buffer for capturing
+//   num: number of bytes to capture
+__WEAK void SWO_Capture_Manchester (uint8_t *buf, uint32_t num) {
+}
+
+// Get SWO Pending Trace Count (Manchester)
+//   return: number of pending trace data bytes
+__WEAK uint32_t SWO_GetCount_Manchester (void) {
+}
+
+#endif  /* (SWO_MANCHESTER != 0) */
+
+
+// Clear Trace Errors and Data
+static void ClearTrace (void) {
+
+#if (SWO_STREAM != 0)
+  if (TraceTransport == 2U) {
+    if (TransferBusy != 0U) {
+      SWO_AbortTransfer();
+      TransferBusy = 0U;
+    }
+  }
+#endif
+
+  TraceError[0] = 0U;
+  TraceError[1] = 0U;
+  TraceError_n  = 0U;
+  TraceIndexI   = 0U;
+  TraceIndexO   = 0U;
+
+#if (TIMESTAMP_CLOCK != 0U)
+  TraceTimestamp.index = 0U;
+  TraceTimestamp.tick  = 0U;
+#endif
+}
+
+// Resume Trace Capture
+static void ResumeTrace (void) {
+  uint32_t index_i;
+  uint32_t index_o;
+
+  if (TraceStatus == (DAP_SWO_CAPTURE_ACTIVE | DAP_SWO_CAPTURE_PAUSED)) {
+    index_i = TraceIndexI;
+    index_o = TraceIndexO;
+    if ((index_i - index_o) < SWO_BUFFER_SIZE) {
+      index_i &= SWO_BUFFER_SIZE - 1U;
+      switch (TraceMode) {
+#if (SWO_UART != 0)
+        case DAP_SWO_UART:
+          TraceStatus = DAP_SWO_CAPTURE_ACTIVE;
+          SWO_Capture_UART(&TraceBuf[index_i], 1U);
+          break;
+#endif
+#if (SWO_MANCHESTER != 0)
+        case DAP_SWO_MANCHESTER:
+          TraceStatus = DAP_SWO_CAPTURE_ACTIVE;
+          SWO_Capture_Manchester(&TraceBuf[index_i], 1U);
+          break;
+#endif
+        default:
+          break;
+      }
+    }
+  }
+}
+
+// Get Trace Count
+//   return: number of available data bytes in trace buffer
+static uint32_t GetTraceCount (void) {
+  uint32_t count;
+
+  if (TraceStatus == DAP_SWO_CAPTURE_ACTIVE) {
+    do {
+      TraceUpdate = 0U;
+      count = TraceIndexI - TraceIndexO;
+      switch (TraceMode) {
+#if (SWO_UART != 0)
+        case DAP_SWO_UART:
+          count += SWO_GetCount_UART();
+          break;
+#endif
+#if (SWO_MANCHESTER != 0)
+        case DAP_SWO_MANCHESTER:
+          count += SWO_GetCount_Manchester();
+          break;
+#endif
+        default:
+          break;
+      }
+    } while (TraceUpdate != 0U);
+  } else {
+    count = TraceIndexI - TraceIndexO;
+  }
+
+  return (count);
+}
+
+// Get Trace Status (clear Error flags)
+//   return: Trace Status (Active flag and Error flags)
+static uint8_t GetTraceStatus (void) {
+  uint8_t  status;
+  uint32_t n;
+
+  n = TraceError_n;
+  TraceError_n ^= 1U;
+  status = TraceStatus | TraceError[n];
+  TraceError[n] = 0U;
+
+  return (status);
+}
+
+// Set Trace Error flag(s)
+//   flag:  error flag(s) to set
+static void SetTraceError (uint8_t flag) {
+  TraceError[TraceError_n] |= flag;
+}
+
+
+// Process SWO Transport command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_Transport (const uint8_t *request, uint8_t *response) {
+  uint8_t  transport;
+  uint32_t result;
+
+  if ((TraceStatus & DAP_SWO_CAPTURE_ACTIVE) == 0U) {
+    transport = *request;
+    switch (transport) {
+      case 0U:
+      case 1U:
+#if (SWO_STREAM != 0)
+      case 2U:
+#endif
+        TraceTransport = transport;
+        result = 1U;
+        break;
+      default:
+        result = 0U;
+        break;
+    }
+  } else {
+    result = 0U;
+  }
+
+  if (result != 0U) {
+    *response = DAP_OK;
+  } else {
+    *response = DAP_ERROR;
+  }
+
+  return ((1U << 16) | 1U);
+}
+
+
+// Process SWO Mode command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_Mode (const uint8_t *request, uint8_t *response) {
+  uint8_t  mode;
+  uint32_t result;
+
+  mode = *request;
+
+  switch (TraceMode) {
+#if (SWO_UART != 0)
+    case DAP_SWO_UART:
+      SWO_Mode_UART(0U);
+      break;
+#endif
+#if (SWO_MANCHESTER != 0)
+    case DAP_SWO_MANCHESTER:
+      SWO_Mode_Manchester(0U);
+      break;
+#endif
+    default:
+      break;
+  }
+
+  switch (mode) {
+    case DAP_SWO_OFF:
+      result = 1U;
+      break;
+#if (SWO_UART != 0)
+    case DAP_SWO_UART:
+      result = SWO_Mode_UART(1U);
+      break;
+#endif
+#if (SWO_MANCHESTER != 0)
+    case DAP_SWO_MANCHESTER:
+      result = SWO_Mode_Manchester(1U);
+      break;
+#endif
+    default:
+      result = 0U;
+      break;
+  }
+  if (result != 0U) {
+    TraceMode = mode;
+  } else {
+    TraceMode = DAP_SWO_OFF;
+  }
+
+  TraceStatus = 0U;
+
+  if (result != 0U) {
+    *response = DAP_OK;
+  } else {
+    *response = DAP_ERROR;
+  }
+
+  return ((1U << 16) | 1U);
+}
+
+
+// Process SWO Baudrate command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_Baudrate (const uint8_t *request, uint8_t *response) {
+  uint32_t baudrate;
+
+  baudrate = (uint32_t)(*(request+0) <<  0) |
+             (uint32_t)(*(request+1) <<  8) |
+             (uint32_t)(*(request+2) << 16) |
+             (uint32_t)(*(request+3) << 24);
+
+  switch (TraceMode) {
+#if (SWO_UART != 0)
+    case DAP_SWO_UART:
+      baudrate = SWO_Baudrate_UART(baudrate);
+      break;
+#endif
+#if (SWO_MANCHESTER != 0)
+    case DAP_SWO_MANCHESTER:
+      baudrate = SWO_Baudrate_Manchester(baudrate);
+      break;
+#endif
+    default:
+      baudrate = 0U;
+      break;
+  }
+
+  if (baudrate == 0U) {
+    TraceStatus = 0U;
+  }
+
+  *response++ = (uint8_t)(baudrate >>  0);
+  *response++ = (uint8_t)(baudrate >>  8);
+  *response++ = (uint8_t)(baudrate >> 16);
+  *response   = (uint8_t)(baudrate >> 24);
+
+  return ((4U << 16) | 4U);
+}
+
+
+// Process SWO Control command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_Control (const uint8_t *request, uint8_t *response) {
+  uint8_t  active;
+  uint32_t result;
+
+  active = *request & DAP_SWO_CAPTURE_ACTIVE;
+
+  if (active != (TraceStatus & DAP_SWO_CAPTURE_ACTIVE)) {
+    if (active) {
+      ClearTrace();
+    }
+    switch (TraceMode) {
+#if (SWO_UART != 0)
+      case DAP_SWO_UART:
+        result = SWO_Control_UART(active);
+        break;
+#endif
+#if (SWO_MANCHESTER != 0)
+      case DAP_SWO_MANCHESTER:
+        result = SWO_Control_Manchester(active);
+        break;
+#endif
+      default:
+        result = 0U;
+        break;
+    }
+    if (result != 0U) {
+      TraceStatus = active;
+#if (SWO_STREAM != 0)
+      if (TraceTransport == 2U) {
+        osThreadFlagsSet(SWO_ThreadId, 1U);
+      }
+#endif
+    }
+  } else {
+    result = 1U;
+  }
+
+  if (result != 0U) {
+    *response = DAP_OK;
+  } else {
+    *response = DAP_ERROR;
+  }
+
+  return ((1U << 16) | 1U);
+}
+
+
+// Process SWO Status command and prepare response
+//   response: pointer to response data
+//   return:   number of bytes in response
+uint32_t SWO_Status (uint8_t *response) {
+  uint8_t  status;
+  uint32_t count;
+
+  status = GetTraceStatus();
+  count  = GetTraceCount();
+
+  *response++ = status;
+  *response++ = (uint8_t)(count >>  0);
+  *response++ = (uint8_t)(count >>  8);
+  *response++ = (uint8_t)(count >> 16);
+  *response   = (uint8_t)(count >> 24);
+
+  return (5U);
+}
+
+
+// Process SWO Extended Status command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_ExtendedStatus (const uint8_t *request, uint8_t *response) {
+  uint8_t  cmd;
+  uint8_t  status;
+  uint32_t count;
+#if (TIMESTAMP_CLOCK != 0U)
+  uint32_t index;
+  uint32_t tick;
+#endif
+  uint32_t num;
+
+  num = 0U;
+  cmd = *request;
+
+  if (cmd & 0x01U) {
+    status = GetTraceStatus();
+    *response++ = status;
+    num += 1U;
+  }
+
+  if (cmd & 0x02U) {
+    count = GetTraceCount();
+    *response++ = (uint8_t)(count >>  0);
+    *response++ = (uint8_t)(count >>  8);
+    *response++ = (uint8_t)(count >> 16);
+    *response++ = (uint8_t)(count >> 24);
+    num += 4U;
+  }
+
+#if (TIMESTAMP_CLOCK != 0U)
+  if (cmd & 0x04U) {
+    do {
+      TraceUpdate = 0U;
+      index = TraceTimestamp.index;
+      tick  = TraceTimestamp.tick;
+    } while (TraceUpdate != 0U);
+    *response++ = (uint8_t)(index >>  0);
+    *response++ = (uint8_t)(index >>  8);
+    *response++ = (uint8_t)(index >> 16);
+    *response++ = (uint8_t)(index >> 24);
+    *response++ = (uint8_t)(tick  >>  0);
+    *response++ = (uint8_t)(tick  >>  8);
+    *response++ = (uint8_t)(tick  >> 16);
+    *response++ = (uint8_t)(tick  >> 24);
+    num += 4U;
+  }
+#endif
+
+  return ((1U << 16) | num);
+}
+
+
+// Process SWO Data command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t SWO_Data (const uint8_t *request, uint8_t *response) {
+  uint8_t  status;
+  uint32_t count;
+  uint32_t index;
+  uint32_t n, i;
+
+  status = GetTraceStatus();
+  count  = GetTraceCount();
+
+  if (TraceTransport == 1U) {
+    n = (uint32_t)(*(request+0) << 0) |
+        (uint32_t)(*(request+1) << 8);
+    if (n > (DAP_PACKET_SIZE - 4U)) {
+      n = DAP_PACKET_SIZE - 4U;
+    }
+    if (count > n) {
+      count = n;
+    }
+  } else {
+    count = 0U;
+  }
+
+  *response++ = status;
+  *response++ = (uint8_t)(count >> 0);
+  *response++ = (uint8_t)(count >> 8);
+
+  if (TraceTransport == 1U) {
+    index = TraceIndexO;
+    for (i = index, n = count; n; n--) {
+      i &= SWO_BUFFER_SIZE - 1U;
+      *response++ = TraceBuf[i++];
+    }
+    TraceIndexO = index + count;
+    ResumeTrace();
+  }
+
+  return ((2U << 16) | (3U + count));
+}
+
+
+#if (SWO_STREAM != 0)
+
+// SWO Data Transfer complete callback
+void SWO_TransferComplete (void) {
+  TraceIndexO += TransferSize;
+  TransferBusy = 0U;
+  ResumeTrace();
+  osThreadFlagsSet(SWO_ThreadId, 1U);
+}
+
+// SWO Thread
+__NO_RETURN void SWO_Thread (void *argument) {
+  uint32_t timeout;
+  uint32_t flags;
+  uint32_t count;
+  uint32_t index;
+  uint32_t i, n;
+  (void)   argument;
+
+  timeout = osWaitForever;
+
+  for (;;) {
+    flags = osThreadFlagsWait(1U, osFlagsWaitAny, timeout);
+    if (TraceStatus & DAP_SWO_CAPTURE_ACTIVE) {
+      timeout = SWO_STREAM_TIMEOUT;
+    } else {
+      timeout = osWaitForever;
+      flags   = osFlagsErrorTimeout;
+    }
+    if (TransferBusy == 0U) {
+      count = GetTraceCount();
+      if (count != 0U) {
+        index = TraceIndexO & (SWO_BUFFER_SIZE - 1U);
+        n = SWO_BUFFER_SIZE - index;
+        if (count > n) {
+          count = n;
+        }
+        if (flags != osFlagsErrorTimeout) {
+          i = index & (USB_BLOCK_SIZE - 1U);
+          if (i == 0U) {
+            count &= ~(USB_BLOCK_SIZE - 1U);
+          } else {
+            n = USB_BLOCK_SIZE - i;
+            if (count >= n) {
+              count = n;
+            } else {
+              count = 0U;
+            }
+          }
+        }
+        if (count != 0U) {
+          TransferSize = count;
+          TransferBusy = 1U;
+          SWO_QueueTransfer(&TraceBuf[index], count);
+        }
+      }
+    }
+  }
+}
+
+#endif  /* (SWO_STREAM != 0) */
+
+
+#endif  /* ((SWO_UART != 0) || (SWO_MANCHESTER != 0)) */
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/SW_DP.c b/third_party/CMSIS-DAP-2.1.1/Source/SW_DP.c
new file mode 100644
index 00000000..803cf421
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/SW_DP.c
@@ -0,0 +1,286 @@
+/*
+ * Copyright (c) 2013-2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        1. December 2017
+ * $Revision:    V2.0.0
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        SW_DP.c CMSIS-DAP SW DP I/O
+ *
+ *---------------------------------------------------------------------------*/
+
+#include "DAP_config.h"
+#include "DAP.h"
+
+
+// SW Macros
+
+#define PIN_SWCLK_SET PIN_SWCLK_TCK_SET
+#define PIN_SWCLK_CLR PIN_SWCLK_TCK_CLR
+
+#define SW_CLOCK_CYCLE()                \
+  PIN_SWCLK_CLR();                      \
+  PIN_DELAY();                          \
+  PIN_SWCLK_SET();                      \
+  PIN_DELAY()
+
+#define SW_WRITE_BIT(bit)               \
+  PIN_SWDIO_OUT(bit);                   \
+  PIN_SWCLK_CLR();                      \
+  PIN_DELAY();                          \
+  PIN_SWCLK_SET();                      \
+  PIN_DELAY()
+
+#define SW_READ_BIT(bit)                \
+  PIN_SWCLK_CLR();                      \
+  PIN_DELAY();                          \
+  bit = PIN_SWDIO_IN();                 \
+  PIN_SWCLK_SET();                      \
+  PIN_DELAY()
+
+#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
+
+
+// Generate SWJ Sequence
+//   count:  sequence bit count
+//   data:   pointer to sequence bit data
+//   return: none
+#if ((DAP_SWD != 0) || (DAP_JTAG != 0))
+void SWJ_Sequence (uint32_t count, const uint8_t *data) {
+  uint32_t val;
+  uint32_t n;
+
+  val = 0U;
+  n = 0U;
+  while (count--) {
+    if (n == 0U) {
+      val = *data++;
+      n = 8U;
+    }
+    if (val & 1U) {
+      PIN_SWDIO_TMS_SET();
+    } else {
+      PIN_SWDIO_TMS_CLR();
+    }
+    SW_CLOCK_CYCLE();
+    val >>= 1;
+    n--;
+  }
+}
+#endif
+
+
+// Generate SWD Sequence
+//   info:   sequence information
+//   swdo:   pointer to SWDIO generated data
+//   swdi:   pointer to SWDIO captured data
+//   return: none
+#if (DAP_SWD != 0)
+void SWD_Sequence (uint32_t info, const uint8_t *swdo, uint8_t *swdi) {
+  uint32_t val;
+  uint32_t bit;
+  uint32_t n, k;
+
+  n = info & SWD_SEQUENCE_CLK;
+  if (n == 0U) {
+    n = 64U;
+  }
+
+  if (info & SWD_SEQUENCE_DIN) {
+    while (n) {
+      val = 0U;
+      for (k = 8U; k && n; k--, n--) {
+        SW_READ_BIT(bit);
+        val >>= 1;
+        val  |= bit << 7;
+      }
+      val >>= k;
+      *swdi++ = (uint8_t)val;
+    }
+  } else {
+    while (n) {
+      val = *swdo++;
+      for (k = 8U; k && n; k--, n--) {
+        SW_WRITE_BIT(val);
+        val >>= 1;
+      }
+    }
+  }
+}
+#endif
+
+
+#if (DAP_SWD != 0)
+
+
+// SWD Transfer I/O
+//   request: A[3:2] RnW APnDP
+//   data:    DATA[31:0]
+//   return:  ACK[2:0]
+#define SWD_TransferFunction(speed)     /**/                                    \
+static uint8_t SWD_Transfer##speed (uint32_t request, uint32_t *data) {         \
+  uint32_t ack;                                                                 \
+  uint32_t bit;                                                                 \
+  uint32_t val;                                                                 \
+  uint32_t parity;                                                              \
+                                                                                \
+  uint32_t n;                                                                   \
+                                                                                \
+  /* Packet Request */                                                          \
+  parity = 0U;                                                                  \
+  SW_WRITE_BIT(1U);                     /* Start Bit */                         \
+  bit = request >> 0;                                                           \
+  SW_WRITE_BIT(bit);                    /* APnDP Bit */                         \
+  parity += bit;                                                                \
+  bit = request >> 1;                                                           \
+  SW_WRITE_BIT(bit);                    /* RnW Bit */                           \
+  parity += bit;                                                                \
+  bit = request >> 2;                                                           \
+  SW_WRITE_BIT(bit);                    /* A2 Bit */                            \
+  parity += bit;                                                                \
+  bit = request >> 3;                                                           \
+  SW_WRITE_BIT(bit);                    /* A3 Bit */                            \
+  parity += bit;                                                                \
+  SW_WRITE_BIT(parity);                 /* Parity Bit */                        \
+  SW_WRITE_BIT(0U);                     /* Stop Bit */                          \
+  SW_WRITE_BIT(1U);                     /* Park Bit */                          \
+                                                                                \
+  /* Turnaround */                                                              \
+  PIN_SWDIO_OUT_DISABLE();                                                      \
+  for (n = DAP_Data.swd_conf.turnaround; n; n--) {                              \
+    SW_CLOCK_CYCLE();                                                           \
+  }                                                                             \
+                                                                                \
+  /* Acknowledge response */                                                    \
+  SW_READ_BIT(bit);                                                             \
+  ack  = bit << 0;                                                              \
+  SW_READ_BIT(bit);                                                             \
+  ack |= bit << 1;                                                              \
+  SW_READ_BIT(bit);                                                             \
+  ack |= bit << 2;                                                              \
+                                                                                \
+  if (ack == DAP_TRANSFER_OK) {         /* OK response */                       \
+    /* Data transfer */                                                         \
+    if (request & DAP_TRANSFER_RnW) {                                           \
+      /* Read data */                                                           \
+      val = 0U;                                                                 \
+      parity = 0U;                                                              \
+      for (n = 32U; n; n--) {                                                   \
+        SW_READ_BIT(bit);               /* Read RDATA[0:31] */                  \
+        parity += bit;                                                          \
+        val >>= 1;                                                              \
+        val  |= bit << 31;                                                      \
+      }                                                                         \
+      SW_READ_BIT(bit);                 /* Read Parity */                       \
+      if ((parity ^ bit) & 1U) {                                                \
+        ack = DAP_TRANSFER_ERROR;                                               \
+      }                                                                         \
+      if (data) { *data = val; }                                                \
+      /* Turnaround */                                                          \
+      for (n = DAP_Data.swd_conf.turnaround; n; n--) {                          \
+        SW_CLOCK_CYCLE();                                                       \
+      }                                                                         \
+      PIN_SWDIO_OUT_ENABLE();                                                   \
+    } else {                                                                    \
+      /* Turnaround */                                                          \
+      for (n = DAP_Data.swd_conf.turnaround; n; n--) {                          \
+        SW_CLOCK_CYCLE();                                                       \
+      }                                                                         \
+      PIN_SWDIO_OUT_ENABLE();                                                   \
+      /* Write data */                                                          \
+      val = *data;                                                              \
+      parity = 0U;                                                              \
+      for (n = 32U; n; n--) {                                                   \
+        SW_WRITE_BIT(val);              /* Write WDATA[0:31] */                 \
+        parity += val;                                                          \
+        val >>= 1;                                                              \
+      }                                                                         \
+      SW_WRITE_BIT(parity);             /* Write Parity Bit */                  \
+    }                                                                           \
+    /* Capture Timestamp */                                                     \
+    if (request & DAP_TRANSFER_TIMESTAMP) {                                     \
+      DAP_Data.timestamp = TIMESTAMP_GET();                                     \
+    }                                                                           \
+    /* Idle cycles */                                                           \
+    n = DAP_Data.transfer.idle_cycles;                                          \
+    if (n) {                                                                    \
+      PIN_SWDIO_OUT(0U);                                                        \
+      for (; n; n--) {                                                          \
+        SW_CLOCK_CYCLE();                                                       \
+      }                                                                         \
+    }                                                                           \
+    PIN_SWDIO_OUT(1U);                                                          \
+    return ((uint8_t)ack);                                                      \
+  }                                                                             \
+                                                                                \
+  if ((ack == DAP_TRANSFER_WAIT) || (ack == DAP_TRANSFER_FAULT)) {              \
+    /* WAIT or FAULT response */                                                \
+    if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) != 0U)) { \
+      for (n = 32U+1U; n; n--) {                                                \
+        SW_CLOCK_CYCLE();               /* Dummy Read RDATA[0:31] + Parity */   \
+      }                                                                         \
+    }                                                                           \
+    /* Turnaround */                                                            \
+    for (n = DAP_Data.swd_conf.turnaround; n; n--) {                            \
+      SW_CLOCK_CYCLE();                                                         \
+    }                                                                           \
+    PIN_SWDIO_OUT_ENABLE();                                                     \
+    if (DAP_Data.swd_conf.data_phase && ((request & DAP_TRANSFER_RnW) == 0U)) { \
+      PIN_SWDIO_OUT(0U);                                                        \
+      for (n = 32U+1U; n; n--) {                                                \
+        SW_CLOCK_CYCLE();               /* Dummy Write WDATA[0:31] + Parity */  \
+      }                                                                         \
+    }                                                                           \
+    PIN_SWDIO_OUT(1U);                                                          \
+    return ((uint8_t)ack);                                                      \
+  }                                                                             \
+                                                                                \
+  /* Protocol error */                                                          \
+  for (n = DAP_Data.swd_conf.turnaround + 32U + 1U; n; n--) {                   \
+    SW_CLOCK_CYCLE();                   /* Back off data phase */               \
+  }                                                                             \
+  PIN_SWDIO_OUT_ENABLE();                                                       \
+  PIN_SWDIO_OUT(1U);                                                            \
+  return ((uint8_t)ack);                                                        \
+}
+
+
+#undef  PIN_DELAY
+#define PIN_DELAY() PIN_DELAY_FAST()
+SWD_TransferFunction(Fast)
+
+#undef  PIN_DELAY
+#define PIN_DELAY() PIN_DELAY_SLOW(DAP_Data.clock_delay)
+SWD_TransferFunction(Slow)
+
+
+// SWD Transfer I/O
+//   request: A[3:2] RnW APnDP
+//   data:    DATA[31:0]
+//   return:  ACK[2:0]
+uint8_t  SWD_Transfer(uint32_t request, uint32_t *data) {
+  if (DAP_Data.fast_clock) {
+    return SWD_TransferFast(request, data);
+  } else {
+    return SWD_TransferSlow(request, data);
+  }
+}
+
+
+#endif  /* (DAP_SWD != 0) */
diff --git a/third_party/CMSIS-DAP-2.1.1/Source/UART.c b/third_party/CMSIS-DAP-2.1.1/Source/UART.c
new file mode 100644
index 00000000..8e9eae50
--- /dev/null
+++ b/third_party/CMSIS-DAP-2.1.1/Source/UART.c
@@ -0,0 +1,652 @@
+/*
+ * Copyright (c) 2021 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * ----------------------------------------------------------------------
+ *
+ * $Date:        1. March 2021
+ * $Revision:    V1.0.0
+ *
+ * Project:      CMSIS-DAP Source
+ * Title:        UART.c CMSIS-DAP UART
+ *
+ *---------------------------------------------------------------------------*/
+
+#include "DAP_config.h"
+#include "DAP.h"
+
+#if (DAP_UART != 0)
+
+#ifdef DAP_FW_V1
+#error "UART Communication Port not supported in DAP V1!"
+#endif
+
+#include "Driver_USART.h"
+
+#include "cmsis_os2.h"
+#include <string.h>
+
+#define UART_RX_BLOCK_SIZE    32U   /* Uart Rx Block Size (must be 2^n) */
+
+// USART Driver
+#define _USART_Driver_(n)  Driver_USART##n
+#define  USART_Driver_(n) _USART_Driver_(n)
+extern ARM_DRIVER_USART    USART_Driver_(DAP_UART_DRIVER);
+#define pUSART           (&USART_Driver_(DAP_UART_DRIVER))
+
+// UART Configuration
+#if (DAP_UART_USB_COM_PORT != 0)
+static uint8_t  UartTransport = DAP_UART_TRANSPORT_USB_COM_PORT;
+#else
+static uint8_t  UartTransport = DAP_UART_TRANSPORT_NONE;
+#endif
+
+// UART Flags
+static uint8_t  UartConfigured = 0U;
+static uint8_t  UartReceiveEnabled = 0U;
+static uint8_t  UartTransmitEnabled = 0U;
+static uint8_t  UartTransmitActive = 0U;
+
+// UART TX Buffer
+static uint8_t  UartTxBuf[DAP_UART_TX_BUFFER_SIZE];
+static volatile uint32_t UartTxIndexI = 0U;
+static volatile uint32_t UartTxIndexO = 0U;
+
+// UART RX Buffer
+static uint8_t  UartRxBuf[DAP_UART_RX_BUFFER_SIZE];
+static volatile uint32_t UartRxIndexI = 0U;
+static volatile uint32_t UartRxIndexO = 0U;
+
+// Uart Errors
+static volatile uint8_t  UartErrorRxDataLost = 0U;
+static volatile uint8_t  UartErrorFraming = 0U;
+static volatile uint8_t  UartErrorParity = 0U;
+
+// UART Transmit
+static uint32_t UartTxNum = 0U;
+
+// Function prototypes
+static uint8_t  UART_Init (void);
+static void     UART_Uninit (void);
+static uint8_t  UART_Get_Status (void);
+static uint8_t  UART_Receive_Enable (void);
+static uint8_t  UART_Transmit_Enable (void);
+static void     UART_Receive_Disable (void);
+static void     UART_Transmit_Disable (void);
+static void     UART_Receive_Flush (void);
+static void     UART_Transmit_Flush (void);
+static void     UART_Receive (void);
+static void     UART_Transmit (void);
+
+
+// USART Driver Callback function
+//   event: event mask
+static void USART_Callback (uint32_t event) {
+  if (event &  ARM_USART_EVENT_SEND_COMPLETE) {
+    UartTxIndexO += UartTxNum;
+    UartTransmitActive = 0U;
+    UART_Transmit();
+  }
+  if (event &  ARM_USART_EVENT_RECEIVE_COMPLETE) {
+    UartRxIndexI += UART_RX_BLOCK_SIZE;
+    UART_Receive();
+  }
+  if (event &  ARM_USART_EVENT_RX_OVERFLOW) {
+    UartErrorRxDataLost = 1U;
+  }
+  if (event &  ARM_USART_EVENT_RX_FRAMING_ERROR) {
+    UartErrorFraming = 1U;
+  }
+  if (event &  ARM_USART_EVENT_RX_PARITY_ERROR) {
+    UartErrorParity = 1U;
+  }
+}
+
+// Init UART
+//   return: DAP_OK or DAP_ERROR
+static uint8_t UART_Init (void) {
+  int32_t status;
+  uint8_t ret = DAP_ERROR;
+
+  UartConfigured = 0U;
+  UartReceiveEnabled = 0U;
+  UartTransmitEnabled = 0U;
+  UartTransmitActive = 0U;
+  UartErrorRxDataLost = 0U;
+  UartErrorFraming = 0U;
+  UartErrorParity = 0U;
+  UartTxIndexI = 0U;
+  UartTxIndexO = 0U;
+  UartRxIndexI = 0U;
+  UartRxIndexO = 0U;
+  UartTxNum = 0U;
+
+  status = pUSART->Initialize(USART_Callback);
+  if (status == ARM_DRIVER_OK) {
+    status = pUSART->PowerControl(ARM_POWER_FULL);
+  }
+  if (status == ARM_DRIVER_OK) {
+    ret = DAP_OK;
+  }
+
+  return (ret);
+}
+
+// Un-Init UART
+static void UART_Uninit (void) {
+  UartConfigured = 0U;
+
+  pUSART->PowerControl(ARM_POWER_OFF);
+  pUSART->Uninitialize();
+}
+
+// Get UART Status
+//   return: status
+static uint8_t UART_Get_Status (void) {
+  uint8_t status = 0U;
+
+  if (UartReceiveEnabled != 0U) {
+    status |= DAP_UART_STATUS_RX_ENABLED;
+  }
+  if (UartErrorRxDataLost != 0U) {
+    UartErrorRxDataLost = 0U;
+    status |= DAP_UART_STATUS_RX_DATA_LOST;
+  }
+  if (UartErrorFraming != 0U) {
+    UartErrorFraming = 0U;
+    status |= DAP_UART_STATUS_FRAMING_ERROR;
+  }
+  if (UartErrorParity != 0U) {
+    UartErrorParity = 0U;
+    status |= DAP_UART_STATUS_PARITY_ERROR;
+  }
+  if (UartTransmitEnabled != 0U) {
+    status |= DAP_UART_STATUS_TX_ENABLED;
+  }
+
+  return (status);
+}
+
+// Enable UART Receive
+//   return: DAP_OK or DAP_ERROR
+static uint8_t UART_Receive_Enable (void) {
+  int32_t status;
+  uint8_t ret = DAP_ERROR;
+
+  if (UartReceiveEnabled == 0U) {
+    // Flush Buffers
+    UartRxIndexI = 0U;
+    UartRxIndexO = 0U;
+
+    UART_Receive();
+    status = pUSART->Control(ARM_USART_CONTROL_RX, 1U);
+    if (status == ARM_DRIVER_OK) {
+      UartReceiveEnabled = 1U;
+      ret = DAP_OK;
+    }
+  } else {
+    ret = DAP_OK;
+  }
+
+  return (ret);
+}
+
+// Enable UART Transmit
+//   return: DAP_OK or DAP_ERROR
+static uint8_t UART_Transmit_Enable (void) {
+  int32_t status;
+  uint8_t ret = DAP_ERROR;
+
+  if (UartTransmitEnabled == 0U) {
+    // Flush Buffers
+    UartTransmitActive = 0U;
+    UartTxIndexI = 0U;
+    UartTxIndexO = 0U;
+    UartTxNum = 0U;
+
+    status = pUSART->Control(ARM_USART_CONTROL_TX, 1U);
+    if (status == ARM_DRIVER_OK) {
+      UartTransmitEnabled = 1U;
+      ret = DAP_OK;
+    }
+  } else {
+    ret = DAP_OK;
+  }
+
+  return (ret);
+}
+
+// Disable UART Receive
+static void UART_Receive_Disable (void) {
+  if (UartReceiveEnabled != 0U) {
+    pUSART->Control(ARM_USART_CONTROL_RX, 0U);
+    pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
+    UartReceiveEnabled = 0U;
+  }
+}
+
+// Disable UART Transmit
+static void UART_Transmit_Disable (void) {
+  if (UartTransmitEnabled != 0U) {
+    pUSART->Control(ARM_USART_ABORT_SEND, 0U);
+    pUSART->Control(ARM_USART_CONTROL_TX, 0U);
+    UartTransmitActive = 0U;
+    UartTransmitEnabled = 0U;
+  }
+}
+
+// Flush UART Receive buffer
+static void UART_Receive_Flush (void) {
+  pUSART->Control(ARM_USART_ABORT_RECEIVE, 0U);
+  UartRxIndexI = 0U;
+  UartRxIndexO = 0U;
+  if (UartReceiveEnabled != 0U) {
+    UART_Receive();
+  }
+}
+
+// Flush UART Transmit buffer
+static void UART_Transmit_Flush (void) {
+  pUSART->Control(ARM_USART_ABORT_SEND, 0U);
+  UartTransmitActive = 0U;
+  UartTxIndexI = 0U;
+  UartTxIndexO = 0U;
+  UartTxNum = 0U;
+}
+
+// Receive data from target via UART
+static void UART_Receive (void) {
+  uint32_t index;
+
+  index = UartRxIndexI & (DAP_UART_RX_BUFFER_SIZE - 1U);
+  pUSART->Receive(&UartRxBuf[index], UART_RX_BLOCK_SIZE);
+}
+
+// Transmit available data to target via UART
+static void UART_Transmit (void) {
+  uint32_t count;
+  uint32_t index;
+
+  count = UartTxIndexI - UartTxIndexO;
+  index = UartTxIndexO & (DAP_UART_TX_BUFFER_SIZE - 1U);
+
+  if (count != 0U) {
+    if ((index + count) <= DAP_UART_TX_BUFFER_SIZE) {
+      UartTxNum = count;
+    } else {
+      UartTxNum = DAP_UART_TX_BUFFER_SIZE - index;
+    }
+    UartTransmitActive = 1U;
+    pUSART->Send(&UartTxBuf[index], UartTxNum);
+  }
+}
+
+// Process UART Transport command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t UART_Transport (const uint8_t *request, uint8_t *response) {
+  uint8_t  transport;
+  uint8_t  ret = DAP_ERROR;
+
+  transport = *request;
+  switch (transport) {
+    case DAP_UART_TRANSPORT_NONE:
+      switch (UartTransport) {
+        case DAP_UART_TRANSPORT_NONE:
+          ret = DAP_OK;
+          break;
+        case DAP_UART_TRANSPORT_USB_COM_PORT:
+#if (DAP_UART_USB_COM_PORT != 0)
+          USB_COM_PORT_Activate(0U);
+          UartTransport = DAP_UART_TRANSPORT_NONE;
+          ret = DAP_OK;
+#endif
+          break;
+        case DAP_UART_TRANSPORT_DAP_COMMAND:
+          UART_Receive_Disable();
+          UART_Transmit_Disable();
+          UART_Uninit();
+          UartTransport = DAP_UART_TRANSPORT_NONE;
+          ret= DAP_OK;
+          break;
+      }
+      break;
+    case DAP_UART_TRANSPORT_USB_COM_PORT:
+      switch (UartTransport) {
+        case DAP_UART_TRANSPORT_NONE:
+#if (DAP_UART_USB_COM_PORT != 0)
+          if (USB_COM_PORT_Activate(1U) == 0U) {
+            UartTransport = DAP_UART_TRANSPORT_USB_COM_PORT;
+            ret = DAP_OK;
+          }
+#endif
+          break;
+        case DAP_UART_TRANSPORT_USB_COM_PORT:
+          ret = DAP_OK;
+          break;
+        case DAP_UART_TRANSPORT_DAP_COMMAND:
+          UART_Receive_Disable();
+          UART_Transmit_Disable();
+          UART_Uninit();
+          UartTransport = DAP_UART_TRANSPORT_NONE;
+#if (DAP_UART_USB_COM_PORT != 0)
+          if (USB_COM_PORT_Activate(1U) == 0U) {
+            UartTransport = DAP_UART_TRANSPORT_USB_COM_PORT;
+            ret = DAP_OK;
+          }
+#endif
+          break;
+      }
+      break;
+    case DAP_UART_TRANSPORT_DAP_COMMAND:
+      switch (UartTransport) {
+        case DAP_UART_TRANSPORT_NONE:
+          ret = UART_Init();
+          if (ret == DAP_OK) {
+            UartTransport = DAP_UART_TRANSPORT_DAP_COMMAND;
+          }
+          break;
+        case DAP_UART_TRANSPORT_USB_COM_PORT:
+#if (DAP_UART_USB_COM_PORT != 0)
+          USB_COM_PORT_Activate(0U);
+          UartTransport = DAP_UART_TRANSPORT_NONE;
+#endif
+          ret = UART_Init();
+          if (ret == DAP_OK) {
+            UartTransport = DAP_UART_TRANSPORT_DAP_COMMAND;
+          }
+          break;
+        case DAP_UART_TRANSPORT_DAP_COMMAND:
+          ret = DAP_OK;
+          break;
+      }
+      break;
+    default:
+      break;
+  }
+
+  *response = ret;
+
+  return ((1U << 16) | 1U);
+}
+
+// Process UART Configure command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t UART_Configure (const uint8_t *request, uint8_t *response) {
+  uint8_t  control, status;
+  uint32_t baudrate;
+  int32_t  result;
+
+  if (UartTransport != DAP_UART_TRANSPORT_DAP_COMMAND) {
+    status = DAP_UART_CFG_ERROR_DATA_BITS |
+             DAP_UART_CFG_ERROR_PARITY    |
+             DAP_UART_CFG_ERROR_STOP_BITS;
+    baudrate = 0U;  // baudrate error
+  } else {
+
+    status   = 0U;
+    control  = *request;
+    baudrate = (uint32_t)(*(request+1) <<  0) |
+               (uint32_t)(*(request+2) <<  8) |
+               (uint32_t)(*(request+3) << 16) |
+               (uint32_t)(*(request+4) << 24);
+
+    result = pUSART->Control(control |
+                             ARM_USART_MODE_ASYNCHRONOUS |
+                             ARM_USART_FLOW_CONTROL_NONE,
+                             baudrate);
+    if (result == ARM_DRIVER_OK) {
+      UartConfigured = 1U;
+    } else {
+      UartConfigured = 0U;
+      switch (result) {
+        case ARM_USART_ERROR_BAUDRATE:
+          status = 0U;
+          baudrate = 0U;
+          break;
+        case ARM_USART_ERROR_DATA_BITS:
+          status = DAP_UART_CFG_ERROR_DATA_BITS;
+          break;
+        case ARM_USART_ERROR_PARITY:
+          status = DAP_UART_CFG_ERROR_PARITY;
+          break;
+        case ARM_USART_ERROR_STOP_BITS:
+          status = DAP_UART_CFG_ERROR_STOP_BITS;
+          break;
+        default:
+          status = DAP_UART_CFG_ERROR_DATA_BITS |
+                   DAP_UART_CFG_ERROR_PARITY    |
+                   DAP_UART_CFG_ERROR_STOP_BITS;
+          baudrate = 0U;
+          break;
+      }
+    }
+  }
+
+  *response++ = status;
+  *response++ = (uint8_t)(baudrate >>  0);
+  *response++ = (uint8_t)(baudrate >>  8);
+  *response++ = (uint8_t)(baudrate >> 16);
+  *response   = (uint8_t)(baudrate >> 24);
+
+  return ((5U << 16) | 5U);
+}
+
+// Process UART Control command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t UART_Control (const uint8_t *request, uint8_t *response) {
+  uint8_t control;
+  uint8_t result;
+  uint8_t ret = DAP_OK;
+
+  if (UartTransport != DAP_UART_TRANSPORT_DAP_COMMAND) {
+    ret = DAP_ERROR;
+  } else {
+ 
+    control = *request;
+
+    if ((control & DAP_UART_CONTROL_RX_DISABLE) != 0U) {
+      // Receive disable
+      UART_Receive_Disable();
+    } else if ((control & DAP_UART_CONTROL_RX_ENABLE) != 0U) {
+      // Receive enable
+      if (UartConfigured != 0U) {
+        result = UART_Receive_Enable();
+        if (result != DAP_OK) {
+          ret = DAP_ERROR;
+        }
+      } else {
+        ret = DAP_ERROR;
+      }
+    }
+    if ((control & DAP_UART_CONTROL_RX_BUF_FLUSH) != 0U) {
+      UART_Receive_Flush();
+    }
+
+    if ((control & DAP_UART_CONTROL_TX_DISABLE) != 0U) {
+      // Transmit disable
+      UART_Transmit_Disable();
+    } else if ((control & DAP_UART_CONTROL_TX_ENABLE) != 0U) {
+      // Transmit enable
+      if (UartConfigured != 0U) {
+        result = UART_Transmit_Enable();
+        if (result != DAP_OK) {
+          ret = DAP_ERROR;
+        }
+      } else {
+        ret = DAP_ERROR;
+      }
+    } 
+    if ((control & DAP_UART_CONTROL_TX_BUF_FLUSH) != 0U) {
+      UART_Transmit_Flush();
+    }
+  }
+
+  *response = ret;
+
+  return ((1U << 16) | 1U);
+}
+
+// Process UART Status command and prepare response
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t UART_Status (uint8_t *response) {
+  uint32_t rx_cnt, tx_cnt;
+  uint32_t cnt;
+  uint8_t  status;
+
+  if ((UartTransport != DAP_UART_TRANSPORT_DAP_COMMAND) ||
+      (UartConfigured == 0U)) {
+    rx_cnt = 0U;
+    tx_cnt = 0U;
+    status = 0U;
+  } else {
+
+    rx_cnt  = UartRxIndexI - UartRxIndexO;
+    rx_cnt += pUSART->GetRxCount();
+    if (rx_cnt > (DAP_UART_RX_BUFFER_SIZE - (UART_RX_BLOCK_SIZE*2))) {
+      // Overflow
+      UartErrorRxDataLost = 1U;
+      rx_cnt = (DAP_UART_RX_BUFFER_SIZE - (UART_RX_BLOCK_SIZE*2));
+      UartRxIndexO = UartRxIndexI - rx_cnt;
+    }
+
+    tx_cnt = UartTxIndexI - UartTxIndexO;
+    cnt = pUSART->GetTxCount();
+    if (UartTransmitActive != 0U) {
+      tx_cnt -= cnt;
+    }
+
+    status = UART_Get_Status();
+  }
+
+  *response++ = status;
+  *response++ = (uint8_t)(rx_cnt >>  0);
+  *response++ = (uint8_t)(rx_cnt >>  8);
+  *response++ = (uint8_t)(rx_cnt >> 16);
+  *response++ = (uint8_t)(rx_cnt >> 24);
+  *response++ = (uint8_t)(tx_cnt >>  0);
+  *response++ = (uint8_t)(tx_cnt >>  8);
+  *response++ = (uint8_t)(tx_cnt >> 16);
+  *response   = (uint8_t)(tx_cnt >> 24);
+
+  return ((0U << 16) | 9U);
+}
+
+// Process UART Transfer command and prepare response
+//   request:  pointer to request data
+//   response: pointer to response data
+//   return:   number of bytes in response (lower 16 bits)
+//             number of bytes in request (upper 16 bits)
+uint32_t UART_Transfer (const uint8_t *request, uint8_t *response) {
+  uint32_t rx_cnt, tx_cnt;
+  uint32_t rx_num, tx_num;
+  uint8_t *rx_data;
+  const
+  uint8_t *tx_data;
+  uint32_t num;
+  uint32_t index;
+  uint8_t  status;
+
+  if (UartTransport != DAP_UART_TRANSPORT_DAP_COMMAND) {
+    status = 0U;
+    rx_cnt = 0U;
+    tx_cnt = 0U;
+  } else {
+
+    // RX Data
+    rx_cnt = ((uint32_t)(*(request+0) << 0)  |
+              (uint32_t)(*(request+1) << 8));
+
+    if (rx_cnt > (DAP_PACKET_SIZE - 6U)) {
+      rx_cnt = (DAP_PACKET_SIZE - 6U);
+    }
+    rx_num  = UartRxIndexI - UartRxIndexO;
+    rx_num += pUSART->GetRxCount();
+    if (rx_num > (DAP_UART_RX_BUFFER_SIZE - (UART_RX_BLOCK_SIZE*2))) {
+      // Overflow
+      UartErrorRxDataLost = 1U;
+      rx_num = (DAP_UART_RX_BUFFER_SIZE - (UART_RX_BLOCK_SIZE*2));
+      UartRxIndexO = UartRxIndexI - rx_num;
+    }
+    if (rx_cnt > rx_num) {
+      rx_cnt = rx_num;
+    }
+
+    rx_data = (response+5);
+    index = UartRxIndexO & (DAP_UART_RX_BUFFER_SIZE - 1U);
+    if ((index + rx_cnt) <= DAP_UART_RX_BUFFER_SIZE) {
+      memcpy( rx_data,      &UartRxBuf[index], rx_cnt);
+    } else {
+      num = DAP_UART_RX_BUFFER_SIZE - index;
+      memcpy( rx_data,      &UartRxBuf[index], num);
+      memcpy(&rx_data[num], &UartRxBuf[0],     rx_cnt - num);
+    }
+    UartRxIndexO += rx_cnt;
+
+    // TX Data
+    tx_cnt  = ((uint32_t)(*(request+2) << 0) |
+               (uint32_t)(*(request+3) << 8));
+    tx_data =              (request+4);
+
+    if (tx_cnt > (DAP_PACKET_SIZE - 5U)) {
+      tx_cnt = (DAP_PACKET_SIZE - 5U);
+    }
+    tx_num = UartTxIndexI - UartTxIndexO;
+    num = pUSART->GetTxCount();
+    if (UartTransmitActive != 0U) {
+      tx_num -= num;
+    }
+    if (tx_cnt > (DAP_UART_TX_BUFFER_SIZE - tx_num)) {
+      tx_cnt = (DAP_UART_TX_BUFFER_SIZE - tx_num);
+    }
+
+    index = UartTxIndexI & (DAP_UART_TX_BUFFER_SIZE - 1U);
+    if ((index + tx_cnt) <= DAP_UART_TX_BUFFER_SIZE) {
+      memcpy(&UartTxBuf[index],  tx_data,      tx_cnt);
+    } else {
+      num = DAP_UART_TX_BUFFER_SIZE - index;
+      memcpy(&UartTxBuf[index],  tx_data,      num);
+      memcpy(&UartTxBuf[0],     &tx_data[num], tx_cnt - num);
+    }
+    UartTxIndexI += tx_cnt;
+
+    if (UartTransmitActive == 0U) {
+      UART_Transmit();
+    }
+
+    status = UART_Get_Status();
+  }
+
+  *response++ = status;
+  *response++ = (uint8_t)(tx_cnt >> 0);
+  *response++ = (uint8_t)(tx_cnt >> 8);
+  *response++ = (uint8_t)(rx_cnt >> 0);
+  *response   = (uint8_t)(rx_cnt >> 8);
+
+  return (((4U + tx_cnt) << 16) | (5U + rx_cnt));
+}
+
+#endif /* DAP_UART */