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