diff --git a/nimble/host/include/host/ble_l2cap.h b/nimble/host/include/host/ble_l2cap.h index aef9682cc..5c5383b26 100644 --- a/nimble/host/include/host/ble_l2cap.h +++ b/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 diff --git a/nimble/host/src/ble_l2cap.c b/nimble/host/src/ble_l2cap.c index 2bc50e0e9..a2c515633 100644 --- a/nimble/host/src/ble_l2cap.c +++ b/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, diff --git a/nimble/host/src/ble_l2cap_sig.c b/nimble/host/src/ble_l2cap_sig.c index aaf9c642a..fab16ea44 100644 --- a/nimble/host/src/ble_l2cap_sig.c +++ b/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) { diff --git a/nimble/host/src/ble_l2cap_sig_priv.h b/nimble/host/src/ble_l2cap_sig_priv.h index a698cd0d8..55b37cd20 100644 --- a/nimble/host/src/ble_l2cap_sig_priv.h +++ b/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);