1 /*
2 * Licensed to the Apache Software Foundation (ASF) under one
3 * or more contributor license agreements. See the NOTICE file
4 * distributed with this work for additional information
5 * regarding copyright ownership. The ASF licenses this file
6 * to you under the Apache License, Version 2.0 (the
7 * "License"); you may not use this file except in compliance
8 * with the License. You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing,
13 * software distributed under the License is distributed on an
14 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
15 * KIND, either express or implied. See the License for the
16 * specific language governing permissions and limitations
17 * under the License.
18 */
19
20 #include "securec.h"
21 #include "nimble/ble.h"
22 #include "ble_hs_priv.h"
23
24 #if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM) != 0
25
26 #define BLE_L2CAP_SDU_SIZE 2
27
28 STAILQ_HEAD(ble_l2cap_coc_srv_list, ble_l2cap_coc_srv);
29
30 static struct ble_l2cap_coc_srv_list ble_l2cap_coc_srvs;
31
32 static os_membuf_t ble_l2cap_coc_srv_mem[
33 OS_MEMPOOL_SIZE(MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
34 sizeof(struct ble_l2cap_coc_srv))
35 ];
36
37 static struct os_mempool ble_l2cap_coc_srv_pool;
38
ble_l2cap_coc_dbg_assert_srv_not_inserted(struct ble_l2cap_coc_srv * srv)39 static void ble_l2cap_coc_dbg_assert_srv_not_inserted(struct ble_l2cap_coc_srv *srv)
40 {
41 #if MYNEWT_VAL(BLE_HS_DEBUG)
42 struct ble_l2cap_coc_srv *cur;
43 STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) {
44 BLE_HS_DBG_ASSERT(cur != srv);
45 }
46 #endif
47 }
48
ble_l2cap_coc_srv_alloc(void)49 static struct ble_l2cap_coc_srv *ble_l2cap_coc_srv_alloc(void)
50 {
51 struct ble_l2cap_coc_srv *srv;
52 srv = os_memblock_get(&ble_l2cap_coc_srv_pool);
53 if (srv != NULL) {
54 memset_s(srv, sizeof(*srv), 0, sizeof(*srv));
55 }
56
57 return srv;
58 }
59
ble_l2cap_coc_create_server(uint16_t psm,uint16_t mtu,ble_l2cap_event_fn * cb,void * cb_arg)60 int ble_l2cap_coc_create_server(uint16_t psm, uint16_t mtu,
61 ble_l2cap_event_fn *cb, void *cb_arg)
62 {
63 struct ble_l2cap_coc_srv *srv;
64 srv = ble_l2cap_coc_srv_alloc();
65 if (!srv) {
66 return BLE_HS_ENOMEM;
67 }
68
69 srv->psm = psm;
70 srv->mtu = mtu;
71 srv->cb = cb;
72 srv->cb_arg = cb_arg;
73 ble_l2cap_coc_dbg_assert_srv_not_inserted(srv);
74 STAILQ_INSERT_HEAD(&ble_l2cap_coc_srvs, srv, next);
75 return 0;
76 }
77
ble_l2cap_set_used_cid(uint32_t * cid_mask,int bit)78 static inline void ble_l2cap_set_used_cid(uint32_t *cid_mask, int bit)
79 {
80 cid_mask[bit / 32] |= (1 << (bit % 32)); // 32:byte alignment
81 }
82
ble_l2cap_clear_used_cid(uint32_t * cid_mask,int bit)83 static inline void ble_l2cap_clear_used_cid(uint32_t *cid_mask, int bit)
84 {
85 cid_mask[bit / 32] &= ~(1 << (bit % 32)); // 32:byte alignment
86 }
87
ble_l2cap_get_first_available_bit(uint32_t * cid_mask)88 static inline int ble_l2cap_get_first_available_bit(uint32_t *cid_mask)
89 {
90 int i;
91 int bit = 0;
92
93 for (i = 0; i < BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN; i++) {
94 /* Find first available index by finding first available bit
95 * in the mask.
96 * Note:
97 * a) If bit == 0 means all the bits are used
98 * b) this function returns 1 + index
99 */
100 bit = __builtin_ffs(~(unsigned int)(cid_mask[i]));
101 if (bit != 0) {
102 break;
103 }
104 }
105
106 if (i == BLE_HS_CONN_L2CAP_COC_CID_MASK_LEN) {
107 return -1;
108 }
109
110 return (i * 32 + bit - 1); // 32:byte alignment
111 }
112
ble_l2cap_coc_get_cid(uint32_t * cid_mask)113 static int ble_l2cap_coc_get_cid(uint32_t *cid_mask)
114 {
115 int bit;
116 bit = ble_l2cap_get_first_available_bit(cid_mask);
117 if (bit < 0) {
118 return -1;
119 }
120
121 ble_l2cap_set_used_cid(cid_mask, bit);
122 return BLE_L2CAP_COC_CID_START + bit;
123 }
124
ble_l2cap_coc_srv_find(uint16_t psm)125 static struct ble_l2cap_coc_srv *ble_l2cap_coc_srv_find(uint16_t psm)
126 {
127 struct ble_l2cap_coc_srv *cur, *srv;
128 srv = NULL;
129 STAILQ_FOREACH(cur, &ble_l2cap_coc_srvs, next) {
130 if (cur->psm == psm) {
131 srv = cur;
132 break;
133 }
134 }
135 return srv;
136 }
137
ble_l2cap_event_coc_received_data(struct ble_l2cap_chan * chan,struct os_mbuf * om)138 static void ble_l2cap_event_coc_received_data(struct ble_l2cap_chan *chan,
139 struct os_mbuf *om)
140 {
141 struct ble_l2cap_event event;
142 event.type = BLE_L2CAP_EVENT_COC_DATA_RECEIVED;
143 event.receive.conn_handle = chan->conn_handle;
144 event.receive.chan = chan;
145 event.receive.sdu_rx = om;
146 chan->cb(&event, chan->cb_arg);
147 }
148
ble_l2cap_coc_rx_fn(struct ble_l2cap_chan * chan)149 static int ble_l2cap_coc_rx_fn(struct ble_l2cap_chan *chan)
150 {
151 int rc;
152 struct os_mbuf **om;
153 struct ble_l2cap_coc_endpoint *rx;
154 uint16_t om_total;
155 /* Create a shortcut to rx_buf */
156 om = &chan->rx_buf;
157 BLE_HS_DBG_ASSERT(*om != NULL);
158 /* Create a shortcut to rx endpoint */
159 rx = &chan->coc_rx;
160 BLE_HS_DBG_ASSERT(rx != NULL);
161 om_total = OS_MBUF_PKTLEN(*om);
162
163 /* First LE frame */
164 if (OS_MBUF_PKTLEN(rx->sdu) == 0) {
165 uint16_t sdu_len;
166 rc = ble_hs_mbuf_pullup_base(om, BLE_L2CAP_SDU_SIZE);
167 if (rc != 0) {
168 return rc;
169 }
170
171 sdu_len = get_le16((*om)->om_data);
172 if (sdu_len > rx->mtu) {
173 BLE_HS_LOG(INFO, "error: sdu_len > rx->mtu (%d>%d)\n",
174 sdu_len, rx->mtu);
175 /* Disconnect peer with invalid behaviour */
176 ble_l2cap_disconnect(chan);
177 return BLE_HS_EBADDATA;
178 }
179
180 BLE_HS_LOG(DEBUG, "sdu_len=%d, received LE frame=%d, credits=%d\n",
181 sdu_len, om_total, rx->credits);
182 os_mbuf_adj(*om, BLE_L2CAP_SDU_SIZE);
183 rc = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total - BLE_L2CAP_SDU_SIZE);
184 if (rc != 0) {
185 /* User shall give us big enough buffer.
186 * need to handle it better
187 */
188 BLE_HS_LOG(INFO, "Could not append data rc=%d\n", rc);
189 assert(0);
190 }
191
192 /* In RX case data_offset keeps incoming SDU len */
193 rx->data_offset = sdu_len;
194 } else {
195 BLE_HS_LOG(DEBUG, "Continuation...received %d\n", (*om)->om_len);
196 rc = os_mbuf_appendfrom(rx->sdu, *om, 0, om_total);
197 if (rc != 0) {
198 /* need to handle it better */
199 BLE_HS_LOG(DEBUG, "Could not append data rc=%d\n", rc);
200 assert(0);
201 }
202 }
203
204 rx->credits--;
205
206 if (OS_MBUF_PKTLEN(rx->sdu) == rx->data_offset) {
207 struct os_mbuf *sdu_rx = rx->sdu;
208 BLE_HS_LOG(DEBUG, "Received sdu_len=%d, credits left=%d\n",
209 OS_MBUF_PKTLEN(rx->sdu), rx->credits);
210 /* Lets get back control to os_mbuf to application.
211 * Since it this callback application might want to set new sdu
212 * we need to prepare space for this. Therefore we need sdu_rx
213 */
214 rx->sdu = NULL;
215 rx->data_offset = 0;
216 ble_l2cap_event_coc_received_data(chan, sdu_rx);
217 return 0;
218 }
219
220 /* If we did not received full SDU and credits are 0 it means
221 * that remote was sending us not fully filled up LE frames.
222 * However, we still have buffer to for next LE Frame so lets give one more
223 * credit to peer so it can send us full SDU
224 */
225 if (rx->credits == 0) {
226 /* Remote did not send full SDU. Lets give him one more credits to do
227 * so since we have still buffer to handle it
228 */
229 rx->credits = 1;
230 ble_l2cap_sig_le_credits(chan->conn_handle, chan->scid, rx->credits);
231 }
232
233 BLE_HS_LOG(DEBUG, "Received partial sdu_len=%d, credits left=%d\n",
234 OS_MBUF_PKTLEN(rx->sdu), rx->credits);
235 return 0;
236 }
237
ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan * chan,uint16_t mtu,uint16_t mps)238 void ble_l2cap_coc_set_new_mtu_mps(struct ble_l2cap_chan *chan, uint16_t mtu, uint16_t mps)
239 {
240 chan->my_coc_mps = mps;
241 chan->coc_rx.mtu = mtu;
242 chan->initial_credits = mtu / chan->my_coc_mps;
243
244 if (mtu % chan->my_coc_mps) {
245 chan->initial_credits++;
246 }
247 }
248
ble_l2cap_coc_chan_alloc(struct ble_hs_conn * conn,uint16_t psm,uint16_t mtu,struct os_mbuf * sdu_rx,ble_l2cap_event_fn * cb,void * cb_arg)249 struct ble_l2cap_chan *ble_l2cap_coc_chan_alloc(struct ble_hs_conn *conn, uint16_t psm, uint16_t mtu,
250 struct os_mbuf *sdu_rx, ble_l2cap_event_fn *cb,
251 void *cb_arg)
252 {
253 struct ble_l2cap_chan *chan;
254 chan = ble_l2cap_chan_alloc(conn->bhc_handle);
255 if (!chan) {
256 return NULL;
257 }
258
259 chan->psm = psm;
260 chan->cb = cb;
261 chan->cb_arg = cb_arg;
262 chan->scid = ble_l2cap_coc_get_cid(conn->l2cap_coc_cid_mask);
263 chan->my_coc_mps = MYNEWT_VAL(BLE_L2CAP_COC_MPS);
264 chan->rx_fn = ble_l2cap_coc_rx_fn;
265 chan->coc_rx.mtu = mtu;
266 chan->coc_rx.sdu = sdu_rx;
267 /* Number of credits should allow to send full SDU with on given
268 * L2CAP MTU
269 */
270 chan->coc_rx.credits = mtu / chan->my_coc_mps;
271
272 if (mtu % chan->my_coc_mps) {
273 chan->coc_rx.credits++;
274 }
275
276 chan->initial_credits = chan->coc_rx.credits;
277 return chan;
278 }
279
ble_l2cap_coc_create_srv_chan(struct ble_hs_conn * conn,uint16_t psm,struct ble_l2cap_chan ** chan)280 int ble_l2cap_coc_create_srv_chan(struct ble_hs_conn *conn, uint16_t psm,
281 struct ble_l2cap_chan **chan)
282 {
283 struct ble_l2cap_coc_srv *srv;
284 /* Check if there is server registered on this PSM */
285 srv = ble_l2cap_coc_srv_find(psm);
286 if (!srv) {
287 return BLE_HS_ENOTSUP;
288 }
289
290 *chan = ble_l2cap_coc_chan_alloc(conn, psm, srv->mtu, NULL, srv->cb,
291 srv->cb_arg);
292 if (!*chan) {
293 return BLE_HS_ENOMEM;
294 }
295
296 return 0;
297 }
298
ble_l2cap_event_coc_disconnected(struct ble_l2cap_chan * chan)299 static void ble_l2cap_event_coc_disconnected(struct ble_l2cap_chan *chan)
300 {
301 struct ble_l2cap_event event = { };
302 if (!chan->cb) {
303 return;
304 }
305
306 event.type = BLE_L2CAP_EVENT_COC_DISCONNECTED;
307 event.disconnect.conn_handle = chan->conn_handle;
308 event.disconnect.chan = chan;
309 chan->cb(&event, chan->cb_arg);
310 }
311
ble_l2cap_coc_cleanup_chan(struct ble_hs_conn * conn,struct ble_l2cap_chan * chan)312 void ble_l2cap_coc_cleanup_chan(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
313 {
314 /* PSM 0 is used for fixed channels. */
315 if (chan->psm == 0) {
316 return;
317 }
318
319 ble_l2cap_event_coc_disconnected(chan);
320
321 if (conn && chan->scid) {
322 ble_l2cap_clear_used_cid(conn->l2cap_coc_cid_mask,
323 chan->scid - BLE_L2CAP_COC_CID_START);
324 }
325
326 os_mbuf_free_chain(chan->coc_rx.sdu);
327 os_mbuf_free_chain(chan->coc_tx.sdu);
328 }
329
ble_l2cap_event_coc_unstalled(struct ble_l2cap_chan * chan,int status)330 static void ble_l2cap_event_coc_unstalled(struct ble_l2cap_chan *chan, int status)
331 {
332 struct ble_l2cap_event event = { };
333
334 if (!chan->cb) {
335 return;
336 }
337
338 event.type = BLE_L2CAP_EVENT_COC_TX_UNSTALLED;
339 event.tx_unstalled.conn_handle = chan->conn_handle;
340 event.tx_unstalled.chan = chan;
341 event.tx_unstalled.status = status;
342 chan->cb(&event, chan->cb_arg);
343 }
344
ble_l2cap_coc_continue_tx(struct ble_l2cap_chan * chan)345 static int ble_l2cap_coc_continue_tx(struct ble_l2cap_chan *chan)
346 {
347 struct ble_l2cap_coc_endpoint *tx;
348 uint16_t len;
349 uint16_t left_to_send;
350 struct os_mbuf *txom;
351 struct ble_hs_conn *conn;
352 uint16_t sdu_size_offset;
353 int rc;
354 /* If there is no data to send, just return success */
355 tx = &chan->coc_tx;
356 if (!tx->sdu) {
357 return 0;
358 }
359
360 while (tx->credits) {
361 sdu_size_offset = 0;
362 BLE_HS_LOG(DEBUG, "Available credits %d\n", tx->credits);
363 /* lets calculate data we are going to send */
364 left_to_send = OS_MBUF_PKTLEN(tx->sdu) - tx->data_offset;
365
366 if (tx->data_offset == 0) {
367 sdu_size_offset = BLE_L2CAP_SDU_SIZE;
368 left_to_send += sdu_size_offset;
369 }
370
371 /* Take into account peer MTU */
372 len = min(left_to_send, chan->peer_coc_mps);
373 /* Prepare packet */
374 txom = ble_hs_mbuf_l2cap_pkt();
375 if (!txom) {
376 BLE_HS_LOG(DEBUG, "Could not prepare l2cap packet len %d", len);
377 rc = BLE_HS_ENOMEM;
378 goto failed;
379 }
380
381 if (tx->data_offset == 0) {
382 /* First packet needs SDU len first. Left to send */
383 uint16_t l = htole16(OS_MBUF_PKTLEN(tx->sdu));
384 BLE_HS_LOG(DEBUG, "Sending SDU len=%d\n", OS_MBUF_PKTLEN(tx->sdu));
385 rc = os_mbuf_append(txom, &l, sizeof(uint16_t));
386 if (rc) {
387 BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc);
388 goto failed;
389 }
390 }
391
392 /* In data_offset we keep track on what we already sent. Need to remember
393 * that for first packet we need to decrease data size by 2 bytes for sdu
394 * size
395 */
396 rc = os_mbuf_appendfrom(txom, tx->sdu, tx->data_offset,
397 len - sdu_size_offset);
398 if (rc) {
399 BLE_HS_LOG(DEBUG, "Could not append data rc=%d", rc);
400 goto failed;
401 }
402
403 ble_hs_lock();
404 conn = ble_hs_conn_find_assert(chan->conn_handle);
405 rc = ble_l2cap_tx(conn, chan, txom);
406 ble_hs_unlock();
407
408 if (rc) {
409 /* txom is consumed by l2cap */
410 txom = NULL;
411 goto failed;
412 } else {
413 tx->credits --;
414 tx->data_offset += len - sdu_size_offset;
415 }
416
417 BLE_HS_LOG(DEBUG, "Sent %d bytes, credits=%d, to send %d bytes \n",
418 len, tx->credits, OS_MBUF_PKTLEN(tx->sdu) - tx->data_offset);
419
420 if (tx->data_offset == OS_MBUF_PKTLEN(tx->sdu)) {
421 BLE_HS_LOG(DEBUG, "Complete package sent\n");
422 os_mbuf_free_chain(tx->sdu);
423 tx->sdu = 0;
424 tx->data_offset = 0;
425
426 if (tx->flags & BLE_L2CAP_COC_FLAG_STALLED) {
427 ble_l2cap_event_coc_unstalled(chan, 0);
428 tx->flags &= ~BLE_L2CAP_COC_FLAG_STALLED;
429 }
430
431 break;
432 }
433 }
434
435 if (tx->sdu) {
436 /* Not complete SDU sent, wait for credits */
437 tx->flags |= BLE_L2CAP_COC_FLAG_STALLED;
438 return BLE_HS_ESTALLED;
439 }
440
441 return 0;
442 failed:
443 os_mbuf_free_chain(tx->sdu);
444 tx->sdu = NULL;
445 os_mbuf_free_chain(txom);
446
447 if (tx->flags & BLE_L2CAP_COC_FLAG_STALLED) {
448 ble_l2cap_event_coc_unstalled(chan, rc);
449 tx->flags &= ~BLE_L2CAP_COC_FLAG_STALLED;
450 }
451
452 return rc;
453 }
454
ble_l2cap_coc_le_credits_update(uint16_t conn_handle,uint16_t dcid,uint16_t credits)455 void ble_l2cap_coc_le_credits_update(uint16_t conn_handle, uint16_t dcid, uint16_t credits)
456 {
457 struct ble_hs_conn *conn;
458 struct ble_l2cap_chan *chan;
459 /* remote updated its credits */
460 ble_hs_lock();
461 conn = ble_hs_conn_find(conn_handle);
462 if (!conn) {
463 ble_hs_unlock();
464 return;
465 }
466
467 chan = ble_hs_conn_chan_find_by_dcid(conn, dcid);
468 if (!chan) {
469 ble_hs_unlock();
470 return;
471 }
472
473 if (chan->coc_tx.credits + credits > 0xFFFF) {
474 BLE_HS_LOG(INFO, "LE CoC credits overflow...disconnecting\n");
475 ble_hs_unlock();
476 ble_l2cap_sig_disconnect(chan);
477 return;
478 }
479
480 chan->coc_tx.credits += credits;
481 ble_hs_unlock();
482 ble_l2cap_coc_continue_tx(chan);
483 }
484
ble_l2cap_coc_recv_ready(struct ble_l2cap_chan * chan,struct os_mbuf * sdu_rx)485 int ble_l2cap_coc_recv_ready(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_rx)
486 {
487 struct ble_hs_conn *conn;
488 struct ble_l2cap_chan *c;
489
490 if (!sdu_rx) {
491 return BLE_HS_EINVAL;
492 }
493
494 chan->coc_rx.sdu = sdu_rx;
495 ble_hs_lock();
496 conn = ble_hs_conn_find_assert(chan->conn_handle);
497 c = ble_hs_conn_chan_find_by_scid(conn, chan->scid);
498 if (!c) {
499 ble_hs_unlock();
500 return BLE_HS_ENOENT;
501 }
502
503 /* We want to back only that much credits which remote side is missing
504 * to be able to send complete SDU.
505 */
506 if (chan->coc_rx.credits < c->initial_credits) {
507 ble_hs_unlock();
508 ble_l2cap_sig_le_credits(chan->conn_handle, chan->scid,
509 c->initial_credits - chan->coc_rx.credits);
510 ble_hs_lock();
511 chan->coc_rx.credits = c->initial_credits;
512 }
513
514 ble_hs_unlock();
515 return 0;
516 }
517
518 /**
519 * Transmits a packet over a connection-oriented channel. This function only
520 * consumes the supplied mbuf on success.
521 */
ble_l2cap_coc_send(struct ble_l2cap_chan * chan,struct os_mbuf * sdu_tx)522 int ble_l2cap_coc_send(struct ble_l2cap_chan *chan, struct os_mbuf *sdu_tx)
523 {
524 struct ble_l2cap_coc_endpoint *tx;
525 tx = &chan->coc_tx;
526
527 if (tx->sdu) {
528 return BLE_HS_EBUSY;
529 }
530
531 if (OS_MBUF_PKTLEN(sdu_tx) > tx->mtu) {
532 return BLE_HS_EBADDATA;
533 }
534
535 tx->sdu = sdu_tx;
536 return ble_l2cap_coc_continue_tx(chan);
537 }
538
ble_l2cap_coc_init(void)539 int ble_l2cap_coc_init(void)
540 {
541 STAILQ_INIT(&ble_l2cap_coc_srvs);
542 return os_mempool_init(&ble_l2cap_coc_srv_pool,
543 MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM),
544 sizeof(struct ble_l2cap_coc_srv),
545 ble_l2cap_coc_srv_mem,
546 "ble_l2cap_coc_srv_pool");
547 }
548
549 #endif