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