stack/bnep/bnep_utils.cc

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