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