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 0x07: 178 printf(" (128-bit Service UUIDs)\n"); 179 if ((sublen - 1) % 16 == 0) { 180 uint8_t uuid[16]; 181 for (i = 0; i < sublen - 1; ++i) { 182 uuid[15 - (i % 16)] = buf[pos+1+i]; 183 if ((i & 15) == 15) { 184 printf(" "); 185 _dump_uuid(uuid); 186 printf("\n"); 187 } 188 } 189 } 190 else { 191 printf("Wrong length (%d, must be divisible by 16)\n", sublen-1); 192 } 193 break; 194 case 0x09: 195 printf(" (Complete Local Name)\n"); 196 printf(" "); 197 for (i = 1; i < sublen; ++i) 198 printf("%c", isprint(buf[pos+i]) ? buf[pos+i] : '.'); 199 printf("\n"); 200 break; 201 case 0x0a: 202 printf(" (Tx Power Level)\n"); 203 printf(" "); 204 if (sublen-1 == 1) { 205 cval = (char *)&buf[pos+1]; 206 printf("%d dBm\n", *cval); 207 } else { 208 printf("Wrong length (%d, should be 1)\n", sublen-1); 209 } 210 break; 211 case 0x12: 212 printf(" (Slave Connection Interval Range)\n"); 213 printf(" "); 214 if (sublen-1 == 4) { 215 val = (buf[pos+2] << 8) | buf[pos+1]; 216 printf("(%0.2f, ", val * 1.25); 217 val = (buf[pos+4] << 8) | buf[pos+3]; 218 printf("%0.2f) ms\n", val * 1.25); 219 } 220 else { 221 printf("Wrong length (%d, should be 4)\n", sublen-1); 222 } 223 break; 224 case 0x16: 225 printf(" (Service Data)\n"); 226 printf(" "); 227 if (sublen-1 >= 2) { 228 val = (buf[pos+2] << 8) | buf[pos+1]; 229 printf("UUID: %02x", val); 230 if (sublen-1 > 2) { 231 printf(", Additional:"); 232 for (i = 3; i < sublen; ++i) 233 printf(" %02x", buf[pos+i]); 234 } 235 printf("\n"); 236 } 237 else { 238 printf("Wrong length (%d, should be >= 2)\n", sublen-1); 239 } 240 break; 241 default: 242 printf("\n"); 243 printf(" "); 244 for (i = 1; i < sublen; ++i) 245 printf(" %02x", buf[pos+i]); 246 printf("\n"); 247 } 248 pos += sublen; 249 } 250 } 251 252 void le_print(le_packet_t *p) { 253 int i; 254 if (le_packet_is_data(p)) { 255 int llid = p->symbols[4] & 0x3; 256 static const char *llid_str[] = { 257 "Reserved", 258 "LL Data PDU / empty or L2CAP continuation", 259 "LL Data PDU / L2CAP start", 260 "LL Control PDU", 261 }; 262 263 printf("Data / AA %08x / %2d bytes\n", p->access_address, p->length); 264 printf(" Channel Index: %d\n", p->channel_idx); 265 printf(" LLID: %d / %s\n", llid, llid_str[llid]); 266 printf(" NESN: %d SN: %d MD: %d\n", (p->symbols[4] >> 2) & 1, 267 (p->symbols[4] >> 3) & 1, 268 (p->symbols[4] >> 4) & 1); 269 } else { 270 printf("Advertising / AA %08x / %2d bytes\n", p->access_address, p->length); 271 printf(" Channel Index: %d\n", p->channel_idx); 272 printf(" Type: %s\n", le_adv_type(p)); 273 274 switch(p->adv_type) { 275 case ADV_IND: 276 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 277 if (p->length-6 > 0) { 278 printf(" AdvData:"); 279 for (i = 0; i < p->length - 6; ++i) 280 printf(" %02x", p->symbols[12+i]); 281 printf("\n"); 282 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 283 } 284 break; 285 case SCAN_REQ: 286 _dump_addr("ScanA: ", p->symbols, 6, p->adv_tx_add); 287 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 288 break; 289 case SCAN_RSP: 290 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 291 printf(" ScanRspData:"); 292 for (i = 0; i < p->length - 6; ++i) 293 printf(" %02x", p->symbols[12+i]); 294 printf("\n"); 295 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 296 break; 297 case CONNECT_REQ: 298 _dump_addr("InitA: ", p->symbols, 6, p->adv_tx_add); 299 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 300 _dump_32("AA: ", p->symbols, 18); 301 _dump_24("CRCInit: ", p->symbols, 22); 302 _dump_8("WinSize: ", p->symbols, 25); 303 _dump_16("WinOffset: ", p->symbols, 26); 304 _dump_16("Interval: ", p->symbols, 28); 305 _dump_16("Latency: ", p->symbols, 30); 306 _dump_16("Timeout: ", p->symbols, 32); 307 308 printf(" ChM:"); 309 for (i = 0; i < 5; ++i) 310 printf(" %02x", p->symbols[34+i]); 311 printf("\n"); 312 313 printf(" Hop: %d\n", p->symbols[37] & 0x1f); 314 printf(" SCA: %d, %s\n", 315 p->symbols[37] >> 5, 316 CONNECT_SCA[p->symbols[37] >> 5]); 317 break; 318 } 319 } 320 321 printf("\n"); 322 printf(" Data: "); 323 for (i = 6; i < 6 + p->length; ++i) 324 printf(" %02x", p->symbols[i]); 325 printf("\n"); 326 327 printf(" CRC: "); 328 for (i = 0; i < 3; ++i) 329 printf(" %02x", p->symbols[6 + p->length + i]); 330 printf("\n"); 331 } 332