l2cap: implement echo request-response procedure

nimble/host/include/host/ble_l2cap.h

@@ -246,6 +246,8 @@ struct ble_l2cap_chan_info {
 
 typedef int ble_l2cap_event_fn(struct ble_l2cap_event *event, void *arg);
 
+typedef void ble_l2cap_ping_fn(uint16_t conn_handle, uint32_t rtt_ms,
+                               struct os_mbuf *om);
 
 uint16_t ble_l2cap_get_conn_handle(struct ble_l2cap_chan *chan);
 int ble_l2cap_create_server(uint16_t psm, uint16_t mtu,
@@ -259,6 +261,26 @@ int ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx);
 int ble_l2cap_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx);
 int ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info *chan_info);
 
+/**
+ * Send an ECHO_REQ packet over the L2CAP signalling channel for the given
+ * connection
+ *
+ * @param conn_handle   Connection handle
+ * @param cb            Function called once the corresponding ECHO_RSP is
+ *                      received. May be NULL.
+ * @param data          User payload appended to the ECHO_REQ packet, may be
+ *                      NULL
+ * @param data_len      Length of @p data in bytes. Set to 0 to omit any user
+ *                      payload
+ *
+ * @return              0 on success
+ *                      BLE_HS_EBADDATA if given payload is invalid
+ *                      BLE_HS_ENOMEM if request packet cannot be allocated
+ *                      BLE_HS_ENOTCONN if not connected
+ */
+int ble_l2cap_ping(uint16_t conn_handle, ble_l2cap_ping_fn cb,
+                   const void *data, uint16_t data_len);
+
 #ifdef __cplusplus
 }
 #endif

nimble/host/src/ble_l2cap.c

@@ -178,6 +178,13 @@ ble_l2cap_get_chan_info(struct ble_l2cap_chan *chan, struct ble_l2cap_chan_info
     return 0;
 }
 
+int
+ble_l2cap_ping(uint16_t conn_handle, ble_l2cap_ping_fn cb,
+               const void *data, uint16_t data_len)
+{
+    return ble_l2cap_sig_ping(conn_handle, cb, data, data_len);
+}
+
 int
 ble_l2cap_enhanced_connect(uint16_t conn_handle,
                                uint16_t psm, uint16_t mtu,

nimble/host/src/ble_l2cap_sig.c

@@ -60,6 +60,7 @@
 #define BLE_L2CAP_SIG_PROC_OP_RECONFIG          2
 #define BLE_L2CAP_SIG_PROC_OP_DISCONNECT        3
 #define BLE_L2CAP_SIG_PROC_OP_MAX               4
+#define BLE_L2CAP_SIG_PROC_OP_PING              5
 
 #if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
 #define BLE_L2CAP_ECOC_MIN_MTU  (64)
@@ -97,6 +98,10 @@ struct ble_l2cap_sig_proc {
             uint16_t new_mtu;
         } reconfig;
 #endif
+        struct {
+            ble_l2cap_ping_fn *cb;
+            ble_npl_time_t time_sent;
+        } ping;
     };
 };
 
@@ -112,6 +117,10 @@ static ble_l2cap_sig_rx_fn ble_l2cap_sig_rx_noop;
 static ble_l2cap_sig_rx_fn ble_l2cap_sig_update_req_rx;
 static ble_l2cap_sig_rx_fn ble_l2cap_sig_update_rsp_rx;
 static ble_l2cap_sig_rx_fn ble_l2cap_sig_rx_reject;
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_echo_req;
+static ble_l2cap_sig_rx_fn ble_l2cap_sig_echo_rsp;
+#endif
 
 #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
 static ble_l2cap_sig_rx_fn ble_l2cap_sig_coc_req_rx;
@@ -145,7 +154,10 @@ static ble_l2cap_sig_rx_fn * const ble_l2cap_sig_dispatch[] = {
     [BLE_L2CAP_SIG_OP_CONFIG_RSP]           = ble_l2cap_sig_rx_noop,
     [BLE_L2CAP_SIG_OP_DISCONN_REQ]          = ble_l2cap_sig_disc_req_rx,
     [BLE_L2CAP_SIG_OP_DISCONN_RSP]          = ble_l2cap_sig_disc_rsp_rx,
-    [BLE_L2CAP_SIG_OP_ECHO_RSP]             = ble_l2cap_sig_rx_noop,
+#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
+    [BLE_L2CAP_SIG_OP_ECHO_REQ]             = ble_l2cap_sig_echo_req,
+    [BLE_L2CAP_SIG_OP_ECHO_RSP]             = ble_l2cap_sig_echo_rsp,
+#endif
     [BLE_L2CAP_SIG_OP_INFO_RSP]             = ble_l2cap_sig_rx_noop,
     [BLE_L2CAP_SIG_OP_CREATE_CHAN_RSP]      = ble_l2cap_sig_rx_noop,
     [BLE_L2CAP_SIG_OP_MOVE_CHAN_RSP]        = ble_l2cap_sig_rx_noop,
@@ -1632,6 +1644,58 @@ done:
     return 0;
 }
 
+static int
+ble_l2cap_sig_echo_req(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                       struct os_mbuf **om)
+{
+    void *rsp;
+    struct os_mbuf *txom;
+    struct ble_l2cap_sig_hdr *rsp_hdr;
+    int rc;
+
+    /* we temporarily set size to 0 as we do not want to allocate additional
+     * space yet */
+    rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_ECHO_RSP,
+                                hdr->identifier, 0, &txom);
+    if (rsp == NULL) {
+        return BLE_HS_ENOMEM;
+    }
+    rc = os_mbuf_appendfrom(txom, *om, 0, OS_MBUF_PKTLEN(*om));
+    if (rc != 0) {
+        os_mbuf_free_chain(txom);
+        return BLE_HS_ENOMEM;
+    }
+    /* after copying the request payload into the response, we need to adjust
+     * the size field in the header to the actual value */
+    rsp_hdr = (struct ble_l2cap_sig_hdr *)txom->om_data;
+    rsp_hdr->length = htole16(OS_MBUF_PKTLEN(*om));
+
+    return ble_l2cap_sig_tx(conn_handle, txom);
+}
+
+static int
+ble_l2cap_sig_echo_rsp(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr,
+                       struct os_mbuf **om)
+{
+    struct ble_l2cap_sig_proc *proc;
+    uint32_t rtt_ms;
+
+    proc = ble_l2cap_sig_proc_extract(conn_handle, BLE_L2CAP_SIG_PROC_OP_PING,
+                                      hdr->identifier);
+    if (proc == NULL) {
+        return BLE_HS_ENOENT;
+    }
+
+    if (proc->ping.cb != NULL) {
+        ble_npl_time_t now = ble_npl_time_get();
+        rtt_ms = ble_npl_time_ticks_to_ms32(now - proc->ping.time_sent);
+        proc->ping.cb(conn_handle, rtt_ms, *om);
+        ble_l2cap_sig_proc_free(proc);
+    }
+
+    return 0;
+}
+
 int
 ble_l2cap_sig_disconnect(struct ble_l2cap_chan *chan)
 {
@@ -1869,6 +1933,60 @@ ble_l2cap_sig_extract_expired(struct ble_l2cap_sig_proc_list *dst_list)
     return next_exp_in;
 }
 
+int
+ble_l2cap_sig_ping(uint16_t conn_handle, ble_l2cap_ping_fn cb,
+                   const void *data, uint16_t data_len)
+{
+    struct ble_l2cap_sig_proc *proc;
+    struct os_mbuf *txom;
+    void *req;
+    struct ble_l2cap_sig_hdr *hdr;
+    int rc;
+
+    if ((data_len > 0) && (data == NULL)) {
+        return BLE_HS_EBADDATA;
+    }
+
+    ble_hs_lock();
+    proc = ble_l2cap_sig_proc_alloc();
+    ble_hs_unlock();
+
+    if (!proc) {
+        return BLE_HS_ENOMEM;
+    }
+
+    /* allocate and fill procedure context */
+    proc->op = BLE_L2CAP_SIG_PROC_OP_PING;
+    proc->id = ble_l2cap_sig_next_id();
+    proc->conn_handle = conn_handle;
+    proc->ping.cb = cb;
+    proc->ping.time_sent = ble_npl_time_get();
+
+    /* allocate signalling packet and copy payload into packet */
+    req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_ECHO_REQ,
+                                proc->id, 0, &txom);
+    if (req == NULL) {
+        ble_l2cap_sig_proc_free(proc);
+        return BLE_HS_ENOMEM;
+    }
+    if (data_len > 0) {
+        rc = os_mbuf_append(txom, data, data_len);
+        if (rc != 0) {
+            os_mbuf_free_chain(txom);
+            ble_l2cap_sig_proc_free(proc);
+            return BLE_HS_ENOMEM;
+        }
+        /* adjust the size field in the signalling header */
+        hdr = (struct ble_l2cap_sig_hdr *)txom->om_data;
+        hdr->length = htole16(data_len);
+    }
+
+
+    rc = ble_l2cap_sig_tx(proc->conn_handle, txom);
+    ble_l2cap_sig_process_status(proc, rc);
+    return rc;
+}
+
 void
 ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason)
 {

nimble/host/src/ble_l2cap_sig_priv.h

@@ -172,6 +172,8 @@ ble_l2cap_sig_coc_reconfig(uint16_t conn_handle, struct ble_l2cap_chan *chans[],
 }
 #endif
 
+int ble_l2cap_sig_ping(uint16_t conn_handle, ble_l2cap_ping_fn cb,
+                       const void *data, uint16_t data_len);
 void ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason);
 int32_t ble_l2cap_sig_timer(void);
 struct ble_l2cap_chan *ble_l2cap_sig_create_chan(uint16_t conn_handle);