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