1 /* -*- c -*- */ 2 /* 3 * Copyright 2007 - 2012 Mike Ryan, Dominic Spill, Michael Ossmann 4 * Copyright 2005, 2006 Free Software Foundation, Inc. 5 * 6 * This file is part of libbtbb 7 * 8 * This program is free software; you can redistribute it and/or modify 9 * it under the terms of the GNU General Public License as published by 10 * the Free Software Foundation; either version 2, or (at your option) 11 * any later version. 12 * 13 * This program is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU General Public License for more details. 17 * 18 * You should have received a copy of the GNU General Public License 19 * along with libbtbb; see the file COPYING. If not, write to 20 * the Free Software Foundation, Inc., 51 Franklin Street, 21 * Boston, MA 02110-1301, USA. 22 */ 23 24 #ifdef HAVE_CONFIG_H 25 #include "config.h" 26 #endif 27 28 #include <string.h> 29 #include "bluetooth_le_packet.h" 30 #include <ctype.h> 31 32 /* string representations of advertising packet type */ 33 static const char *ADV_TYPE_NAMES[] = { 34 "ADV_IND", "ADV_DIRECT_IND", "ADV_NONCONN_IND", "SCAN_REQ", 35 "SCAN_RSP", "CONNECT_REQ", "ADV_SCAN_IND", 36 }; 37 38 /* source clock accuracy in a connect packet */ 39 static const char *CONNECT_SCA[] = { 40 "251 ppm to 500 ppm", "151 ppm to 250 ppm", "101 ppm to 150 ppm", 41 "76 ppm to 100 ppm", "51 ppm to 75 ppm", "31 ppm to 50 ppm", 42 "21 ppm to 30 ppm", "0 ppm to 20 ppm", 43 }; 44 45 // count of objects in an array, shamelessly stolen from Chrome 46 #define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x]))))) 47 48 void decode_le(uint8_t *stream, uint16_t phys_channel, uint32_t clk100ns, le_packet_t *p) { 49 memcpy(p->symbols, stream, MAX_LE_SYMBOLS); 50 51 p->channel_idx = le_channel_index(phys_channel); 52 p->clk100ns = clk100ns; 53 54 p->access_address = 0; 55 p->access_address |= p->symbols[0]; 56 p->access_address |= p->symbols[1] << 8; 57 p->access_address |= p->symbols[2] << 16; 58 p->access_address |= p->symbols[3] << 24; 59 60 if (le_packet_is_data(p)) { 61 // data PDU 62 p->length = p->symbols[5] & 0x1f; 63 } else { 64 // advertising PDU 65 p->length = p->symbols[5] & 0x3f; 66 p->adv_type = p->symbols[4] & 0xf; 67 p->adv_tx_add = p->symbols[4] & 0x40 ? 1 : 0; 68 p->adv_rx_add = p->symbols[4] & 0x80 ? 1 : 0; 69 } 70 } 71 72 int le_packet_is_data(le_packet_t *p) { 73 return p->channel_idx < 37; 74 } 75 76 uint8_t le_channel_index(uint16_t phys_channel) { 77 uint8_t ret; 78 if (phys_channel == 2402) { 79 ret = 37; 80 } else if (phys_channel < 2426) { // 0 - 10 81 ret = (phys_channel - 2404) / 2; 82 } else if (phys_channel == 2426) { 83 ret = 38; 84 } else if (phys_channel < 2480) { // 11 - 36 85 ret = 11 + (phys_channel - 2428) / 2; 86 } else { 87 ret = 39; 88 } 89 return ret; 90 } 91 92 const char *le_adv_type(le_packet_t *p) { 93 if (le_packet_is_data(p)) 94 return NULL; 95 if (p->adv_type < COUNT_OF(ADV_TYPE_NAMES)) 96 return ADV_TYPE_NAMES[p->adv_type]; 97 return "UNKNOWN"; 98 } 99 100 static void _dump_addr(char *name, uint8_t *buf, int offset, int random) { 101 int i; 102 printf(" %s%02x", name, buf[offset+5]); 103 for (i = 4; i >= 0; --i) 104 printf(":%02x", buf[offset+i]); 105 printf(" (%s)\n", random ? "random" : "public"); 106 } 107 108 static void _dump_8(char *name, uint8_t *buf, int offset) { 109 printf(" %s%02x (%d)\n", name, buf[offset], buf[offset]); 110 } 111 112 static void _dump_16(char *name, uint8_t *buf, int offset) { 113 uint16_t val = buf[offset+1] << 8 | buf[offset]; 114 printf(" %s%04x (%d)\n", name, val, val); 115 } 116 117 static void _dump_24(char *name, uint8_t *buf, int offset) { 118 uint16_t val = buf[offset+2] << 16 | buf[offset+1] << 8 | buf[offset]; 119 printf(" %s%06x\n", name, val); 120 } 121 122 static void _dump_32(char *name, uint8_t *buf, int offset) { 123 uint32_t val = buf[offset+3] << 24 | 124 buf[offset+2] << 16 | 125 buf[offset+1] << 8 | 126 buf[offset+0]; 127 printf(" %s%08x\n", name, val); 128 } 129 130 static void _dump_uuid(uint8_t *uuid) { 131 int i; 132 for (i = 0; i < 4; ++i) 133 printf("%02x", uuid[i]); 134 printf("-"); 135 for (i = 4; i < 6; ++i) 136 printf("%02x", uuid[i]); 137 printf("-"); 138 for (i = 6; i < 8; ++i) 139 printf("%02x", uuid[i]); 140 printf("-"); 141 for (i = 8; i < 10; ++i) 142 printf("%02x", uuid[i]); 143 printf("-"); 144 for (i = 10; i < 16; ++i) 145 printf("%02x", uuid[i]); 146 } 147 148 // Refer to pg 1735 of Bluetooth Core Spec 4.0 149 static void _dump_scan_rsp_data(uint8_t *buf, int len) { 150 int pos = 0; 151 int sublen, i; 152 uint8_t type; 153 uint16_t val; 154 char *cval; 155 156 while (pos < len) { 157 sublen = buf[pos]; 158 ++pos; 159 if (pos + sublen > len) { 160 printf("Error: attempt to read past end of buffer (%d + %d > %d)\n", pos, sublen, len); 161 return; 162 } 163 if (sublen == 0) { 164 printf("Early return due to 0 length\n"); 165 return; 166 } 167 type = buf[pos]; 168 printf(" Type %02x", type); 169 switch (type) { 170 case 0x01: 171 printf(" (Flags)\n"); 172 printf(" "); 173 for (i = 0; i < 8; ++i) 174 printf("%d", buf[pos+1] & (1 << (7-i)) ? 1 : 0); 175 printf("\n"); 176 break; 177 case 0x06: 178 printf(" (128-bit Service UUIDs, more available)\n"); 179 goto print128; 180 case 0x07: 181 printf(" (128-bit Service UUIDs)\n"); 182 print128: 183 if ((sublen - 1) % 16 == 0) { 184 uint8_t uuid[16]; 185 for (i = 0; i < sublen - 1; ++i) { 186 uuid[15 - (i % 16)] = buf[pos+1+i]; 187 if ((i & 15) == 15) { 188 printf(" "); 189 _dump_uuid(uuid); 190 printf("\n"); 191 } 192 } 193 } 194 else { 195 printf("Wrong length (%d, must be divisible by 16)\n", sublen-1); 196 } 197 break; 198 case 0x09: 199 printf(" (Complete Local Name)\n"); 200 printf(" "); 201 for (i = 1; i < sublen; ++i) 202 printf("%c", isprint(buf[pos+i]) ? buf[pos+i] : '.'); 203 printf("\n"); 204 break; 205 case 0x0a: 206 printf(" (Tx Power Level)\n"); 207 printf(" "); 208 if (sublen-1 == 1) { 209 cval = (char *)&buf[pos+1]; 210 printf("%d dBm\n", *cval); 211 } else { 212 printf("Wrong length (%d, should be 1)\n", sublen-1); 213 } 214 break; 215 case 0x12: 216 printf(" (Slave Connection Interval Range)\n"); 217 printf(" "); 218 if (sublen-1 == 4) { 219 val = (buf[pos+2] << 8) | buf[pos+1]; 220 printf("(%0.2f, ", val * 1.25); 221 val = (buf[pos+4] << 8) | buf[pos+3]; 222 printf("%0.2f) ms\n", val * 1.25); 223 } 224 else { 225 printf("Wrong length (%d, should be 4)\n", sublen-1); 226 } 227 break; 228 case 0x16: 229 printf(" (Service Data)\n"); 230 printf(" "); 231 if (sublen-1 >= 2) { 232 val = (buf[pos+2] << 8) | buf[pos+1]; 233 printf("UUID: %02x", val); 234 if (sublen-1 > 2) { 235 printf(", Additional:"); 236 for (i = 3; i < sublen; ++i) 237 printf(" %02x", buf[pos+i]); 238 } 239 printf("\n"); 240 } 241 else { 242 printf("Wrong length (%d, should be >= 2)\n", sublen-1); 243 } 244 break; 245 default: 246 printf("\n"); 247 printf(" "); 248 for (i = 1; i < sublen; ++i) 249 printf(" %02x", buf[pos+i]); 250 printf("\n"); 251 } 252 pos += sublen; 253 } 254 } 255 256 void le_print(le_packet_t *p) { 257 int i, opcode; 258 if (le_packet_is_data(p)) { 259 int llid = p->symbols[4] & 0x3; 260 static const char *llid_str[] = { 261 "Reserved", 262 "LL Data PDU / empty or L2CAP continuation", 263 "LL Data PDU / L2CAP start", 264 "LL Control PDU", 265 }; 266 267 printf("Data / AA %08x / %2d bytes\n", p->access_address, p->length); 268 printf(" Channel Index: %d\n", p->channel_idx); 269 printf(" LLID: %d / %s\n", llid, llid_str[llid]); 270 printf(" NESN: %d SN: %d MD: %d\n", (p->symbols[4] >> 2) & 1, 271 (p->symbols[4] >> 3) & 1, 272 (p->symbols[4] >> 4) & 1); 273 switch (llid) { 274 case 3: // LL Control PDU 275 opcode = p->symbols[6]; 276 static const char *opcode_str[] = { 277 "LL_CONNECTION_UPDATE_REQ", 278 "LL_CHANNEL_MAP_REQ", 279 "LL_TERMINATE_IND", 280 "LL_ENC_REQ", 281 "LL_ENC_RSP", 282 "LL_START_ENC_REQ", 283 "LL_START_ENC_RSP", 284 "LL_UNKNOWN_RSP", 285 "LL_FEATURE_REQ", 286 "LL_FEATURE_RSP", 287 "LL_PAUSE_ENC_REQ", 288 "LL_PAUSE_ENC_RSP", 289 "LL_VERSION_IND", 290 "LL_REJECT_IND", 291 "LL_SLAVE_FEATURE_REQ", 292 "LL_CONNECTION_PARAM_REQ", 293 "LL_CONNECTION_PARAM_RSP", 294 "LL_REJECT_IND_EXT", 295 "LL_PING_REQ", 296 "LL_PING_RSP", 297 "Reserved for Future Use", 298 }; 299 printf(" Opcode: %d / %s\n", opcode, opcode_str[(opcode<0x14)?opcode:0x14]); 300 break; 301 default: 302 break; 303 } 304 } else { 305 printf("Advertising / AA %08x / %2d bytes\n", p->access_address, p->length); 306 printf(" Channel Index: %d\n", p->channel_idx); 307 printf(" Type: %s\n", le_adv_type(p)); 308 309 switch(p->adv_type) { 310 case ADV_IND: 311 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 312 if (p->length-6 > 0) { 313 printf(" AdvData:"); 314 for (i = 0; i < p->length - 6; ++i) 315 printf(" %02x", p->symbols[12+i]); 316 printf("\n"); 317 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 318 } 319 break; 320 case SCAN_REQ: 321 _dump_addr("ScanA: ", p->symbols, 6, p->adv_tx_add); 322 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 323 break; 324 case SCAN_RSP: 325 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 326 printf(" ScanRspData:"); 327 for (i = 0; i < p->length - 6; ++i) 328 printf(" %02x", p->symbols[12+i]); 329 printf("\n"); 330 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 331 break; 332 case CONNECT_REQ: 333 _dump_addr("InitA: ", p->symbols, 6, p->adv_tx_add); 334 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 335 _dump_32("AA: ", p->symbols, 18); 336 _dump_24("CRCInit: ", p->symbols, 22); 337 _dump_8("WinSize: ", p->symbols, 25); 338 _dump_16("WinOffset: ", p->symbols, 26); 339 _dump_16("Interval: ", p->symbols, 28); 340 _dump_16("Latency: ", p->symbols, 30); 341 _dump_16("Timeout: ", p->symbols, 32); 342 343 printf(" ChM:"); 344 for (i = 0; i < 5; ++i) 345 printf(" %02x", p->symbols[34+i]); 346 printf("\n"); 347 348 printf(" Hop: %d\n", p->symbols[39] & 0x1f); 349 printf(" SCA: %d, %s\n", 350 p->symbols[39] >> 5, 351 CONNECT_SCA[p->symbols[39] >> 5]); 352 break; 353 } 354 } 355 356 printf("\n"); 357 printf(" Data: "); 358 for (i = 6; i < 6 + p->length; ++i) 359 printf(" %02x", p->symbols[i]); 360 printf("\n"); 361 362 printf(" CRC: "); 363 for (i = 0; i < 3; ++i) 364 printf(" %02x", p->symbols[6 + p->length + i]); 365 printf("\n"); 366 } 367