From 98a683e1b4f3d8c8847ad5790c2e16b11899c5ee Mon Sep 17 00:00:00 2001
From: Szymon Janc <szymon.janc@codecoup.pl>
Date: Wed, 26 Oct 2022 13:07:03 +0200
Subject: [PATCH] nimble/host: Add support for enabling controller DTM

This allows to controll DTM from application.
---
 nimble/host/include/host/ble_dtm.h | 49 +++++++++++++++++++++
 nimble/host/src/ble_dtm.c          | 71 ++++++++++++++++++++++++++++++
 2 files changed, 120 insertions(+)
 create mode 100644 nimble/host/include/host/ble_dtm.h
 create mode 100644 nimble/host/src/ble_dtm.c

diff --git a/nimble/host/include/host/ble_dtm.h b/nimble/host/include/host/ble_dtm.h
new file mode 100644
index 000000000..e340a3069
--- /dev/null
+++ b/nimble/host/include/host/ble_dtm.h
@@ -0,0 +1,49 @@
+/*
+ * 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_DTM_
+#define H_BLE_DTM_
+
+#include <inttypes.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct ble_dtm_rx_params {
+    uint8_t channel;
+    uint8_t phy;
+    uint8_t modulation_index;
+};
+
+int ble_dtm_rx_start(const struct ble_dtm_rx_params *params);
+
+struct ble_dtm_tx_params {
+    uint8_t channel;
+    uint8_t test_data_len;
+    uint8_t payload;
+    uint8_t phy;
+};
+int ble_dtm_tx_start(const struct ble_dtm_tx_params *params);
+
+int ble_dtm_stop(uint16_t *num_packets);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/nimble/host/src/ble_dtm.c b/nimble/host/src/ble_dtm.c
new file mode 100644
index 000000000..d88bee270
--- /dev/null
+++ b/nimble/host/src/ble_dtm.c
@@ -0,0 +1,71 @@
+/*
+ * 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 <nimble/hci_common.h>
+#include <host/ble_dtm.h>
+#include <os/endian.h>
+#include "ble_hs_hci_priv.h"
+
+int
+ble_dtm_rx_start(const struct ble_dtm_rx_params *params)
+{
+    struct ble_hci_le_rx_test_v2_cp cmd;
+
+    cmd.rx_chan = params->channel;
+    cmd.phy = params->phy;
+    cmd.index = params->modulation_index;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_RX_TEST_V2),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_dtm_tx_start(const struct ble_dtm_tx_params *params)
+{
+    struct ble_hci_le_tx_test_v2_cp cmd;
+
+    cmd.tx_chan = params->channel;
+    cmd.test_data_len = params->test_data_len;
+    cmd.payload = params->payload;
+    cmd.phy = params->phy;
+
+    return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                        BLE_HCI_OCF_LE_TX_TEST_V2),
+                             &cmd, sizeof(cmd), NULL, 0);
+}
+
+int
+ble_dtm_stop(uint16_t *num_packets)
+{
+    struct ble_hci_le_test_end_rp rsp;
+    int rc;
+
+    rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE,
+                                      BLE_HCI_OCF_LE_TEST_END),
+                           NULL, 0, &rsp, sizeof(rsp));
+
+    if (rc) {
+        *num_packets = 0;
+    } else {
+        *num_packets = le16toh(rsp.num_packets);
+    }
+
+    return rc;
+}