monitor/bnep.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 "uuid.h"
  31 #include "keys.h"
  32 #include "sdp.h"
  33 #include "bnep.h"
  34
  35 #define GET_PKT_TYPE(type) (type & 0x7f)
  36 #define GET_EXTENSION(type) (type & 0x80)
  37
  38 /* BNEP Extension Type */
  39 #define BNEP_EXTENSION_CONTROL          0x00
  40
  41 #define BNEP_CONTROL                    0x01
  42
  43 uint16_t proto = 0x0000;
  44
  45 struct bnep_frame {
  46         uint8_t type;
  47         int extension;
  48         struct l2cap_frame l2cap_frame;
  49 };
  50
  51 static bool get_macaddr(struct bnep_frame *bnep_frame, char *str)
  52 {
  53         uint8_t addr[6];
  54         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
  55         int i;
  56
  57         for (i = 0; i < 6; i++)
  58                 if (!l2cap_frame_get_u8(frame, &addr[i]))
  59                         return false;
  60
  61         sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
  62                 addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
  63
  64         return true;
  65 }
  66
  67 static bool bnep_general(struct bnep_frame *bnep_frame,
  68                                         uint8_t indent, int hdr_len)
  69 {
  70         struct l2cap_frame *frame;
  71         char src_addr[20], dest_addr[20];
  72
  73         if (!get_macaddr(bnep_frame, dest_addr))
  74                 return false;
  75
  76         if (!get_macaddr(bnep_frame, src_addr))
  77                 return false;
  78
  79         frame = &bnep_frame->l2cap_frame;
  80
  81         if (!l2cap_frame_get_be16(frame, &proto))
  82                 return false;
  83
  84         print_field("%*cdst %s src %s [proto 0x%04x] ", indent,
  85                                         ' ', dest_addr, src_addr, proto);
  86
  87         return true;
  88
  89 }
  90
  91 static bool cmd_nt_understood(struct bnep_frame *bnep_frame, uint8_t indent)
  92 {
  93         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
  94         uint8_t ptype;
  95
  96         if (!l2cap_frame_get_u8(frame, &ptype))
  97                 return false;
  98
  99         print_field("%*cType: 0x%02x ", indent, ' ', ptype);
 100
 101         return true;
 102 }
 103
 104 static bool setup_conn_req(struct bnep_frame *bnep_frame, uint8_t indent)
 105 {
 106
 107         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 108         uint8_t uuid_size;
 109         uint32_t src_uuid = 0, dst_uuid = 0;
 110
 111         if (!l2cap_frame_get_u8(frame, &uuid_size))
 112                 return false;
 113
 114         print_field("%*cSize: 0x%02x ", indent, ' ', uuid_size);
 115
 116         switch (uuid_size) {
 117         case 2:
 118                 if (!l2cap_frame_get_be16(frame, (uint16_t *) &dst_uuid))
 119                         return false;
 120
 121                 if (!l2cap_frame_get_be16(frame, (uint16_t *) &src_uuid))
 122                         return false;
 123                 break;
 124         case 4:
 125                 if (!l2cap_frame_get_be32(frame, &dst_uuid))
 126                         return false;
 127
 128                 if (!l2cap_frame_get_be32(frame, &src_uuid))
 129                         return false;
 130                 break;
 131         case 16:
 132                 if (!l2cap_frame_get_be32(frame, &dst_uuid))
 133                         return false;
 134
 135                 l2cap_frame_pull(frame, frame, 12);
 136
 137                 if (!l2cap_frame_get_be32(frame, &src_uuid))
 138                         return false;
 139
 140                 l2cap_frame_pull(frame, frame, 12);
 141                 break;
 142         default:
 143                 l2cap_frame_pull(frame, frame, (uuid_size * 2));
 144                 return true;
 145         }
 146
 147         print_field("%*cDst: 0x%x(%s)", indent, ' ', dst_uuid,
 148                                                 bt_uuid32_to_str(dst_uuid));
 149         print_field("%*cSrc: 0x%x(%s)", indent, ' ', src_uuid,
 150                                                 bt_uuid32_to_str(src_uuid));
 151         return true;
 152 }
 153
 154 static const char *value2str(uint16_t value)
 155 {
 156         switch (value) {
 157         case 0x00:
 158                 return "Operation Successful";
 159         case 0x01:
 160                 return "Operation Failed - Invalid Dst Srv UUID";
 161         case 0x02:
 162                 return "Operation Failed - Invalid Src Srv UUID";
 163         case 0x03:
 164                 return "Operation Failed - Invalid Srv UUID size";
 165         case 0x04:
 166                 return "Operation Failed - Conn not allowed";
 167         default:
 168                 return "Unknown";
 169         }
 170 }
 171
 172 static bool print_rsp_msg(struct bnep_frame *bnep_frame, uint8_t indent)
 173 {
 174         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 175         uint16_t rsp_msg;
 176
 177         if (!l2cap_frame_get_be16(frame, &rsp_msg))
 178                 return false;
 179
 180         print_field("%*cRsp msg: %s(0x%04x) ", indent, ' ',
 181                                         value2str(rsp_msg), rsp_msg);
 182
 183         return true;
 184 }
 185
 186 static bool filter_nettype_req(struct bnep_frame *bnep_frame, uint8_t indent)
 187 {
 188         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 189         uint16_t length, start_range, end_range;
 190         int i;
 191
 192         if (!l2cap_frame_get_be16(frame, &length))
 193                 return false;
 194
 195         print_field("%*cLength: 0x%04x", indent, ' ', length);
 196
 197         for (i = 0; i < length / 4; i++) {
 198
 199                 if (!l2cap_frame_get_be16(frame, &start_range))
 200                         return false;
 201
 202                 if (!l2cap_frame_get_be16(frame, &end_range))
 203                         return false;
 204
 205                 print_field("%*c0x%04x - 0x%04x", indent, ' ',
 206                                                 start_range, end_range);
 207         }
 208
 209         return true;
 210 }
 211
 212 static bool filter_multaddr_req(struct bnep_frame *bnep_frame, uint8_t indent)
 213 {
 214         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 215         uint16_t length;
 216         char start_addr[20], end_addr[20];
 217         int i;
 218
 219         if (!l2cap_frame_get_be16(frame, &length))
 220                 return false;
 221
 222         print_field("%*cLength: 0x%04x", indent, ' ', length);
 223
 224         for (i = 0; i < length / 12; i++) {
 225
 226                 if (!get_macaddr(bnep_frame, start_addr))
 227                         return false;
 228
 229                 if (!get_macaddr(bnep_frame, end_addr))
 230                         return false;
 231
 232                 print_field("%*c%s - %s", indent, ' ', start_addr, end_addr);
 233         }
 234
 235         return true;
 236 }
 237
 238 struct bnep_control_data {
 239         uint8_t type;
 240         const char *str;
 241         bool (*func) (struct bnep_frame *frame, uint8_t indent);
 242 };
 243
 244 static const struct bnep_control_data bnep_control_table[] = {
 245         { 0x00, "Command Not Understood",       cmd_nt_understood       },
 246         { 0x01, "Setup Conn Req",               setup_conn_req          },
 247         { 0x02, "Setup Conn Rsp",               print_rsp_msg           },
 248         { 0x03, "Filter NetType Set",           filter_nettype_req      },
 249         { 0x04, "Filter NetType Rsp",           print_rsp_msg           },
 250         { 0x05, "Filter MultAddr Set",          filter_multaddr_req     },
 251         { 0x06, "Filter MultAddr Rsp",          print_rsp_msg           },
 252         { }
 253 };
 254
 255 static bool bnep_control(struct bnep_frame *bnep_frame,
 256                                         uint8_t indent, int hdr_len)
 257 {
 258         uint8_t ctype;
 259         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 260         const struct bnep_control_data *bnep_control_data = NULL;
 261         const char *type_str;
 262         int i;
 263
 264         if (!l2cap_frame_get_u8(frame, &ctype))
 265                 return false;
 266
 267         for (i = 0; bnep_control_table[i].str; i++) {
 268                 if (bnep_control_table[i].type == ctype) {
 269                         bnep_control_data = &bnep_control_table[i];
 270                         break;
 271                 }
 272         }
 273
 274         if (bnep_control_data)
 275                 type_str = bnep_control_data->str;
 276         else
 277                 type_str = "Unknown control type";
 278
 279         print_field("%*c%s (0x%02x) ", indent, ' ', type_str, ctype);
 280
 281         if (!bnep_control_data || !bnep_control_data->func) {
 282                 packet_hexdump(frame->data, hdr_len - 1);
 283                 l2cap_frame_pull(frame, frame, hdr_len - 1);
 284                 goto done;
 285         }
 286
 287         if (!bnep_control_data->func(bnep_frame, indent+2))
 288                 return false;
 289
 290 done:
 291         return true;
 292 }
 293
 294 static bool bnep_compressed(struct bnep_frame *bnep_frame,
 295                                         uint8_t indent, int hdr_len)
 296 {
 297
 298         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 299
 300         if (!l2cap_frame_get_be16(frame, &proto))
 301                 return false;
 302
 303         print_field("%*c[proto 0x%04x] ", indent, ' ', proto);
 304
 305         return true;
 306 }
 307
 308 static bool bnep_src_only(struct bnep_frame *bnep_frame,
 309                                         uint8_t indent, int hdr_len)
 310 {
 311
 312         struct l2cap_frame *frame;
 313         char src_addr[20];
 314
 315         if (!get_macaddr(bnep_frame, src_addr))
 316                 return false;
 317
 318         frame = &bnep_frame->l2cap_frame;
 319
 320         if (!l2cap_frame_get_be16(frame, &proto))
 321                 return false;
 322
 323         print_field("%*csrc %s [proto 0x%04x] ", indent,
 324                                         ' ', src_addr, proto);
 325
 326         return true;
 327 }
 328
 329 static bool bnep_dst_only(struct bnep_frame *bnep_frame,
 330                                         uint8_t indent, int hdr_len)
 331 {
 332
 333         struct l2cap_frame *frame;
 334         char dest_addr[20];
 335
 336         if (!get_macaddr(bnep_frame, dest_addr))
 337                 return false;
 338
 339         frame = &bnep_frame->l2cap_frame;
 340
 341         if (!l2cap_frame_get_be16(frame, &proto))
 342                 return false;
 343
 344         print_field("%*cdst %s [proto 0x%04x] ", indent,
 345                                         ' ', dest_addr, proto);
 346
 347         return true;
 348 }
 349
 350 static bool bnep_eval_extension(struct bnep_frame *bnep_frame, uint8_t indent)
 351 {
 352         struct l2cap_frame *frame = &bnep_frame->l2cap_frame;
 353         uint8_t type, length;
 354         int extension;
 355
 356         if (!l2cap_frame_get_u8(frame, &type))
 357                 return false;
 358
 359         if (!l2cap_frame_get_u8(frame, &length))
 360                 return false;
 361
 362         extension = GET_EXTENSION(type);
 363         type = GET_PKT_TYPE(type);
 364
 365         switch (type) {
 366         case BNEP_EXTENSION_CONTROL:
 367                 print_field("%*cExt Control(0x%02x|%s) len 0x%02x", indent,
 368                                 ' ', type, extension ? "1" : "0", length);
 369                 if (!bnep_control(bnep_frame, indent+2, length))
 370                         return false;
 371                 break;
 372
 373         default:
 374                 print_field("%*cExt Unknown(0x%02x|%s) len 0x%02x", indent,
 375                                 ' ', type, extension ? "1" : "0", length);
 376                 packet_hexdump(frame->data, length);
 377                 l2cap_frame_pull(frame, frame, length);
 378         }
 379
 380         if (extension)
 381                 if (!bnep_eval_extension(bnep_frame, indent))
 382                         return false;
 383
 384         return true;
 385 }
 386
 387 struct bnep_data {
 388         uint8_t type;
 389         const char *str;
 390         bool (*func) (struct bnep_frame *frame, uint8_t indent, int hdr_len);
 391 };
 392
 393 static const struct bnep_data bnep_table[] = {
 394         { 0x00, "General Ethernet",             bnep_general    },
 395         { 0x01, "Control",                      bnep_control    },
 396         { 0x02, "Compressed Ethernet",          bnep_compressed },
 397         { 0x03, "Compressed Ethernet SrcOnly",  bnep_src_only   },
 398         { 0x04, "Compressed Ethernet DestOnly", bnep_dst_only   },
 399         { }
 400 };
 401
 402 void bnep_packet(const struct l2cap_frame *frame)
 403 {
 404         uint8_t type, indent = 1;
 405         struct bnep_frame bnep_frame;
 406         struct l2cap_frame *l2cap_frame;
 407         const struct bnep_data *bnep_data = NULL;
 408         const char *pdu_color, *pdu_str;
 409         int i;
 410
 411         l2cap_frame_pull(&bnep_frame.l2cap_frame, frame, 0);
 412         l2cap_frame = &bnep_frame.l2cap_frame;
 413
 414         if (!l2cap_frame_get_u8(l2cap_frame, &type))
 415                 goto fail;
 416
 417         bnep_frame.extension = GET_EXTENSION(type);
 418         bnep_frame.type = GET_PKT_TYPE(type);
 419
 420         for (i = 0; bnep_table[i].str; i++) {
 421                 if (bnep_table[i].type == bnep_frame.type) {
 422                         bnep_data = &bnep_table[i];
 423                         break;
 424                 }
 425         }
 426
 427         if (bnep_data) {
 428                 if (bnep_data->func) {
 429                         if (frame->in)
 430                                 pdu_color = COLOR_MAGENTA;
 431                         else
 432                                 pdu_color = COLOR_BLUE;
 433                 } else
 434                         pdu_color = COLOR_WHITE_BG;
 435                 pdu_str = bnep_data->str;
 436         } else {
 437                 pdu_color = COLOR_WHITE_BG;
 438                 pdu_str = "Unknown packet type";
 439         }
 440
 441         print_indent(6, pdu_color, "BNEP: ", pdu_str, COLOR_OFF,
 442                                 " (0x%02x|%s)", bnep_frame.type,
 443                                 bnep_frame.extension ? "1" : "0");
 444
 445         if (!bnep_data || !bnep_data->func) {
 446                 packet_hexdump(l2cap_frame->data, l2cap_frame->size);
 447                 return;
 448         }
 449
 450         if (!bnep_data->func(&bnep_frame, indent, -1))
 451                 goto fail;
 452
 453         /* Extension info */
 454         if (bnep_frame.extension)
 455                 if (!bnep_eval_extension(&bnep_frame, indent+2))
 456                         goto fail;
 457
 458         /* Control packet => No payload info */
 459         if (bnep_frame.type == BNEP_CONTROL)
 460                 return;
 461
 462         /* TODO: Handle BNEP IP packet */
 463         packet_hexdump(l2cap_frame->data, l2cap_frame->size);
 464
 465         return;
 466
 467 fail:
 468         print_text(COLOR_ERROR, "frame too short");
 469         packet_hexdump(frame->data, frame->size);
 470 }