nimble/l2cap: Add support to connect L2CAP LE COC With this patch nibmle is support to create L2CAP LE COC and handle incoming connection request.
Note: New error codes (BLE_HS_EFOO) has been added in order to translate L2CAP COC error codes to user 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/696e79d3 Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/696e79d3 Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/696e79d3 Branch: refs/heads/develop Commit: 696e79d3455d4141b6a17e649219140db9f7e66f Parents: 1afc08a Author: Åukasz Rymanowski <[email protected]> Authored: Tue Jan 31 00:08:13 2017 +0100 Committer: Åukasz Rymanowski <[email protected]> Committed: Thu Feb 2 12:59:59 2017 +0100 ---------------------------------------------------------------------- net/nimble/host/include/host/ble_hs.h | 4 + net/nimble/host/src/ble_hs_priv.h | 1 + net/nimble/host/src/ble_l2cap.c | 15 +- net/nimble/host/src/ble_l2cap_coc.c | 131 +++++++++ net/nimble/host/src/ble_l2cap_coc_priv.h | 68 +++++ net/nimble/host/src/ble_l2cap_priv.h | 9 +- net/nimble/host/src/ble_l2cap_sig.c | 365 +++++++++++++++++++++++++- net/nimble/host/src/ble_l2cap_sig_cmd.c | 29 +- net/nimble/host/src/ble_l2cap_sig_priv.h | 31 ++- 9 files changed, 633 insertions(+), 20 deletions(-) ---------------------------------------------------------------------- http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/include/host/ble_hs.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/include/host/ble_hs.h b/net/nimble/host/include/host/ble_hs.h index 4ee6c79..8dd2296 100644 --- a/net/nimble/host/include/host/ble_hs.h +++ b/net/nimble/host/include/host/ble_hs.h @@ -66,6 +66,10 @@ struct os_event; #define BLE_HS_ENOMEM_EVT 20 #define BLE_HS_ENOADDR 21 #define BLE_HS_ENOTSYNCED 22 +#define BLE_HS_EAUTHEN 23 +#define BLE_HS_EAUTHOR 24 +#define BLE_HS_EENCRYPT 25 +#define BLE_HS_EENCRYPT_KEY_SZ 26 #define BLE_HS_ERR_ATT_BASE 0x100 /* 256 */ #define BLE_HS_ATT_ERR(x) ((x) ? BLE_HS_ERR_ATT_BASE + (x) : 0) http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/src/ble_hs_priv.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_hs_priv.h b/net/nimble/host/src/ble_hs_priv.h index 2cc8b40..4bdb22c 100644 --- a/net/nimble/host/src/ble_hs_priv.h +++ b/net/nimble/host/src/ble_hs_priv.h @@ -37,6 +37,7 @@ #include "ble_hs_startup_priv.h" #include "ble_l2cap_priv.h" #include "ble_l2cap_sig_priv.h" +#include "ble_l2cap_coc_priv.h" #include "ble_sm_priv.h" #include "ble_hs_adv_priv.h" #include "ble_hs_pvcy_priv.h" http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/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 bf279de..d881acb 100644 --- a/net/nimble/host/src/ble_l2cap.c +++ b/net/nimble/host/src/ble_l2cap.c @@ -24,6 +24,7 @@ #include "nimble/ble.h" #include "nimble/hci_common.h" #include "ble_hs_priv.h" +#include "ble_l2cap_coc_priv.h" _Static_assert(sizeof (struct ble_l2cap_hdr) == BLE_L2CAP_HDR_SZ, "struct ble_l2cap_hdr must be 4 bytes"); @@ -127,19 +128,14 @@ int ble_l2cap_create_server(uint16_t psm, uint16_t mtu, ble_l2cap_event_fn *cb, void *cb_arg) { - /*TODO: Create server object and put it on the queue */ - return BLE_HS_ENOTSUP; + return ble_l2cap_coc_create_server(psm, mtu, cb, cb_arg); } int ble_l2cap_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb, void *cb_arg) { - /* - * TODO In here we are going to create l2cap channel and send - * BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ - */ - return BLE_HS_ENOTSUP; + return ble_l2cap_sig_coc_connect(conn_handle, psm, mtu, sdu_rx, cb, cb_arg); } int ble_l2cap_disconnect(struct ble_l2cap_chan *chan) @@ -375,6 +371,11 @@ ble_l2cap_init(void) return rc; } + rc = ble_l2cap_coc_init(); + if (rc != 0) { + return rc; + } + rc = ble_sm_init(); if (rc != 0) { return rc; http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/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 new file mode 100644 index 0000000..6286d43 --- /dev/null +++ b/net/nimble/host/src/ble_l2cap_coc.c @@ -0,0 +1,131 @@ +/* + * 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. + */ + +#include <string.h> +#include <errno.h> +#include "console/console.h" +#include "nimble/ble.h" +#include "ble_hs_priv.h" +#include "ble_l2cap_priv.h" +#include "ble_l2cap_coc_priv.h" +#include "ble_l2cap_sig_priv.h" + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 + +STAILQ_HEAD(ble_l2cap_coc_srv_list, ble_l2cap_coc_srv); + +static struct ble_l2cap_coc_srv_list ble_l2cap_coc_srvs; + +static os_membuf_t ble_l2cap_coc_srv_mem[ + OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM), + sizeof (struct ble_l2cap_coc_srv)) +]; + +static struct os_mempool ble_l2cap_coc_srv_pool; + +static void +ble_l2cap_coc_dbg_assert_srv_not_inserted(struct ble_l2cap_coc_srv *srv) +{ +#if MYNEWT_VAL(BLE_HS_DEBUG) + struct ble_l2cap_coc_srv *cur; + + STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) { + BLE_HS_DBG_ASSERT(cur != srv); + } +#endif +} + +static struct ble_l2cap_coc_srv * +ble_l2cap_coc_srv_alloc(void) +{ + struct ble_l2cap_coc_srv *srv; + + srv = os_memblock_get(&ble_l2cap_coc_srv_pool); + if (srv != NULL) { + memset(srv, 0, sizeof(*srv)); + } + + return srv; +} + +int +ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, + ble_l2cap_event_fn *cb, void *cb_arg) +{ + struct ble_l2cap_coc_srv * srv; + + srv = ble_l2cap_coc_srv_alloc(); + if (!srv) { + return BLE_HS_ENOMEM; + } + + srv->psm = psm; + srv->mtu = mtu; + srv->cb = cb; + srv->cb_arg = cb_arg; + + ble_l2cap_coc_dbg_assert_srv_not_inserted(srv); + + STAILQ_INSERT_HEAD(&ble_l2cap_coc_srvs, srv, next); + + return 0; +} + +uint16_t +ble_l2cap_coc_get_cid(void) +{ + static uint16_t next_cid = BLE_L2CAP_COC_CID_START; + + if (next_cid > BLE_L2CAP_COC_CID_END) { + next_cid = BLE_L2CAP_COC_CID_START; + } + + /*TODO: Make it smarter*/ + return next_cid++; +} + +struct ble_l2cap_coc_srv * +ble_l2cap_coc_srv_find(uint16_t psm) +{ + struct ble_l2cap_coc_srv *cur, *srv; + + srv = NULL; + STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) { + if (cur->psm == psm) { + srv = cur; + break; + } + } + + return srv; +} + +int +ble_l2cap_coc_init(void) +{ + STAILQ_INIT(&ble_l2cap_coc_srvs); + + return os_mempool_init(&ble_l2cap_coc_srv_pool, + MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM), + sizeof (struct ble_l2cap_coc_srv), + ble_l2cap_coc_srv_mem, + "ble_l2cap_coc_srv_pool"); +} + +#endif http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/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 new file mode 100644 index 0000000..31e81e7 --- /dev/null +++ b/net/nimble/host/src/ble_l2cap_coc_priv.h @@ -0,0 +1,68 @@ +/* + * 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_L2CAP_COC_PRIV_ +#define H_L2CAP_COC_PRIV_ + +#include <inttypes.h> +#include "syscfg/syscfg.h" +#include "os/queue.h" +#include "host/ble_l2cap.h" +#include "ble_l2cap_sig_priv.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_L2CAP_COC_MTU 100 +#define BLE_L2CAP_COC_CID_START 0x0040 +#define BLE_L2CAP_COC_CID_END 0x007F + +struct ble_l2cap_chan; + +struct ble_l2cap_coc_endpoint { + uint16_t mtu; + uint16_t credits; + struct os_mbuf *sdu; +}; + +struct ble_l2cap_coc_srv { + STAILQ_ENTRY(ble_l2cap_coc_srv) next; + uint16_t psm; + uint16_t mtu; + ble_l2cap_event_fn *cb; + void *cb_arg; +}; + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +int ble_l2cap_coc_init(void); +uint16_t ble_l2cap_coc_get_cid(void); +int ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu, + ble_l2cap_event_fn *cb, void *cb_arg); +struct ble_l2cap_coc_srv * ble_l2cap_coc_srv_find(uint16_t psm); +#else +#define ble_l2cap_coc_init() 0 +#define ble_l2cap_coc_create_server(psm, mtu, cb, cb_arg) BLE_HS_ENOTSUP +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* H_L2CAP_COC_PRIV_ */ http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/src/ble_l2cap_priv.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_priv.h b/net/nimble/host/src/ble_l2cap_priv.h index 900d749..cc4e0ed 100644 --- a/net/nimble/host/src/ble_l2cap_priv.h +++ b/net/nimble/host/src/ble_l2cap_priv.h @@ -20,6 +20,7 @@ #ifndef H_L2CAP_PRIV_ #define H_L2CAP_PRIV_ +#include "ble_l2cap_coc_priv.h" #include "host/ble_l2cap.h" #include <inttypes.h> #include "stats/stats.h" @@ -63,14 +64,6 @@ typedef uint8_t ble_l2cap_chan_flags; typedef int ble_l2cap_rx_fn(uint16_t conn_handle, struct os_mbuf **rxom); -#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 -struct ble_l2cap_coc_endpoint { - uint16_t mtu; - uint16_t credits; - struct os_mbuf *sdu; -}; -#endif - struct ble_l2cap_chan { SLIST_ENTRY(ble_l2cap_chan) next; uint16_t scid; http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/src/ble_l2cap_sig.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_sig.c b/net/nimble/host/src/ble_l2cap_sig.c index 09df2c3..fe9362f 100644 --- a/net/nimble/host/src/ble_l2cap_sig.c +++ b/net/nimble/host/src/ble_l2cap_sig.c @@ -55,7 +55,8 @@ #define BLE_L2CAP_SIG_UNRESPONSIVE_TIMEOUT 30000 /* Milliseconds. */ #define BLE_L2CAP_SIG_PROC_OP_UPDATE 0 -#define BLE_L2CAP_SIG_PROC_OP_MAX 1 +#define BLE_L2CAP_SIG_PROC_OP_CONNECT 1 +#define BLE_L2CAP_SIG_PROC_OP_MAX 2 struct ble_l2cap_sig_proc { STAILQ_ENTRY(ble_l2cap_sig_proc) next; @@ -70,6 +71,9 @@ struct ble_l2cap_sig_proc { ble_l2cap_sig_update_fn *cb; void *cb_arg; } update; + struct { + struct ble_l2cap_chan *chan; + } connect; }; }; @@ -85,6 +89,14 @@ 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; +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +static ble_l2cap_sig_rx_fn ble_l2cap_sig_coc_req_rx; +static ble_l2cap_sig_rx_fn ble_l2cap_sig_coc_rsp_rx; +#else +#define ble_l2cap_sig_coc_req_rx ble_l2cap_sig_rx_noop +#define ble_l2cap_sig_coc_rsp_rx ble_l2cap_sig_rx_noop +#endif + static ble_l2cap_sig_rx_fn * const ble_l2cap_sig_dispatch[] = { [BLE_L2CAP_SIG_OP_REJECT] = ble_l2cap_sig_rx_noop, [BLE_L2CAP_SIG_OP_CONNECT_RSP] = ble_l2cap_sig_rx_noop, @@ -97,7 +109,8 @@ static ble_l2cap_sig_rx_fn * const ble_l2cap_sig_dispatch[] = { [BLE_L2CAP_SIG_OP_MOVE_CHAN_CONF_RSP] = ble_l2cap_sig_rx_noop, [BLE_L2CAP_SIG_OP_UPDATE_REQ] = ble_l2cap_sig_update_req_rx, [BLE_L2CAP_SIG_OP_UPDATE_RSP] = ble_l2cap_sig_update_rsp_rx, - [BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP] = ble_l2cap_sig_rx_noop, + [BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ] = ble_l2cap_sig_coc_req_rx, + [BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP] = ble_l2cap_sig_coc_rsp_rx, [BLE_L2CAP_SIG_OP_FLOW_CTRL_CREDIT] = ble_l2cap_sig_rx_noop, }; @@ -471,6 +484,343 @@ done: return rc; } +/***************************************************************************** + * $connect * + *****************************************************************************/ + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 + +static int +ble_l2cap_sig_coc_err2ble_hs_err(uint16_t l2cap_coc_err) +{ + switch (l2cap_coc_err) { + case BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS: + return 0; + case BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM: + return BLE_HS_ENOTSUP; + case BLE_L2CAP_COC_ERR_NO_RESOURCES: + return BLE_HS_ENOMEM; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN: + return BLE_HS_EAUTHEN; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR: + return BLE_HS_EAUTHOR; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ: + return BLE_HS_EENCRYPT_KEY_SZ; + case BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC: + return BLE_HS_EENCRYPT; + case BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID: + return BLE_HS_EREJECT; + case BLE_L2CAP_COC_ERR_SOURCE_CID_ALREADY_USED: + return BLE_HS_EALREADY; + case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS: + return BLE_HS_EINVAL; + default: + return BLE_HS_EUNKNOWN; + } +} + +static int +ble_l2cap_sig_ble_hs_err2coc_err(uint16_t ble_hs_err) +{ + switch (ble_hs_err) { + case BLE_HS_ENOMEM: + return BLE_L2CAP_COC_ERR_NO_RESOURCES; + case BLE_HS_EAUTHEN: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN; + case BLE_HS_EAUTHOR: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR; + case BLE_HS_EENCRYPT: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC; + case BLE_HS_EENCRYPT_KEY_SZ: + return BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ; + case BLE_HS_EINVAL: + return BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS; + default: + return BLE_L2CAP_COC_ERR_NO_RESOURCES; + } +} + +static void +ble_l2cap_event_coc_connected(struct ble_l2cap_chan *chan, uint16_t status) +{ + struct ble_l2cap_event event = { }; + + event.type = BLE_L2CAP_EVENT_COC_CONNECTED; + event.connect.conn_handle = chan->conn_handle; + event.connect.chan = chan; + event.connect.status = status; + + chan->cb(&event, chan->cb_arg); +} + +static int +ble_l2cap_event_coc_accept(struct ble_l2cap_chan *chan, uint16_t peer_sdu_size) +{ + struct ble_l2cap_event event = { }; + + event.type = BLE_L2CAP_EVENT_COC_ACCEPT; + event.accept.chan = chan; + event.accept.conn_handle = chan->conn_handle; + event.accept.peer_sdu_size = peer_sdu_size; + + return chan->cb(&event, chan->cb_arg); +} + +static void +ble_l2cap_sig_coc_connect_cb(struct ble_l2cap_sig_proc *proc, int status) +{ + struct ble_l2cap_chan *chan; + + if (!proc) { + return; + } + + chan = proc->connect.chan; + if (!chan || !chan->cb) { + return; + } + + ble_l2cap_event_coc_connected(chan, status); +} + +static int +ble_l2cap_sig_coc_req_rx(uint16_t conn_handle, struct ble_l2cap_sig_hdr *hdr, + struct os_mbuf **om) +{ + int rc; + struct ble_l2cap_sig_le_con_req *req; + struct os_mbuf *txom; + struct ble_l2cap_sig_le_con_rsp *rsp; + struct ble_l2cap_coc_srv *srv; + struct ble_l2cap_chan *chan; + struct ble_hs_conn *conn; + uint16_t scid; + + rc = ble_hs_mbuf_pullup_base(om, sizeof(req)); + if (rc != 0) { + return rc; + } + + rsp = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_RSP, + hdr->identifier, sizeof(*rsp), &txom); + if (!rsp) { + /* Well, nothing smart we can do if there is no memory for response. + * Remote will timeout. + */ + return 0; + } + + req = (struct ble_l2cap_sig_le_con_req *)(*om)->om_data; + + ble_hs_lock(); + conn = ble_hs_conn_find_assert(conn_handle); + + /* Check if there is server registered on this PSM */ + srv = ble_l2cap_coc_srv_find(le16toh(req->psm)); + if (!srv) { + rsp->result = htole16(BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM); + goto failed; + } + + /* Verify CID */ + scid = le16toh(req->scid); + if (scid < BLE_L2CAP_COC_CID_START || scid > BLE_L2CAP_COC_CID_END) { + /*FIXME: Check if SCID is not already used */ + rsp->result = htole16(BLE_L2CAP_COC_ERR_INVALID_SOURCE_CID); + goto failed; + } + + chan = ble_l2cap_chan_alloc(); + if (!chan) { + rsp->result = htole16(BLE_L2CAP_COC_ERR_NO_RESOURCES); + goto failed; + } + + chan->cb = srv->cb; + chan->cb_arg = srv->cb_arg; + chan->conn_handle = conn_handle; + chan->dcid = scid; + chan->my_mtu = BLE_L2CAP_COC_MTU; + + /* Fill up remote configuration. Note MPS is the L2CAP MTU*/ + chan->peer_mtu = le16toh(req->mps); + chan->coc_tx.credits = le16toh(req->credits); + chan->coc_tx.mtu = le16toh(req->mtu); + + rc = ble_l2cap_event_coc_accept(chan, le16toh(req->mtu)); + if (rc != 0) { + uint16_t coc_err = ble_l2cap_sig_ble_hs_err2coc_err(rc); + ble_l2cap_chan_free(chan); + rsp->result = htole16(coc_err); + goto failed; + } + + chan->scid = ble_l2cap_coc_get_cid(); + chan->coc_rx.mtu = srv->mtu; + chan->coc_rx.credits = 10; //FIXME Calculate it + + rsp->dcid = htole16(chan->scid); + rsp->credits = htole16(chan->coc_rx.credits); + rsp->mps = htole16(chan->my_mtu); + rsp->mtu = htole16(chan->coc_rx.mtu); + rsp->result = htole16(BLE_L2CAP_COC_ERR_CONNECTION_SUCCESS); + + rc = ble_l2cap_sig_tx(conn_handle, txom); + if (rc == 0) { + /* Response sent out with a success. We are connected now*/ + ble_hs_lock(); + ble_hs_conn_chan_insert(conn, chan); + ble_hs_unlock(); + } else { + ble_l2cap_chan_free(chan); + } + + /* Notify user about connection status */ + ble_l2cap_event_coc_connected(chan, rc); + ble_hs_unlock(); + + return 0; + +failed: + ble_hs_unlock(); + ble_l2cap_sig_tx(conn_handle, txom); + return 0; +} + +static int +ble_l2cap_sig_coc_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_le_con_rsp *rsp; + struct ble_l2cap_chan *chan; + struct ble_hs_conn *conn; + int rc; + + BLE_HS_LOG(DEBUG, "L2CAP LE COC connection response received\n"); + + 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, sizeof(*rsp)); + if (rc != 0) { + goto done; + } + + rsp = (struct ble_l2cap_sig_le_con_rsp *)(*om)->om_data; + + chan = proc->connect.chan; + + if (rsp->result) { + rc = ble_l2cap_sig_coc_err2ble_hs_err(le16toh(rsp->result)); + goto done; + } + + /* Fill up remote configuration + * Note MPS is the L2CAP MTU + */ + chan->peer_mtu = le16toh(rsp->mps); + chan->dcid = le16toh(rsp->dcid); + chan->coc_tx.mtu = le16toh(rsp->mtu); + chan->coc_tx.credits = le16toh(rsp->credits); + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + assert(conn != NULL); + ble_hs_conn_chan_insert(conn, chan); + ble_hs_unlock(); + + rc = 0; + +done: + ble_l2cap_sig_coc_connect_cb(proc, rc); + ble_l2cap_sig_proc_free(proc); + + /* Silently ignore errors as this is response signal */ + return 0; +} + +int +ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, + struct os_mbuf *sdu_rx, + ble_l2cap_event_fn *cb, void *cb_arg) +{ + struct ble_hs_conn *conn; + struct ble_l2cap_sig_proc *proc; + struct os_mbuf *txom; + struct ble_l2cap_sig_le_con_req *req; + struct ble_l2cap_chan *chan; + int rc; + + if (!sdu_rx || !cb) { + return BLE_HS_EINVAL; + } + + ble_hs_lock(); + conn = ble_hs_conn_find(conn_handle); + + if (!conn) { + ble_hs_unlock(); + return BLE_HS_ENOTCONN; + } + + chan = ble_l2cap_chan_alloc(); + if (!chan) { + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } + + proc = ble_l2cap_sig_proc_alloc(); + if (!proc) { + ble_l2cap_chan_free(chan); + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } + + chan->scid = ble_l2cap_coc_get_cid(); + chan->my_mtu = BLE_L2CAP_COC_MTU; + chan->coc_rx.credits = 10; + chan->coc_rx.mtu = mtu; + chan->coc_rx.sdu = sdu_rx; + chan->cb = cb; + chan->cb_arg = cb_arg; + chan->conn_handle = conn_handle; + + proc->op = BLE_L2CAP_SIG_PROC_OP_CONNECT; + proc->id = ble_l2cap_sig_next_id(); + proc->conn_handle = conn_handle; + proc->connect.chan = chan; + + req = ble_l2cap_sig_cmd_get(BLE_L2CAP_SIG_OP_CREDIT_CONNECT_REQ, proc->id, + sizeof(*req), &txom); + if (!req) { + ble_l2cap_chan_free(chan); + ble_hs_unlock(); + return BLE_HS_ENOMEM; + } + + req->psm = htole16(psm); + req->scid = htole16(chan->scid); + req->mtu = htole16(chan->coc_rx.mtu); + req->mps = htole16(chan->my_mtu); + req->credits = htole16(chan->coc_rx.credits); + + rc = ble_l2cap_sig_tx(proc->conn_handle, txom); + if (rc != 0) { + ble_l2cap_chan_free(chan); + } + + ble_l2cap_sig_process_status(proc, rc); + ble_hs_unlock(); + + return rc; +} +#endif + static int ble_l2cap_sig_rx(uint16_t conn_handle, struct os_mbuf **om) { @@ -620,7 +970,16 @@ ble_l2cap_sig_timer(void) /* Report a failure for each timed out procedure. */ while ((proc = STAILQ_FIRST(&temp_list)) != NULL) { STATS_INC(ble_l2cap_stats, proc_timeout); - ble_l2cap_sig_update_call_cb(proc, BLE_HS_ETIMEOUT); + switch(proc->op) { + case BLE_L2CAP_SIG_PROC_OP_UPDATE: + ble_l2cap_sig_update_call_cb(proc, BLE_HS_ETIMEOUT); + break; +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 + case BLE_L2CAP_SIG_PROC_OP_CONNECT: + ble_l2cap_sig_coc_connect_cb(proc, BLE_HS_ETIMEOUT); + break; +#endif + } STAILQ_REMOVE_HEAD(&temp_list, next); ble_l2cap_sig_proc_free(proc); http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/src/ble_l2cap_sig_cmd.c ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_sig_cmd.c b/net/nimble/host/src/ble_l2cap_sig_cmd.c index 6b58f92..f7e68c8 100644 --- a/net/nimble/host/src/ble_l2cap_sig_cmd.c +++ b/net/nimble/host/src/ble_l2cap_sig_cmd.c @@ -54,7 +54,7 @@ ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len, return 0; } -static int +int ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom) { struct ble_l2cap_chan *chan; @@ -249,3 +249,30 @@ ble_l2cap_sig_update_rsp_tx(uint16_t conn_handle, uint8_t id, uint16_t result) return ble_l2cap_sig_tx(conn_handle, txom); } + +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +void * +ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len, + struct os_mbuf **txom) +{ + struct ble_l2cap_sig_hdr *hdr; + + *txom = ble_hs_mbuf_l2cap_pkt(); + if (*txom == NULL) { + return NULL; + } + + if (os_mbuf_extend(*txom, sizeof(*hdr) + len) == NULL) { + os_mbuf_free_chain(*txom); + return NULL; + } + + hdr = (struct ble_l2cap_sig_hdr *)(*txom)->om_data; + + hdr->op = opcode; + hdr->identifier = id; + hdr->length = htole16(len); + + return hdr->data; +} +#endif http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/696e79d3/net/nimble/host/src/ble_l2cap_sig_priv.h ---------------------------------------------------------------------- diff --git a/net/nimble/host/src/ble_l2cap_sig_priv.h b/net/nimble/host/src/ble_l2cap_sig_priv.h index 72819de..56f1af0 100644 --- a/net/nimble/host/src/ble_l2cap_sig_priv.h +++ b/net/nimble/host/src/ble_l2cap_sig_priv.h @@ -20,6 +20,8 @@ #ifndef H_BLE_L2CAP_SIG_ #define H_BLE_L2CAP_SIG_ +#include "syscfg/syscfg.h" + #ifdef __cplusplus extern "C" { #endif @@ -31,6 +33,7 @@ struct ble_l2cap_sig_hdr { uint8_t op; uint8_t identifier; uint16_t length; + uint8_t data[0]; } __attribute__((packed)); #define BLE_L2CAP_SIG_REJECT_MIN_SZ 2 @@ -54,6 +57,22 @@ struct ble_l2cap_sig_update_rsp { #define BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT 0x0000 #define BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT 0x0001 +struct ble_l2cap_sig_le_con_req { + uint16_t psm; + uint16_t scid; + uint16_t mtu; + uint16_t mps; + uint16_t credits; +} __attribute__((packed)); + +struct ble_l2cap_sig_le_con_rsp { + uint16_t dcid; + uint16_t mtu; + uint16_t mps; + uint16_t credits; + uint16_t result; +} __attribute__((packed)); + int ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len, struct os_mbuf **out_om, void **out_payload_buf); void ble_l2cap_sig_hdr_parse(void *payload, uint16_t len, @@ -75,9 +94,19 @@ void ble_l2cap_sig_update_rsp_write(void *payload, int len, struct ble_l2cap_sig_update_rsp *src); int ble_l2cap_sig_update_rsp_tx(uint16_t conn_handle, uint8_t id, uint16_t result); - int ble_l2cap_sig_reject_invalid_cid_tx(uint16_t conn_handle, uint8_t id, uint16_t src_cid, uint16_t dst_cid); +int ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom); +#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0 +int ble_l2cap_sig_coc_connect(uint16_t conn_handle, uint16_t psm, uint16_t mtu, + struct os_mbuf *sdu_rx, + ble_l2cap_event_fn *cb, void *cb_arg); +void *ble_l2cap_sig_cmd_get(uint8_t opcode, uint8_t id, uint16_t len, + struct os_mbuf **txom); +#else +#define ble_l2cap_sig_coc_connect(conn_handle, psm, mtu, sdu_rx, cb, cb_arg) \ + BLE_HS_ENOTSUP +#endif void ble_l2cap_sig_conn_broken(uint16_t conn_handle, int reason); int32_t ble_l2cap_sig_timer(void);
