stack/rfcomm/rfc_ts_frames.cc

   1 /******************************************************************************
   2  *
   3  *  Copyright (C) 1999-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 functions to send TS 07.10 frames
  22  *
  23  ******************************************************************************/
  24
  25 #include <stddef.h>
  26 #include "bt_common.h"
  27 #include "bt_target.h"
  28 #include "l2c_api.h"
  29 #include "port_api.h"
  30 #include "port_int.h"
  31 #include "rfc_int.h"
  32 #include "rfcdefs.h"
  33
  34 /*******************************************************************************
  35  *
  36  * Function         rfc_send_sabme
  37  *
  38  * Description      This function sends SABME frame.
  39  *
  40  ******************************************************************************/
  41 void rfc_send_sabme(tRFC_MCB* p_mcb, uint8_t dlci) {
  42   uint8_t* p_data;
  43   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, true);
  44   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
  45
  46   p_buf->offset = L2CAP_MIN_OFFSET;
  47   p_data = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
  48
  49   /* SABME frame, command, PF = 1, dlci */
  50   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
  51   *p_data++ = RFCOMM_SABME | RFCOMM_PF;
  52   *p_data++ = RFCOMM_EA | 0;
  53
  54   *p_data =
  55       RFCOMM_SABME_FCS((uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET, cr, dlci);
  56
  57   p_buf->len = 4;
  58
  59   rfc_check_send_cmd(p_mcb, p_buf);
  60 }
  61
  62 /*******************************************************************************
  63  *
  64  * Function         rfc_send_ua
  65  *
  66  * Description      This function sends UA frame.
  67  *
  68  ******************************************************************************/
  69 void rfc_send_ua(tRFC_MCB* p_mcb, uint8_t dlci) {
  70   uint8_t* p_data;
  71   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, false);
  72   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
  73
  74   p_buf->offset = L2CAP_MIN_OFFSET;
  75   p_data = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
  76
  77   /* ua frame, response, PF = 1, dlci */
  78   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
  79   *p_data++ = RFCOMM_UA | RFCOMM_PF;
  80   *p_data++ = RFCOMM_EA | 0;
  81
  82   *p_data = RFCOMM_UA_FCS((uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET, cr, dlci);
  83
  84   p_buf->len = 4;
  85
  86   rfc_check_send_cmd(p_mcb, p_buf);
  87 }
  88
  89 /*******************************************************************************
  90  *
  91  * Function         rfc_send_dm
  92  *
  93  * Description      This function sends DM frame.
  94  *
  95  ******************************************************************************/
  96 void rfc_send_dm(tRFC_MCB* p_mcb, uint8_t dlci, bool pf) {
  97   uint8_t* p_data;
  98   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, false);
  99   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 100
 101   p_buf->offset = L2CAP_MIN_OFFSET;
 102   p_data = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
 103
 104   /* DM frame, response, PF = 1, dlci */
 105   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
 106   *p_data++ = RFCOMM_DM | ((pf) ? RFCOMM_PF : 0);
 107   *p_data++ = RFCOMM_EA | 0;
 108
 109   *p_data = RFCOMM_DM_FCS((uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET, cr, dlci);
 110
 111   p_buf->len = 4;
 112
 113   rfc_check_send_cmd(p_mcb, p_buf);
 114 }
 115
 116 /*******************************************************************************
 117  *
 118  * Function         rfc_send_disc
 119  *
 120  * Description      This function sends DISC frame.
 121  *
 122  ******************************************************************************/
 123 void rfc_send_disc(tRFC_MCB* p_mcb, uint8_t dlci) {
 124   uint8_t* p_data;
 125   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, true);
 126   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 127
 128   p_buf->offset = L2CAP_MIN_OFFSET;
 129   p_data = (uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET;
 130
 131   /* DISC frame, command, PF = 1, dlci */
 132   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
 133   *p_data++ = RFCOMM_DISC | RFCOMM_PF;
 134   *p_data++ = RFCOMM_EA | 0;
 135
 136   *p_data = RFCOMM_DISC_FCS((uint8_t*)(p_buf + 1) + L2CAP_MIN_OFFSET, cr, dlci);
 137
 138   p_buf->len = 4;
 139
 140   rfc_check_send_cmd(p_mcb, p_buf);
 141 }
 142
 143 /*******************************************************************************
 144  *
 145  * Function         rfc_send_buf_uih
 146  *
 147  * Description      This function sends UIH frame.
 148  *
 149  ******************************************************************************/
 150 void rfc_send_buf_uih(tRFC_MCB* p_mcb, uint8_t dlci, BT_HDR* p_buf) {
 151   uint8_t* p_data;
 152   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, true);
 153   uint8_t credits;
 154
 155   p_buf->offset -= RFCOMM_CTRL_FRAME_LEN;
 156   if (p_buf->len > 127) p_buf->offset--;
 157
 158   if (dlci)
 159     credits = (uint8_t)p_buf->layer_specific;
 160   else
 161     credits = 0;
 162
 163   if (credits) p_buf->offset--;
 164
 165   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 166
 167   /* UIH frame, command, PF = 0, dlci */
 168   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
 169   *p_data++ = RFCOMM_UIH | ((credits) ? RFCOMM_PF : 0);
 170   if (p_buf->len <= 127) {
 171     *p_data++ = RFCOMM_EA | (p_buf->len << 1);
 172     p_buf->len += 3;
 173   } else {
 174     *p_data++ = (p_buf->len & 0x7f) << 1;
 175     *p_data++ = p_buf->len >> RFCOMM_SHIFT_LENGTH2;
 176     p_buf->len += 4;
 177   }
 178
 179   if (credits) {
 180     *p_data++ = credits;
 181     p_buf->len++;
 182   }
 183
 184   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len++;
 185
 186   *p_data = RFCOMM_UIH_FCS((uint8_t*)(p_buf + 1) + p_buf->offset, dlci);
 187
 188   if (dlci == RFCOMM_MX_DLCI) {
 189     rfc_check_send_cmd(p_mcb, p_buf);
 190   } else {
 191     L2CA_DataWrite(p_mcb->lcid, p_buf);
 192   }
 193 }
 194
 195 /*******************************************************************************
 196  *
 197  * Function         rfc_send_pn
 198  *
 199  * Description      This function sends DLC Parameters Negotiation Frame.
 200  *
 201  ******************************************************************************/
 202 void rfc_send_pn(tRFC_MCB* p_mcb, uint8_t dlci, bool is_command, uint16_t mtu,
 203                  uint8_t cl, uint8_t k) {
 204   uint8_t* p_data;
 205   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 206
 207   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 208   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 209
 210   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_PN;
 211   *p_data++ = RFCOMM_EA | (RFCOMM_MX_PN_LEN << 1);
 212
 213   *p_data++ = dlci;
 214   *p_data++ = RFCOMM_PN_FRAM_TYPE_UIH | cl;
 215
 216   /* It appeared that we need to reply with the same priority bits as we
 217   *received.
 218   ** We will use the fact that we reply in the same context so rx_frame can
 219   *still be used.
 220   */
 221   if (is_command)
 222     *p_data++ = RFCOMM_PN_PRIORITY_0;
 223   else
 224     *p_data++ = rfc_cb.rfc.rx_frame.u.pn.priority;
 225
 226   *p_data++ = RFCOMM_T1_DSEC;
 227   *p_data++ = mtu & 0xFF;
 228   *p_data++ = mtu >> 8;
 229   *p_data++ = RFCOMM_N2;
 230   *p_data = k;
 231
 232   /* Total length is sizeof PN data + mx header 2 */
 233   p_buf->len = RFCOMM_MX_PN_LEN + 2;
 234
 235   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 236 }
 237
 238 /*******************************************************************************
 239  *
 240  * Function         rfc_send_fcon
 241  *
 242  * Description      This function sends Flow Control On Command.
 243  *
 244  ******************************************************************************/
 245 void rfc_send_fcon(tRFC_MCB* p_mcb, bool is_command) {
 246   uint8_t* p_data;
 247   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 248
 249   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 250   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 251
 252   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_FCON;
 253   *p_data++ = RFCOMM_EA | (RFCOMM_MX_FCON_LEN << 1);
 254
 255   /* Total length is sizeof FCON data + mx header 2 */
 256   p_buf->len = RFCOMM_MX_FCON_LEN + 2;
 257
 258   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 259 }
 260
 261 /*******************************************************************************
 262  *
 263  * Function         rfc_send_fcoff
 264  *
 265  * Description      This function sends Flow Control Off Command.
 266  *
 267  ******************************************************************************/
 268 void rfc_send_fcoff(tRFC_MCB* p_mcb, bool is_command) {
 269   uint8_t* p_data;
 270   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 271
 272   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 273   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 274
 275   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_FCOFF;
 276   *p_data++ = RFCOMM_EA | (RFCOMM_MX_FCOFF_LEN << 1);
 277
 278   /* Total length is sizeof FCOFF data + mx header 2 */
 279   p_buf->len = RFCOMM_MX_FCOFF_LEN + 2;
 280
 281   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 282 }
 283
 284 /*******************************************************************************
 285  *
 286  * Function         rfc_send_msc
 287  *
 288  * Description      This function sends Modem Status Command Frame.
 289  *
 290  ******************************************************************************/
 291 void rfc_send_msc(tRFC_MCB* p_mcb, uint8_t dlci, bool is_command,
 292                   tPORT_CTRL* p_pars) {
 293   uint8_t* p_data;
 294   uint8_t signals;
 295   uint8_t break_duration;
 296   uint8_t len;
 297   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 298
 299   signals = p_pars->modem_signal;
 300   break_duration = p_pars->break_signal;
 301
 302   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 303   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 304
 305   if (break_duration)
 306     len = RFCOMM_MX_MSC_LEN_WITH_BREAK;
 307   else
 308     len = RFCOMM_MX_MSC_LEN_NO_BREAK;
 309
 310   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_MSC;
 311   *p_data++ = RFCOMM_EA | (len << 1);
 312
 313   *p_data++ = RFCOMM_EA | RFCOMM_CR_MASK | (dlci << RFCOMM_SHIFT_DLCI);
 314   *p_data++ = RFCOMM_EA | ((p_pars->fc) ? RFCOMM_MSC_FC : 0) |
 315               ((signals & MODEM_SIGNAL_DTRDSR) ? RFCOMM_MSC_RTC : 0) |
 316               ((signals & MODEM_SIGNAL_RTSCTS) ? RFCOMM_MSC_RTR : 0) |
 317               ((signals & MODEM_SIGNAL_RI) ? RFCOMM_MSC_IC : 0) |
 318               ((signals & MODEM_SIGNAL_DCD) ? RFCOMM_MSC_DV : 0);
 319
 320   if (break_duration) {
 321     *p_data++ = RFCOMM_EA | RFCOMM_MSC_BREAK_PRESENT_MASK |
 322                 (break_duration << RFCOMM_MSC_SHIFT_BREAK);
 323   }
 324
 325   /* Total length is sizeof MSC data + mx header 2 */
 326   p_buf->len = len + 2;
 327
 328   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 329 }
 330
 331 /*******************************************************************************
 332  *
 333  * Function         rfc_send_rls
 334  *
 335  * Description      This function sends Remote Line Status Command Frame.
 336  *
 337  ******************************************************************************/
 338 void rfc_send_rls(tRFC_MCB* p_mcb, uint8_t dlci, bool is_command,
 339                   uint8_t status) {
 340   uint8_t* p_data;
 341   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 342
 343   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 344   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 345
 346   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_RLS;
 347   *p_data++ = RFCOMM_EA | (RFCOMM_MX_RLS_LEN << 1);
 348
 349   *p_data++ = RFCOMM_EA | RFCOMM_CR_MASK | (dlci << RFCOMM_SHIFT_DLCI);
 350   *p_data++ = RFCOMM_RLS_ERROR | status;
 351
 352   /* Total length is sizeof RLS data + mx header 2 */
 353   p_buf->len = RFCOMM_MX_RLS_LEN + 2;
 354
 355   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 356 }
 357
 358 /*******************************************************************************
 359  *
 360  * Function         rfc_send_nsc
 361  *
 362  * Description      This function sends Non Supported Command Response.
 363  *
 364  ******************************************************************************/
 365 void rfc_send_nsc(tRFC_MCB* p_mcb) {
 366   uint8_t* p_data;
 367   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 368
 369   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 370   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 371
 372   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(false) | RFCOMM_MX_NSC;
 373   *p_data++ = RFCOMM_EA | (RFCOMM_MX_NSC_LEN << 1);
 374
 375   *p_data++ = rfc_cb.rfc.rx_frame.ea |
 376               (rfc_cb.rfc.rx_frame.cr << RFCOMM_SHIFT_CR) |
 377               rfc_cb.rfc.rx_frame.type;
 378
 379   /* Total length is sizeof NSC data + mx header 2 */
 380   p_buf->len = RFCOMM_MX_NSC_LEN + 2;
 381
 382   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 383 }
 384
 385 /*******************************************************************************
 386  *
 387  * Function         rfc_send_rpn
 388  *
 389  * Description      This function sends Remote Port Negotiation Command
 390  *
 391  ******************************************************************************/
 392 void rfc_send_rpn(tRFC_MCB* p_mcb, uint8_t dlci, bool is_command,
 393                   tPORT_STATE* p_pars, uint16_t mask) {
 394   uint8_t* p_data;
 395   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 396
 397   p_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_CTRL_FRAME_LEN;
 398   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 399
 400   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_RPN;
 401
 402   if (!p_pars) {
 403     *p_data++ = RFCOMM_EA | (RFCOMM_MX_RPN_REQ_LEN << 1);
 404
 405     *p_data++ = RFCOMM_EA | RFCOMM_CR_MASK | (dlci << RFCOMM_SHIFT_DLCI);
 406
 407     p_buf->len = RFCOMM_MX_RPN_REQ_LEN + 2;
 408   } else {
 409     *p_data++ = RFCOMM_EA | (RFCOMM_MX_RPN_LEN << 1);
 410
 411     *p_data++ = RFCOMM_EA | RFCOMM_CR_MASK | (dlci << RFCOMM_SHIFT_DLCI);
 412     *p_data++ = p_pars->baud_rate;
 413     *p_data++ = (p_pars->byte_size << RFCOMM_RPN_BITS_SHIFT) |
 414                 (p_pars->stop_bits << RFCOMM_RPN_STOP_BITS_SHIFT) |
 415                 (p_pars->parity << RFCOMM_RPN_PARITY_SHIFT) |
 416                 (p_pars->parity_type << RFCOMM_RPN_PARITY_TYPE_SHIFT);
 417     *p_data++ = p_pars->fc_type;
 418     *p_data++ = p_pars->xon_char;
 419     *p_data++ = p_pars->xoff_char;
 420     *p_data++ = (mask & 0xFF);
 421     *p_data++ = (mask >> 8);
 422
 423     /* Total length is sizeof RPN data + mx header 2 */
 424     p_buf->len = RFCOMM_MX_RPN_LEN + 2;
 425   }
 426
 427   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 428 }
 429
 430 /*******************************************************************************
 431  *
 432  * Function         rfc_send_test
 433  *
 434  * Description      This function sends Test frame.
 435  *
 436  ******************************************************************************/
 437 void rfc_send_test(tRFC_MCB* p_mcb, bool is_command, BT_HDR* p_buf) {
 438   /* Shift buffer to give space for header */
 439   if (p_buf->offset < (L2CAP_MIN_OFFSET + RFCOMM_MIN_OFFSET + 2)) {
 440     uint8_t* p_src = (uint8_t*)(p_buf + 1) + p_buf->offset + p_buf->len - 1;
 441     BT_HDR* p_new_buf =
 442         (BT_HDR*)osi_malloc(p_buf->len + (L2CAP_MIN_OFFSET + RFCOMM_MIN_OFFSET +
 443                                           2 + sizeof(BT_HDR) + 1));
 444
 445     p_new_buf->offset = L2CAP_MIN_OFFSET + RFCOMM_MIN_OFFSET + 2;
 446     p_new_buf->len = p_buf->len;
 447
 448     uint8_t* p_dest =
 449         (uint8_t*)(p_new_buf + 1) + p_new_buf->offset + p_new_buf->len - 1;
 450
 451     for (uint16_t xx = 0; xx < p_buf->len; xx++) *p_dest-- = *p_src--;
 452
 453     osi_free(p_buf);
 454     p_buf = p_new_buf;
 455   }
 456
 457   /* Adjust offset by number of bytes we are going to fill */
 458   p_buf->offset -= 2;
 459   uint8_t* p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 460
 461   *p_data++ = RFCOMM_EA | RFCOMM_I_CR(is_command) | RFCOMM_MX_TEST;
 462   *p_data++ = RFCOMM_EA | (p_buf->len << 1);
 463
 464   p_buf->len += 2;
 465
 466   rfc_send_buf_uih(p_mcb, RFCOMM_MX_DLCI, p_buf);
 467 }
 468
 469 /*******************************************************************************
 470  *
 471  * Function         rfc_send_credit
 472  *
 473  * Description      This function sends a flow control credit in UIH frame.
 474  *
 475  ******************************************************************************/
 476 void rfc_send_credit(tRFC_MCB* p_mcb, uint8_t dlci, uint8_t credit) {
 477   uint8_t* p_data;
 478   uint8_t cr = RFCOMM_CR(p_mcb->is_initiator, true);
 479   BT_HDR* p_buf = (BT_HDR*)osi_malloc(RFCOMM_CMD_BUF_SIZE);
 480
 481   p_buf->offset = L2CAP_MIN_OFFSET;
 482   p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 483
 484   *p_data++ = RFCOMM_EA | cr | (dlci << RFCOMM_SHIFT_DLCI);
 485   *p_data++ = RFCOMM_UIH | RFCOMM_PF;
 486   *p_data++ = RFCOMM_EA | 0;
 487   *p_data++ = credit;
 488   *p_data = RFCOMM_UIH_FCS((uint8_t*)(p_buf + 1) + p_buf->offset, dlci);
 489
 490   p_buf->len = 5;
 491
 492   rfc_check_send_cmd(p_mcb, p_buf);
 493 }
 494
 495 /*******************************************************************************
 496  *
 497  * Function         rfc_parse_data
 498  *
 499  * Description      This function processes data packet received from L2CAP
 500  *
 501  ******************************************************************************/
 502 uint8_t rfc_parse_data(tRFC_MCB* p_mcb, MX_FRAME* p_frame, BT_HDR* p_buf) {
 503   uint8_t ead, eal, fcs;
 504   uint8_t* p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 505   uint8_t* p_start = p_data;
 506   uint16_t len;
 507
 508   if (p_buf->len < RFCOMM_CTRL_FRAME_LEN) {
 509     RFCOMM_TRACE_ERROR("Bad Length1: %d", p_buf->len);
 510     return (RFC_EVENT_BAD_FRAME);
 511   }
 512
 513   RFCOMM_PARSE_CTRL_FIELD(ead, p_frame->cr, p_frame->dlci, p_data);
 514   if (!ead) {
 515     RFCOMM_TRACE_ERROR("Bad Address(EA must be 1)");
 516     return (RFC_EVENT_BAD_FRAME);
 517   }
 518   RFCOMM_PARSE_TYPE_FIELD(p_frame->type, p_frame->pf, p_data);
 519   RFCOMM_PARSE_LEN_FIELD(eal, len, p_data);
 520
 521   p_buf->len -= (3 + !ead + !eal + 1); /* Additional 1 for FCS */
 522   p_buf->offset += (3 + !ead + !eal);
 523
 524   /* handle credit if credit based flow control */
 525   if ((p_mcb->flow == PORT_FC_CREDIT) && (p_frame->type == RFCOMM_UIH) &&
 526       (p_frame->dlci != RFCOMM_MX_DLCI) && (p_frame->pf == 1)) {
 527     p_frame->credit = *p_data++;
 528     p_buf->len--;
 529     p_buf->offset++;
 530   } else
 531     p_frame->credit = 0;
 532
 533   if (p_buf->len != len) {
 534     RFCOMM_TRACE_ERROR("Bad Length2 %d %d", p_buf->len, len);
 535     return (RFC_EVENT_BAD_FRAME);
 536   }
 537
 538   fcs = *(p_data + len);
 539
 540   /* All control frames that we are sending are sent with P=1, expect */
 541   /* reply with F=1 */
 542   /* According to TS 07.10 spec ivalid frames are discarded without */
 543   /* notification to the sender */
 544   switch (p_frame->type) {
 545     case RFCOMM_SABME:
 546       if (RFCOMM_FRAME_IS_RSP(p_mcb->is_initiator, p_frame->cr) ||
 547           !p_frame->pf || len || !RFCOMM_VALID_DLCI(p_frame->dlci) ||
 548           !rfc_check_fcs(RFCOMM_CTRL_FRAME_LEN, p_start, fcs)) {
 549         RFCOMM_TRACE_ERROR("Bad SABME");
 550         return (RFC_EVENT_BAD_FRAME);
 551       } else
 552         return (RFC_EVENT_SABME);
 553
 554     case RFCOMM_UA:
 555       if (RFCOMM_FRAME_IS_CMD(p_mcb->is_initiator, p_frame->cr) ||
 556           !p_frame->pf || len || !RFCOMM_VALID_DLCI(p_frame->dlci) ||
 557           !rfc_check_fcs(RFCOMM_CTRL_FRAME_LEN, p_start, fcs)) {
 558         RFCOMM_TRACE_ERROR("Bad UA");
 559         return (RFC_EVENT_BAD_FRAME);
 560       } else
 561         return (RFC_EVENT_UA);
 562
 563     case RFCOMM_DM:
 564       if (RFCOMM_FRAME_IS_CMD(p_mcb->is_initiator, p_frame->cr) || len ||
 565           !RFCOMM_VALID_DLCI(p_frame->dlci) ||
 566           !rfc_check_fcs(RFCOMM_CTRL_FRAME_LEN, p_start, fcs)) {
 567         RFCOMM_TRACE_ERROR("Bad DM");
 568         return (RFC_EVENT_BAD_FRAME);
 569       } else
 570         return (RFC_EVENT_DM);
 571
 572     case RFCOMM_DISC:
 573       if (RFCOMM_FRAME_IS_RSP(p_mcb->is_initiator, p_frame->cr) ||
 574           !p_frame->pf || len || !RFCOMM_VALID_DLCI(p_frame->dlci) ||
 575           !rfc_check_fcs(RFCOMM_CTRL_FRAME_LEN, p_start, fcs)) {
 576         RFCOMM_TRACE_ERROR("Bad DISC");
 577         return (RFC_EVENT_BAD_FRAME);
 578       } else
 579         return (RFC_EVENT_DISC);
 580
 581     case RFCOMM_UIH:
 582       if (!RFCOMM_VALID_DLCI(p_frame->dlci)) {
 583         RFCOMM_TRACE_ERROR("Bad UIH - invalid DLCI");
 584         return (RFC_EVENT_BAD_FRAME);
 585       } else if (!rfc_check_fcs(2, p_start, fcs)) {
 586         RFCOMM_TRACE_ERROR("Bad UIH - FCS");
 587         return (RFC_EVENT_BAD_FRAME);
 588       } else if (RFCOMM_FRAME_IS_RSP(p_mcb->is_initiator, p_frame->cr)) {
 589         /* we assume that this is ok to allow bad implementations to work */
 590         RFCOMM_TRACE_ERROR("Bad UIH - response");
 591         return (RFC_EVENT_UIH);
 592       } else
 593         return (RFC_EVENT_UIH);
 594   }
 595
 596   return (RFC_EVENT_BAD_FRAME);
 597 }
 598
 599 /*******************************************************************************
 600  *
 601  * Function         rfc_process_mx_message
 602  *
 603  * Description      This function processes UIH frames received on the
 604  *                  multiplexer control channel.
 605  *
 606  ******************************************************************************/
 607 void rfc_process_mx_message(tRFC_MCB* p_mcb, BT_HDR* p_buf) {
 608   uint8_t* p_data = (uint8_t*)(p_buf + 1) + p_buf->offset;
 609   MX_FRAME* p_rx_frame = &rfc_cb.rfc.rx_frame;
 610   uint16_t length = p_buf->len;
 611   uint8_t ea, cr, mx_len;
 612   bool is_command;
 613
 614   p_rx_frame->ea = *p_data & RFCOMM_EA;
 615   p_rx_frame->cr = (*p_data & RFCOMM_CR_MASK) >> RFCOMM_SHIFT_CR;
 616   p_rx_frame->type = *p_data++ & ~(RFCOMM_CR_MASK | RFCOMM_EA_MASK);
 617
 618   if (!p_rx_frame->ea || !length) {
 619     RFCOMM_TRACE_ERROR("Illegal MX Frame ea:%d len:%d", p_rx_frame->ea, length);
 620     osi_free(p_buf);
 621     return;
 622   }
 623
 624   length--;
 625
 626   is_command = p_rx_frame->cr;
 627
 628   ea = *p_data & RFCOMM_EA;
 629
 630   mx_len = *p_data++ >> RFCOMM_SHIFT_LENGTH1;
 631   length--;
 632
 633   if (!ea) {
 634     mx_len += *p_data++ << RFCOMM_SHIFT_LENGTH2;
 635     length--;
 636   }
 637
 638   if (mx_len != length) {
 639     RFCOMM_TRACE_ERROR("Bad MX frame");
 640     osi_free(p_buf);
 641     return;
 642   }
 643
 644   switch (p_rx_frame->type) {
 645     case RFCOMM_MX_PN:
 646       if (length != RFCOMM_MX_PN_LEN) break;
 647
 648       p_rx_frame->dlci = *p_data++ & RFCOMM_PN_DLCI_MASK;
 649       p_rx_frame->u.pn.frame_type = *p_data & RFCOMM_PN_FRAME_TYPE_MASK;
 650       p_rx_frame->u.pn.conv_layer = *p_data++ & RFCOMM_PN_CONV_LAYER_MASK;
 651       p_rx_frame->u.pn.priority = *p_data++ & RFCOMM_PN_PRIORITY_MASK;
 652       p_rx_frame->u.pn.t1 = *p_data++;
 653       p_rx_frame->u.pn.mtu = *p_data + (*(p_data + 1) << 8);
 654       p_data += 2;
 655       p_rx_frame->u.pn.n2 = *p_data++;
 656       p_rx_frame->u.pn.k = *p_data++ & RFCOMM_PN_K_MASK;
 657
 658       if (!p_rx_frame->dlci || !RFCOMM_VALID_DLCI(p_rx_frame->dlci) ||
 659           (p_rx_frame->u.pn.mtu < RFCOMM_MIN_MTU) ||
 660           (p_rx_frame->u.pn.mtu > RFCOMM_MAX_MTU)) {
 661         RFCOMM_TRACE_ERROR("Bad PN frame");
 662         break;
 663       }
 664
 665       osi_free(p_buf);
 666
 667       rfc_process_pn(p_mcb, is_command, p_rx_frame);
 668       return;
 669
 670     case RFCOMM_MX_TEST:
 671       if (!length) break;
 672
 673       p_rx_frame->u.test.p_data = p_data;
 674       p_rx_frame->u.test.data_len = length;
 675
 676       p_buf->offset += 2;
 677       p_buf->len -= 2;
 678
 679       if (is_command)
 680         rfc_send_test(p_mcb, false, p_buf);
 681       else
 682         rfc_process_test_rsp(p_mcb, p_buf);
 683       return;
 684
 685     case RFCOMM_MX_FCON:
 686       if (length != RFCOMM_MX_FCON_LEN) break;
 687
 688       osi_free(p_buf);
 689
 690       rfc_process_fcon(p_mcb, is_command);
 691       return;
 692
 693     case RFCOMM_MX_FCOFF:
 694       if (length != RFCOMM_MX_FCOFF_LEN) break;
 695
 696       osi_free(p_buf);
 697
 698       rfc_process_fcoff(p_mcb, is_command);
 699       return;
 700
 701     case RFCOMM_MX_MSC:
 702
 703       ea = *p_data & RFCOMM_EA;
 704       cr = (*p_data & RFCOMM_CR_MASK) >> RFCOMM_SHIFT_CR;
 705       p_rx_frame->dlci = *p_data++ >> RFCOMM_SHIFT_DLCI;
 706
 707       if (!ea || !cr || !p_rx_frame->dlci ||
 708           !RFCOMM_VALID_DLCI(p_rx_frame->dlci)) {
 709         RFCOMM_TRACE_ERROR("Bad MSC frame");
 710         break;
 711       }
 712
 713       p_rx_frame->u.msc.signals = *p_data++;
 714
 715       if (mx_len == RFCOMM_MX_MSC_LEN_WITH_BREAK) {
 716         p_rx_frame->u.msc.break_present =
 717             *p_data & RFCOMM_MSC_BREAK_PRESENT_MASK;
 718         p_rx_frame->u.msc.break_duration =
 719             (*p_data & RFCOMM_MSC_BREAK_MASK) >> RFCOMM_MSC_SHIFT_BREAK;
 720       } else {
 721         p_rx_frame->u.msc.break_present = false;
 722         p_rx_frame->u.msc.break_duration = 0;
 723       }
 724       osi_free(p_buf);
 725
 726       rfc_process_msc(p_mcb, is_command, p_rx_frame);
 727       return;
 728
 729     case RFCOMM_MX_NSC:
 730       if ((length != RFCOMM_MX_NSC_LEN) || !is_command) break;
 731
 732       p_rx_frame->u.nsc.ea = *p_data & RFCOMM_EA;
 733       p_rx_frame->u.nsc.cr = (*p_data & RFCOMM_CR_MASK) >> RFCOMM_SHIFT_CR;
 734       p_rx_frame->u.nsc.type = *p_data++ >> RFCOMM_SHIFT_DLCI;
 735
 736       osi_free(p_buf);
 737
 738       rfc_process_nsc(p_mcb, p_rx_frame);
 739       return;
 740
 741     case RFCOMM_MX_RPN:
 742       if ((length != RFCOMM_MX_RPN_REQ_LEN) && (length != RFCOMM_MX_RPN_LEN))
 743         break;
 744
 745       ea = *p_data & RFCOMM_EA;
 746       cr = (*p_data & RFCOMM_CR_MASK) >> RFCOMM_SHIFT_CR;
 747       p_rx_frame->dlci = *p_data++ >> RFCOMM_SHIFT_DLCI;
 748
 749       if (!ea || !cr || !p_rx_frame->dlci ||
 750           !RFCOMM_VALID_DLCI(p_rx_frame->dlci)) {
 751         RFCOMM_TRACE_ERROR("Bad RPN frame");
 752         break;
 753       }
 754
 755       p_rx_frame->u.rpn.is_request = (length == RFCOMM_MX_RPN_REQ_LEN);
 756
 757       if (!p_rx_frame->u.rpn.is_request) {
 758         p_rx_frame->u.rpn.baud_rate = *p_data++;
 759         p_rx_frame->u.rpn.byte_size =
 760             (*p_data >> RFCOMM_RPN_BITS_SHIFT) & RFCOMM_RPN_BITS_MASK;
 761         p_rx_frame->u.rpn.stop_bits =
 762             (*p_data >> RFCOMM_RPN_STOP_BITS_SHIFT) & RFCOMM_RPN_STOP_BITS_MASK;
 763         p_rx_frame->u.rpn.parity =
 764             (*p_data >> RFCOMM_RPN_PARITY_SHIFT) & RFCOMM_RPN_PARITY_MASK;
 765         p_rx_frame->u.rpn.parity_type =
 766             (*p_data++ >> RFCOMM_RPN_PARITY_TYPE_SHIFT) &
 767             RFCOMM_RPN_PARITY_TYPE_MASK;
 768
 769         p_rx_frame->u.rpn.fc_type = *p_data++ & RFCOMM_FC_MASK;
 770         p_rx_frame->u.rpn.xon_char = *p_data++;
 771         p_rx_frame->u.rpn.xoff_char = *p_data++;
 772         p_rx_frame->u.rpn.param_mask =
 773             (*p_data + (*(p_data + 1) << 8)) & RFCOMM_RPN_PM_MASK;
 774       }
 775       osi_free(p_buf);
 776
 777       rfc_process_rpn(p_mcb, is_command, p_rx_frame->u.rpn.is_request,
 778                       p_rx_frame);
 779       return;
 780
 781     case RFCOMM_MX_RLS:
 782       if (length != RFCOMM_MX_RLS_LEN) break;
 783
 784       ea = *p_data & RFCOMM_EA;
 785       cr = (*p_data & RFCOMM_CR_MASK) >> RFCOMM_SHIFT_CR;
 786
 787       p_rx_frame->dlci = *p_data++ >> RFCOMM_SHIFT_DLCI;
 788       p_rx_frame->u.rls.line_status = (*p_data & ~0x01);
 789
 790       if (!ea || !cr || !p_rx_frame->dlci ||
 791           !RFCOMM_VALID_DLCI(p_rx_frame->dlci)) {
 792         RFCOMM_TRACE_ERROR("Bad RPN frame");
 793         break;
 794       }
 795
 796       osi_free(p_buf);
 797
 798       rfc_process_rls(p_mcb, is_command, p_rx_frame);
 799       return;
 800   }
 801
 802   osi_free(p_buf);
 803
 804   if (is_command) rfc_send_nsc(p_mcb);
 805 }