stack/bnep/bnep_utils.cc

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