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