stack/bnep/bnep_utils.c

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