monitor/rfcomm.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2014  Intel Corporation
   6  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <stdlib.h>
  31 #include <string.h>
  32 #include <ctype.h>
  33 #include <inttypes.h>
  34
  35 #include "lib/bluetooth.h"
  36
  37 #include "src/shared/util.h"
  38 #include "bt.h"
  39 #include "packet.h"
  40 #include "display.h"
  41 #include "l2cap.h"
  42 #include "uuid.h"
  43 #include "keys.h"
  44 #include "sdp.h"
  45 #include "rfcomm.h"
  46
  47 static char *cr_str[] = {
  48         "RSP",
  49         "CMD"
  50 };
  51
  52 /* RFCOMM frame parsing macros */
  53 #define CR_STR(type)            cr_str[GET_CR(type)]
  54 #define GET_LEN8(length)        ((length & 0xfe) >> 1)
  55 #define GET_LEN16(length)       ((length & 0xfffe) >> 1)
  56 #define GET_CR(type)            ((type & 0x02) >> 1)
  57 #define GET_PF(ctr)             (((ctr) >> 4) & 0x1)
  58
  59 /* MSC macros */
  60 #define GET_V24_FC(sigs)        ((sigs & 0x02) >> 1)
  61 #define GET_V24_RTC(sigs)       ((sigs & 0x04) >> 2)
  62 #define GET_V24_RTR(sigs)       ((sigs & 0x08) >> 3)
  63 #define GET_V24_IC(sigs)        ((sigs & 0x40) >> 6)
  64 #define GET_V24_DV(sigs)        ((sigs & 0x80) >> 7)
  65
  66 /* RPN macros */
  67 #define GET_RPN_DB(parity)      (parity & 0x03)
  68 #define GET_RPN_SB(parity)      ((parity & 0x04) >> 2)
  69 #define GET_RPN_PARITY(parity)  ((parity & 0x08) >> 3)
  70 #define GET_RPN_PTYPE(parity)   ((parity & 0x30) >> 4)
  71 #define GET_RPN_XIN(io)         (io & 0x01)
  72 #define GET_RPN_XOUT(io)        ((io & 0x02) >> 1)
  73 #define GET_RPN_RTRI(io)        ((io & 0x04) >> 2)
  74 #define GET_RPN_RTRO(io)        ((io & 0x08) >> 3)
  75 #define GET_RPN_RTCI(io)        ((io & 0x10) >> 4)
  76 #define GET_RPN_RTCO(io)        ((io & 0x20) >> 5)
  77
  78 /* RLS macro */
  79 #define GET_ERROR(err)          (err & 0x0f)
  80
  81 /* PN macros */
  82 #define GET_FRM_TYPE(ctrl)      ((ctrl & 0x0f))
  83 #define GET_CRT_FLOW(ctrl)      ((ctrl & 0xf0) >> 4)
  84 #define GET_PRIORITY(prio)      ((prio & 0x3f))
  85 #define GET_PN_DLCI(dlci)       ((dlci & 0x3f))
  86
  87 struct rfcomm_lhdr {
  88         uint8_t address;
  89         uint8_t control;
  90         uint16_t length;
  91         uint8_t fcs;
  92         uint8_t credits; /* only for UIH frame */
  93 } __attribute__((packed));
  94
  95 struct rfcomm_lmsc {
  96         uint8_t dlci;
  97         uint8_t v24_sig;
  98         uint8_t break_sig;
  99 } __attribute__((packed));
 100
 101 struct rfcomm_rpn {
 102         uint8_t dlci;
 103         uint8_t bit_rate;
 104         uint8_t parity;
 105         uint8_t io;
 106         uint8_t xon;
 107         uint8_t xoff;
 108         uint16_t pm;
 109 } __attribute__ ((packed));
 110
 111 struct rfcomm_rls {
 112         uint8_t dlci;
 113         uint8_t error;
 114 } __attribute__((packed));
 115
 116 struct rfcomm_nsc {
 117         uint8_t cmd_type;
 118 } __attribute__((packed));
 119
 120 struct rfcomm_lmcc {
 121         uint8_t type;
 122         uint16_t length;
 123 } __attribute__((packed));
 124
 125 struct rfcomm_frame {
 126         struct rfcomm_lhdr hdr;
 127         struct rfcomm_lmcc mcc;
 128         struct l2cap_frame l2cap_frame;
 129 };
 130
 131 static void print_rfcomm_hdr(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 132 {
 133         struct rfcomm_lhdr hdr = rfcomm_frame->hdr;
 134
 135         /* Address field */
 136         print_field("%*cAddress: 0x%2.2x cr %d dlci 0x%2.2x", indent, ' ',
 137                                         hdr.address, GET_CR(hdr.address),
 138                                         RFCOMM_GET_DLCI(hdr.address));
 139
 140         /* Control field */
 141         print_field("%*cControl: 0x%2.2x poll/final %d", indent, ' ',
 142                                         hdr.control, GET_PF(hdr.control));
 143
 144         /* Length and FCS */
 145         print_field("%*cLength: %d", indent, ' ', hdr.length);
 146         print_field("%*cFCS: 0x%2.2x", indent, ' ', hdr.fcs);
 147 }
 148
 149 static inline bool mcc_test(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 150 {
 151         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 152         uint8_t data;
 153
 154         printf("%*cTest Data: 0x ", indent, ' ');
 155
 156         while (frame->size > 1) {
 157                 if (!l2cap_frame_get_u8(frame, &data))
 158                         return false;
 159                 printf("%2.2x ", data);
 160         }
 161
 162         printf("\n");
 163         return true;
 164 }
 165
 166 static inline bool mcc_msc(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 167 {
 168         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 169         struct rfcomm_lmsc msc;
 170
 171         if (!l2cap_frame_get_u8(frame, &msc.dlci))
 172                 return false;
 173
 174         print_field("%*cdlci %d ", indent, ' ', RFCOMM_GET_DLCI(msc.dlci));
 175
 176         if (!l2cap_frame_get_u8(frame, &msc.v24_sig))
 177                 return false;
 178
 179         /* v24 control signals */
 180         print_field("%*cfc %d rtc %d rtr %d ic %d dv %d", indent, ' ',
 181                 GET_V24_FC(msc.v24_sig), GET_V24_RTC(msc.v24_sig),
 182                 GET_V24_RTR(msc.v24_sig), GET_V24_IC(msc.v24_sig),
 183                                         GET_V24_DV(msc.v24_sig));
 184
 185         if (frame->size < 2)
 186                 goto done;
 187
 188         /*
 189          * TODO: Implement the break signals decoding.
 190          */
 191
 192         packet_hexdump(frame->data, frame->size);
 193
 194 done:
 195         return true;
 196 }
 197
 198 static inline bool mcc_rpn(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 199 {
 200         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 201         struct rfcomm_rpn rpn;
 202
 203         if (!l2cap_frame_get_u8(frame, &rpn.dlci))
 204                 return false;
 205
 206         print_field("%*cdlci %d", indent, ' ', RFCOMM_GET_DLCI(rpn.dlci));
 207
 208         if (frame->size < 7)
 209                 goto done;
 210
 211         /* port value octets (optional) */
 212
 213         if (!l2cap_frame_get_u8(frame, &rpn.bit_rate))
 214                 return false;
 215
 216         if (!l2cap_frame_get_u8(frame, &rpn.parity))
 217                 return false;
 218
 219         if (!l2cap_frame_get_u8(frame, &rpn.io))
 220                 return false;
 221
 222         print_field("%*cbr %d db %d sb %d p %d pt %d xi %d xo %d", indent, ' ',
 223                 rpn.bit_rate, GET_RPN_DB(rpn.parity), GET_RPN_SB(rpn.parity),
 224                 GET_RPN_PARITY(rpn.parity), GET_RPN_PTYPE(rpn.parity),
 225                 GET_RPN_XIN(rpn.io), GET_RPN_XOUT(rpn.io));
 226
 227         if (!l2cap_frame_get_u8(frame, &rpn.xon))
 228                 return false;
 229
 230         if (!l2cap_frame_get_u8(frame, &rpn.xoff))
 231                 return false;
 232
 233         print_field("%*crtri %d rtro %d rtci %d rtco %d xon %d xoff %d",
 234                 indent, ' ', GET_RPN_RTRI(rpn.io), GET_RPN_RTRO(rpn.io),
 235                 GET_RPN_RTCI(rpn.io), GET_RPN_RTCO(rpn.io), rpn.xon,
 236                 rpn.xoff);
 237
 238         if (!l2cap_frame_get_le16(frame, &rpn.pm))
 239                 return false;
 240
 241         print_field("%*cpm 0x%04x", indent, ' ', rpn.pm);
 242
 243 done:
 244         return true;
 245 }
 246
 247 static inline bool mcc_rls(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 248 {
 249         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 250         struct rfcomm_rls rls;
 251
 252         if (!l2cap_frame_get_u8(frame, &rls.dlci))
 253                 return false;
 254
 255         if (!l2cap_frame_get_u8(frame, &rls.error))
 256                 return false;
 257
 258         print_field("%*cdlci %d error: %d", indent, ' ',
 259                         RFCOMM_GET_DLCI(rls.dlci), GET_ERROR(rls.error));
 260
 261         return true;
 262 }
 263
 264 static inline bool mcc_pn(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 265 {
 266         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 267         struct rfcomm_pn pn;
 268
 269         /* rfcomm_pn struct is defined in rfcomm.h */
 270
 271         if (!l2cap_frame_get_u8(frame, &pn.dlci))
 272                 return false;
 273
 274         if (!l2cap_frame_get_u8(frame, &pn.flow_ctrl))
 275                 return false;
 276
 277         if (!l2cap_frame_get_u8(frame, &pn.priority))
 278                 return false;
 279
 280         print_field("%*cdlci %d frame_type %d credit_flow %d pri %d", indent,
 281                         ' ', GET_PN_DLCI(pn.dlci), GET_FRM_TYPE(pn.flow_ctrl),
 282                         GET_CRT_FLOW(pn.flow_ctrl), GET_PRIORITY(pn.priority));
 283
 284         if (!l2cap_frame_get_u8(frame, &pn.ack_timer))
 285                 return false;
 286
 287         if (!l2cap_frame_get_le16(frame, &pn.mtu))
 288                 return false;
 289
 290         if (!l2cap_frame_get_u8(frame, &pn.max_retrans))
 291                 return false;
 292
 293         if (!l2cap_frame_get_u8(frame, &pn.credits))
 294                 return false;
 295
 296         print_field("%*cack_timer %d frame_size %d max_retrans %d credits %d",
 297                         indent, ' ', pn.ack_timer, pn.mtu, pn.max_retrans,
 298                         pn.credits);
 299
 300         return true;
 301 }
 302
 303 static inline bool mcc_nsc(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 304 {
 305         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 306         struct rfcomm_nsc nsc;
 307
 308         if (!l2cap_frame_get_u8(frame, &nsc.cmd_type))
 309                 return false;
 310
 311         print_field("%*ccr %d, mcc_cmd_type %x", indent, ' ',
 312                 GET_CR(nsc.cmd_type), RFCOMM_GET_MCC_TYPE(nsc.cmd_type));
 313
 314         return true;
 315 }
 316
 317 struct mcc_data {
 318         uint8_t type;
 319         const char *str;
 320 };
 321
 322 static const struct mcc_data mcc_table[] = {
 323         { 0x08, "Test Command" },
 324         { 0x28, "Flow Control On Command" },
 325         { 0x18, "Flow Control Off Command" },
 326         { 0x38, "Modem Status Command" },
 327         { 0x24, "Remote Port Negotiation Command" },
 328         { 0x14, "Remote Line Status" },
 329         { 0x20, "DLC Parameter Negotiation" },
 330         { 0x04, "Non Supported Command" },
 331         { }
 332 };
 333
 334 static inline bool mcc_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 335 {
 336         uint8_t length, ex_length, type;
 337         const char *type_str;
 338         int i;
 339         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 340         struct rfcomm_lmcc mcc;
 341         const struct mcc_data *mcc_data = NULL;
 342
 343         if (!l2cap_frame_get_u8(frame, &mcc.type) ||
 344                         !l2cap_frame_get_u8(frame, &length))
 345                 return false;
 346
 347         if (RFCOMM_TEST_EA(length))
 348                 mcc.length = (uint16_t) GET_LEN8(length);
 349         else {
 350                 if (!l2cap_frame_get_u8(frame, &ex_length))
 351                         return false;
 352                 mcc.length = ((uint16_t) length << 8) | ex_length;
 353                 mcc.length = GET_LEN16(mcc.length);
 354         }
 355
 356         type = RFCOMM_GET_MCC_TYPE(mcc.type);
 357
 358         for (i = 0; mcc_table[i].str; i++) {
 359                 if (mcc_table[i].type == type) {
 360                         mcc_data = &mcc_table[i];
 361                         break;
 362                 }
 363         }
 364
 365         if (mcc_data)
 366                 type_str = mcc_data->str;
 367         else
 368                 type_str = "Unknown";
 369
 370         print_field("%*cMCC Message type: %s %s(0x%2.2x)", indent, ' ',
 371                                 type_str, CR_STR(mcc.type), type);
 372
 373         print_field("%*cLength: %d", indent+2, ' ', mcc.length);
 374
 375         rfcomm_frame->mcc = mcc;
 376
 377         switch (type) {
 378         case RFCOMM_TEST:
 379                 return mcc_test(rfcomm_frame, indent+10);
 380         case RFCOMM_MSC:
 381                 return mcc_msc(rfcomm_frame, indent+2);
 382         case RFCOMM_RPN:
 383                 return mcc_rpn(rfcomm_frame, indent+2);
 384         case RFCOMM_RLS:
 385                 return mcc_rls(rfcomm_frame, indent+2);
 386         case RFCOMM_PN:
 387                 return mcc_pn(rfcomm_frame, indent+2);
 388         case RFCOMM_NSC:
 389                 return mcc_nsc(rfcomm_frame, indent+2);
 390         default:
 391                 packet_hexdump(frame->data, frame->size);
 392         }
 393
 394         return true;
 395 }
 396
 397 static bool uih_frame(struct rfcomm_frame *rfcomm_frame, uint8_t indent)
 398 {
 399         uint8_t credits;
 400         struct l2cap_frame *frame = &rfcomm_frame->l2cap_frame;
 401         struct rfcomm_lhdr *hdr = &rfcomm_frame->hdr;
 402
 403         if (!RFCOMM_GET_CHANNEL(hdr->address))
 404                 return mcc_frame(rfcomm_frame, indent);
 405
 406         /* fetching credits from UIH frame */
 407         if (GET_PF(hdr->control)) {
 408                 if (!l2cap_frame_get_u8(frame, &credits))
 409                         return false;
 410                 hdr->credits = credits;
 411                 print_field("%*cCredits: %d", indent, ' ', hdr->credits);
 412         }
 413
 414         packet_hexdump(frame->data, frame->size);
 415         return true;
 416 }
 417
 418 struct rfcomm_data {
 419         uint8_t frame;
 420         const char *str;
 421 };
 422
 423 static const struct rfcomm_data rfcomm_table[] = {
 424         { 0x2f, "Set Async Balance Mode (SABM) " },
 425         { 0x63, "Unnumbered Ack (UA)" },
 426         { 0x0f, "Disconnect Mode (DM)" },
 427         { 0x43, "Disconnect (DISC)" },
 428         { 0xef, "Unnumbered Info with Header Check (UIH)" },
 429         { }
 430 };
 431
 432 void rfcomm_packet(const struct l2cap_frame *frame)
 433 {
 434         uint8_t ctype, length, ex_length, indent = 1;
 435         const char *frame_str, *frame_color;
 436         struct l2cap_frame *l2cap_frame, tmp_frame;
 437         struct rfcomm_frame rfcomm_frame;
 438         struct rfcomm_lhdr hdr;
 439         const struct rfcomm_data *rfcomm_data = NULL;
 440         int i;
 441
 442         l2cap_frame_pull(&rfcomm_frame.l2cap_frame, frame, 0);
 443
 444         l2cap_frame = &rfcomm_frame.l2cap_frame;
 445
 446         if (frame->size < 4)
 447                 goto fail;
 448
 449         if (!l2cap_frame_get_u8(l2cap_frame, &hdr.address) ||
 450                         !l2cap_frame_get_u8(l2cap_frame, &hdr.control) ||
 451                         !l2cap_frame_get_u8(l2cap_frame, &length))
 452                 goto fail;
 453
 454         /* length maybe 1 or 2 octets */
 455         if (RFCOMM_TEST_EA(length))
 456                 hdr.length = (uint16_t) GET_LEN8(length);
 457         else {
 458                 if (!l2cap_frame_get_u8(l2cap_frame, &ex_length))
 459                         goto fail;
 460                 hdr.length = ((uint16_t)length << 8) | ex_length;
 461                 hdr.length = GET_LEN16(hdr.length);
 462         }
 463
 464         l2cap_frame_pull(&tmp_frame, l2cap_frame, l2cap_frame->size-1);
 465
 466         if (!l2cap_frame_get_u8(&tmp_frame, &hdr.fcs))
 467                 goto fail;
 468
 469         /* Decoding frame type */
 470         ctype = RFCOMM_GET_TYPE(hdr.control);
 471
 472         for (i = 0; rfcomm_table[i].str; i++) {
 473                 if (rfcomm_table[i].frame == ctype) {
 474                         rfcomm_data = &rfcomm_table[i];
 475                         break;
 476                 }
 477         }
 478
 479         if (rfcomm_data) {
 480                 if (frame->in)
 481                         frame_color = COLOR_MAGENTA;
 482                 else
 483                         frame_color = COLOR_BLUE;
 484                 frame_str = rfcomm_data->str;
 485         } else {
 486                 frame_color = COLOR_WHITE_BG;
 487                 frame_str = "Unknown";
 488         }
 489
 490         if (!rfcomm_data) {
 491                 packet_hexdump(frame->data, frame->size);
 492                 return;
 493         }
 494
 495         print_indent(6, frame_color, "RFCOMM: ", frame_str, COLOR_OFF,
 496                                                 "(0x%2.2x)", ctype);
 497
 498         rfcomm_frame.hdr = hdr;
 499         print_rfcomm_hdr(&rfcomm_frame, indent);
 500
 501         /* UIH frame */
 502         if (ctype == 0xef)
 503                 if (!uih_frame(&rfcomm_frame, indent))
 504                         goto fail;
 505
 506         return;
 507
 508 fail:
 509         print_text(COLOR_ERROR, "Frame too short");
 510         packet_hexdump(frame->data, frame->size);
 511         return;
 512 }