[wip]apps/bttester: Add RAP/RAS support

apps/bttester/pkg.yml

@@ -45,3 +45,6 @@ pkg.deps.BLE_AUDIO:
     - "@apache-mynewt-nimble/nimble/host/audio/services/bass"
     - "@apache-mynewt-nimble/nimble/host/audio/services/pacs"
     - "@apache-mynewt-nimble/nimble/host/audio/services/pacs/lc3"
+
+pkg.deps.BTTESTER_CHANNEL_SOUNDING:
+    - "@apache-mynewt-nimble/nimble/host/services/ras"

apps/bttester/src/btp/btp.h

@@ -35,6 +35,7 @@
 #include "btp_mesh.h"
 #include "btp_pacs.h"
 #include "btp_bap.h"
+#include "btp_rap.h"
 
 #define BTP_MTU MYNEWT_VAL(BTTESTER_BTP_DATA_SIZE_MAX)
 #define BTP_DATA_MAX_SIZE (BTP_MTU - sizeof(struct btp_hdr))
@@ -50,8 +51,9 @@
 #define BTP_SERVICE_ID_GATTC    6
 #define BTP_SERVICE_ID_PACS     12
 #define BTP_SERVICE_ID_BAP     14
+#define BTP_SERVICE_ID_RAP     33
 
-#define BTP_SERVICE_ID_MAX    BTP_SERVICE_ID_BAP
+#define BTP_SERVICE_ID_MAX    BTP_SERVICE_ID_RAP
 
 #define BTP_STATUS_SUCCESS    0x00
 #define BTP_STATUS_FAILED    0x01

apps/bttester/src/btp/btp_rap.h

@@ -0,0 +1,69 @@
+/*
+ * 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_BTP_RAP_
+#define H_BTP_RAP_
+
+#include "nimble/ble.h"
+#include <stdint.h>
+
+#ifndef __packed
+#define __packed    __attribute__((__packed__))
+#endif
+
+/* RAP Service */
+/* commands */
+#define BTP_RAP_READ_SUPPORTED_COMMANDS 0x01
+struct btp_rap_read_supported_commands_rp {
+    uint8_t data[0];
+} __packed;
+
+#define BTP_RAP_SET_TEST_METHOD 0x02
+struct btp_rap_set_test_method_cmd {
+    ble_addr_t address;
+    uint8_t test_method;
+} __packed;
+
+#define BTP_RAP_START_RANGING 0x03
+struct btp_rap_start_ranging_cmd {
+    ble_addr_t address;
+    uint8_t flags;
+    uint8_t local_role;
+} __packed;
+
+#define BTP_RAP_STOP_RANGING 0x04
+struct btp_rap_stop_ranging_cmd {
+    ble_addr_t address;
+} __packed;
+
+#define BTP_RAP_SET_TEST_CS_SUBEVENT_DATA 0x05
+struct btp_rap_set_test_cs_subevent_data_cmd {
+    ble_addr_t address;
+    uint8_t data_len;
+    uint8_t data[0];
+} __packed;
+
+/* RAP events */
+#define BTP_RAP_EV_RANGING_COMPLETE 0x80
+struct btp_rap_ranging_complete_ev {
+    ble_addr_t address;
+    uint8_t status;
+} __packed;
+
+#endif /* H_BTP_RAP_                        */

apps/bttester/src/btp/bttester.h

@@ -159,5 +159,12 @@ uint8_t
 tester_unregister_pacs(void);
 #endif /* MYNEWT_VAL(BLE_AUDIO) */
 
+#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
+uint8_t
+tester_init_rap(void);
+uint8_t
+tester_unregister_rap(void);
+#endif /* MYNEWT_VAL(BLE_CHANNEL_SOUNDING) */
+
 #endif /* __BTTESTER_H__ */
 

apps/bttester/src/btp_core.c

@@ -111,6 +111,11 @@ register_service(const void *cmd, uint16_t cmd_len,
     case BTP_SERVICE_ID_GATTC:
         status = tester_init_gatt_cl();
         break;
+#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
+    case BTP_SERVICE_ID_RAP:
+        status = tester_init_rap();
+        break;
+#endif /* MYNEWT_VAL (BLE_CHANNEL_SOUNDING) */
     default:
         status = BTP_STATUS_FAILED;
         break;
@@ -170,6 +175,11 @@ unregister_service(const void *cmd, uint16_t cmd_len,
         status = tester_unregister_pacs();
         break;
 #endif /* MYNEWT_VAL (BLE_AUDIO) */
+#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
+    case BTP_SERVICE_ID_RAP:
+        status = tester_unregister_rap();
+        break;
+#endif /* MYNEWT_VAL (BLE_CHANNEL_SOUNDING) */
     default:
         status = BTP_STATUS_FAILED;
         break;

apps/bttester/src/btp_rap.c

@@ -0,0 +1,238 @@
+/*
+ * 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.
+ */
+
+/* btp_rap.c - Bluetooth Ranging Profile Tester */
+
+#include "btp/bttester.h"
+#include "syscfg/syscfg.h"
+#include <string.h>
+
+
+#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING)
+
+#include "btp/btp_rap.h"
+
+#include "btp/btp.h"
+#include "console/console.h"
+
+#include "host/ble_hs.h"
+#include "host/ble_cs.h"
+#include "host/util/util.h"
+#include "math.h"
+#if MYNEWT_VAL(BLE_SVC_RAS_CLIENT) || MYNEWT_VAL(BLE_SVC_RAS_SERVER)
+#include "services/ras/ble_svc_ras.h"
+#endif
+
+int ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, unsigned int len);
+
+static void
+rap_ranging_complete_ev(uint16_t conn_handle, uint8_t status)
+{
+    struct btp_rap_ranging_complete_ev *ev;
+    struct os_mbuf *buf = os_msys_get(0, 0);
+    struct ble_gap_conn_desc conn;
+    const ble_addr_t *addr;
+
+    SYS_LOG_DBG("");
+
+    if (ble_gap_conn_find(conn_handle, &conn)) {
+        return;
+    }
+
+    ev = os_mbuf_extend(buf, sizeof(*ev));
+    if (!ev) {
+        return;
+    }
+
+    addr = &conn.peer_ota_addr;
+    memcpy(&ev->address, addr, sizeof(ev->address));
+    ev->status = status;
+
+    tester_event(BTP_SERVICE_ID_RAP, BTP_RAP_EV_RANGING_COMPLETE,
+                 buf->om_data, buf->om_len);
+}
+
+static int
+btp_rap_event(struct ble_cs_event *event, void *arg, uint16_t conn_handle)
+{
+    switch (event->type) {
+    case BLE_CS_EVENT_CS_PROCEDURE_COMPLETE:
+        rap_ranging_complete_ev(conn_handle, event->procedure_complete.status);
+        break;
+    }
+
+    return 0;
+}
+
+static uint8_t
+supported_commands(const void *cmd, uint16_t cmd_len,
+                   void *rsp, uint16_t *rsp_len)
+{
+    struct btp_rap_read_supported_commands_rp *rp = rsp;
+
+    *rsp_len = tester_supported_commands(BTP_SERVICE_ID_RAP, rp->data);
+    *rsp_len += sizeof(*rp);
+
+    return BTP_STATUS_SUCCESS;
+}
+
+static uint8_t
+btp_rap_set_test_method(const void *cmd, uint16_t cmd_len,
+                        void *rsp, uint16_t *rsp_len)
+{
+    SYS_LOG_DBG("");
+//    struct btp_rap_set_test_method_cmd *cp = cmd;
+
+    return 0;
+}
+
+static uint8_t
+btp_rap_start_ranging(const void *cmd, uint16_t cmd_len,
+                      void *rsp, uint16_t *rsp_len)
+{
+    int rc;
+//    struct ble_cs_procedure_start_params start_cmd;
+    const struct btp_rap_start_ranging_cmd *cp = cmd;
+    struct ble_cs_setup_params setup_cmd;
+    struct ble_gap_conn_desc conn;
+
+    SYS_LOG_DBG("");
+
+    rc = ble_gap_conn_find_by_addr(&cp->address, &conn);
+    if (rc) {
+        SYS_LOG_DBG("failed, disconnected, %u", rc);
+        return BTP_STATUS_FAILED;
+    }
+
+    setup_cmd.cb = btp_rap_event;
+    setup_cmd.cb_arg = NULL;
+    setup_cmd.local_role = cp->local_role;
+    ble_cs_setup(&setup_cmd, conn.conn_handle);
+
+#if MYNEWT_VAL(BLE_SVC_RAS_SERVER)
+    ble_svc_ras_ranging_data_body_init(conn.conn_handle, 0, 0, 0, 1);
+#endif
+//    rc = ble_cs_procedure_start(&start_cmd, conn.conn_handle);
+//    if (rc) {
+//        // send response with error.
+//        return rc;
+//    }
+
+    return 0;
+}
+
+static uint8_t
+btp_rap_stop_ranging(const void *cmd, uint16_t cmd_len,
+                     void *rsp, uint16_t *rsp_len)
+{
+    SYS_LOG_DBG("");
+
+    return 0;
+}
+
+static uint8_t
+btp_rap_set_test_cs_subevent_data(const void *cmd, uint16_t cmd_len,
+                                  void *rsp, uint16_t *rsp_len)
+{
+    int rc;
+    struct ble_hci_ev *hci_ev;
+    const struct btp_rap_set_test_cs_subevent_data_cmd *cp = cmd;
+    struct ble_hci_ev_le_subev_cs_subevent_result *ev;
+    struct ble_gap_conn_desc conn;
+
+    SYS_LOG_DBG("");
+
+    if (BLE_HCI_MAX_DATA_LEN - sizeof(ev->subev_code) - sizeof(ev->conn_handle) < cp->data_len) {
+        return BTP_STATUS_FAILED;
+    }
+
+    rc = ble_gap_conn_find_by_addr(&cp->address, &conn);
+    if (rc) {
+        return BTP_STATUS_FAILED;
+    }
+
+    hci_ev = ble_transport_alloc_evt(0);
+    if (!hci_ev) {
+        return BTP_STATUS_FAILED;
+    }
+
+    hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+    hci_ev->length = sizeof(ev->subev_code) + sizeof(ev->conn_handle) + cp->data_len;
+    memset(hci_ev->data, 0, BLE_HCI_MAX_DATA_LEN);
+    ev = (struct ble_hci_ev_le_subev_cs_subevent_result *)hci_ev->data;
+
+    ev->subev_code = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT;
+    ev->conn_handle = htole16(conn.conn_handle);
+    memcpy(&ev->config_id, cp->data, cp->data_len);
+
+    ble_hs_hci_evt_le_cs_subevent_result(BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT, ev, hci_ev->length);
+
+    return 0;
+}
+
+static const struct btp_handler handlers[] = {
+    {
+        .opcode = BTP_RAP_READ_SUPPORTED_COMMANDS,
+        .index = BTP_INDEX_NONE,
+        .expect_len = 0,
+        .func = supported_commands,
+    },
+    {
+        .opcode = BTP_RAP_SET_TEST_METHOD,
+        .index = BTP_INDEX,
+        .expect_len = sizeof(struct btp_rap_set_test_method_cmd),
+        .func = btp_rap_set_test_method,
+    },
+    {
+        .opcode = BTP_RAP_START_RANGING,
+        .index = BTP_INDEX,
+        .expect_len = sizeof(struct btp_rap_start_ranging_cmd),
+        .func = btp_rap_start_ranging,
+    },
+    {
+        .opcode = BTP_RAP_STOP_RANGING,
+        .index = BTP_INDEX,
+        .expect_len = sizeof(struct btp_rap_stop_ranging_cmd),
+        .func = btp_rap_stop_ranging,
+    },
+    {
+        .opcode = BTP_RAP_SET_TEST_CS_SUBEVENT_DATA,
+        .index = BTP_INDEX,
+        .expect_len = BTP_HANDLER_LENGTH_VARIABLE,
+        .func = btp_rap_set_test_cs_subevent_data,
+    },
+};
+
+uint8_t
+tester_init_rap(void)
+{
+    tester_register_command_handlers(BTP_SERVICE_ID_RAP, handlers,
+                                     ARRAY_SIZE(handlers));
+
+    return BTP_STATUS_SUCCESS;
+}
+
+uint8_t
+tester_unregister_rap(void)
+{
+    return BTP_STATUS_SUCCESS;
+}
+
+#endif /* MYNEWT_VAL(BLE_CHANNEL_SOUNDING) */
+