michal-narajowski commented on a change in pull request #744: Add support for
Enhanced LE CoC as per BT 5.2
URL: https://github.com/apache/mynewt-nimble/pull/744#discussion_r385106852
##########
File path: nimble/host/src/ble_l2cap_sig.c
##########
@@ -609,31 +643,456 @@ ble_l2cap_sig_coc_connect_cb(struct ble_l2cap_sig_proc
*proc, int status)
{
struct ble_hs_conn *conn;
struct ble_l2cap_chan *chan;
+ int i;
+ bool some_not_connected = false;
if (!proc) {
return;
}
- chan = proc->connect.chan;
- if (!chan || !chan->cb) {
+ for (i = 0; i < proc->connect.chan_cnt; i++) {
+ chan = proc->connect.chan[i];
+ if (!chan || !chan->cb) {
+ continue;
+ }
+
+ if ((status == 0) && (chan->dcid != 0)) {
+ ble_l2cap_event_coc_connected(chan, status);
+ /* Let's forget about connected channel now.
+ * Not connected will be freed later on.
+ */
+ proc->connect.chan[i] = NULL;
+ continue;
+ }
+ some_not_connected = true;
+ ble_l2cap_event_coc_connected(chan, status ? status : BLE_HS_EREJECT);
+ }
+
+ if (!some_not_connected) {
return;
}
- ble_l2cap_event_coc_connected(chan, status);
+ /* Free not connected channels*/
+
+ ble_hs_lock();
+ conn = ble_hs_conn_find(chan->conn_handle);
+ for (i = 0; i < proc->connect.chan_cnt; i++) {
+ chan = proc->connect.chan[i];
+ if (chan) {
+ /* Normally in channel free we send disconnected event to
application.
+ * However in case on error during creation connection we send
connected
+ * event with error status. To avoid additional disconnected event
lets
+ * clear callbacks since we don't needed it anymore.
+ */
+ chan->cb = NULL;
+ ble_l2cap_chan_free(conn, chan);
+ }
+ }
+ ble_hs_unlock();
+}
+
+#if MYNEWT_VAL(BLE_L2CAP_ENHANCED_COC)
+static void
+ble_l2cap_event_coc_reconfigured(uint16_t conn_handle, uint16_t status,
+ struct ble_l2cap_chan *chan, bool peer)
+{
+ struct ble_l2cap_event event = { };
+
+ if (peer) {
+ event.type = BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED;
+ } else {
+ event.type = BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED;
+ }
+ event.reconfigured.conn_handle = conn_handle;
+ event.reconfigured.chan = chan;
+ event.reconfigured.status = status;
+
+ chan->cb(&event, chan->cb_arg);
+}
+
+static int
+ble_l2cap_sig_credit_base_reconfig_req_rx(uint16_t conn_handle,
+ struct ble_l2cap_sig_hdr *hdr,
+ struct os_mbuf **om)
+{
+ struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ] = {0};
+ struct ble_l2cap_sig_credit_base_reconfig_req *req;
+ struct ble_l2cap_sig_credit_base_reconfig_rsp *rsp;
+ struct ble_hs_conn *conn;
+ struct os_mbuf *txom;
+ int i;
+ int rc;
+ uint8_t cid_cnt;
+ uint8_t reduction_mps = 0;
+
+ rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+ if (rc != 0) {
+ return rc;
+ }
+
+ ble_hs_lock();
+ conn = ble_hs_conn_find(conn_handle);
+ if (!conn) {
+ ble_hs_unlock();
+ return 0;
+ }
+
+ rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_RECONFIG_RSP,
+ hdr->identifier, sizeof(*rsp) , &txom);
+ if (!rsp) {
+ /* TODO: Reuse req bufor for the response. For now is such a case.
+ * remote will timeout.
+ */
+ ble_hs_unlock();
+ return 0;
+ }
+
+ if (hdr->length <= sizeof(*req)) {
+ rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+ goto done;
+ }
+
+ req = (struct ble_l2cap_sig_credit_base_reconfig_req *)(*om)->om_data;
+
+ if ((req->mps < BLE_L2CAP_ECOC_MIN_MTU) || (req->mtu <
BLE_L2CAP_ECOC_MIN_MTU)) {
+ rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+ goto done;
+ }
+
+ /* Assume request will succeed. If not, result will be updated */
+ rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_SUCCEED);
+
+ cid_cnt = (hdr->length - sizeof(*req)) / sizeof(uint16_t);
+ if (cid_cnt > BLE_L2CAP_MAX_COC_CONN_REQ) {
+ rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_UNACCAPTED_PARAM);
+ goto done;
+ }
+
+ for (i = 0; i < cid_cnt; i++) {
+ chan[i] = ble_hs_conn_chan_find_by_dcid(conn, req->dcids[i]);
+ if (!chan[i]) {
+ rsp->result = htole16(BLE_L2CAP_ERR_RECONFIG_INVALID_DCID);
+ ble_hs_unlock();
+ goto done;
+ }
+
+ if (chan[i]->peer_coc_mps > req->mps) {
+ reduction_mps++;
+ if (reduction_mps > 1) {
+ rsp->result =
htole16(BLE_L2CAP_ERR_RECONFIG_REDUCTION_MPS_NOT_ALLOWED);
+ ble_hs_unlock();
+ goto done;
+ }
+ }
+
+ if (chan[i]->coc_tx.mtu > req->mtu) {
+ rsp->result =
htole16(BLE_L2CAP_ERR_RECONFIG_REDUCTION_MTU_NOT_ALLOWED);
+ ble_hs_unlock();
+ goto done;
+ }
+ }
+
+ for (i = 0; i < cid_cnt; i++) {
+ chan[i]->coc_tx.mtu = req->mtu;
+ chan[i]->peer_coc_mps = req->mps;
+ ble_l2cap_event_coc_reconfigured(conn_handle, 0, chan[i], true);
+ }
+
+ ble_hs_unlock();
+done:
+ ble_l2cap_sig_tx(conn_handle, txom);
+ return 0;
+}
+
+static void
+ble_l2cap_sig_coc_reconfig_cb(struct ble_l2cap_sig_proc *proc, int status)
+{
+ int i;
+ struct ble_l2cap_chan *chan[BLE_L2CAP_MAX_COC_CONN_REQ] = {0};
+ struct ble_hs_conn *conn;
+
+ ble_hs_lock();
+
+ conn = ble_hs_conn_find(proc->conn_handle);
+ if (!conn) {
+ ble_hs_unlock();
+ return;
+ }
+
+ for (i = 0; i< proc->reconfig.cid_cnt; i++) {
+ chan[i] = ble_hs_conn_chan_find_by_scid(conn, proc->reconfig.cids[i]);
+ if (status == 0) {
+ ble_l2cap_coc_set_new_mtu_mps(chan[i], proc->reconfig.new_mtu,
proc->reconfig.new_mps);
+ }
+ }
+
+ ble_hs_unlock();
+
+ for (i = 0; i < proc->reconfig.cid_cnt; i++) {
+ ble_l2cap_event_coc_reconfigured(proc->conn_handle, status, chan[i],
false);
+ }
+}
+
+static int
+ble_l2cap_sig_credit_base_reconfig_rsp_rx(uint16_t conn_handle,
+ struct ble_l2cap_sig_hdr *hdr,
+ struct os_mbuf **om)
+{
+ struct ble_l2cap_sig_proc *proc;
+ struct ble_l2cap_sig_credit_base_reconfig_rsp *rsp;
+ int rc;
+
+ proc = ble_l2cap_sig_proc_extract(conn_handle,
+ BLE_L2CAP_SIG_PROC_OP_RECONFIG,
+ hdr->identifier);
+ if (!proc) {
+ return 0;
+ }
+
+ rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+ if (rc != 0) {
+ return rc;
+ }
+
+ rsp = (struct ble_l2cap_sig_credit_base_reconfig_rsp *)(*om)->om_data;
+ ble_l2cap_sig_coc_reconfig_cb(proc, (rsp->result > 0) ? BLE_HS_EREJECT :
0);
+
+ return 0;
+}
+
+static int
+ble_l2cap_sig_credit_base_con_req_rx(uint16_t conn_handle,
+ struct ble_l2cap_sig_hdr *hdr,
+ struct os_mbuf **om)
+{
+ int rc;
+ struct ble_l2cap_sig_credit_base_connect_req *req;
+ struct os_mbuf *txom;
+ struct ble_l2cap_sig_credit_base_connect_rsp *rsp;
+ struct ble_l2cap_chan *chans[5] = { 0 };
+ struct ble_hs_conn *conn;
+ uint16_t scid;
+ uint8_t num_of_scids;
+ uint8_t chan_created = 0;
+ int i;
+ uint8_t len;
+
+ rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+ if (rc != 0) {
+ return rc;
+ }
+
+ len = (hdr->length > sizeof(*req)) ? hdr->length : sizeof(*req);
+
+ rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP,
+ hdr->identifier, len , &txom);
+ if (!rsp) {
+ /* Well, nothing smart we can do if there is no memory for response.
+ * Remote will timeout.
+ */
+ return 0;
+ }
+
+ ble_hs_lock();
+
+ memset(rsp, 0, len);
+
+ /* Initial dummy values in case of error, just to satisfy PTS */
+ rsp->credits = htole16(1);
+ rsp->mps = htole16(BLE_L2CAP_ECOC_MIN_MTU);
+ rsp->mtu = htole16(BLE_L2CAP_ECOC_MIN_MTU);
+
+ if (hdr->length <= sizeof(*req)) {
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+ goto failed;
+ }
+
+ req = (struct ble_l2cap_sig_credit_base_connect_req *)(*om)->om_data;
+
+ num_of_scids = (hdr->length - sizeof(*req)) / sizeof(uint16_t);
+ if (num_of_scids > 5) {
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+ goto failed;
+ }
+
+ if ((req->mtu < BLE_L2CAP_ECOC_MIN_MTU) || (req->mps <
BLE_L2CAP_ECOC_MIN_MTU)) {
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_PARAMETERS);
+ goto failed;
+ }
+
+ conn = ble_hs_conn_find_assert(conn_handle);
+
+ /* First verify that provided SCIDs are good */
+ for (i = 0; i < num_of_scids; i++) {
+ scid = le16toh(req->scids[i]);
+ if (scid < BLE_L2CAP_COC_CID_START || scid > BLE_L2CAP_COC_CID_END) {
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID);
+ goto failed;
+ }
+ }
+
+ /* Let us try to connect channels */
+ for (i = 0; i < num_of_scids; i++) {
+ /* Verify CID. Note, scid in the request is dcid for out local channel
*/
+ scid = le16toh(req->scids[i]);
+ chans[i] = ble_hs_conn_chan_find_by_dcid(conn, scid);
+ if (chans[i]) {
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED);
+ rsp->dcids[i] = htole16(chans[i]->scid);
+ continue;
+ }
+
+ rc = ble_l2cap_coc_create_srv_chan(conn, le16toh(req->psm), &chans[i]);
+ if (rc != 0) {
+ if (i == 0) {
+ /* In case it is very first channel we cannot create it means
PSM is incorrect
+ * or we are out of resources. Just send a response now.
+ */
+ rsp->result = htole16(ble_l2cap_sig_ble_hs_err2coc_err(rc));
+ goto failed;
+ } else {
+ /* We cannot create number of channels req by peer due to
limited resources. */
+ rsp->result = htole16(BLE_L2CAP_COC_ERR_NO_RESOURCES);
+ goto done;
+ }
+ }
+
+ /* Fill up remote configuration. Note MPS is the L2CAP MTU*/
+ chans[i]->dcid = scid;
+ chans[i]->peer_coc_mps = le16toh(req->mps);
+ chans[i]->coc_tx.credits = le16toh(req->credits);
+ chans[i]->coc_tx.mtu = le16toh(req->mtu);
+
+ ble_hs_conn_chan_insert(conn, chans[i]);
+ /* Sending event to the app. Unlock hs */
+ ble_hs_unlock();
+
+ rc = ble_l2cap_event_coc_accept(chans[i], le16toh(req->mtu));
+ if (rc == 0) {
+ rsp->dcids[i] = htole16(chans[i]->scid);
+ chan_created++;
+ if (chan_created == 1) {
+ /* We need to set it once as there are same initial parameters
+ * for all the channels
+ */
+ rsp->credits = htole16(chans[i]->coc_rx.credits);
+ rsp->mps = htole16(chans[i]->my_mtu);
+ rsp->mtu = htole16(chans[i]->coc_rx.mtu);
+ }
+ } else {
+ /* Make sure we do not send disconnect event when removing channel
*/
+ chans[i]->cb = NULL;
+
+ ble_hs_lock();
+ conn = ble_hs_conn_find_assert(conn_handle);
+ ble_hs_conn_delete_chan(conn, chans[i]);
+ chans[i] = NULL;
+ rsp->result = htole16(ble_l2cap_sig_ble_hs_err2coc_err(rc));
+ rc = 0;
+ ble_hs_unlock();
+ }
- if (status) {
- /* Normally in channel free we send disconnected event to application.
- * However in case on error during creation connection we send
connected
- * event with error status. To avoid additional disconnected event lets
- * clear callbacks since we don't needed it anymore.*/
- chan->cb = NULL;
ble_hs_lock();
- conn = ble_hs_conn_find(chan->conn_handle);
- ble_l2cap_chan_free(conn, chan);
+ conn = ble_hs_conn_find_assert(conn_handle);
+ }
+
+done:
+ ble_hs_unlock();
+ rc = ble_l2cap_sig_tx(conn_handle, txom);
+ if (rc != 0) {
+ ble_hs_lock();
+ conn = ble_hs_conn_find_assert(conn_handle);
+ for (i = 0; i < num_of_scids; i++) {
+ if (chans[i]) {
+ ble_hs_conn_delete_chan(conn, chans[i]);
+ }
+ }
ble_hs_unlock();
+ return 0;
+ }
+
+ /* Notify user about connection status */
+ for (i = 0; i < num_of_scids; i++) {
+ if (chans[i]) {
+ ble_l2cap_event_coc_connected(chans[i], rc);
+ }
}
+
+ return 0;
+
+failed:
+ ble_hs_unlock();
+ ble_l2cap_sig_tx(conn_handle, txom);
+ return 0;
}
+static int
+ble_l2cap_sig_credit_base_con_rsp_rx(uint16_t conn_handle,
+ struct ble_l2cap_sig_hdr *hdr,
+ struct os_mbuf **om)
+{
+ struct ble_l2cap_sig_proc *proc;
+ struct ble_l2cap_sig_credit_base_connect_rsp *rsp;
+ struct ble_l2cap_chan *chan;
+ struct ble_hs_conn *conn;
+ int rc;
+ int i;
+
+#if !BLE_MONITOR
+ BLE_HS_LOG(DEBUG, "L2CAP LE COC connection response received\n");
+#endif
+
+ proc = ble_l2cap_sig_proc_extract(conn_handle,
+ BLE_L2CAP_SIG_PROC_OP_CONNECT,
+ hdr->identifier);
+ if (!proc) {
+ return 0;
+ }
+
+ rc = ble_hs_mbuf_pullup_base(om, hdr->length);
+ if (rc != 0) {
+ goto done;
+ }
+
+ rsp = (struct ble_l2cap_sig_credit_base_connect_rsp *)(*om)->om_data;
+
+ if (rsp->result) {
+ rc = ble_l2cap_sig_coc_err2ble_hs_err(le16toh(rsp->result));
+ goto done;
+ }
+
+ ble_hs_lock();
+ conn = ble_hs_conn_find(conn_handle);
+ assert(conn != NULL);
+
+ for (i = 0; i < proc->connect.chan_cnt; i++) {
+ chan = proc->connect.chan[i];
+ if (rsp->dcids[i] == 0) {
+ /* Channel rejected, dont put it on the list.
+ * User will get notified later in that function
+ */
+ chan->dcid = 0;
+ continue;
+ }
+ chan->peer_coc_mps = le16toh(rsp->mps);
+ chan->dcid = le16toh(rsp->dcids[i]);
+ chan->coc_tx.mtu = le16toh(rsp->mtu);
+ chan->coc_tx.credits = le16toh(rsp->credits);
+
+ ble_hs_conn_chan_insert(conn, chan);
+ }
+
+ ble_hs_unlock();
+
+done:
+ ble_l2cap_sig_coc_connect_cb(proc, rc);
+ ble_l2cap_sig_proc_free(proc);
+
+ /* Silently ignore errors as this is response signal */
Review comment:
Error log here?
----------------------------------------------------------------
This is an automated message from the Apache Git Service.
To respond to the message, please log on to GitHub and use the
URL above to go to the specific comment.
For queries about this service, please contact Infrastructure at:
[email protected]
With regards,
Apache Git Services
