1 /******************************************************************************
2 *
3 * Copyright 2001-2012 Broadcom Corporation
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 /******************************************************************************
20 *
21 * This file contains BNEP utility functions
22 *
23 ******************************************************************************/
24
25 #include <bluetooth/log.h>
26 #include <string.h>
27
28 #include "bnep_int.h"
29 #include "hci/controller_interface.h"
30 #include "internal_include/bt_target.h"
31 #include "main/shim/entry.h"
32 #include "main/shim/helpers.h"
33 #include "os/log.h"
34 #include "osi/include/allocator.h"
35 #include "stack/include/bt_hdr.h"
36 #include "stack/include/bt_types.h"
37 #include "types/bluetooth/uuid.h"
38 #include "types/raw_address.h"
39
40 using namespace bluetooth;
41 using bluetooth::Uuid;
42
43 /******************************************************************************/
44 /* L O C A L F U N C T I O N P R O T O T Y P E S */
45 /******************************************************************************/
46 static uint8_t* bnepu_init_hdr(BT_HDR* p_buf, uint16_t hdr_len,
47 uint8_t pkt_type);
48
49 void bnepu_process_peer_multicast_filter_set(tBNEP_CONN* p_bcb,
50 uint8_t* p_filters, uint16_t len);
51 void bnepu_send_peer_multicast_filter_rsp(tBNEP_CONN* p_bcb,
52 uint16_t response_code);
53
54 /*******************************************************************************
55 *
56 * Function bnepu_find_bcb_by_cid
57 *
58 * Description This function searches the bcb table for an entry with the
59 * passed CID.
60 *
61 * Returns the BCB address, or NULL if not found.
62 *
63 ******************************************************************************/
bnepu_find_bcb_by_cid(uint16_t cid)64 tBNEP_CONN* bnepu_find_bcb_by_cid(uint16_t cid) {
65 uint16_t xx;
66 tBNEP_CONN* p_bcb;
67
68 /* Look through each connection control block */
69 for (xx = 0, p_bcb = bnep_cb.bcb; xx < BNEP_MAX_CONNECTIONS; xx++, p_bcb++) {
70 if ((p_bcb->con_state != BNEP_STATE_IDLE) && (p_bcb->l2cap_cid == cid))
71 return (p_bcb);
72 }
73
74 /* If here, not found */
75 return (NULL);
76 }
77
78 /*******************************************************************************
79 *
80 * Function bnepu_find_bcb_by_bd_addr
81 *
82 * Description This function searches the BCB table for an entry with the
83 * passed Bluetooth Address.
84 *
85 * Returns the BCB address, or NULL if not found.
86 *
87 ******************************************************************************/
bnepu_find_bcb_by_bd_addr(const RawAddress & p_bda)88 tBNEP_CONN* bnepu_find_bcb_by_bd_addr(const RawAddress& p_bda) {
89 uint16_t xx;
90 tBNEP_CONN* p_bcb;
91
92 /* Look through each connection control block */
93 for (xx = 0, p_bcb = bnep_cb.bcb; xx < BNEP_MAX_CONNECTIONS; xx++, p_bcb++) {
94 if (p_bcb->con_state != BNEP_STATE_IDLE) {
95 if (p_bcb->rem_bda == p_bda) return (p_bcb);
96 }
97 }
98
99 /* If here, not found */
100 return (NULL);
101 }
102
103 /*******************************************************************************
104 *
105 * Function bnepu_allocate_bcb
106 *
107 * Description This function allocates a new BCB.
108 *
109 * Returns BCB address, or NULL if none available.
110 *
111 ******************************************************************************/
bnepu_allocate_bcb(const RawAddress & p_rem_bda)112 tBNEP_CONN* bnepu_allocate_bcb(const RawAddress& p_rem_bda) {
113 uint16_t xx;
114 tBNEP_CONN* p_bcb;
115
116 /* Look through each connection control block for a free one */
117 for (xx = 0, p_bcb = bnep_cb.bcb; xx < BNEP_MAX_CONNECTIONS; xx++, p_bcb++) {
118 if (p_bcb->con_state == BNEP_STATE_IDLE) {
119 alarm_free(p_bcb->conn_timer);
120 memset((uint8_t*)p_bcb, 0, sizeof(tBNEP_CONN));
121 p_bcb->conn_timer = alarm_new("bnep.conn_timer");
122
123 p_bcb->rem_bda = p_rem_bda;
124 p_bcb->handle = xx + 1;
125 p_bcb->xmit_q = fixed_queue_new(SIZE_MAX);
126
127 return (p_bcb);
128 }
129 }
130
131 /* If here, no free BCB found */
132 return (NULL);
133 }
134
135 /*******************************************************************************
136 *
137 * Function bnepu_release_bcb
138 *
139 * Description This function releases a BCB.
140 *
141 * Returns void
142 *
143 ******************************************************************************/
bnepu_release_bcb(tBNEP_CONN * p_bcb)144 void bnepu_release_bcb(tBNEP_CONN* p_bcb) {
145 /* Ensure timer is stopped */
146 alarm_free(p_bcb->conn_timer);
147 p_bcb->conn_timer = NULL;
148
149 /* Drop any response pointer we may be holding */
150 p_bcb->con_state = BNEP_STATE_IDLE;
151 osi_free_and_reset((void**)&p_bcb->p_pending_data);
152
153 /* Free transmit queue */
154 while (!fixed_queue_is_empty(p_bcb->xmit_q)) {
155 osi_free(fixed_queue_try_dequeue(p_bcb->xmit_q));
156 }
157 fixed_queue_free(p_bcb->xmit_q, NULL);
158 p_bcb->xmit_q = NULL;
159 }
160
161 /*******************************************************************************
162 *
163 * Function bnep_send_conn_req
164 *
165 * Description This function sends a BNEP connection request to peer
166 *
167 * Returns void
168 *
169 ******************************************************************************/
bnep_send_conn_req(tBNEP_CONN * p_bcb)170 void bnep_send_conn_req(tBNEP_CONN* p_bcb) {
171 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
172 uint8_t *p, *p_start;
173
174 log::verbose("sending setup req with dst uuid {}",
175 p_bcb->dst_uuid.ToString());
176
177 p_buf->offset = L2CAP_MIN_OFFSET;
178 p = p_start = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
179
180 /* Put in BNEP frame type - filter control */
181 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
182
183 /* Put in filter message type - set filters */
184 UINT8_TO_BE_STREAM(p, BNEP_SETUP_CONNECTION_REQUEST_MSG);
185
186 int len = std::max(p_bcb->dst_uuid.GetShortestRepresentationSize(),
187 p_bcb->src_uuid.GetShortestRepresentationSize());
188
189 UINT8_TO_BE_STREAM(p, len);
190
191 if (len == Uuid::kNumBytes16) {
192 UINT16_TO_BE_STREAM(p, p_bcb->dst_uuid.As16Bit());
193 UINT16_TO_BE_STREAM(p, p_bcb->src_uuid.As16Bit());
194 } else if (len == Uuid::kNumBytes32) {
195 UINT32_TO_BE_STREAM(p, p_bcb->dst_uuid.As32Bit());
196 UINT32_TO_BE_STREAM(p, p_bcb->src_uuid.As32Bit());
197 } else if (len == Uuid::kNumBytes128) {
198 memcpy(p, p_bcb->dst_uuid.To128BitBE().data(), Uuid::kNumBytes128);
199 p += Uuid::kNumBytes128;
200 memcpy(p, p_bcb->src_uuid.To128BitBE().data(), Uuid::kNumBytes128);
201 p += Uuid::kNumBytes128;
202 } else {
203 log::error("uuid: {}, invalid length: {}", p_bcb->dst_uuid.ToString(),
204 p_bcb->dst_uuid.GetShortestRepresentationSize());
205 }
206
207 p_buf->len = (uint16_t)(p - p_start);
208
209 bnepu_check_send_packet(p_bcb, p_buf);
210 }
211
212 /*******************************************************************************
213 *
214 * Function bnep_send_conn_response
215 *
216 * Description This function sends a BNEP setup response to peer
217 *
218 * Returns void
219 *
220 ******************************************************************************/
bnep_send_conn_response(tBNEP_CONN * p_bcb,uint16_t resp_code)221 void bnep_send_conn_response(tBNEP_CONN* p_bcb, uint16_t resp_code) {
222 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
223 uint8_t* p;
224
225 log::debug("BNEP - bnep_send_conn_response for CID: 0x{:x}",
226 p_bcb->l2cap_cid);
227
228 p_buf->offset = L2CAP_MIN_OFFSET;
229 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
230
231 /* Put in BNEP frame type - filter control */
232 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
233
234 /* Put in filter message type - set filters */
235 UINT8_TO_BE_STREAM(p, BNEP_SETUP_CONNECTION_RESPONSE_MSG);
236
237 UINT16_TO_BE_STREAM(p, resp_code);
238
239 p_buf->len = 4;
240
241 bnepu_check_send_packet(p_bcb, p_buf);
242 }
243
244 /*******************************************************************************
245 *
246 * Function bnepu_send_peer_our_filters
247 *
248 * Description This function sends our filters to a peer
249 *
250 * Returns void
251 *
252 ******************************************************************************/
bnepu_send_peer_our_filters(tBNEP_CONN * p_bcb)253 void bnepu_send_peer_our_filters(tBNEP_CONN* p_bcb) {
254 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
255 uint8_t* p;
256 uint16_t xx;
257
258 log::verbose("BNEP sending peer our filters");
259
260 p_buf->offset = L2CAP_MIN_OFFSET;
261 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
262
263 /* Put in BNEP frame type - filter control */
264 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
265
266 /* Put in filter message type - set filters */
267 UINT8_TO_BE_STREAM(p, BNEP_FILTER_NET_TYPE_SET_MSG);
268
269 UINT16_TO_BE_STREAM(p, (4 * p_bcb->sent_num_filters));
270 for (xx = 0; xx < p_bcb->sent_num_filters; xx++) {
271 UINT16_TO_BE_STREAM(p, p_bcb->sent_prot_filter_start[xx]);
272 UINT16_TO_BE_STREAM(p, p_bcb->sent_prot_filter_end[xx]);
273 }
274
275 p_buf->len = 4 + (4 * p_bcb->sent_num_filters);
276
277 bnepu_check_send_packet(p_bcb, p_buf);
278
279 p_bcb->con_flags |= BNEP_FLAGS_FILTER_RESP_PEND;
280
281 /* Start timer waiting for setup response */
282 alarm_set_on_mloop(p_bcb->conn_timer, BNEP_FILTER_SET_TIMEOUT_MS,
283 bnep_conn_timer_timeout, p_bcb);
284 }
285
286 /*******************************************************************************
287 *
288 * Function bnepu_send_peer_our_multi_filters
289 *
290 * Description This function sends our multicast filters to a peer
291 *
292 * Returns void
293 *
294 ******************************************************************************/
bnepu_send_peer_our_multi_filters(tBNEP_CONN * p_bcb)295 void bnepu_send_peer_our_multi_filters(tBNEP_CONN* p_bcb) {
296 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
297 uint8_t* p;
298 uint16_t xx;
299
300 log::verbose("BNEP sending peer our multicast filters");
301
302 p_buf->offset = L2CAP_MIN_OFFSET;
303 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
304
305 /* Put in BNEP frame type - filter control */
306 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
307
308 /* Put in filter message type - set filters */
309 UINT8_TO_BE_STREAM(p, BNEP_FILTER_MULTI_ADDR_SET_MSG);
310
311 UINT16_TO_BE_STREAM(p, (2 * BD_ADDR_LEN * p_bcb->sent_mcast_filters));
312 for (xx = 0; xx < p_bcb->sent_mcast_filters; xx++) {
313 memcpy(p, p_bcb->sent_mcast_filter_start[xx].address, BD_ADDR_LEN);
314 p += BD_ADDR_LEN;
315 memcpy(p, p_bcb->sent_mcast_filter_end[xx].address, BD_ADDR_LEN);
316 p += BD_ADDR_LEN;
317 }
318
319 p_buf->len = 4 + (2 * BD_ADDR_LEN * p_bcb->sent_mcast_filters);
320
321 bnepu_check_send_packet(p_bcb, p_buf);
322
323 p_bcb->con_flags |= BNEP_FLAGS_MULTI_RESP_PEND;
324
325 /* Start timer waiting for setup response */
326 alarm_set_on_mloop(p_bcb->conn_timer, BNEP_FILTER_SET_TIMEOUT_MS,
327 bnep_conn_timer_timeout, p_bcb);
328 }
329
330 /*******************************************************************************
331 *
332 * Function bnepu_send_peer_filter_rsp
333 *
334 * Description This function sends a filter response to a peer
335 *
336 * Returns void
337 *
338 ******************************************************************************/
bnepu_send_peer_filter_rsp(tBNEP_CONN * p_bcb,uint16_t response_code)339 void bnepu_send_peer_filter_rsp(tBNEP_CONN* p_bcb, uint16_t response_code) {
340 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
341 uint8_t* p;
342
343 log::verbose("BNEP sending filter response");
344
345 p_buf->offset = L2CAP_MIN_OFFSET;
346 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
347
348 /* Put in BNEP frame type - filter control */
349 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
350
351 /* Put in filter message type - set filters */
352 UINT8_TO_BE_STREAM(p, BNEP_FILTER_NET_TYPE_RESPONSE_MSG);
353
354 UINT16_TO_BE_STREAM(p, response_code);
355
356 p_buf->len = 4;
357
358 bnepu_check_send_packet(p_bcb, p_buf);
359 }
360
361 /*******************************************************************************
362 *
363 * Function bnep_send_command_not_understood
364 *
365 * Description This function sends a BNEP command not understood message
366 *
367 * Returns void
368 *
369 ******************************************************************************/
bnep_send_command_not_understood(tBNEP_CONN * p_bcb,uint8_t cmd_code)370 void bnep_send_command_not_understood(tBNEP_CONN* p_bcb, uint8_t cmd_code) {
371 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
372 uint8_t* p;
373
374 log::verbose(
375 "BNEP - bnep_send_command_not_understood for CID: 0x{:x}, cmd 0x{:x}",
376 p_bcb->l2cap_cid, cmd_code);
377
378 p_buf->offset = L2CAP_MIN_OFFSET;
379 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
380
381 /* Put in BNEP frame type - filter control */
382 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
383
384 /* Put in filter message type - set filters */
385 UINT8_TO_BE_STREAM(p, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD);
386
387 UINT8_TO_BE_STREAM(p, cmd_code);
388
389 p_buf->len = 3;
390
391 bnepu_check_send_packet(p_bcb, p_buf);
392 }
393
394 /*******************************************************************************
395 *
396 * Function bnepu_check_send_packet
397 *
398 * Description This function tries to send a packet to L2CAP.
399 * If L2CAP is flow controlled, it enqueues the
400 * packet to the transmit queue
401 *
402 * Returns void
403 *
404 ******************************************************************************/
bnepu_check_send_packet(tBNEP_CONN * p_bcb,BT_HDR * p_buf)405 void bnepu_check_send_packet(tBNEP_CONN* p_bcb, BT_HDR* p_buf) {
406 log::debug("BNEP - bnepu_check_send_packet for CID: 0x{:x}",
407 p_bcb->l2cap_cid);
408 if (p_bcb->con_flags & BNEP_FLAGS_L2CAP_CONGESTED) {
409 if (fixed_queue_length(p_bcb->xmit_q) >= BNEP_MAX_XMITQ_DEPTH) {
410 log::warn("BNEP - congested, dropping buf, CID: 0x{:x}",
411 p_bcb->l2cap_cid);
412
413 osi_free(p_buf);
414 } else {
415 fixed_queue_enqueue(p_bcb->xmit_q, p_buf);
416 }
417 } else {
418 if (L2CA_DataWrite(p_bcb->l2cap_cid, p_buf) != L2CAP_DW_SUCCESS) {
419 log::warn("Unable to write L2CAP data peer:{} cid:{} len:{}",
420 p_bcb->rem_bda, p_bcb->l2cap_cid, p_buf->len);
421 }
422 }
423 }
424
425 /*******************************************************************************
426 *
427 * Function bnepu_build_bnep_hdr
428 *
429 * Description This function builds the BNEP header for a packet
430 * Extension headers are not sent yet, so there is no
431 * check for that.
432 *
433 * Returns void
434 *
435 ******************************************************************************/
bnepu_build_bnep_hdr(tBNEP_CONN * p_bcb,BT_HDR * p_buf,uint16_t protocol,const RawAddress & src_addr,const RawAddress & dest_addr,bool fw_ext_present)436 void bnepu_build_bnep_hdr(tBNEP_CONN* p_bcb, BT_HDR* p_buf, uint16_t protocol,
437 const RawAddress& src_addr,
438 const RawAddress& dest_addr, bool fw_ext_present) {
439 uint8_t ext_bit, *p = (uint8_t*)NULL;
440 uint8_t type = BNEP_FRAME_COMPRESSED_ETHERNET;
441 RawAddress source_addr = src_addr;
442
443 ext_bit = fw_ext_present ? 0x80 : 0x00;
444
445 if (source_addr != RawAddress::kEmpty &&
446 source_addr != bluetooth::ToRawAddress(
447 bluetooth::shim::GetController()->GetMacAddress()))
448 type = BNEP_FRAME_COMPRESSED_ETHERNET_SRC_ONLY;
449
450 if (dest_addr != p_bcb->rem_bda)
451 type = (type == BNEP_FRAME_COMPRESSED_ETHERNET)
452 ? BNEP_FRAME_COMPRESSED_ETHERNET_DEST_ONLY
453 : BNEP_FRAME_GENERAL_ETHERNET;
454
455 if (source_addr == RawAddress::kEmpty)
456 source_addr = bluetooth::ToRawAddress(
457 bluetooth::shim::GetController()->GetMacAddress());
458
459 switch (type) {
460 case BNEP_FRAME_GENERAL_ETHERNET:
461 p = bnepu_init_hdr(p_buf, 15,
462 (uint8_t)(ext_bit | BNEP_FRAME_GENERAL_ETHERNET));
463
464 memcpy(p, dest_addr.address, BD_ADDR_LEN);
465 p += BD_ADDR_LEN;
466
467 memcpy(p, source_addr.address, BD_ADDR_LEN);
468 p += BD_ADDR_LEN;
469 break;
470
471 case BNEP_FRAME_COMPRESSED_ETHERNET:
472 p = bnepu_init_hdr(p_buf, 3,
473 (uint8_t)(ext_bit | BNEP_FRAME_COMPRESSED_ETHERNET));
474 break;
475
476 case BNEP_FRAME_COMPRESSED_ETHERNET_SRC_ONLY:
477 p = bnepu_init_hdr(
478 p_buf, 9,
479 (uint8_t)(ext_bit | BNEP_FRAME_COMPRESSED_ETHERNET_SRC_ONLY));
480
481 memcpy(p, source_addr.address, BD_ADDR_LEN);
482 p += BD_ADDR_LEN;
483 break;
484
485 case BNEP_FRAME_COMPRESSED_ETHERNET_DEST_ONLY:
486 p = bnepu_init_hdr(
487 p_buf, 9,
488 (uint8_t)(ext_bit | BNEP_FRAME_COMPRESSED_ETHERNET_DEST_ONLY));
489
490 memcpy(p, dest_addr.address, BD_ADDR_LEN);
491 p += BD_ADDR_LEN;
492 break;
493 }
494
495 UINT16_TO_BE_STREAM(p, protocol);
496 }
497
498 /*******************************************************************************
499 *
500 * Function bnepu_init_hdr
501 *
502 * Description This function initializes the BNEP header
503 *
504 * Returns pointer to header in buffer
505 *
506 ******************************************************************************/
bnepu_init_hdr(BT_HDR * p_buf,uint16_t hdr_len,uint8_t pkt_type)507 static uint8_t* bnepu_init_hdr(BT_HDR* p_buf, uint16_t hdr_len,
508 uint8_t pkt_type) {
509 uint8_t* p = (uint8_t*)(p_buf + 1) + p_buf->offset;
510
511 /* See if we need to make space in the buffer */
512 if (p_buf->offset < (hdr_len + L2CAP_MIN_OFFSET)) {
513 uint16_t xx, diff = BNEP_MINIMUM_OFFSET - p_buf->offset;
514 p = p + p_buf->len - 1;
515 for (xx = 0; xx < p_buf->len; xx++, p--) p[diff] = *p;
516
517 p_buf->offset = BNEP_MINIMUM_OFFSET;
518 p = (uint8_t*)(p_buf + 1) + p_buf->offset;
519 }
520
521 p_buf->len += hdr_len;
522 p_buf->offset -= hdr_len;
523 p -= hdr_len;
524
525 *p++ = pkt_type;
526
527 return (p);
528 }
529
530 /*******************************************************************************
531 *
532 * Function bnep_process_setup_conn_req
533 *
534 * Description This function processes a peer's setup connection request
535 * message. The destination UUID is verified and response sent
536 * Connection open indication will be given to PAN profile
537 *
538 * Returns void
539 *
540 ******************************************************************************/
bnep_process_setup_conn_req(tBNEP_CONN * p_bcb,uint8_t * p_setup,uint8_t len)541 void bnep_process_setup_conn_req(tBNEP_CONN* p_bcb, uint8_t* p_setup,
542 uint8_t len) {
543 log::debug("BNEP - for CID: 0x{:x}", p_bcb->l2cap_cid);
544
545 if (p_bcb->con_state != BNEP_STATE_CONN_SETUP &&
546 p_bcb->con_state != BNEP_STATE_SEC_CHECKING &&
547 p_bcb->con_state != BNEP_STATE_CONNECTED) {
548 log::error("BNEP - setup request in bad state {}", p_bcb->con_state);
549 bnep_send_conn_response(p_bcb, BNEP_SETUP_CONN_NOT_ALLOWED);
550 return;
551 }
552
553 /* Check if we already initiated security check or if waiting for user
554 * responce */
555 if (p_bcb->con_flags & BNEP_FLAGS_SETUP_RCVD) {
556 log::warn(
557 "BNEP - Duplicate Setup message received while doing security check");
558 return;
559 }
560
561 /* Check if peer is the originator */
562 if (p_bcb->con_state != BNEP_STATE_CONNECTED &&
563 (!(p_bcb->con_flags & BNEP_FLAGS_SETUP_RCVD)) &&
564 (p_bcb->con_flags & BNEP_FLAGS_IS_ORIG)) {
565 log::error("BNEP - setup request when we are originator state:{}",
566 p_bcb->con_state);
567 bnep_send_conn_response(p_bcb, BNEP_SETUP_CONN_NOT_ALLOWED);
568 return;
569 }
570
571 if (p_bcb->con_state == BNEP_STATE_CONNECTED) {
572 p_bcb->prv_src_uuid = p_bcb->src_uuid;
573 p_bcb->prv_dst_uuid = p_bcb->dst_uuid;
574 }
575
576 if (len == Uuid::kNumBytes16) {
577 /* because peer initiated connection keep src uuid as dst uuid */
578 uint16_t tmp;
579
580 BE_STREAM_TO_UINT16(tmp, p_setup);
581 p_bcb->src_uuid = Uuid::From16Bit(tmp);
582
583 BE_STREAM_TO_UINT16(tmp, p_setup);
584 p_bcb->dst_uuid = Uuid::From16Bit(tmp);
585
586 /* If nothing has changed don't bother the profile */
587 if (p_bcb->con_state == BNEP_STATE_CONNECTED &&
588 p_bcb->src_uuid == p_bcb->prv_src_uuid &&
589 p_bcb->dst_uuid == p_bcb->prv_dst_uuid) {
590 bnep_send_conn_response(p_bcb, BNEP_SETUP_CONN_OK);
591 return;
592 }
593 } else if (len == Uuid::kNumBytes32) {
594 uint32_t tmp;
595
596 BE_STREAM_TO_UINT32(tmp, p_setup);
597 p_bcb->src_uuid = Uuid::From32Bit(tmp);
598
599 BE_STREAM_TO_UINT32(tmp, p_setup);
600 p_bcb->dst_uuid = Uuid::From32Bit(tmp);
601 } else if (len == Uuid::kNumBytes128) {
602 p_bcb->src_uuid = Uuid::From128BitBE(p_setup);
603 p_setup += len;
604
605 p_bcb->dst_uuid = Uuid::From128BitBE(p_setup);
606 p_setup += len;
607 } else {
608 log::error("BNEP - Bad UID len {} in ConnReq", len);
609 bnep_send_conn_response(p_bcb, BNEP_SETUP_INVALID_UUID_SIZE);
610 return;
611 }
612
613 p_bcb->con_state = BNEP_STATE_SEC_CHECKING;
614 p_bcb->con_flags |= BNEP_FLAGS_SETUP_RCVD;
615
616 log::debug("BNEP initiating security check for incoming call for uuid {}",
617 p_bcb->src_uuid.ToString());
618 bnep_sec_check_complete(&p_bcb->rem_bda, BT_TRANSPORT_BR_EDR, p_bcb);
619 }
620
621 /*******************************************************************************
622 *
623 * Function bnep_process_setup_conn_responce
624 *
625 * Description This function processes a peer's setup connection response
626 * message. The response code is verified and
627 * Connection open indication will be given to PAN profile
628 *
629 * Returns void
630 *
631 ******************************************************************************/
bnep_process_setup_conn_responce(tBNEP_CONN * p_bcb,uint8_t * p_setup)632 void bnep_process_setup_conn_responce(tBNEP_CONN* p_bcb, uint8_t* p_setup) {
633 tBNEP_RESULT resp;
634 uint16_t resp_code;
635
636 log::verbose("BNEP received setup responce");
637 /* The state should be either SETUP or CONNECTED */
638 if (p_bcb->con_state != BNEP_STATE_CONN_SETUP) {
639 /* Should we disconnect ? */
640 log::error("BNEP - setup response in bad state {}", p_bcb->con_state);
641 return;
642 }
643
644 /* Check if we are the originator */
645 if (!(p_bcb->con_flags & BNEP_FLAGS_IS_ORIG)) {
646 log::error("BNEP - setup response when we are not originator");
647 return;
648 }
649
650 BE_STREAM_TO_UINT16(resp_code, p_setup);
651
652 switch (resp_code) {
653 case BNEP_SETUP_INVALID_SRC_UUID:
654 resp = BNEP_CONN_FAILED_SRC_UUID;
655 break;
656
657 case BNEP_SETUP_INVALID_DEST_UUID:
658 resp = BNEP_CONN_FAILED_DST_UUID;
659 break;
660
661 case BNEP_SETUP_INVALID_UUID_SIZE:
662 resp = BNEP_CONN_FAILED_UUID_SIZE;
663 break;
664
665 case BNEP_SETUP_CONN_NOT_ALLOWED:
666 default:
667 resp = BNEP_CONN_FAILED;
668 break;
669 }
670
671 /* Check the responce code */
672 if (resp_code != BNEP_SETUP_CONN_OK) {
673 if (p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED) {
674 log::verbose("BNEP - role change response is {}", resp_code);
675
676 /* Restore the earlier BNEP status */
677 p_bcb->con_state = BNEP_STATE_CONNECTED;
678 p_bcb->con_flags &= (~BNEP_FLAGS_SETUP_RCVD);
679 p_bcb->src_uuid = p_bcb->prv_src_uuid;
680 p_bcb->dst_uuid = p_bcb->prv_dst_uuid;
681
682 /* Ensure timer is stopped */
683 alarm_cancel(p_bcb->conn_timer);
684 p_bcb->re_transmits = 0;
685
686 /* Tell the user if there is a callback */
687 if (bnep_cb.p_conn_state_cb)
688 (*bnep_cb.p_conn_state_cb)(p_bcb->handle, p_bcb->rem_bda, resp, true);
689
690 return;
691 } else {
692 log::error("BNEP - setup response {} is not OK", resp_code);
693
694 if (!L2CA_DisconnectReq(p_bcb->l2cap_cid)) {
695 log::warn("Unable to request L2CAP disconnect peer:{} cid:{}",
696 p_bcb->rem_bda, p_bcb->l2cap_cid);
697 }
698
699 /* Tell the user if there is a callback */
700 if ((p_bcb->con_flags & BNEP_FLAGS_IS_ORIG) && (bnep_cb.p_conn_state_cb))
701 (*bnep_cb.p_conn_state_cb)(p_bcb->handle, p_bcb->rem_bda, resp, false);
702
703 bnepu_release_bcb(p_bcb);
704 return;
705 }
706 }
707
708 /* Received successful responce */
709 bnep_connected(p_bcb);
710 }
711
712 /*******************************************************************************
713 *
714 * Function bnep_process_control_packet
715 *
716 * Description This function processes a peer's setup connection request
717 * message. The destination UUID is verified and response sent
718 * Connection open indication will be given to PAN profile
719 *
720 * Returns void
721 *
722 ******************************************************************************/
bnep_process_control_packet(tBNEP_CONN * p_bcb,uint8_t * p,uint16_t * rem_len,bool is_ext)723 uint8_t* bnep_process_control_packet(tBNEP_CONN* p_bcb, uint8_t* p,
724 uint16_t* rem_len, bool is_ext) {
725 uint8_t control_type;
726 uint16_t len, ext_len = 0;
727
728 if (p == NULL || rem_len == NULL) {
729 if (rem_len != NULL) *rem_len = 0;
730 log::verbose("invalid packet: p = {} rem_len = {}", fmt::ptr(p),
731 fmt::ptr(rem_len));
732 return NULL;
733 }
734 uint16_t rem_len_orig = *rem_len;
735
736 if (is_ext) {
737 if (*rem_len < 1) goto bad_packet_length;
738 ext_len = *p++;
739 *rem_len = *rem_len - 1;
740 }
741
742 if (*rem_len < 1) goto bad_packet_length;
743 control_type = *p++;
744 *rem_len = *rem_len - 1;
745
746 log::verbose(
747 "BNEP processing control packet rem_len {}, is_ext {}, ctrl_type {}",
748 *rem_len, is_ext, control_type);
749
750 switch (control_type) {
751 case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD:
752 if (*rem_len < 1) {
753 log::error(
754 "Received BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD with bad length");
755 goto bad_packet_length;
756 }
757 log::error(
758 "Received BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD for pkt type: {}", *p);
759 p++;
760 *rem_len = *rem_len - 1;
761 break;
762
763 case BNEP_SETUP_CONNECTION_REQUEST_MSG:
764 if (*rem_len < 1) {
765 log::error(
766 "Received BNEP_SETUP_CONNECTION_REQUEST_MSG with bad length");
767 goto bad_packet_length;
768 }
769 len = *p++;
770 if (*rem_len < ((2 * len) + 1)) {
771 log::error(
772 "Received BNEP_SETUP_CONNECTION_REQUEST_MSG with bad length");
773 goto bad_packet_length;
774 }
775 if (!is_ext) bnep_process_setup_conn_req(p_bcb, p, (uint8_t)len);
776 p += (2 * len);
777 *rem_len = *rem_len - (2 * len) - 1;
778 break;
779
780 case BNEP_SETUP_CONNECTION_RESPONSE_MSG:
781 if (*rem_len < 2) {
782 log::error(
783 "Received BNEP_SETUP_CONNECTION_RESPONSE_MSG with bad length");
784 goto bad_packet_length;
785 }
786 if (!is_ext) bnep_process_setup_conn_responce(p_bcb, p);
787 p += 2;
788 *rem_len = *rem_len - 2;
789 break;
790
791 case BNEP_FILTER_NET_TYPE_SET_MSG:
792 if (*rem_len < 2) {
793 log::error("Received BNEP_FILTER_NET_TYPE_SET_MSG with bad length");
794 goto bad_packet_length;
795 }
796 BE_STREAM_TO_UINT16(len, p);
797 if (*rem_len < (len + 2)) {
798 log::error("Received BNEP_FILTER_NET_TYPE_SET_MSG with bad length");
799 goto bad_packet_length;
800 }
801 bnepu_process_peer_filter_set(p_bcb, p, len);
802 p += len;
803 *rem_len = *rem_len - len - 2;
804 break;
805
806 case BNEP_FILTER_NET_TYPE_RESPONSE_MSG:
807 if (*rem_len < 2) {
808 log::error(
809 "Received BNEP_FILTER_NET_TYPE_RESPONSE_MSG with bad length");
810 goto bad_packet_length;
811 }
812 bnepu_process_peer_filter_rsp(p_bcb, p);
813 p += 2;
814 *rem_len = *rem_len - 2;
815 break;
816
817 case BNEP_FILTER_MULTI_ADDR_SET_MSG:
818 if (*rem_len < 2) {
819 log::error("Received BNEP_FILTER_MULTI_ADDR_SET_MSG with bad length");
820 goto bad_packet_length;
821 }
822 BE_STREAM_TO_UINT16(len, p);
823 if (*rem_len < (len + 2)) {
824 log::error("Received BNEP_FILTER_MULTI_ADDR_SET_MSG with bad length");
825 goto bad_packet_length;
826 }
827 bnepu_process_peer_multicast_filter_set(p_bcb, p, len);
828 p += len;
829 *rem_len = *rem_len - len - 2;
830 break;
831
832 case BNEP_FILTER_MULTI_ADDR_RESPONSE_MSG:
833 if (*rem_len < 2) {
834 log::error(
835 "Received BNEP_FILTER_MULTI_ADDR_RESPONSE_MSG with bad length");
836 goto bad_packet_length;
837 }
838 bnepu_process_multicast_filter_rsp(p_bcb, p);
839 p += 2;
840 *rem_len = *rem_len - 2;
841 break;
842
843 default:
844 log::error("BNEP - bad ctl pkt type: {}", control_type);
845 bnep_send_command_not_understood(p_bcb, control_type);
846 if (is_ext && (ext_len > 0)) {
847 if (*rem_len < (ext_len - 1)) {
848 goto bad_packet_length;
849 }
850 p += (ext_len - 1);
851 *rem_len -= (ext_len - 1);
852 }
853 break;
854 }
855 return p;
856
857 bad_packet_length:
858 log::error("bad control packet length: original={} remaining={}",
859 rem_len_orig, *rem_len);
860 *rem_len = 0;
861 return NULL;
862 }
863
864 /*******************************************************************************
865 *
866 * Function bnepu_process_peer_filter_set
867 *
868 * Description This function processes a peer's filter control
869 * 'set' message. The filters are stored in the BCB,
870 * and an appropriate filter response message sent.
871 *
872 * Returns void
873 *
874 ******************************************************************************/
bnepu_process_peer_filter_set(tBNEP_CONN * p_bcb,uint8_t * p_filters,uint16_t len)875 void bnepu_process_peer_filter_set(tBNEP_CONN* p_bcb, uint8_t* p_filters,
876 uint16_t len) {
877 uint16_t num_filters = 0;
878 uint16_t xx, resp_code = BNEP_FILTER_CRL_OK;
879 uint16_t start, end;
880 uint8_t* p_temp_filters;
881
882 if ((p_bcb->con_state != BNEP_STATE_CONNECTED) &&
883 (!(p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED))) {
884 log::verbose(
885 "BNEP received filter set from peer when there is no connection");
886 return;
887 }
888
889 log::verbose("BNEP received filter set from peer");
890 /* Check for length not a multiple of 4 */
891 if (len & 3) {
892 log::verbose("BNEP - bad filter len: {}", len);
893 bnepu_send_peer_filter_rsp(p_bcb, BNEP_FILTER_CRL_BAD_RANGE);
894 return;
895 }
896
897 if (len) num_filters = (uint16_t)(len >> 2);
898
899 /* Validate filter values */
900 if (num_filters <= BNEP_MAX_PROT_FILTERS) {
901 p_temp_filters = p_filters;
902 for (xx = 0; xx < num_filters; xx++) {
903 BE_STREAM_TO_UINT16(start, p_temp_filters);
904 BE_STREAM_TO_UINT16(end, p_temp_filters);
905
906 if (start > end) {
907 resp_code = BNEP_FILTER_CRL_BAD_RANGE;
908 break;
909 }
910 }
911 } else
912 resp_code = BNEP_FILTER_CRL_MAX_REACHED;
913
914 if (resp_code != BNEP_FILTER_CRL_OK) {
915 bnepu_send_peer_filter_rsp(p_bcb, resp_code);
916 return;
917 }
918
919 if (bnep_cb.p_filter_ind_cb)
920 (*bnep_cb.p_filter_ind_cb)(p_bcb->handle, true, 0, len, p_filters);
921
922 p_bcb->rcvd_num_filters = num_filters;
923 for (xx = 0; xx < num_filters; xx++) {
924 BE_STREAM_TO_UINT16(start, p_filters);
925 BE_STREAM_TO_UINT16(end, p_filters);
926
927 p_bcb->rcvd_prot_filter_start[xx] = start;
928 p_bcb->rcvd_prot_filter_end[xx] = end;
929 }
930
931 bnepu_send_peer_filter_rsp(p_bcb, resp_code);
932 }
933
934 /*******************************************************************************
935 *
936 * Function bnepu_process_peer_filter_rsp
937 *
938 * Description This function processes a peer's filter control
939 * 'response' message.
940 *
941 * Returns void
942 *
943 ******************************************************************************/
bnepu_process_peer_filter_rsp(tBNEP_CONN * p_bcb,uint8_t * p_data)944 void bnepu_process_peer_filter_rsp(tBNEP_CONN* p_bcb, uint8_t* p_data) {
945 uint16_t resp_code;
946 tBNEP_RESULT result;
947
948 log::verbose("BNEP received filter responce");
949 /* The state should be CONNECTED */
950 if ((p_bcb->con_state != BNEP_STATE_CONNECTED) &&
951 (!(p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED))) {
952 log::error("BNEP - filter response in bad state {}", p_bcb->con_state);
953 return;
954 }
955
956 /* Check if we are the originator */
957 if (!(p_bcb->con_flags & BNEP_FLAGS_FILTER_RESP_PEND)) {
958 log::error("BNEP - filter response when not expecting");
959 return;
960 }
961
962 /* Ensure timer is stopped */
963 alarm_cancel(p_bcb->conn_timer);
964 p_bcb->con_flags &= ~BNEP_FLAGS_FILTER_RESP_PEND;
965 p_bcb->re_transmits = 0;
966
967 BE_STREAM_TO_UINT16(resp_code, p_data);
968
969 result = BNEP_SUCCESS;
970 if (resp_code != BNEP_FILTER_CRL_OK) result = BNEP_SET_FILTER_FAIL;
971
972 if (bnep_cb.p_filter_ind_cb)
973 (*bnep_cb.p_filter_ind_cb)(p_bcb->handle, false, result, 0, NULL);
974 }
975
976 /*******************************************************************************
977 *
978 * Function bnepu_process_multicast_filter_rsp
979 *
980 * Description This function processes multicast filter control
981 * 'response' message.
982 *
983 * Returns void
984 *
985 ******************************************************************************/
bnepu_process_multicast_filter_rsp(tBNEP_CONN * p_bcb,uint8_t * p_data)986 void bnepu_process_multicast_filter_rsp(tBNEP_CONN* p_bcb, uint8_t* p_data) {
987 uint16_t resp_code;
988 tBNEP_RESULT result;
989
990 log::verbose("BNEP received multicast filter responce");
991 /* The state should be CONNECTED */
992 if ((p_bcb->con_state != BNEP_STATE_CONNECTED) &&
993 (!(p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED))) {
994 log::error("BNEP - multicast filter response in bad state {}",
995 p_bcb->con_state);
996 return;
997 }
998
999 /* Check if we are the originator */
1000 if (!(p_bcb->con_flags & BNEP_FLAGS_MULTI_RESP_PEND)) {
1001 log::error("BNEP - multicast filter response when not expecting");
1002 return;
1003 }
1004
1005 /* Ensure timer is stopped */
1006 alarm_cancel(p_bcb->conn_timer);
1007 p_bcb->con_flags &= ~BNEP_FLAGS_MULTI_RESP_PEND;
1008 p_bcb->re_transmits = 0;
1009
1010 BE_STREAM_TO_UINT16(resp_code, p_data);
1011
1012 result = BNEP_SUCCESS;
1013 if (resp_code != BNEP_FILTER_CRL_OK) result = BNEP_SET_FILTER_FAIL;
1014
1015 if (bnep_cb.p_mfilter_ind_cb)
1016 (*bnep_cb.p_mfilter_ind_cb)(p_bcb->handle, false, result, 0, NULL);
1017 }
1018
1019 /*******************************************************************************
1020 *
1021 * Function bnepu_process_peer_multicast_filter_set
1022 *
1023 * Description This function processes a peer's filter control
1024 * 'set' message. The filters are stored in the BCB,
1025 * and an appropriate filter response message sent.
1026 *
1027 * Returns void
1028 *
1029 ******************************************************************************/
bnepu_process_peer_multicast_filter_set(tBNEP_CONN * p_bcb,uint8_t * p_filters,uint16_t len)1030 void bnepu_process_peer_multicast_filter_set(tBNEP_CONN* p_bcb,
1031 uint8_t* p_filters, uint16_t len) {
1032 uint16_t resp_code = BNEP_FILTER_CRL_OK;
1033 uint16_t num_filters, xx;
1034 uint8_t *p_temp_filters, null_bda[BD_ADDR_LEN] = {0, 0, 0, 0, 0, 0};
1035
1036 if ((p_bcb->con_state != BNEP_STATE_CONNECTED) &&
1037 (!(p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED))) {
1038 log::verbose(
1039 "BNEP received multicast filter set from peer when there is no "
1040 "connection");
1041 return;
1042 }
1043
1044 if (len % 12) {
1045 log::verbose("BNEP - bad filter len: {}", len);
1046 bnepu_send_peer_multicast_filter_rsp(p_bcb, BNEP_FILTER_CRL_BAD_RANGE);
1047 return;
1048 }
1049
1050 if (len > (BNEP_MAX_MULTI_FILTERS * 2 * BD_ADDR_LEN)) {
1051 log::verbose("BNEP - Too many filters");
1052 bnepu_send_peer_multicast_filter_rsp(p_bcb, BNEP_FILTER_CRL_MAX_REACHED);
1053 return;
1054 }
1055
1056 num_filters = 0;
1057 if (len) num_filters = (uint16_t)(len / 12);
1058
1059 /* Validate filter values */
1060 if (num_filters <= BNEP_MAX_MULTI_FILTERS) {
1061 p_temp_filters = p_filters;
1062 for (xx = 0; xx < num_filters; xx++) {
1063 if (memcmp(p_temp_filters, p_temp_filters + BD_ADDR_LEN, BD_ADDR_LEN) >
1064 0) {
1065 bnepu_send_peer_multicast_filter_rsp(p_bcb, BNEP_FILTER_CRL_BAD_RANGE);
1066 return;
1067 }
1068
1069 p_temp_filters += (BD_ADDR_LEN * 2);
1070 }
1071 }
1072
1073 p_bcb->rcvd_mcast_filters = num_filters;
1074 p_temp_filters = p_filters;
1075 for (xx = 0; xx < num_filters; xx++) {
1076 memcpy(p_bcb->rcvd_mcast_filter_start[xx].address, p_temp_filters, BD_ADDR_LEN);
1077 memcpy(p_bcb->rcvd_mcast_filter_end[xx].address, p_temp_filters + BD_ADDR_LEN,
1078 BD_ADDR_LEN);
1079 p_temp_filters += (BD_ADDR_LEN * 2);
1080
1081 /* Check if any of the ranges have all zeros as both starting and ending
1082 * addresses */
1083 if ((memcmp(null_bda, p_bcb->rcvd_mcast_filter_start[xx].address,
1084 BD_ADDR_LEN) == 0) &&
1085 (memcmp(null_bda, p_bcb->rcvd_mcast_filter_end[xx].address,
1086 BD_ADDR_LEN) == 0)) {
1087 p_bcb->rcvd_mcast_filters = 0xFFFF;
1088 break;
1089 }
1090 }
1091
1092 log::verbose("BNEP multicast filters {}", p_bcb->rcvd_mcast_filters);
1093 bnepu_send_peer_multicast_filter_rsp(p_bcb, resp_code);
1094
1095 if (bnep_cb.p_mfilter_ind_cb)
1096 (*bnep_cb.p_mfilter_ind_cb)(p_bcb->handle, true, 0, len, p_filters);
1097 }
1098
1099 /*******************************************************************************
1100 *
1101 * Function bnepu_send_peer_multicast_filter_rsp
1102 *
1103 * Description This function sends a filter response to a peer
1104 *
1105 * Returns void
1106 *
1107 ******************************************************************************/
bnepu_send_peer_multicast_filter_rsp(tBNEP_CONN * p_bcb,uint16_t response_code)1108 void bnepu_send_peer_multicast_filter_rsp(tBNEP_CONN* p_bcb,
1109 uint16_t response_code) {
1110 BT_HDR* p_buf = (BT_HDR*)osi_malloc(BNEP_BUF_SIZE);
1111 uint8_t* p;
1112
1113 log::verbose("BNEP sending multicast filter response {}", response_code);
1114
1115 p_buf->offset = L2CAP_MIN_OFFSET;
1116 p = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
1117
1118 /* Put in BNEP frame type - filter control */
1119 UINT8_TO_BE_STREAM(p, BNEP_FRAME_CONTROL);
1120
1121 /* Put in filter message type - set filters */
1122 UINT8_TO_BE_STREAM(p, BNEP_FILTER_MULTI_ADDR_RESPONSE_MSG);
1123
1124 UINT16_TO_BE_STREAM(p, response_code);
1125
1126 p_buf->len = 4;
1127
1128 bnepu_check_send_packet(p_bcb, p_buf);
1129 }
1130
1131 /*******************************************************************************
1132 *
1133 * Function bnep_sec_check_complete
1134 *
1135 * Description This function is registered with BTM and will be called
1136 * after completing the security procedures
1137 *
1138 * Returns void
1139 *
1140 ******************************************************************************/
bnep_sec_check_complete(const RawAddress * bd_addr,tBT_TRANSPORT trasnport,void * p_ref_data)1141 void bnep_sec_check_complete(const RawAddress* bd_addr, tBT_TRANSPORT trasnport,
1142 void* p_ref_data) {
1143 tBNEP_CONN* p_bcb = (tBNEP_CONN*)p_ref_data;
1144 uint16_t resp_code = BNEP_SETUP_CONN_OK;
1145 bool is_role_change;
1146
1147 if (p_bcb->con_flags & BNEP_FLAGS_CONN_COMPLETED)
1148 is_role_change = true;
1149 else
1150 is_role_change = false;
1151
1152 /* check if the port is still waiting for security to complete */
1153 if (p_bcb->con_state != BNEP_STATE_SEC_CHECKING) {
1154 log::error("BNEP Connection in wrong state {} when security is completed",
1155 p_bcb->con_state);
1156 return;
1157 }
1158
1159 /* if it is outgoing call and result is FAILURE return security fail error */
1160 if (!(p_bcb->con_flags & BNEP_FLAGS_SETUP_RCVD)) {
1161 /* Transition to the next appropriate state, waiting for connection confirm.
1162 */
1163 p_bcb->con_state = BNEP_STATE_CONN_SETUP;
1164
1165 bnep_send_conn_req(p_bcb);
1166 alarm_set_on_mloop(p_bcb->conn_timer, BNEP_CONN_TIMEOUT_MS,
1167 bnep_conn_timer_timeout, p_bcb);
1168 return;
1169 }
1170
1171 if (bnep_cb.p_conn_ind_cb) {
1172 p_bcb->con_state = BNEP_STATE_CONN_SETUP;
1173 (*bnep_cb.p_conn_ind_cb)(p_bcb->handle, p_bcb->rem_bda, p_bcb->dst_uuid,
1174 p_bcb->src_uuid, is_role_change);
1175 } else {
1176 /* Profile didn't register connection indication call back */
1177 bnep_send_conn_response(p_bcb, resp_code);
1178 bnep_connected(p_bcb);
1179 }
1180 }
1181
1182 /*******************************************************************************
1183 *
1184 * Function bnep_is_packet_allowed
1185 *
1186 * Description This function verifies whether the protocol passes through
1187 * the protocol filters set by the peer
1188 *
1189 * Returns BNEP_SUCCESS - if the protocol is allowed
1190 * BNEP_IGNORE_CMD - if the protocol is filtered out
1191 *
1192 ******************************************************************************/
bnep_is_packet_allowed(tBNEP_CONN * p_bcb,const RawAddress & dest_addr,uint16_t protocol,bool fw_ext_present,uint8_t * p_data,uint16_t org_len)1193 tBNEP_RESULT bnep_is_packet_allowed(tBNEP_CONN* p_bcb,
1194 const RawAddress& dest_addr,
1195 uint16_t protocol, bool fw_ext_present,
1196 uint8_t* p_data, uint16_t org_len) {
1197 if (p_bcb->rcvd_num_filters) {
1198 uint16_t i, proto;
1199
1200 /* Findout the actual protocol to check for the filtering */
1201 proto = protocol;
1202 if (proto == BNEP_802_1_P_PROTOCOL) {
1203 uint16_t new_len = 0;
1204 if (fw_ext_present) {
1205 uint8_t len, ext;
1206 /* parse the extension headers and findout actual protocol */
1207 do {
1208 if ((new_len + 2) > org_len) {
1209 return BNEP_IGNORE_CMD;
1210 }
1211
1212 ext = *p_data++;
1213 len = *p_data++;
1214 p_data += len;
1215
1216 new_len += (len + 2);
1217
1218 } while (ext & 0x80);
1219 }
1220 if ((new_len + 4) > org_len) {
1221 return BNEP_IGNORE_CMD;
1222 }
1223 p_data += 2;
1224 BE_STREAM_TO_UINT16(proto, p_data);
1225 }
1226
1227 for (i = 0; i < p_bcb->rcvd_num_filters; i++) {
1228 if ((p_bcb->rcvd_prot_filter_start[i] <= proto) &&
1229 (proto <= p_bcb->rcvd_prot_filter_end[i]))
1230 break;
1231 }
1232
1233 if (i == p_bcb->rcvd_num_filters) {
1234 log::verbose("Ignoring protocol 0x{:x} in BNEP data write", proto);
1235 return BNEP_IGNORE_CMD;
1236 }
1237 }
1238
1239 /* Ckeck for multicast address filtering */
1240 if ((dest_addr.address[0] & 0x01) && p_bcb->rcvd_mcast_filters) {
1241 uint16_t i;
1242
1243 /* Check if every multicast should be filtered */
1244 if (p_bcb->rcvd_mcast_filters != 0xFFFF) {
1245 /* Check if the address is mentioned in the filter range */
1246 for (i = 0; i < p_bcb->rcvd_mcast_filters; i++) {
1247 if ((memcmp(p_bcb->rcvd_mcast_filter_start[i].address,
1248 dest_addr.address, BD_ADDR_LEN) <= 0) &&
1249 (memcmp(p_bcb->rcvd_mcast_filter_end[i].address, dest_addr.address,
1250 BD_ADDR_LEN) >= 0))
1251 break;
1252 }
1253 }
1254
1255 /*
1256 ** If every multicast should be filtered or the address is not in the filter
1257 *range
1258 ** drop the packet
1259 */
1260 if ((p_bcb->rcvd_mcast_filters == 0xFFFF) ||
1261 (i == p_bcb->rcvd_mcast_filters)) {
1262 log::verbose("Ignoring multicast address {} in BNEP data write",
1263 dest_addr);
1264 return BNEP_IGNORE_CMD;
1265 }
1266 }
1267
1268 return BNEP_SUCCESS;
1269 }
1270