monitor/ll.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 <inttypes.h>
  31
  32 #include "src/shared/util.h"
  33 #include "display.h"
  34 #include "packet.h"
  35 #include "crc.h"
  36 #include "bt.h"
  37 #include "ll.h"
  38
  39 #define COLOR_OPCODE            COLOR_MAGENTA
  40 #define COLOR_OPCODE_UNKNOWN    COLOR_WHITE_BG
  41
  42 #define MAX_CHANNEL 16
  43
  44 struct channel_data {
  45         uint32_t access_addr;
  46         uint32_t crc_init;
  47 };
  48
  49 static struct channel_data channel_list[MAX_CHANNEL];
  50
  51 static void set_crc_init(uint32_t access_addr, uint32_t crc_init)
  52 {
  53         int i;
  54
  55         for (i = 0; i < MAX_CHANNEL; i++) {
  56                 if (channel_list[i].access_addr == 0x00000000 ||
  57                                 channel_list[i].access_addr == access_addr) {
  58                         channel_list[i].access_addr = access_addr;
  59                         channel_list[i].crc_init = crc_init;
  60                         break;
  61                 }
  62         }
  63 }
  64
  65 static uint32_t get_crc_init(uint32_t access_addr)
  66 {
  67         int i;
  68
  69         for (i = 0; i < MAX_CHANNEL; i++) {
  70                 if (channel_list[i].access_addr == access_addr)
  71                         return channel_list[i].crc_init;
  72         }
  73
  74         return 0x00000000;
  75 }
  76
  77 static void advertising_packet(const void *data, uint8_t size)
  78 {
  79         const uint8_t *ptr = data;
  80         uint8_t pdu_type, length, win_size, hop, sca;
  81         bool tx_add, rx_add;
  82         uint32_t access_addr, crc_init;
  83         uint16_t win_offset, interval, latency, timeout;
  84         const char *str;
  85
  86         if (size < 2) {
  87                 print_text(COLOR_ERROR, "packet too short");
  88                 packet_hexdump(data, size);
  89                 return;
  90         }
  91
  92         pdu_type = ptr[0] & 0x0f;
  93         tx_add = !!(ptr[0] & 0x40);
  94         rx_add = !!(ptr[0] & 0x80);
  95         length = ptr[1] & 0x3f;
  96
  97         switch (pdu_type) {
  98         case 0x00:
  99                 str = "ADV_IND";
 100                 break;
 101         case 0x01:
 102                 str = "ADV_DIRECT_IND";
 103                 break;
 104         case 0x02:
 105                 str = "ADV_NONCONN_IND";
 106                 break;
 107         case 0x03:
 108                 str = "SCAN_REQ";
 109                 break;
 110         case 0x04:
 111                 str = "SCAN_RSP";
 112                 break;
 113         case 0x05:
 114                 str = "CONNECT_REQ";
 115                 break;
 116         case 0x06:
 117                 str = "ADV_SCAN_IND";
 118                 break;
 119         default:
 120                 str = "Reserved";
 121                 break;
 122         }
 123
 124         print_field("Type: %s (0x%2.2x)", str, pdu_type);
 125         print_field("TxAdd: %u", tx_add);
 126         print_field("RxAdd: %u", rx_add);
 127         print_field("Length: %u", length);
 128
 129         if (length != size - 2) {
 130                 print_text(COLOR_ERROR, "packet size mismatch");
 131                 packet_hexdump(data + 2, size - 2);
 132                 return;
 133         }
 134
 135         switch (pdu_type) {
 136         case 0x00:      /* ADV_IND */
 137         case 0x02:      /* AVD_NONCONN_IND */
 138         case 0x06:      /* ADV_SCAN_IND */
 139         case 0x04:      /* SCAN_RSP */
 140                 if (length < 6) {
 141                         print_text(COLOR_ERROR, "payload too short");
 142                         packet_hexdump(data + 2, length);
 143                         return;
 144                 }
 145
 146                 packet_print_addr("Advertiser address", data + 2, tx_add);
 147                 packet_print_ad(data + 8, length - 6);
 148                 break;
 149
 150         case 0x01:      /* ADV_DIRECT_IND */
 151                 if (length < 12) {
 152                         print_text(COLOR_ERROR, "payload too short");
 153                         packet_hexdump(data + 2, length);
 154                         return;
 155                 }
 156
 157                 packet_print_addr("Advertiser address", data + 2, tx_add);
 158                 packet_print_addr("Inititator address", data + 8, rx_add);
 159                 break;
 160
 161         case 0x03:      /* SCAN_REQ */
 162                 if (length < 12) {
 163                         print_text(COLOR_ERROR, "payload too short");
 164                         packet_hexdump(data + 2, length);
 165                         return;
 166                 }
 167
 168                 packet_print_addr("Scanner address", data + 2, tx_add);
 169                 packet_print_addr("Advertiser address", data + 8, rx_add);
 170                 break;
 171
 172         case 0x05:      /* CONNECT_REQ */
 173                 if (length < 34) {
 174                         print_text(COLOR_ERROR, "payload too short");
 175                         packet_hexdump(data + 2, length);
 176                         return;
 177                 }
 178
 179                 packet_print_addr("Inititator address", data + 2, tx_add);
 180                 packet_print_addr("Advertiser address", data + 8, rx_add);
 181
 182                 access_addr = ptr[14] | ptr[15] << 8 |
 183                                         ptr[16] << 16 | ptr[17] << 24;
 184                 crc_init = ptr[18] | ptr[19] << 8 | ptr[20] << 16;
 185
 186                 print_field("Access address: 0x%8.8x", access_addr);
 187                 print_field("CRC init: 0x%6.6x", crc_init);
 188
 189                 set_crc_init(access_addr, crc24_bit_reverse(crc_init));
 190
 191                 win_size = ptr[21];
 192                 win_offset = ptr[22] | ptr[23] << 8;
 193                 interval = ptr[24] | ptr[25] << 8;
 194                 latency = ptr[26] | ptr[27] << 8;
 195                 timeout = ptr[28] | ptr[29] << 8;
 196
 197                 print_field("Transmit window size: %u", win_size);
 198                 print_field("Transmit window offset: %u", win_offset);
 199                 print_field("Connection interval: %u", interval);
 200                 print_field("Connection slave latency: %u", latency);
 201                 print_field("Connection supervision timeout: %u", timeout);
 202
 203                 packet_print_channel_map_ll(ptr + 30);
 204
 205                 hop = ptr[35] & 0x1f;
 206                 sca = (ptr[35] & 0xe0) >> 5;
 207
 208                 switch (sca) {
 209                 case 0:
 210                         str = "251 ppm to 500 ppm";
 211                         break;
 212                 case 1:
 213                         str = "151 ppm to 250 ppm";
 214                         break;
 215                 case 2:
 216                         str = "101 ppm to 150ppm";
 217                         break;
 218                 case 3:
 219                         str = "76 ppm to 100 ppm";
 220                         break;
 221                 case 4:
 222                         str = "51 ppm to 75 ppm";
 223                         break;
 224                 case 5:
 225                         str = "31 ppm to 50 ppm";
 226                         break;
 227                 case 6:
 228                         str = "21 ppm to 30 ppm";
 229                         break;
 230                 case 7:
 231                         str = "0 ppm to 20 ppm";
 232                         break;
 233                 default:
 234                         str = "Invalid";
 235                         break;
 236                 }
 237
 238                 print_field("Hop increment: %u", hop);
 239                 print_field("Sleep clock accuracy: %s (%u)", str, sca);
 240                 break;
 241
 242         default:
 243                 packet_hexdump(data + 2, length);
 244                 break;
 245         }
 246 }
 247
 248 static void data_packet(const void *data, uint8_t size, bool padded)
 249 {
 250         const uint8_t *ptr = data;
 251         uint8_t llid, length;
 252         bool nesn, sn, md;
 253         const char *str;
 254
 255         if (size < 2) {
 256                 print_text(COLOR_ERROR, "packet too short");
 257                 packet_hexdump(data, size);
 258                 return;
 259         }
 260
 261         llid = ptr[0] & 0x03;
 262         nesn = !!(ptr[0] & 0x04);
 263         sn = !!(ptr[0] & 0x08);
 264         md = !!(ptr[0] & 0x10);
 265         length = ptr[1] & 0x1f;
 266
 267         switch (llid) {
 268         case 0x01:
 269                 if (length > 0)
 270                         str = "Continuation fragement of L2CAP message";
 271                 else
 272                         str = "Empty message";
 273                 break;
 274         case 0x02:
 275                 str = "Start of L2CAP message";
 276                 break;
 277         case 0x03:
 278                 str = "Control";
 279                 break;
 280         default:
 281                 str = "Reserved";
 282                 break;
 283         }
 284
 285         print_field("LLID: %s (0x%2.2x)", str, llid);
 286         print_field("Next expected sequence number: %u", nesn);
 287         print_field("Sequence number: %u", sn);
 288         print_field("More data: %u", md);
 289         print_field("Length: %u", length);
 290
 291         switch (llid) {
 292         case 0x03:
 293                 llcp_packet(data + 2, size - 2, padded);
 294                 break;
 295
 296         default:
 297                 packet_hexdump(data + 2, size - 2);
 298                 break;
 299         }
 300 }
 301
 302 void ll_packet(uint16_t frequency, const void *data, uint8_t size, bool padded)
 303 {
 304         const struct bt_ll_hdr *hdr = data;
 305         uint8_t channel = (frequency - 2402) / 2;
 306         uint32_t access_addr;
 307         char access_str[12];
 308         const char *channel_label, *channel_color;
 309         const uint8_t *pdu_data;
 310         uint8_t pdu_len;
 311         uint32_t pdu_crc, crc, crc_init;
 312
 313         if (size < sizeof(*hdr)) {
 314                 print_text(COLOR_ERROR, "packet missing header");
 315                 packet_hexdump(data, size);
 316                 return;
 317         }
 318
 319         if (size < sizeof(*hdr) + 3) {
 320                 print_text(COLOR_ERROR, "packet missing checksum");
 321                 packet_hexdump(data, size);
 322                 return;
 323         }
 324
 325         if (hdr->preamble != 0xaa && hdr->preamble != 0x55) {
 326                 print_text(COLOR_ERROR, "invalid preamble");
 327                 packet_hexdump(data, size);
 328                 return;
 329         }
 330
 331         access_addr = le32_to_cpu(hdr->access_addr);
 332
 333         pdu_data = data + sizeof(*hdr);
 334         pdu_len = size - sizeof(*hdr) - 3;
 335
 336         pdu_crc = pdu_data[pdu_len + 0] | (pdu_data[pdu_len + 1] << 8) |
 337                                                 (pdu_data[pdu_len + 2] << 16);
 338
 339         if (access_addr == 0x8e89bed6) {
 340                 channel_label = "Advertising channel: ";
 341                 channel_color = COLOR_MAGENTA;
 342         } else {
 343                 channel_label = "Data channel: ";
 344                 channel_color = COLOR_CYAN;
 345         }
 346
 347         sprintf(access_str, "0x%8.8x", access_addr);
 348
 349         print_indent(6, channel_color, channel_label, access_str, COLOR_OFF,
 350                 " (channel %d) len %d crc 0x%6.6x", channel, pdu_len, pdu_crc);
 351
 352         if (access_addr == 0x8e89bed6)
 353                 crc_init = 0xaaaaaa;
 354         else
 355                 crc_init = get_crc_init(access_addr);
 356
 357         if (crc_init) {
 358                 crc = crc24_calculate(crc_init, pdu_data, pdu_len);
 359
 360                 if (crc != pdu_crc) {
 361                         print_text(COLOR_ERROR, "invalid checksum");
 362                         packet_hexdump(pdu_data, pdu_len);
 363                         return;
 364                 }
 365         } else
 366                 print_text(COLOR_ERROR, "unknown access address");
 367
 368         if (access_addr == 0x8e89bed6)
 369                 advertising_packet(pdu_data, pdu_len);
 370         else
 371                 data_packet(pdu_data, pdu_len, padded);
 372 }
 373
 374 static void null_pdu(const void *data, uint8_t size)
 375 {
 376 }
 377
 378 static void conn_update_req(const void *data, uint8_t size)
 379 {
 380         const struct bt_ll_conn_update_req *pdu = data;
 381
 382         print_field("Transmit window size: %u", pdu->win_size);
 383         print_field("Transmit window offset: %u", le16_to_cpu(pdu->win_offset));
 384         print_field("Connection interval: %u", le16_to_cpu(pdu->interval));
 385         print_field("Connection slave latency: %u", le16_to_cpu(pdu->latency));
 386         print_field("Connection supervision timeout: %u", le16_to_cpu(pdu->timeout));
 387         print_field("Connection instant: %u", le16_to_cpu(pdu->instant));
 388 }
 389
 390 static void channel_map_req(const void *data, uint8_t size)
 391 {
 392         const struct bt_ll_channel_map_req *pdu = data;
 393
 394         packet_print_channel_map_ll(pdu->map);
 395         print_field("Connection instant: %u", le16_to_cpu(pdu->instant));
 396 }
 397
 398 static void terminate_ind(const void *data, uint8_t size)
 399 {
 400         const struct bt_ll_terminate_ind *pdu = data;
 401
 402         packet_print_error("Error code", pdu->error);
 403 }
 404
 405 static void enc_req(const void *data, uint8_t size)
 406 {
 407         const struct bt_ll_enc_req *pdu = data;
 408
 409         print_field("Rand: 0x%16.16" PRIx64, le64_to_cpu(pdu->rand));
 410         print_field("EDIV: 0x%4.4x", le16_to_cpu(pdu->ediv));
 411         print_field("SKD (master): 0x%16.16" PRIx64, le64_to_cpu(pdu->skd));
 412         print_field("IV (master): 0x%8.8x", le32_to_cpu(pdu->iv));
 413 }
 414
 415 static void enc_rsp(const void *data, uint8_t size)
 416 {
 417         const struct bt_ll_enc_rsp *pdu = data;
 418
 419         print_field("SKD (slave): 0x%16.16" PRIx64, le64_to_cpu(pdu->skd));
 420         print_field("IV (slave): 0x%8.8x", le32_to_cpu(pdu->iv));
 421 }
 422
 423 static const char *opcode_to_string(uint8_t opcode);
 424
 425 static void unknown_rsp(const void *data, uint8_t size)
 426 {
 427         const struct bt_ll_unknown_rsp *pdu = data;
 428
 429         print_field("Unknown type: %s (0x%2.2x)",
 430                                 opcode_to_string(pdu->type), pdu->type);
 431 }
 432
 433 static void feature_req(const void *data, uint8_t size)
 434 {
 435         const struct bt_ll_feature_req *pdu = data;
 436
 437         packet_print_features_ll(pdu->features);
 438 }
 439
 440 static void feature_rsp(const void *data, uint8_t size)
 441 {
 442         const struct bt_ll_feature_rsp *pdu = data;
 443
 444         packet_print_features_ll(pdu->features);
 445 }
 446
 447 static void version_ind(const void *data, uint8_t size)
 448 {
 449         const struct bt_ll_version_ind *pdu = data;
 450
 451         packet_print_version("Version", pdu->version,
 452                                 "Subversion", le16_to_cpu(pdu->subversion));
 453         packet_print_company("Company", le16_to_cpu(pdu->company));
 454 }
 455
 456 static void reject_ind(const void *data, uint8_t size)
 457 {
 458         const struct bt_ll_reject_ind *pdu = data;
 459
 460         packet_print_error("Error code", pdu->error);
 461 }
 462
 463 static void slave_feature_req(const void *data, uint8_t size)
 464 {
 465         const struct bt_ll_slave_feature_req *pdu = data;
 466
 467         packet_print_features_ll(pdu->features);
 468 }
 469
 470 static void reject_ind_ext(const void *data, uint8_t size)
 471 {
 472         const struct bt_ll_reject_ind_ext *pdu = data;
 473
 474         print_field("Reject opcode: %u (0x%2.2x)", pdu->opcode, pdu->opcode);
 475         packet_print_error("Error code", pdu->error);
 476 }
 477
 478 struct llcp_data {
 479         uint8_t opcode;
 480         const char *str;
 481         void (*func) (const void *data, uint8_t size);
 482         uint8_t size;
 483         bool fixed;
 484 };
 485
 486 static const struct llcp_data llcp_table[] = {
 487         { 0x00, "LL_CONNECTION_UPDATE_REQ", conn_update_req,   11, true },
 488         { 0x01, "LL_CHANNEL_MAP_REQ",       channel_map_req,    7, true },
 489         { 0x02, "LL_TERMINATE_IND",         terminate_ind,      1, true },
 490         { 0x03, "LL_ENC_REQ",               enc_req,           22, true },
 491         { 0x04, "LL_ENC_RSP",               enc_rsp,           12, true },
 492         { 0x05, "LL_START_ENC_REQ",         null_pdu,           0, true },
 493         { 0x06, "LL_START_ENC_RSP",         null_pdu,           0, true },
 494         { 0x07, "LL_UNKNOWN_RSP",           unknown_rsp,        1, true },
 495         { 0x08, "LL_FEATURE_REQ",           feature_req,        8, true },
 496         { 0x09, "LL_FEATURE_RSP",           feature_rsp,        8, true },
 497         { 0x0a, "LL_PAUSE_ENC_REQ",         null_pdu,           0, true },
 498         { 0x0b, "LL_PAUSE_ENC_RSP",         null_pdu,           0, true },
 499         { 0x0c, "LL_VERSION_IND",           version_ind,        5, true },
 500         { 0x0d, "LL_REJECT_IND",            reject_ind,         1, true },
 501         { 0x0e, "LL_SLAVE_FEATURE_REQ",     slave_feature_req,  8, true },
 502         { 0x0f, "LL_CONNECTION_PARAM_REQ",  NULL,              23, true },
 503         { 0x10, "LL_CONNECTION_PARAM_RSP",  NULL,              23, true },
 504         { 0x11, "LL_REJECT_IND_EXT",        reject_ind_ext,     2, true },
 505         { 0x12, "LL_PING_REQ",              null_pdu,           0, true },
 506         { 0x13, "LL_PING_RSP",              null_pdu,           0, true },
 507         { 0x14, "LL_LENGTH_REQ",            NULL,               8, true },
 508         { 0x15, "LL_LENGTH_RSP",            NULL,               8, true },
 509         { }
 510 };
 511
 512 static const char *opcode_to_string(uint8_t opcode)
 513 {
 514         int i;
 515
 516         for (i = 0; llcp_table[i].str; i++) {
 517                 if (llcp_table[i].opcode == opcode)
 518                         return llcp_table[i].str;
 519         }
 520
 521         return "Unknown";
 522 }
 523
 524 void llcp_packet(const void *data, uint8_t size, bool padded)
 525 {
 526         uint8_t opcode = ((const uint8_t *) data)[0];
 527         const struct llcp_data *llcp_data = NULL;
 528         const char *opcode_color, *opcode_str;
 529         int i;
 530
 531         for (i = 0; llcp_table[i].str; i++) {
 532                 if (llcp_table[i].opcode == opcode) {
 533                         llcp_data = &llcp_table[i];
 534                         break;
 535                 }
 536         }
 537
 538         if (llcp_data) {
 539                 if (llcp_data->func)
 540                         opcode_color = COLOR_OPCODE;
 541                 else
 542                         opcode_color = COLOR_OPCODE_UNKNOWN;
 543                 opcode_str = llcp_data->str;
 544         } else {
 545                 opcode_color = COLOR_OPCODE_UNKNOWN;
 546                 opcode_str = "Unknown";
 547         }
 548
 549         print_indent(6, opcode_color, "", opcode_str, COLOR_OFF,
 550                                                 " (0x%2.2x)", opcode);
 551
 552         if (!llcp_data || !llcp_data->func) {
 553                 packet_hexdump(data + 1, size - 1);
 554                 return;
 555         }
 556
 557         if (llcp_data->fixed && !padded) {
 558                 if (size - 1 != llcp_data->size) {
 559                         print_text(COLOR_ERROR, "invalid packet size");
 560                         packet_hexdump(data + 1, size - 1);
 561                         return;
 562                 }
 563         } else {
 564                 if (size - 1 < llcp_data->size) {
 565                         print_text(COLOR_ERROR, "too short packet");
 566                         packet_hexdump(data + 1, size - 1);
 567                         return;
 568                 }
 569         }
 570
 571         llcp_data->func(data + 1, size - 1);
 572 }