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