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