nimble/l2cap: Add suppport to send data over L2CAP LE CoC
Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/repo Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/commit/93ac0dfa Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/93ac0dfa Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/93ac0dfa Branch: refs/heads/master Commit: 93ac0dfaf68e8b5614413b03b0ca787dc9f6142b Parents: fbceba5 Author: Åukasz Rymanowski <[email protected]> Authored: Sun Feb 12 15:26:25 2017 +0100 Committer: Åukasz Rymanowski <[email protected]> Committed: Fri Mar 3 12:40:42 2017 +0100 ---------------------------------------------------------------------- net/nimble/host/src/ble_l2cap.c | 3 +- net/nimble/host/src/ble_l2cap_coc.c | 124 ++++++++++++++++++++++++++ net/nimble/host/src/ble_l2cap_coc_priv.h | 2 + 3 files changed, 127 insertions(+), 2 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/93ac0dfa/net/nimble/host/src/ble_l2cap.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap.c b/net/nimble/host/src/ble_l2cap.c index 046a136..b19a348 100644 --- a/net/nimble/host/src/ble_l2cap.c +++ b/net/nimble/host/src/ble_l2cap.c @@ -160,8 +160,7 @@ int ble_l2cap_disconnect(struct ble_l2cap_chan *chan) int ble_l2cap_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu) { - /*TODO Implement */ - return BLE_HS_ENOTSUP; + return ble_l2cap_coc_send(chan, sdu); } void http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/93ac0dfa/net/nimble/host/src/ble_l2cap_coc.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_coc.c b/net/nimble/host/src/ble_l2cap_coc.c index 9794a00..12d79d6 100644 --- a/net/nimble/host/src/ble_l2cap_coc.c +++ b/net/nimble/host/src/ble_l2cap_coc.c @@ -288,6 +288,108 @@ ble_l2cap_coc_cleanup_chan(struct ble_l2cap_chan *chan) os_mbuf_free_chain(chan->coc_tx.sdu); } +static int +ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan) +{ + struct ble_l2cap_coc_endpoint *tx; + uint16_t len; + uint16_t left_to_send; + struct os_mbuf *txom; + struct ble_hs_conn *conn; + uint16_t sdu_size_offset; + int rc; + + /* If there is no data to send, just return success */ + tx = &chan->coc_tx; + if (!tx->sdu) { + return 0; + } + + while (tx->credits) { + sdu_size_offset = 0; + + BLE_HS_LOG(DEBUG, "Available credits %d\n", tx->credits); + + /* lets calculate data we are going to send */ + left_to_send = OS_MBUF_PKTLEN(tx->sdu) - tx->data_offset; + + if (tx->data_offset == 0) { + sdu_size_offset = BLE_L2CAP_SDU_SIZE; + left_to_send += sdu_size_offset; + } + + /* Take into account peer MTU */ + len = min(left_to_send, chan->peer_mtu); + + /* Prepare packet */ + txom = ble_hs_mbuf_l2cap_pkt(); + if (!txom) { + BLE_HS_LOG(DEBUG, "Could not prepare l2cap packet len %d, err=%d", + len, rc); + + rc = BLE_HS_ENOMEM; + goto failed; + } + + if (tx->data_offset == 0) { + /* First packet needs SDU len first. Left to send */ + uint16_t l = htole16(OS_MBUF_PKTLEN(tx->sdu)); + + BLE_HS_LOG(DEBUG, "Sending SDU len=%d\n", OS_MBUF_PKTLEN(tx->sdu)); + rc = os_mbuf_append(txom, &l, sizeof(uint16_t)); + if (rc) { + BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc); + goto failed; + } + } + + /* In data_offset we keep track on what we already sent. Need to remember + * that for first packet we need to decrease data size by 2 bytes for sdu + * size + */ + rc = os_mbuf_appendfrom(txom, tx->sdu, tx->data_offset, + len - sdu_size_offset); + if (rc) { + BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc); + goto failed; + } + + ble_hs_lock(); + conn = ble_hs_conn_find_assert(chan->conn_handle); + rc = ble_l2cap_tx(conn, chan, txom); + ble_hs_unlock(); + + if (rc) { + /* txom is consumed by l2cap */ + txom = NULL; + goto failed; + } else { + tx->credits --; + tx->data_offset += len - sdu_size_offset; + } + + BLE_HS_LOG(DEBUG, "Sent %d bytes, credits=%d, to send %d bytes \n", + len, tx->credits, OS_MBUF_PKTLEN(tx->sdu)- tx->data_offset ); + + if (tx->data_offset == OS_MBUF_PKTLEN(tx->sdu)) { + BLE_HS_LOG(DEBUG, "Complete package sent"); + os_mbuf_free_chain(tx->sdu); + tx->sdu = 0; + tx->data_offset = 0; + break; + } + } + + return 0; + +failed: + os_mbuf_free_chain(tx->sdu); + tx->sdu = NULL; + os_mbuf_free_chain(txom); + + return rc; +} + void ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid, uint16_t credits) @@ -317,6 +419,8 @@ ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid, } chan->coc_tx.credits += credits; + ble_l2cap_coc_continue_tx(chan); + ble_hs_unlock(); } @@ -349,6 +453,26 @@ ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx) } int +ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx) +{ + struct ble_l2cap_coc_endpoint *tx; + + tx = &chan->coc_tx; + + if (tx->sdu) { + return BLE_HS_EBUSY; + } + + tx->sdu = sdu_tx; + + if (OS_MBUF_PKTLEN(sdu_tx) > tx->mtu) { + return BLE_HS_EBADDATA; + } + + return ble_l2cap_coc_continue_tx(chan); +} + +int ble_l2cap_coc_init(void) { STAILQ_INIT(&ble_l2cap_coc_srvs); http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/93ac0dfa/net/nimble/host/src/ble_l2cap_coc_priv.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_coc_priv.h b/net/nimble/host/src/ble_l2cap_coc_priv.h index 36ab4dc..4fe75e9 100644 --- a/net/nimble/host/src/ble_l2cap_coc_priv.h +++ b/net/nimble/host/src/ble_l2cap_coc_priv.h @@ -67,11 +67,13 @@ void ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid, uint16_t credits); void ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx); +int ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx); #else #define ble_l2cap_coc_init() 0 #define ble_l2cap_coc_create_server(psm, mtu, cb, cb_arg) BLE_HS_ENOTSUP #define ble_l2cap_coc_recv_ready(chan, sdu_rx) #define ble_l2cap_coc_cleanup_chan(chan) +#define ble_l2cap_coc_send(chan, sdu_tx) BLE_HS_ENOTSUP #endif #ifdef __cplusplus
