monitor/rfcomm.c

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