1 /* -*- c -*- */ 2 /* 3 * Copyright 2014 Christopher D. Kilgour techie AT whiterocker.com 4 * 5 * This file is part of libbtbb 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License as published by 9 * the Free Software Foundation; either version 2, or (at your option) 10 * any later version. 11 * 12 * This program is distributed in the hope that it will be useful, 13 * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 * GNU General Public License for more details. 16 * 17 * You should have received a copy of the GNU General Public License 18 * along with libbtbb; see the file COPYING. If not, write to 19 * the Free Software Foundation, Inc., 51 Franklin Street, 20 * Boston, MA 02110-1301, USA. 21 */ 22 23 #include "btbb.h" 24 #include "bluetooth_le_packet.h" 25 #include "bluetooth_packet.h" 26 #include "pcapng-bt.h" 27 #include <stdlib.h> 28 #include <string.h> 29 #include <time.h> 30 #include <unistd.h> 31 #include <stdio.h> 32 #include <assert.h> 33 34 /* generic section options indicating libbtbb */ 35 const struct { 36 struct { 37 option_header hdr; 38 char libname[8]; 39 } libopt; 40 struct { 41 option_header hdr; 42 } termopt; 43 } libbtbb_section_options = { 44 .libopt = { 45 .hdr = { 46 .option_code = SHB_USERAPPL, 47 .option_length = 7 }, 48 .libname = "libbtbb" 49 }, 50 .termopt = { 51 .hdr = { 52 .option_code = OPT_ENDOFOPT, 53 .option_length = 0 54 } 55 } 56 }; 57 58 static PCAPNG_RESULT 59 check_and_fix_tsresol( PCAPNG_HANDLE * handle, 60 const option_header * interface_options ) 61 { 62 PCAPNG_RESULT retval = PCAPNG_OK; 63 int got_tsresol = 0; 64 65 while( !got_tsresol && 66 interface_options && 67 interface_options->option_code && 68 interface_options->option_length) { 69 if (interface_options->option_code == IF_TSRESOL) { 70 got_tsresol = 1; 71 } 72 else { 73 size_t step = 4+4*((interface_options->option_length+3)/4); 74 uint8_t * next = &((uint8_t *)interface_options)[step]; 75 interface_options = (const option_header *) next; 76 } 77 } 78 79 if (!got_tsresol) { 80 const struct { 81 option_header hdr; 82 uint8_t resol; 83 } tsresol = { 84 .hdr = { 85 .option_code = IF_TSRESOL, 86 .option_length = 1, 87 }, 88 .resol = 9 /* 10^-9 is nanoseconds */ 89 }; 90 91 retval = pcapng_append_interface_option( handle, 92 (const option_header *) &tsresol ); 93 } 94 95 return retval; 96 } 97 98 /* --------------------------------- BR/EDR ----------------------------- */ 99 100 static PCAPNG_RESULT 101 create_bredr_capture_file_single_interface( PCAPNG_HANDLE * handle, 102 const char * filename, 103 const option_header * interface_options ) 104 { 105 PCAPNG_RESULT retval = PCAPNG_OK; 106 107 retval = pcapng_create( handle, 108 filename, 109 (const option_header *) &libbtbb_section_options, 110 (size_t) getpagesize( ), 111 DLT_BLUETOOTH_BREDR_BB, 112 BREDR_MAX_PAYLOAD, 113 interface_options, 114 (size_t) getpagesize( ) ); 115 116 if (retval == PCAPNG_OK) { 117 /* if there is no timestamp resolution alread in the 118 interface options, record nanosecond resolution */ 119 retval = check_and_fix_tsresol( handle, interface_options ); 120 121 if (retval != PCAPNG_OK) { 122 (void) pcapng_close( handle ); 123 } 124 } 125 126 return retval; 127 } 128 129 int btbb_pcapng_create_file( const char *filename, 130 const char *interface_desc, 131 btbb_pcapng_handle ** ph ) 132 { 133 int retval = PCAPNG_OK; 134 PCAPNG_HANDLE * handle = malloc( sizeof(PCAPNG_HANDLE) ); 135 if (handle) { 136 const option_header * popt = NULL; 137 struct { 138 option_header header; 139 char desc[256]; 140 } ifopt = { 141 .header = { 142 .option_code = IF_DESCRIPTION, 143 } 144 }; 145 if (interface_desc) { 146 (void) strncpy( &ifopt.desc[0], interface_desc, 256 ); 147 ifopt.desc[255] = '\0'; 148 ifopt.header.option_length = strlen( ifopt.desc ); 149 popt = (const option_header *) &ifopt; 150 } 151 152 retval = -create_bredr_capture_file_single_interface( handle, 153 filename, 154 popt ); 155 if (retval == PCAPNG_OK) { 156 *ph = (btbb_pcapng_handle *) handle; 157 } 158 else { 159 free( handle ); 160 } 161 } 162 else { 163 retval = -PCAPNG_NO_MEMORY; 164 } 165 return retval; 166 } 167 168 static PCAPNG_RESULT 169 append_bredr_packet( PCAPNG_HANDLE * handle, 170 pcapng_bredr_packet * pkt ) 171 { 172 return pcapng_append_packet( handle, ( const enhanced_packet_block *) pkt ); 173 } 174 175 static void 176 assemble_pcapng_bredr_packet( pcapng_bredr_packet * pkt, 177 const uint32_t interface_id, 178 const uint64_t ns, 179 const uint32_t caplen, 180 const uint8_t rf_channel, 181 const int8_t signal_power, 182 const int8_t noise_power, 183 const uint8_t access_code_offenses, 184 const uint8_t payload_transport, 185 const uint8_t payload_rate, 186 const uint8_t corrected_header_bits, 187 const int16_t corrected_payload_bits, 188 const uint32_t lap, 189 const uint32_t ref_lap, 190 const uint8_t ref_uap, 191 const uint32_t bt_header, 192 const uint16_t flags, 193 const char * payload ) 194 { 195 uint32_t pcapng_caplen = sizeof(pcap_bluetooth_bredr_bb_header) 196 - sizeof(pkt->bredr_bb_header.bredr_payload) 197 + caplen; 198 uint32_t block_length = 4*((36+pcapng_caplen+3)/4); 199 uint32_t reflapuap = (ref_lap&0xffffff) | (ref_uap<<24); 200 201 pkt->blk_header.block_type = BLOCK_TYPE_ENHANCED_PACKET; 202 pkt->blk_header.block_total_length = block_length; 203 pkt->blk_header.interface_id = interface_id; 204 pkt->blk_header.timestamp_high = (uint32_t) (ns >> 32); 205 pkt->blk_header.timestamp_low = (uint32_t) (ns & 0x0ffffffffull); 206 pkt->blk_header.captured_len = pcapng_caplen; 207 pkt->blk_header.packet_len = pcapng_caplen; 208 pkt->bredr_bb_header.rf_channel = rf_channel; 209 pkt->bredr_bb_header.signal_power = signal_power; 210 pkt->bredr_bb_header.noise_power = noise_power; 211 pkt->bredr_bb_header.access_code_offenses = access_code_offenses; 212 pkt->bredr_bb_header.payload_transport_rate = 213 (payload_transport << 4) | payload_rate; 214 pkt->bredr_bb_header.corrected_header_bits = corrected_header_bits; 215 pkt->bredr_bb_header.corrected_payload_bits = htole16( corrected_payload_bits ); 216 pkt->bredr_bb_header.lap = htole32( lap ); 217 pkt->bredr_bb_header.ref_lap_uap = htole32( reflapuap ); 218 pkt->bredr_bb_header.bt_header = htole32( bt_header ); 219 pkt->bredr_bb_header.flags = htole16( flags ); 220 if (caplen) { 221 assert(caplen <= sizeof(pkt->bredr_bb_header.bredr_payload)); // caller ensures this, but to be safe.. 222 (void) memcpy( &pkt->bredr_bb_header.bredr_payload[0], payload, caplen ); 223 pkt->bredr_bb_header.flags |= htole16( BREDR_PAYLOAD_PRESENT ); 224 } 225 else { 226 pkt->bredr_bb_header.flags &= htole16( ~BREDR_PAYLOAD_PRESENT ); 227 } 228 ((uint32_t *)pkt)[block_length/4-2] = 0x00000000; /* no-options */ 229 ((uint32_t *)pkt)[block_length/4-1] = block_length; 230 } 231 232 int btbb_pcapng_append_packet(btbb_pcapng_handle * h, const uint64_t ns, 233 const int8_t sigdbm, const int8_t noisedbm, 234 const uint32_t reflap, const uint8_t refuap, 235 const btbb_packet *pkt) 236 { 237 uint16_t flags = BREDR_DEWHITENED | BREDR_SIGPOWER_VALID | 238 ((noisedbm < sigdbm) ? BREDR_NOISEPOWER_VALID : 0) | 239 ((reflap != LAP_ANY) ? BREDR_REFLAP_VALID : 0) | 240 ((refuap != UAP_ANY) ? BREDR_REFUAP_VALID : 0); 241 int caplen = btbb_packet_get_payload_length(pkt); 242 char payload_bytes[caplen]; 243 btbb_get_payload_packed( pkt, &payload_bytes[0] ); 244 caplen = MIN(BREDR_MAX_PAYLOAD, caplen); 245 pcapng_bredr_packet pcapng_pkt; 246 assemble_pcapng_bredr_packet( &pcapng_pkt, 247 0, 248 ns, 249 caplen, 250 btbb_packet_get_channel(pkt), 251 sigdbm, 252 noisedbm, 253 btbb_packet_get_ac_errors(pkt), 254 btbb_packet_get_transport(pkt), 255 btbb_packet_get_modulation(pkt), 256 0, /* TODO: corrected header bits */ 257 0, /* TODO: corrected payload bits */ 258 btbb_packet_get_lap(pkt), 259 reflap, 260 refuap, 261 btbb_packet_get_header_packed(pkt), 262 flags, 263 payload_bytes ); 264 return -append_bredr_packet( (PCAPNG_HANDLE *)h, &pcapng_pkt ); 265 } 266 267 static PCAPNG_RESULT 268 record_bd_addr_info( PCAPNG_HANDLE * handle, 269 const uint64_t bd_addr, 270 const uint8_t uap_mask, 271 const uint8_t nap_valid ) 272 { 273 const bredr_br_addr_option bdopt = { 274 .header = { 275 .option_code = PCAPNG_BREDR_OPTION_BD_ADDR, 276 .option_length = sizeof(bredr_br_addr_option), 277 }, 278 .bd_addr_info = { 279 .bd_addr = { 280 ((bd_addr>>0) & 0xff), 281 ((bd_addr>>8) & 0xff), 282 ((bd_addr>>16) & 0xff), 283 ((bd_addr>>24) & 0xff), 284 ((bd_addr>>32) & 0xff), 285 ((bd_addr>>40) & 0xff) 286 }, 287 .uap_mask = uap_mask, 288 .nap_valid = nap_valid, 289 } 290 }; 291 return pcapng_append_interface_option( handle, 292 (const option_header *) &bdopt ); 293 } 294 295 int btbb_pcapng_record_bdaddr(btbb_pcapng_handle * h, const uint64_t bdaddr, 296 const uint8_t uapmask, const uint8_t napvalid) 297 { 298 return -record_bd_addr_info( (PCAPNG_HANDLE *) h, 299 bdaddr, uapmask, napvalid ); 300 } 301 302 static PCAPNG_RESULT 303 record_bredr_master_clock_info( PCAPNG_HANDLE * handle, 304 const uint64_t bd_addr, 305 const uint64_t ns, 306 const uint32_t clk, 307 const uint32_t clk_mask) 308 { 309 const bredr_clk_option mcopt = { 310 .header = { 311 .option_code = PCAPNG_BREDR_OPTION_MASTER_CLOCK_INFO, 312 .option_length = sizeof(bredr_clk_option) 313 }, 314 .clock_info = { 315 .ts = ns, 316 .lap_uap = htole32(bd_addr & 0xffffffff), 317 .clk = clk, 318 .clk_mask = clk_mask 319 } 320 }; 321 return pcapng_append_interface_option( handle, 322 (const option_header *) &mcopt ); 323 } 324 325 int btbb_pcapng_record_btclock(btbb_pcapng_handle * h, const uint64_t bdaddr, 326 const uint64_t ns, const uint32_t clk, 327 const uint32_t clkmask) 328 { 329 return -record_bredr_master_clock_info( (PCAPNG_HANDLE *) h, 330 bdaddr, ns, clk, clkmask ); 331 } 332 333 int btbb_pcapng_close(btbb_pcapng_handle * h) 334 { 335 pcapng_close( (PCAPNG_HANDLE *) h ); 336 if (h) { 337 free( h ); 338 } 339 return -PCAPNG_INVALID_HANDLE; 340 } 341 342 /* --------------------------------- Low Energy ---------------------------- */ 343 344 static PCAPNG_RESULT 345 create_le_capture_file_single_interface( PCAPNG_HANDLE * handle, 346 const char * filename, 347 const option_header * interface_options ) 348 { 349 PCAPNG_RESULT retval = PCAPNG_OK; 350 351 retval = pcapng_create( handle, 352 filename, 353 (const option_header *) &libbtbb_section_options, 354 (size_t) getpagesize( ), 355 DLT_BLUETOOTH_LE_LL_WITH_PHDR, 356 64, 357 interface_options, 358 (size_t) getpagesize( ) ); 359 360 if (retval == PCAPNG_OK) { 361 /* if there is no timestamp resolution alread in the 362 interface options, record nanosecond resolution */ 363 retval = check_and_fix_tsresol( handle, interface_options ); 364 365 if (retval != PCAPNG_OK) { 366 (void) pcapng_close( handle ); 367 } 368 } 369 370 return retval; 371 } 372 373 int 374 lell_pcapng_create_file(const char *filename, const char *interface_desc, 375 lell_pcapng_handle ** ph) 376 { 377 int retval = PCAPNG_OK; 378 PCAPNG_HANDLE * handle = malloc( sizeof(PCAPNG_HANDLE) ); 379 if (handle) { 380 const option_header * popt = NULL; 381 struct { 382 option_header header; 383 char desc[256]; 384 } ifopt = { 385 .header = { 386 .option_code = IF_DESCRIPTION, 387 } 388 }; 389 if (interface_desc) { 390 (void) strncpy( &ifopt.desc[0], interface_desc, 256 ); 391 ifopt.desc[255] = '\0'; 392 ifopt.header.option_length = strlen( ifopt.desc ); 393 popt = (const option_header *) &ifopt; 394 } 395 396 retval = -create_le_capture_file_single_interface( handle, 397 filename, 398 popt ); 399 if (retval == PCAPNG_OK) { 400 *ph = (lell_pcapng_handle *) handle; 401 } 402 else { 403 free( handle ); 404 } 405 } 406 else { 407 retval = -PCAPNG_NO_MEMORY; 408 } 409 return retval; 410 } 411 412 static PCAPNG_RESULT 413 append_le_packet( PCAPNG_HANDLE * handle, 414 pcapng_le_packet * pkt ) 415 { 416 return pcapng_append_packet( handle, ( const enhanced_packet_block *) pkt ); 417 } 418 419 /* Size of a PCAPNG enhanced packet block with no packet data. 420 NOTE: The pcap_bluetooth_le_ll_header is part of the packet data of 421 the enhanced block. */ 422 #define PCAPNG_ENHANCED_BLK_SZ 36 423 424 static void 425 assemble_pcapng_le_packet( pcapng_le_packet * pkt, 426 const uint32_t interface_id, 427 const uint64_t ns, 428 const uint32_t caplen, 429 const uint8_t rf_channel, 430 const int8_t signal_power, 431 const int8_t noise_power, 432 const uint8_t access_address_offenses, 433 const uint32_t ref_access_address, 434 const uint16_t flags, 435 const uint8_t * lepkt ) 436 { 437 uint32_t pcapng_caplen = sizeof(pcap_bluetooth_le_ll_header)+caplen; 438 uint32_t block_length = 4*((PCAPNG_ENHANCED_BLK_SZ+pcapng_caplen+3)/4); 439 440 assert(caplen <= LE_MAX_PAYLOAD); 441 442 pkt->blk_header.block_type = BLOCK_TYPE_ENHANCED_PACKET; 443 pkt->blk_header.block_total_length = block_length; 444 pkt->blk_header.interface_id = interface_id; 445 pkt->blk_header.timestamp_high = (uint32_t) (ns >> 32); 446 pkt->blk_header.timestamp_low = (uint32_t) (ns & 0x0ffffffffull); 447 pkt->blk_header.captured_len = pcapng_caplen; 448 pkt->blk_header.packet_len = pcapng_caplen; 449 pkt->le_ll_header.rf_channel = rf_channel; 450 pkt->le_ll_header.signal_power = signal_power; 451 pkt->le_ll_header.noise_power = noise_power; 452 pkt->le_ll_header.access_address_offenses = access_address_offenses; 453 pkt->le_ll_header.ref_access_address = htole32( ref_access_address ); 454 pkt->le_ll_header.flags = htole16( flags ); 455 (void) memcpy( &pkt->le_packet[0], lepkt, caplen ); 456 ((uint32_t *)pkt)[block_length/4-2] = 0x00000000; /* no-options */ 457 ((uint32_t *)pkt)[block_length/4-1] = block_length; 458 } 459 460 int 461 lell_pcapng_append_packet(lell_pcapng_handle * h, const uint64_t ns, 462 const int8_t sigdbm, const int8_t noisedbm, 463 const uint32_t refAA, const lell_packet *pkt) 464 { 465 uint16_t flags = LE_DEWHITENED | LE_AA_OFFENSES_VALID | 466 LE_SIGPOWER_VALID | 467 ((noisedbm < sigdbm) ? LE_NOISEPOWER_VALID : 0) | 468 (lell_packet_is_data(pkt) ? 0 : LE_REF_AA_VALID); 469 pcapng_le_packet pcapng_pkt; 470 471 /* The extra 9 bytes added to the packet length are for: 472 4 bytes for Access Address 473 2 bytes for PDU header 474 3 bytes for CRC */ 475 assemble_pcapng_le_packet( &pcapng_pkt, 476 0, 477 ns, 478 9+pkt->length, 479 pkt->channel_k, 480 sigdbm, 481 noisedbm, 482 pkt->access_address_offenses, 483 refAA, 484 flags, 485 &pkt->symbols[0] ); 486 int retval = -append_le_packet( (PCAPNG_HANDLE *) h, &pcapng_pkt ); 487 if ((retval == 0) && !lell_packet_is_data(pkt) && (pkt->adv_type == CONNECT_REQ)) { 488 (void) lell_pcapng_record_connect_req(h, ns, &pkt->symbols[0]); 489 } 490 return retval; 491 } 492 493 static PCAPNG_RESULT 494 record_le_connect_req_info( PCAPNG_HANDLE * handle, 495 const uint64_t ns, 496 const uint8_t * pdu ) 497 { 498 le_ll_connection_info_option cropt = { 499 .header = { 500 .option_code = PCAPNG_LE_LL_CONNECTION_INFO, 501 .option_length = sizeof(le_ll_connection_info_option) 502 }, 503 .connection_info = { 504 .ns = ns 505 } 506 }; 507 (void) memcpy( &cropt.connection_info.pdu.bytes[0], pdu, 34 ); 508 return pcapng_append_interface_option( handle, 509 (const option_header *) &cropt ); 510 } 511 512 int 513 lell_pcapng_record_connect_req(lell_pcapng_handle * h, const uint64_t ns, 514 const uint8_t * pdu) 515 { 516 return -record_le_connect_req_info( (PCAPNG_HANDLE *) h, ns, pdu ); 517 } 518 519 int lell_pcapng_close(lell_pcapng_handle *h) 520 { 521 pcapng_close( (PCAPNG_HANDLE *) h ); 522 if (h) { 523 free( h ); 524 } 525 return -PCAPNG_INVALID_HANDLE; 526 } 527