diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h
index 3e3d2e517..12b978653 100644
--- a/nimble/controller/include/controller/ble_ll.h
+++ b/nimble/controller/include/controller/ble_ll.h
@@ -187,6 +187,7 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats;
 #define BLE_LL_STATE_SCANNING       (2)
 #define BLE_LL_STATE_INITIATING     (3)
 #define BLE_LL_STATE_CONNECTION     (4)
+#define BLE_LL_STATE_DTM            (5)
 
 /* LL Features */
 #define BLE_LL_FEAT_LE_ENCRYPTION    (0x00000001)
diff --git a/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h
index 2a971c554..3545a4008 100644
--- a/nimble/controller/include/controller/ble_ll_sched.h
+++ b/nimble/controller/include/controller/ble_ll_sched.h
@@ -73,6 +73,7 @@ extern uint8_t g_ble_ll_sched_offset_ticks;
 #define BLE_LL_SCHED_TYPE_SCAN      (2)
 #define BLE_LL_SCHED_TYPE_CONN      (3)
 #define BLE_LL_SCHED_TYPE_AUX_SCAN  (4)
+#define BLE_LL_SCHED_TYPE_DTM       (5)
 
 /* Return values for schedule callback. */
 #define BLE_LL_SCHED_STATE_RUNNING  (0)
@@ -196,6 +197,10 @@ void ble_ll_sched_stop(void);
 void ble_ll_sched_rfclk_chk_restart(void);
 #endif
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+int ble_ll_sched_dtm(struct ble_ll_sched_item *sch);
+#endif
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/nimble/controller/include/controller/ble_phy.h b/nimble/controller/include/controller/ble_phy.h
index b459f548c..3887a2a3e 100644
--- a/nimble/controller/include/controller/ble_phy.h
+++ b/nimble/controller/include/controller/ble_phy.h
@@ -240,6 +240,11 @@ static inline int ble_ll_phy_to_phy_mode(int phy, int phy_options)
     return phy_mode;
 }
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+void ble_phy_enable_dtm(void);
+void ble_phy_disable_dtm(void);
+#endif
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c
index 81fccc778..3674a19b1 100644
--- a/nimble/controller/src/ble_ll.c
+++ b/nimble/controller/src/ble_ll.c
@@ -42,6 +42,10 @@
 #include "controller/ble_ll_xcvr.h"
 #include "ble_ll_conn_priv.h"
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#include <ble_ll_dtm_priv.h>
+#endif
+
 /* XXX:
  *
  * 1) use the sanity task!
@@ -569,6 +573,11 @@ ble_ll_wfr_timer_exp(void *arg)
         case BLE_LL_STATE_INITIATING:
             ble_ll_conn_init_wfr_timer_exp();
             break;
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+        case BLE_LL_STATE_DTM:
+            ble_ll_dtm_wfr_timer_exp();
+            break;
+#endif
         default:
             break;
         }
@@ -658,6 +667,12 @@ ble_ll_count_rx_stats(struct ble_mbuf_hdr *hdr, uint16_t len, uint8_t pdu_type)
 
     crcok = BLE_MBUF_HDR_CRC_OK(hdr);
     connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_CONNECTION);
+
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+    /* Reuse connection stats for DTM */
+    connection_data = (BLE_MBUF_HDR_RX_STATE(hdr) == BLE_LL_STATE_DTM);
+#endif
+
     if (crcok) {
         if (connection_data) {
             STATS_INC(ble_ll_stats, rx_data_pdu_crc_ok);
@@ -730,6 +745,11 @@ ble_ll_rx_pkt_in(void)
         case BLE_LL_STATE_INITIATING:
             ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr);
             break;
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+        case BLE_LL_STATE_DTM:
+            ble_ll_dtm_rx_pkt_in(m, ble_hdr);
+            break;
+#endif
         default:
             /* Any other state should never occur */
             STATS_INC(ble_ll_stats, bad_ll_state);
@@ -856,6 +876,11 @@ ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr)
     case BLE_LL_STATE_SCANNING:
         rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags);
         break;
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+    case BLE_LL_STATE_DTM:
+        rc = ble_ll_dtm_rx_isr_start(rxhdr, ble_phy_access_addr_get());
+        break;
+#endif
     default:
         /* Should not be in this state! */
         rc = -1;
@@ -896,6 +921,13 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
                ((uint16_t)rxhdr->rxinfo.flags << 8) | rxbuf[1],
                rxhdr->beg_cputime);
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+    if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_DTM) {
+        rc = ble_ll_dtm_rx_isr_end(rxbuf, rxhdr);
+        return rc;
+    }
+#endif
+
     if (BLE_MBUF_HDR_RX_STATE(rxhdr) == BLE_LL_STATE_CONNECTION) {
         rc = ble_ll_conn_rx_isr_end(rxbuf, rxhdr);
         return rc;
@@ -1179,6 +1211,10 @@ ble_ll_reset(void)
     /* Stop any advertising */
     ble_ll_adv_reset();
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE)
+    ble_ll_dtm_reset();
+#endif
+
     /* FLush all packets from Link layer queues */
     ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_tx_pkt_q);
     ble_ll_flush_pkt_queue(&g_ble_ll_data.ll_rx_pkt_q);
diff --git a/nimble/controller/src/ble_ll_dtm.c b/nimble/controller/src/ble_ll_dtm.c
new file mode 100644
index 000000000..f893f7680
--- /dev/null
+++ b/nimble/controller/src/ble_ll_dtm.c
@@ -0,0 +1,494 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you 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.
+ */
+
+#include <syscfg/syscfg.h>
+
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+
+#include "assert.h"
+#include "os/os.h"
+#include "controller/ble_ll.h"
+#include "controller/ble_phy.h"
+#include "controller/ble_ll_sched.h"
+#include "ble_ll_dtm_priv.h"
+
+struct dtm_ctx {
+    uint8_t payload_packet;
+    uint8_t itvl_rem_usec;
+    uint16_t num_of_packets;
+    uint32_t itvl_ticks;
+    int active;
+    int chan;
+    int phy_mode;
+    struct os_mbuf *om;
+    struct os_event evt;
+    struct ble_ll_sched_item sch;
+};
+
+static struct dtm_ctx g_ble_ll_dtm_ctx;
+
+static const uint8_t g_ble_ll_dtm_prbs9_data[] =
+{
+    0xff, 0xc1, 0xfb, 0xe8, 0x4c, 0x90, 0x72, 0x8b,
+    0xe7, 0xb3, 0x51, 0x89, 0x63, 0xab, 0x23, 0x23,
+    0x02, 0x84, 0x18, 0x72, 0xaa, 0x61, 0x2f, 0x3b,
+    0x51, 0xa8, 0xe5, 0x37, 0x49, 0xfb, 0xc9, 0xca,
+    0x0c, 0x18, 0x53, 0x2c, 0xfd, 0x45, 0xe3, 0x9a,
+    0xe6, 0xf1, 0x5d, 0xb0, 0xb6, 0x1b, 0xb4, 0xbe,
+    0x2a, 0x50, 0xea, 0xe9, 0x0e, 0x9c, 0x4b, 0x5e,
+    0x57, 0x24, 0xcc, 0xa1, 0xb7, 0x59, 0xb8, 0x87,
+    0xff, 0xe0, 0x7d, 0x74, 0x26, 0x48, 0xb9, 0xc5,
+    0xf3, 0xd9, 0xa8, 0xc4, 0xb1, 0xd5, 0x91, 0x11,
+    0x01, 0x42, 0x0c, 0x39, 0xd5, 0xb0, 0x97, 0x9d,
+    0x28, 0xd4, 0xf2, 0x9b, 0xa4, 0xfd, 0x64, 0x65,
+    0x06, 0x8c, 0x29, 0x96, 0xfe, 0xa2, 0x71, 0x4d,
+    0xf3, 0xf8, 0x2e, 0x58, 0xdb, 0x0d, 0x5a, 0x5f,
+    0x15, 0x28, 0xf5, 0x74, 0x07, 0xce, 0x25, 0xaf,
+    0x2b, 0x12, 0xe6, 0xd0, 0xdb, 0x2c, 0xdc, 0xc3,
+    0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc, 0xe2,
+    0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8, 0x88,
+    0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb, 0x4e,
+    0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2, 0x32,
+    0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8, 0xa6,
+    0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad, 0xaf,
+    0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92, 0xd7,
+    0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee, 0xe1,
+    0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e, 0xf1,
+    0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64, 0x44,
+    0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65, 0x27,
+    0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59, 0x99,
+    0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c, 0xd3,
+    0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6, 0x57,
+    0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9, 0xeb,
+    0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7
+};
+
+static const uint8_t g_ble_ll_dtm_prbs15_data[] =
+{
+    0xff, 0x7f, 0xf0, 0x3e, 0x3a, 0x13, 0xa4, 0xdc,
+    0xe2, 0xf9, 0x6c, 0x54, 0xe2, 0xd8, 0xea, 0xc8,
+    0x88, 0x00, 0x21, 0x86, 0x9c, 0x6a, 0xd8, 0xcb,
+    0x4e, 0x14, 0x6a, 0xf9, 0x4d, 0xd2, 0x7e, 0xb2,
+    0x32, 0x03, 0xc6, 0x14, 0x4b, 0x7f, 0xd1, 0xb8,
+    0xa6, 0x79, 0x7c, 0x17, 0xac, 0xed, 0x06, 0xad,
+    0xaf, 0x0a, 0x94, 0x7a, 0xba, 0x03, 0xe7, 0x92,
+    0xd7, 0x15, 0x09, 0x73, 0xe8, 0x6d, 0x16, 0xee,
+    0xe1, 0x3f, 0x78, 0x1f, 0x9d, 0x09, 0x52, 0x6e,
+    0xf1, 0x7c, 0x36, 0x2a, 0x71, 0x6c, 0x75, 0x64,
+    0x44, 0x80, 0x10, 0x43, 0x4e, 0x35, 0xec, 0x65,
+    0x27, 0x0a, 0xb5, 0xfc, 0x26, 0x69, 0x3f, 0x59,
+    0x99, 0x01, 0x63, 0x8a, 0xa5, 0xbf, 0x68, 0x5c,
+    0xd3, 0x3c, 0xbe, 0x0b, 0xd6, 0x76, 0x83, 0xd6,
+    0x57, 0x05, 0x4a, 0x3d, 0xdd, 0x81, 0x73, 0xc9,
+    0xeb, 0x8a, 0x84, 0x39, 0xf4, 0x36, 0x0b, 0xf7,
+    0xf0, 0x1f, 0xbc, 0x8f, 0xce, 0x04, 0x29, 0xb7,
+    0x78, 0x3e, 0x1b, 0x95, 0x38, 0xb6, 0x3a, 0x32,
+    0x22, 0x40, 0x88, 0x21, 0xa7, 0x1a, 0xf6, 0xb2,
+    0x13, 0x85, 0x5a, 0x7e, 0x93, 0xb4, 0x9f, 0xac,
+    0xcc, 0x80, 0x31, 0xc5, 0xd2, 0x5f, 0x34, 0xae,
+    0x69, 0x1e, 0xdf, 0x05, 0x6b, 0xbb, 0x41, 0xeb,
+    0xab, 0x02, 0xa5, 0x9e, 0xee, 0xc0, 0xb9, 0xe4,
+    0x75, 0x45, 0xc2, 0x1c, 0x7a, 0x9b, 0x85, 0x7b,
+    0xf8, 0x0f, 0xde, 0x47, 0x67, 0x82, 0x94, 0x5b,
+    0x3c, 0x9f, 0x8d, 0x4a, 0x1c, 0x5b, 0x1d, 0x19,
+    0x11, 0x20, 0xc4, 0x90, 0x53, 0x0d, 0x7b, 0xd9,
+    0x89, 0x42, 0x2d, 0xbf, 0x49, 0xda, 0x4f, 0x56,
+    0x66, 0xc0, 0x98, 0x62, 0xe9, 0x2f, 0x1a, 0xd7,
+    0x34, 0x8f, 0xef, 0x82, 0xb5, 0xdd, 0xa0, 0xf5,
+    0x55, 0x81, 0x52, 0x4f, 0x77, 0xe0, 0x5c, 0xf2,
+    0xba, 0x22, 0x61, 0x0e, 0xbd, 0xcd, 0xc2
+};
+
+#define BLE_DTM_SYNC_WORD          (0x71764129)
+#define BLE_DTM_CRC                (0x555555)
+
+static void
+ble_ll_dtm_set_next(struct dtm_ctx *ctx)
+{
+    struct ble_ll_sched_item *sch = &ctx->sch;
+
+    sch->start_time += ctx->itvl_ticks;
+    sch->remainder += ctx->itvl_rem_usec;
+    if (sch->remainder > 30) {
+       sch->start_time++;
+       sch->remainder -= 30;
+    }
+
+    sch->start_time -= g_ble_ll_sched_offset_ticks;
+}
+
+static void
+ble_ll_dtm_event(struct os_event *evt) {
+    /* It is called in LL context */
+    struct dtm_ctx *ctx = evt->ev_arg;
+    int rc;
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    if (!ctx->active || !ctx->om) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    ble_ll_dtm_set_next(ctx);
+    rc = ble_ll_sched_dtm(&ctx->sch);
+    assert(rc == 0);
+}
+
+static void
+ble_ll_dtm_tx_done(void *arg)
+{
+    struct dtm_ctx *ctx;
+
+    ctx = arg;
+    if (!ctx->evt.ev_cb) {
+        return;
+    }
+    ctx->num_of_packets++;
+    /* Reschedule event in LL context */
+    os_eventq_put(&g_ble_ll_data.ll_evq, &ctx->evt);
+
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+}
+
+static int
+ble_ll_dtm_tx_sched_cb(struct ble_ll_sched_item *sch)
+{
+    struct dtm_ctx *ctx = sch->cb_arg;
+    int rc;
+
+    if (!ctx->active) {
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+    rc = ble_phy_setchan(ctx->chan, BLE_DTM_SYNC_WORD, BLE_DTM_CRC);
+    if (rc != 0) {
+        assert(0);
+        return BLE_LL_SCHED_STATE_DONE;
+    }
+
+    ble_phy_mode_set(ctx->phy_mode, ctx->phy_mode);
+    ble_phy_set_txend_cb(ble_ll_dtm_tx_done, ctx);
+    ble_phy_txpwr_set(0);
+
+    sch->start_time += g_ble_ll_sched_offset_ticks;
+
+    /*XXX Maybe reschedule if too late */
+    rc = ble_phy_tx_set_start_time(sch->start_time, sch->remainder);
+    assert(rc == 0);
+
+    rc = ble_phy_tx(ctx->om, BLE_PHY_TRANSITION_NONE);
+    assert(rc == 0);
+
+    ble_ll_state_set(BLE_LL_STATE_DTM);
+
+    return BLE_LL_SCHED_STATE_DONE;
+}
+
+static void
+ble_ll_dtm_calculate_itvl(struct dtm_ctx *ctx, uint8_t len, int phy_mode)
+{
+    uint32_t l;
+    uint32_t itvl_usec;
+    uint32_t itvl_ticks;
+
+    /* Calculate interval as per spec Bluetooth 5.0 Vol 6. Part F, 4.1.6 */
+    l = ble_ll_pdu_tx_time_get(len + BLE_LL_PDU_HDR_LEN, phy_mode);
+    itvl_usec = ((l + 249 + 624) / 625) * 625;
+
+    itvl_ticks = os_cputime_usecs_to_ticks(itvl_usec);
+    ctx->itvl_rem_usec = (itvl_usec - os_cputime_ticks_to_usecs(itvl_ticks));
+    if (ctx->itvl_rem_usec == 31) {
+        ctx->itvl_rem_usec = 0;
+        ++itvl_ticks;
+    }
+    ctx->itvl_ticks = itvl_ticks;
+}
+
+static int
+ble_ll_dtm_tx_create_ctx(uint8_t packet_payload, uint8_t len, int chan,
+                         int phy_mode)
+{
+    int rc = 0;
+    uint8_t byte_pattern;
+    struct ble_ll_sched_item *sch = &g_ble_ll_dtm_ctx.sch;
+
+    /* MSYS is big enough to get continues memory */
+    g_ble_ll_dtm_ctx.om = os_msys_get_pkthdr(len, sizeof(struct ble_mbuf_hdr));
+    assert(g_ble_ll_dtm_ctx.om);
+
+    g_ble_ll_dtm_ctx.phy_mode = phy_mode;
+    g_ble_ll_dtm_ctx.chan = chan;
+
+    /* Set BLE transmit header */
+    ble_ll_mbuf_init(g_ble_ll_dtm_ctx.om, len, packet_payload);
+
+    switch(packet_payload) {
+    case 0x00:
+        memcpy(g_ble_ll_dtm_ctx.om->om_data, &g_ble_ll_dtm_prbs9_data, len);
+        goto schedule;
+    case 0x01:
+        byte_pattern = 0x0F;
+        break;
+    case 0x02:
+        byte_pattern = 0x55;
+        break;
+    case 0x03:
+        memcpy(g_ble_ll_dtm_ctx.om->om_data, &g_ble_ll_dtm_prbs15_data, len);
+        goto schedule;
+    case 0x04:
+        byte_pattern = 0xFF;
+        break;
+    case 0x05:
+        byte_pattern = 0x00;
+        break;
+    case 0x06:
+        byte_pattern = 0xF0;
+        break;
+    case 0x07:
+        byte_pattern = 0xAA;
+        break;
+    default:
+        return 1;
+    }
+
+    memset(g_ble_ll_dtm_ctx.om->om_data, byte_pattern, len);
+
+schedule:
+
+    sch->sched_cb = ble_ll_dtm_tx_sched_cb;
+    sch->cb_arg = &g_ble_ll_dtm_ctx;
+    sch->sched_type = BLE_LL_SCHED_TYPE_DTM;
+    sch->start_time =  os_cputime_get32() +
+                                       os_cputime_usecs_to_ticks(5000);
+
+    /* Prepare os_event */
+    g_ble_ll_dtm_ctx.evt.ev_cb = ble_ll_dtm_event;
+    g_ble_ll_dtm_ctx.evt.ev_arg = &g_ble_ll_dtm_ctx;
+
+    ble_ll_dtm_calculate_itvl(&g_ble_ll_dtm_ctx, len, phy_mode);
+
+    /* Set some start point for TX packets */
+    rc = ble_ll_sched_dtm(sch);
+    assert(rc == 0);
+
+    g_ble_ll_dtm_ctx.active = 1;
+    return 0;
+}
+
+static int
+ble_ll_dtm_rx_start(void)
+{
+    int rc;
+
+    rc = ble_phy_setchan(g_ble_ll_dtm_ctx.chan, BLE_DTM_SYNC_WORD,
+                         BLE_DTM_CRC);
+    assert(rc == 0);
+
+    ble_phy_mode_set(g_ble_ll_dtm_ctx.phy_mode, g_ble_ll_dtm_ctx.phy_mode);
+
+    rc = ble_phy_rx_set_start_time(os_cputime_get32() +
+                                   g_ble_ll_sched_offset_ticks, 0);
+    assert(rc == 0);
+
+    ble_ll_state_set(BLE_LL_STATE_DTM);
+
+    return 0;
+}
+
+static int
+ble_ll_dtm_rx_create_ctx(int chan, int phy_mode)
+{
+    g_ble_ll_dtm_ctx.phy_mode = phy_mode;
+    g_ble_ll_dtm_ctx.chan = chan;
+    g_ble_ll_dtm_ctx.active = 1;
+
+    if (ble_ll_dtm_rx_start() != 0) {
+        assert(0);
+        return 1;
+    }
+
+    return 0;
+}
+
+static void
+ble_ll_dtm_ctx_free(struct dtm_ctx * ctx)
+{
+    os_sr_t sr;
+
+    OS_ENTER_CRITICAL(sr);
+    if (!ctx->active) {
+        OS_EXIT_CRITICAL(sr);
+        return;
+    }
+    OS_EXIT_CRITICAL(sr);
+
+    ble_phy_disable();
+    ble_ll_state_set(BLE_LL_STATE_STANDBY);
+#ifdef BLE_XCVR_RFCLK
+    ble_ll_xcvr_rfclk_stop();
+#endif
+
+    ble_ll_sched_rmv_elem(&ctx->sch);
+    os_mbuf_free_chain(ctx->om);
+    memset(ctx, 0, sizeof(*ctx));
+}
+
+int
+ble_ll_dtm_tx_test(uint8_t *cmdbuf, bool enhanced)
+{
+    uint8_t tx_chan = cmdbuf[0];
+    uint8_t len = cmdbuf[1];
+    uint8_t packet_payload = cmdbuf[2];
+    uint8_t phy_mode = BLE_PHY_MODE_1M;
+
+    if (g_ble_ll_dtm_ctx.active) {
+        return BLE_ERR_CTLR_BUSY;
+    }
+
+    if (enhanced) {
+        switch (cmdbuf[3]) {
+        case BLE_HCI_LE_PHY_1M:
+            phy_mode = BLE_PHY_MODE_1M;
+            break;
+        case BLE_HCI_LE_PHY_2M:
+            phy_mode = BLE_PHY_MODE_2M;
+            break;
+        case BLE_HCI_LE_PHY_CODED_S8:
+            phy_mode = BLE_PHY_MODE_CODED_125KBPS;
+            break;
+        case BLE_HCI_LE_PHY_CODED_S2:
+            phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+            break;
+        default:
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    if (tx_chan > 0x27 || packet_payload > 0x07) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_dtm_tx_create_ctx(packet_payload, len, tx_chan, phy_mode)) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int ble_ll_dtm_rx_test(uint8_t *cmdbuf, bool enhanced)
+{
+    uint8_t rx_chan = cmdbuf[0];
+    uint8_t phy_mode = BLE_PHY_MODE_1M;
+
+    if (g_ble_ll_dtm_ctx.active) {
+        return BLE_ERR_CTLR_BUSY;
+    }
+
+    /*XXX What to do with modulation cmdbuf[2]? */
+
+    if (enhanced) {
+        switch (cmdbuf[1]) {
+        case BLE_HCI_LE_PHY_1M:
+            phy_mode = BLE_PHY_MODE_1M;
+            break;
+        case BLE_HCI_LE_PHY_2M:
+            phy_mode = BLE_PHY_MODE_2M;
+            break;
+        case BLE_HCI_LE_PHY_CODED:
+            phy_mode = BLE_PHY_MODE_CODED_500KBPS;
+            break;
+        default:
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+    }
+
+    if (rx_chan > 0x27) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_dtm_rx_create_ctx(rx_chan, phy_mode)) {
+        return BLE_ERR_UNSPECIFIED;
+    }
+
+    return BLE_ERR_SUCCESS;
+}
+
+int ble_ll_dtm_end_test(uint8_t *cmdbuf, uint8_t *rsp, uint8_t *rsplen)
+{
+    put_le16(rsp, g_ble_ll_dtm_ctx. num_of_packets);
+    *rsplen = 2;
+
+    ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx);
+    return BLE_ERR_SUCCESS;
+}
+
+int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa)
+{
+    return 0;
+}
+
+void
+ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
+{
+    if (BLE_MBUF_HDR_CRC_OK(hdr)) {
+        /* XXX Compare data. */
+        g_ble_ll_dtm_ctx.num_of_packets++;
+    }
+
+    if (ble_ll_dtm_rx_start() != 0) {
+        assert(0);
+    }
+}
+
+int
+ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
+{
+    struct os_mbuf *rxpdu;
+
+    if (!g_ble_ll_dtm_ctx.active) {
+        return -1;
+    }
+
+    rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN);
+
+    /* Copy the received pdu and hand it up */
+    if (rxpdu) {
+        ble_phy_rxpdu_copy(rxbuf, rxpdu);
+        ble_ll_rx_pdu_in(rxpdu);
+    }
+
+    return 0;
+}
+
+void
+ble_ll_dtm_wfr_timer_exp(void)
+{
+    /* Should not be needed */
+    assert(0);
+}
+
+
+void
+ble_ll_dtm_reset(void)
+{
+    ble_ll_dtm_ctx_free(&g_ble_ll_dtm_ctx);
+}
+#endif
diff --git a/nimble/controller/src/ble_ll_dtm_priv.h b/nimble/controller/src/ble_ll_dtm_priv.h
new file mode 100644
index 000000000..4f21a9ca9
--- /dev/null
+++ b/nimble/controller/src/ble_ll_dtm_priv.h
@@ -0,0 +1,36 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you 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.
+ */
+
+#ifndef H_BLE_LL_TEST_PRIV_
+#define H_BLE_LL_TEST_PRIV_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "nimble/ble.h"
+
+int ble_ll_dtm_tx_test(uint8_t *cmdbuf, bool enhanced);
+int ble_ll_dtm_rx_test(uint8_t *cmdbuf, bool enhanced);
+int ble_ll_dtm_end_test(uint8_t *cmdbuf, uint8_t *rsp, uint8_t *rsplen);
+
+int ble_ll_dtm_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa);
+int ble_ll_dtm_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr);
+void ble_ll_dtm_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr);
+void ble_ll_dtm_wfr_timer_exp(void);
+void ble_ll_dtm_reset(void);
+#endif
diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c
index 07e89a0b1..6e29df078 100644
--- a/nimble/controller/src/ble_ll_hci.c
+++ b/nimble/controller/src/ble_ll_hci.c
@@ -34,6 +34,10 @@
 #include "controller/ble_ll_resolv.h"
 #include "ble_ll_conn_priv.h"
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+#include <ble_ll_dtm_priv.h>
+#endif
+
 static void ble_ll_hci_cmd_proc(struct os_event *ev);
 
 /* OS event to enqueue command */
@@ -859,6 +863,23 @@ ble_ll_hci_le_cmd_proc(uint8_t *cmdbuf, uint16_t ocf, uint8_t *rsplen)
     case BLE_HCI_OCF_LE_SET_PHY:
         rc = ble_ll_conn_hci_le_set_phy(cmdbuf);
         break;
+#endif
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+    case BLE_HCI_OCF_LE_TX_TEST:
+        rc = ble_ll_dtm_tx_test(cmdbuf, false);
+        break;
+    case BLE_HCI_OCF_LE_RX_TEST:
+        rc = ble_ll_dtm_rx_test(cmdbuf, false);
+        break;
+    case BLE_HCI_OCF_LE_TEST_END:
+        rc = ble_ll_dtm_end_test(cmdbuf, rspbuf, rsplen);
+        break;
+    case BLE_HCI_OCF_LE_ENH_RX_TEST:
+        rc = ble_ll_dtm_rx_test(cmdbuf, true);
+        break;
+    case BLE_HCI_OCF_LE_ENH_TX_TEST:
+        rc = ble_ll_dtm_tx_test(cmdbuf, true);
+        break;
 #endif
     default:
         rc = BLE_ERR_UNKNOWN_HCI_CMD;
diff --git a/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c
index 8db9ced02..9530d906b 100644
--- a/nimble/controller/src/ble_ll_sched.c
+++ b/nimble/controller/src/ble_ll_sched.c
@@ -1432,6 +1432,66 @@ done:
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+int ble_ll_sched_dtm(struct ble_ll_sched_item *sch)
+{
+    int rc;
+    os_sr_t sr;
+    struct ble_ll_sched_item *entry;
+
+    OS_ENTER_CRITICAL(sr);
+
+    if (!ble_ll_sched_insert_if_empty(sch)) {
+        /* Nothing in schedule. Schedule as soon as possible
+         * If we are here it means sch has been added to the scheduler */
+        rc = 0;
+        goto done;
+    }
+
+    /* Try to find slot for test. */
+    os_cputime_timer_stop(&g_ble_ll_sched_timer);
+    TAILQ_FOREACH(entry, &g_ble_ll_sched_q, link) {
+        /* We can insert if before entry in list */
+        if (sch->end_time <= entry->start_time) {
+            rc = 0;
+            TAILQ_INSERT_BEFORE(entry, sch, link);
+            sch->enqueued = 1;
+            break;
+        }
+
+        /* Check for overlapping events. For now drop if it overlaps with
+         * anything. We can make it smarter later on
+         */
+        if (ble_ll_sched_is_overlap(sch, entry)) {
+            OS_EXIT_CRITICAL(sr);
+            return -1;
+        }
+    }
+
+    if (!entry) {
+        rc = 0;
+        TAILQ_INSERT_TAIL(&g_ble_ll_sched_q, sch, link);
+        sch->enqueued = 1;
+    }
+
+done:
+
+    /* Get head of list to restart timer */
+    sch = TAILQ_FIRST(&g_ble_ll_sched_q);
+
+#ifdef BLE_XCVR_RFCLK
+        ble_ll_xcvr_rfclk_timer_start(sch->start_time);
+#endif
+
+    OS_EXIT_CRITICAL(sr);
+
+    /* Restart timer */
+    assert(sch != NULL);
+    os_cputime_timer_start(&g_ble_ll_sched_timer, sch->start_time);
+
+    return rc;
+}
+#endif
 /**
  * Stop the scheduler
  *
diff --git a/nimble/controller/src/ble_ll_supp_cmd.c b/nimble/controller/src/ble_ll_supp_cmd.c
index 7383a66c2..64de46679 100644
--- a/nimble/controller/src/ble_ll_supp_cmd.c
+++ b/nimble/controller/src/ble_ll_supp_cmd.c
@@ -136,9 +136,16 @@
 #define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY   (0 << 2)
 #endif
 #define BLE_SUPP_CMD_LE_READ_SUPP_STATES    (1 << 3)
+
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
 #define BLE_SUPP_CMD_LE_RX_TEST             (0 << 4)
 #define BLE_SUPP_CMD_LE_TX_TEST             (0 << 5)
 #define BLE_SUPP_CMD_LE_TEST_END            (0 << 6)
+#else
+#define BLE_SUPP_CMD_LE_RX_TEST             (1 << 4)
+#define BLE_SUPP_CMD_LE_TX_TEST             (1 << 5)
+#define BLE_SUPP_CMD_LE_TEST_END            (1 << 6)
+#endif
 
 #define BLE_LL_SUPP_CMD_OCTET_28            \
 (                                           \
@@ -224,7 +231,12 @@
 #define BLE_SUPP_CMD_LE_SET_DEFAULT_PHY     (0 << 5)
 #define BLE_SUPP_CMD_LE_SET_PHY             (0 << 6)
 #endif
+
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
 #define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (0 << 7)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_RX_TEST    (1 << 7)
+#endif
 
 #define BLE_LL_SUPP_CMD_OCTET_35            \
 (                                           \
@@ -239,7 +251,12 @@
 )
 
 /* Octet 36 */
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 0
 #define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (0 << 0)
+#else
+#define BLE_SUPP_CMD_LE_ENHANCED_TX_TEST    (1 << 0)
+#endif
+
 #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) == 1)
 #define BLE_SUPP_CMD_LE_SET_ADVS_RAND_ADDR  (1 << 1)
 #define BLE_SUPP_CMD_LE_SET_EXT_ADV_PARAM   (1 << 2)
diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml
index 07c2d1b3c..00cb1e8cc 100644
--- a/nimble/controller/syscfg.yml
+++ b/nimble/controller/syscfg.yml
@@ -283,6 +283,11 @@ syscfg.defs:
             be used.
         value: "(uint8_t[6]){0x00, 0x00, 0x00, 0x00, 0x00, 0x00}"
 
+    BLE_LL_DIRECT_TEST_MODE:
+        description: >
+             Enables HCI Test commands needed for Bluetooth SIG certification
+        value: 0
+
 syscfg.vals.BLE_LL_CFG_FEAT_LL_EXT_ADV:
     BLE_LL_CFG_FEAT_LE_CSA2: 1
     BLE_HW_WHITELIST_ENABLE: 0
diff --git a/nimble/drivers/nrf52/src/ble_phy.c b/nimble/drivers/nrf52/src/ble_phy.c
index 690bbd79f..010b744dc 100644
--- a/nimble/drivers/nrf52/src/ble_phy.c
+++ b/nimble/drivers/nrf52/src/ble_phy.c
@@ -1824,6 +1824,19 @@ ble_phy_resolv_list_disable(void)
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_DIRECT_TEST_MODE) == 1
+void ble_phy_enable_dtm(void)
+{
+    /* Disable whitening as per Bluetooth v5.0 Vol 6. Part F. 4.1.1*/
+    NRF_RADIO->PCNF1 &= ~RADIO_PCNF1_WHITEEN_Msk;
+}
+
+void ble_phy_disable_dtm(void)
+{
+    /* Enable whitening */
+    NRF_RADIO->PCNF1 |= RADIO_PCNF1_WHITEEN_Msk;
+}
+#endif
 #ifdef BLE_XCVR_RFCLK
 void
 ble_phy_rfclk_enable(void)
diff --git a/nimble/host/src/ble_hs_hci_cmd.c b/nimble/host/src/ble_hs_hci_cmd.c
index 7f062c48c..fa9814677 100644
--- a/nimble/host/src/ble_hs_hci_cmd.c
+++ b/nimble/host/src/ble_hs_hci_cmd.c
@@ -1179,7 +1179,7 @@ ble_hs_hci_cmd_build_le_enh_recv_test(uint8_t rx_chan, uint8_t phy,
                                       uint8_t mod_idx,
                                       uint8_t *dst, uint16_t dst_len)
 {
-    BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_LE_ENH_RCVR_TEST_LEN);
+    BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_LE_ENH_RX_TEST_LEN);
 
     return ble_hs_hci_cmd_body_le_enhanced_recv_test(rx_chan, phy, mod_idx, dst);
 }
@@ -1214,7 +1214,7 @@ ble_hs_hci_cmd_build_le_enh_trans_test(uint8_t tx_chan, uint8_t test_data_len,
                                        uint8_t packet_payload_idx, uint8_t phy,
                                        uint8_t *dst, uint16_t dst_len)
 {
-    BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_LE_ENH_TRANS_TEST_LEN);
+    BLE_HS_DBG_ASSERT(dst_len >= BLE_HCI_LE_ENH_TX_TEST_LEN);
 
     return ble_hs_hci_cmd_body_le_enhanced_trans_test(tx_chan, test_data_len,
                                                       packet_payload_idx,
diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h
index 1907bf0d5..4dd779b44 100644
--- a/nimble/include/nimble/hci_common.h
+++ b/nimble/include/nimble/hci_common.h
@@ -138,8 +138,8 @@ extern "C" {
 #define BLE_HCI_OCF_LE_RD_PHY                       (0x0030)
 #define BLE_HCI_OCF_LE_SET_DEFAULT_PHY              (0x0031)
 #define BLE_HCI_OCF_LE_SET_PHY                      (0x0032)
-#define BLE_HCI_OCF_LE_ENH_RCVR_TEST                (0x0033)
-#define BLE_HCI_OCF_LE_ENH_TRANS_TEST               (0x0034)
+#define BLE_HCI_OCF_LE_ENH_RX_TEST                  (0x0033)
+#define BLE_HCI_OCF_LE_ENH_TX_TEST                  (0x0034)
 #define BLE_HCI_OCF_LE_SET_ADV_SET_RND_ADDR         (0x0035)
 #define BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM            (0x0036)
 #define BLE_HCI_OCF_LE_SET_EXT_ADV_DATA             (0x0037)
@@ -478,13 +478,13 @@ extern "C" {
 #define BLE_HCI_LE_PHY_CODED_S8_PREF                (0x0002)
 
 /* --- LE enhanced receiver test (OCF 0x0033) */
-#define BLE_HCI_LE_ENH_RCVR_TEST_LEN                (3)
+#define BLE_HCI_LE_ENH_RX_TEST_LEN                  (3)
 #define BLE_HCI_LE_PHY_1M                           (1)
 #define BLE_HCI_LE_PHY_2M                           (2)
 #define BLE_HCI_LE_PHY_CODED                        (3)
 
 /* --- LE enhanced transmitter test (OCF 0x0034) */
-#define BLE_HCI_LE_ENH_TRANS_TEST_LEN               (4)
+#define BLE_HCI_LE_ENH_TX_TEST_LEN                  (4)
 #define BLE_HCI_LE_PHY_CODED_S8                     (3)
 #define BLE_HCI_LE_PHY_CODED_S2                     (4)
 
diff --git a/nimble/src/hci_common.c b/nimble/src/hci_common.c
index a09271457..2216e9f49 100644
--- a/nimble/src/hci_common.c
+++ b/nimble/src/hci_common.c
@@ -78,8 +78,8 @@ const uint8_t g_ble_hci_le_cmd_len[BLE_HCI_NUM_LE_CMDS] =
     BLE_HCI_LE_RD_PHY_LEN,              /* 0x0030: read maximum default PHY */
     BLE_HCI_LE_SET_DEFAULT_PHY_LEN,     /* 0x0031: set default PHY */
     BLE_HCI_LE_SET_PHY_LEN,             /* 0x0032: set PHY */
-    BLE_HCI_LE_ENH_RCVR_TEST_LEN,       /* 0x0033: enhanced receiver test */
-    BLE_HCI_LE_ENH_TRANS_TEST_LEN,      /* 0x0034: enhanced transmitter test */
+    BLE_HCI_LE_ENH_RX_TEST_LEN,       /* 0x0033: enhanced receiver test */
+    BLE_HCI_LE_ENH_TX_TEST_LEN,      /* 0x0034: enhanced transmitter test */
     BLE_HCI_LE_SET_ADV_SET_RND_ADDR_LEN,/* 0x0035: set adv. set random address */
     BLE_HCI_LE_SET_EXT_ADV_PARAM_LEN,   /* 0x0036: set ext. adv params */
     BLE_HCI_LE_SET_EXT_ADV_DATA_LEN,    /* 0x0037: set ext. adv. data */