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 #include "bluetooth_le_packet.h" 23 #include "bluetooth_packet.h" 24 #include "btbb.h" 25 #include "pcap-common.h" 26 27 #include <stdlib.h> 28 #include <string.h> 29 #include <assert.h> 30 #include <sys/types.h> 31 32 typedef enum { 33 PCAP_OK = 0, 34 PCAP_INVALID_HANDLE, 35 PCAP_FILE_NOT_ALLOWED, 36 PCAP_NO_MEMORY, 37 } PCAP_RESULT; 38 39 typedef struct __attribute__((packed)) pcap_hdr_s { 40 uint32_t magic_number; /* magic number */ 41 uint16_t version_major; /* major version number */ 42 uint16_t version_minor; /* minor version number */ 43 int32_t thiszone; /* GMT to local correction */ 44 uint32_t sigfigs; /* accuracy of timestamps */ 45 uint32_t snaplen; /* max length of captured packets, in octets */ 46 uint32_t network; /* data link type */ 47 } pcap_hdr_t; 48 49 FILE *btbb_pcap_open(const char *filename, uint32_t dlt, uint32_t snaplen) { 50 pcap_hdr_t pcap_header = { 51 .magic_number = 0xa1b23c4d, 52 .version_major = 2, 53 .version_minor = 4, 54 .thiszone = 0, 55 .sigfigs = 0, 56 .snaplen = snaplen, 57 .network = dlt, 58 }; 59 60 FILE *pcap_file = fopen(filename, "w"); 61 if (pcap_file == NULL) return NULL; 62 63 fwrite(&pcap_header, sizeof(pcap_header), 1, pcap_file); 64 65 return pcap_file; 66 } 67 68 /* BT BR/EDR support */ 69 70 struct btbb_pcap_handle { 71 FILE *pcap_file; 72 }; 73 74 int 75 btbb_pcap_create_file(const char *filename, btbb_pcap_handle ** ph) 76 { 77 int retval = 0; 78 btbb_pcap_handle * handle = malloc( sizeof(btbb_pcap_handle) ); 79 if (handle) { 80 memset(handle, 0, sizeof(*handle)); 81 handle->pcap_file = btbb_pcap_open(filename, DLT_BLUETOOTH_BREDR_BB, 82 BREDR_MAX_PAYLOAD); 83 if (handle->pcap_file) { 84 *ph = handle; 85 } 86 else { 87 perror("PCAP error:"); 88 retval = -PCAP_FILE_NOT_ALLOWED; 89 goto fail; 90 } 91 } 92 else { 93 retval = -PCAP_NO_MEMORY; 94 goto fail; 95 } 96 return retval; 97 fail: 98 (void) btbb_pcap_close( handle ); 99 return retval; 100 } 101 102 typedef struct __attribute__((packed)) pcaprec_hdr_s { 103 uint32_t ts_sec; /* timestamp seconds */ 104 uint32_t ts_usec; /* timestamp microseconds */ 105 uint32_t incl_len; /* number of octets of packet saved in file */ 106 uint32_t orig_len; /* actual length of packet */ 107 } pcaprec_hdr_t; 108 109 typedef struct { 110 pcaprec_hdr_t pcap_header; 111 pcap_bluetooth_bredr_bb_header bredr_bb_header; 112 } pcap_bredr_packet; 113 114 void btbb_pcap_dump(FILE *file, pcaprec_hdr_t *pcap_header, u_char *data) { 115 fwrite(pcap_header, sizeof(*pcap_header), 1, file); 116 fwrite(data, pcap_header->incl_len, 1, file); 117 fflush(file); 118 } 119 120 static void 121 assemble_pcapng_bredr_packet( pcap_bredr_packet * pkt, 122 const uint32_t interface_id __attribute__((unused)), 123 const uint64_t ns, 124 const uint32_t caplen, 125 const uint8_t rf_channel, 126 const int8_t signal_power, 127 const int8_t noise_power, 128 const uint8_t access_code_offenses, 129 const uint8_t payload_transport, 130 const uint8_t payload_rate, 131 const uint8_t corrected_header_bits, 132 const int16_t corrected_payload_bits, 133 const uint32_t lap, 134 const uint32_t ref_lap, 135 const uint8_t ref_uap, 136 const uint32_t bt_header, 137 const uint16_t flags, 138 const uint8_t * payload ) 139 { 140 uint32_t pcap_caplen = sizeof(pcap_bluetooth_bredr_bb_header) - 141 sizeof(pkt->bredr_bb_header.bredr_payload) 142 + caplen; 143 uint32_t reflapuap = (ref_lap&0xffffff) | (ref_uap<<24); 144 145 pkt->pcap_header.ts_sec = ns / 1000000000ull; 146 pkt->pcap_header.ts_usec = ns % 1000000000ull; 147 pkt->pcap_header.incl_len = pcap_caplen; 148 pkt->pcap_header.orig_len = pcap_caplen; 149 150 pkt->bredr_bb_header.rf_channel = rf_channel; 151 pkt->bredr_bb_header.signal_power = signal_power; 152 pkt->bredr_bb_header.noise_power = noise_power; 153 pkt->bredr_bb_header.access_code_offenses = access_code_offenses; 154 pkt->bredr_bb_header.payload_transport_rate = 155 (payload_transport << 4) | payload_rate; 156 pkt->bredr_bb_header.corrected_header_bits = corrected_header_bits; 157 pkt->bredr_bb_header.corrected_payload_bits = htole16( corrected_payload_bits ); 158 pkt->bredr_bb_header.lap = htole32( lap ); 159 pkt->bredr_bb_header.ref_lap_uap = htole32( reflapuap ); 160 pkt->bredr_bb_header.bt_header = htole32( bt_header ); 161 pkt->bredr_bb_header.flags = htole16( flags ); 162 if (caplen) { 163 assert(caplen <= sizeof(pkt->bredr_bb_header.bredr_payload)); // caller ensures this, but to be safe.. 164 (void) memcpy( &pkt->bredr_bb_header.bredr_payload[0], payload, caplen ); 165 pkt->bredr_bb_header.flags |= htole16( BREDR_PAYLOAD_PRESENT ); 166 } 167 else { 168 pkt->bredr_bb_header.flags &= htole16( ~BREDR_PAYLOAD_PRESENT ); 169 } 170 } 171 172 int 173 btbb_pcap_append_packet(btbb_pcap_handle * h, const uint64_t ns, 174 const int8_t sigdbm, const int8_t noisedbm, 175 const uint32_t reflap, const uint8_t refuap, 176 const btbb_packet *pkt) 177 { 178 if (h && h->pcap_file) { 179 uint16_t flags = BREDR_DEWHITENED | BREDR_SIGPOWER_VALID | 180 ((noisedbm < sigdbm) ? BREDR_NOISEPOWER_VALID : 0) | 181 ((reflap != LAP_ANY) ? BREDR_REFLAP_VALID : 0) | 182 ((refuap != UAP_ANY) ? BREDR_REFUAP_VALID : 0); 183 uint32_t caplen = (uint32_t) btbb_packet_get_payload_length(pkt); 184 uint8_t payload_bytes[caplen]; 185 btbb_get_payload_packed( pkt, (char *) &payload_bytes[0] ); 186 caplen = MIN(BREDR_MAX_PAYLOAD, caplen); 187 pcap_bredr_packet pcap_pkt; 188 assemble_pcapng_bredr_packet( &pcap_pkt, 189 0, 190 ns, 191 caplen, 192 btbb_packet_get_channel(pkt), 193 sigdbm, 194 noisedbm, 195 btbb_packet_get_ac_errors(pkt), 196 btbb_packet_get_transport(pkt), 197 btbb_packet_get_modulation(pkt), 198 0, /* TODO: corrected header bits */ 199 0, /* TODO: corrected payload bits */ 200 btbb_packet_get_lap(pkt), 201 reflap, 202 refuap, 203 btbb_packet_get_header_packed(pkt), 204 flags, 205 payload_bytes ); 206 btbb_pcap_dump(h->pcap_file, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.bredr_bb_header); 207 return 0; 208 } 209 return -PCAP_INVALID_HANDLE; 210 } 211 212 int 213 btbb_pcap_close(btbb_pcap_handle * h) 214 { 215 if (h && h->pcap_file) { 216 fclose(h->pcap_file); 217 } 218 if (h) { 219 free(h); 220 return 0; 221 } 222 return -PCAP_INVALID_HANDLE; 223 } 224 225 /* BTLE support */ 226 227 struct lell_pcap_handle { 228 FILE *pcap_file; 229 int dlt; 230 uint8_t btle_ppi_version; 231 }; 232 233 static int 234 lell_pcap_create_file_dlt(const char *filename, int dlt, lell_pcap_handle ** ph) 235 { 236 int retval = 0; 237 lell_pcap_handle * handle = malloc( sizeof(lell_pcap_handle) ); 238 if (handle) { 239 memset(handle, 0, sizeof(*handle)); 240 handle->pcap_file = btbb_pcap_open(filename, dlt, BREDR_MAX_PAYLOAD); 241 if (handle->pcap_file) { 242 handle->dlt = dlt; 243 *ph = handle; 244 } 245 else { 246 retval = -PCAP_FILE_NOT_ALLOWED; 247 goto fail; 248 } 249 } 250 else { 251 retval = -PCAP_NO_MEMORY; 252 goto fail; 253 } 254 return retval; 255 fail: 256 (void) lell_pcap_close( handle ); 257 return retval; 258 } 259 260 int 261 lell_pcap_create_file(const char *filename, lell_pcap_handle ** ph) 262 { 263 return lell_pcap_create_file_dlt(filename, DLT_BLUETOOTH_LE_LL_WITH_PHDR, ph); 264 } 265 266 int 267 lell_pcap_ppi_create_file(const char *filename, int btle_ppi_version, 268 lell_pcap_handle ** ph) 269 { 270 int retval = lell_pcap_create_file_dlt(filename, DLT_PPI, ph); 271 if (!retval) { 272 (*ph)->btle_ppi_version = btle_ppi_version; 273 } 274 return retval; 275 } 276 277 typedef struct { 278 pcaprec_hdr_t pcap_header; 279 pcap_bluetooth_le_ll_header le_ll_header; 280 uint8_t le_packet[LE_MAX_PAYLOAD]; 281 } pcap_le_packet; 282 283 static void 284 assemble_pcapng_le_packet( pcap_le_packet * pkt, 285 const uint32_t interface_id __attribute__((unused)), 286 const uint64_t ns, 287 const uint32_t caplen, 288 const uint8_t rf_channel, 289 const int8_t signal_power, 290 const int8_t noise_power, 291 const uint8_t access_address_offenses, 292 const uint32_t ref_access_address, 293 const uint16_t flags, 294 const uint8_t * lepkt ) 295 { 296 uint32_t incl_len = MIN(LE_MAX_PAYLOAD, caplen); 297 298 pkt->pcap_header.ts_sec = ns / 1000000000ull; 299 pkt->pcap_header.ts_usec = ns % 1000000000ull; 300 pkt->pcap_header.incl_len = sizeof(pcap_bluetooth_le_ll_header)+caplen; 301 pkt->pcap_header.orig_len = sizeof(pcap_bluetooth_le_ll_header)+incl_len; 302 303 pkt->le_ll_header.rf_channel = rf_channel; 304 pkt->le_ll_header.signal_power = signal_power; 305 pkt->le_ll_header.noise_power = noise_power; 306 pkt->le_ll_header.access_address_offenses = access_address_offenses; 307 pkt->le_ll_header.ref_access_address = htole32( ref_access_address ); 308 pkt->le_ll_header.flags = htole16( flags ); 309 (void) memcpy( &pkt->le_packet[0], lepkt, incl_len ); 310 } 311 312 int 313 lell_pcap_append_packet(lell_pcap_handle * h, const uint64_t ns, 314 const int8_t sigdbm, const int8_t noisedbm, 315 const uint32_t refAA, const lell_packet *pkt) 316 { 317 if (h && h->pcap_file && 318 (h->dlt == DLT_BLUETOOTH_LE_LL_WITH_PHDR)) { 319 uint16_t flags = LE_DEWHITENED | LE_AA_OFFENSES_VALID | 320 LE_SIGPOWER_VALID | 321 ((noisedbm < sigdbm) ? LE_NOISEPOWER_VALID : 0) | 322 (lell_packet_is_data(pkt) ? 0 : LE_REF_AA_VALID); 323 pcap_le_packet pcap_pkt; 324 assemble_pcapng_le_packet( &pcap_pkt, 325 0, 326 ns, 327 pkt->length + 4 + 2 + 3, // AA + header + CRC 328 pkt->channel_k, 329 sigdbm, 330 noisedbm, 331 pkt->access_address_offenses, 332 refAA, 333 flags, 334 &pkt->symbols[0] ); 335 btbb_pcap_dump(h->pcap_file, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.le_ll_header); 336 return 0; 337 } 338 return -PCAP_INVALID_HANDLE; 339 } 340 341 #define PPI_BTLE 30006 342 343 typedef struct __attribute__((packed)) { 344 uint8_t pph_version; 345 uint8_t pph_flags; 346 uint16_t pph_len; 347 uint32_t pph_dlt; 348 } ppi_packet_header_t; 349 350 typedef struct __attribute__((packed)) { 351 uint16_t pfh_type; 352 uint16_t pfh_datalen; 353 } ppi_fieldheader_t; 354 355 typedef struct __attribute__((packed)) { 356 uint8_t btle_version; 357 uint16_t btle_channel; 358 uint8_t btle_clkn_high; 359 uint32_t btle_clk100ns; 360 int8_t rssi_max; 361 int8_t rssi_min; 362 int8_t rssi_avg; 363 uint8_t rssi_count; 364 } ppi_btle_t; 365 366 typedef struct __attribute__((packed)) { 367 pcaprec_hdr_t pcap_header; 368 ppi_packet_header_t ppi_packet_header; 369 ppi_fieldheader_t ppi_fieldheader; 370 ppi_btle_t le_ll_ppi_header; 371 uint8_t le_packet[LE_MAX_PAYLOAD]; 372 } pcap_ppi_le_packet; 373 374 int 375 lell_pcap_append_ppi_packet(lell_pcap_handle * h, const uint64_t ns, 376 const uint8_t clkn_high, 377 const int8_t rssi_min, const int8_t rssi_max, 378 const int8_t rssi_avg, const uint8_t rssi_count, 379 const lell_packet *pkt) 380 { 381 if (h && h->pcap_file && 382 (h->dlt == DLT_PPI)) { 383 pcap_ppi_le_packet pcap_pkt; 384 const uint16_t pcap_headerlen = 385 sizeof(ppi_packet_header_t) + 386 sizeof(ppi_fieldheader_t) + 387 sizeof(ppi_btle_t); 388 uint16_t MHz = 2402 + 2*lell_get_channel_k(pkt); 389 unsigned packet_len = pkt->length + 4 + 2 + 3; // AA + header + CRC 390 unsigned incl_len = MIN(LE_MAX_PAYLOAD, packet_len); 391 392 pcap_pkt.pcap_header.ts_sec = ns / 1000000000ull; 393 pcap_pkt.pcap_header.ts_usec = ns % 1000000000ull; 394 pcap_pkt.pcap_header.incl_len = pcap_headerlen + incl_len; 395 pcap_pkt.pcap_header.orig_len = pcap_headerlen + packet_len; 396 397 pcap_pkt.ppi_packet_header.pph_version = 0; 398 pcap_pkt.ppi_packet_header.pph_flags = 0; 399 pcap_pkt.ppi_packet_header.pph_len = htole16(pcap_headerlen); 400 pcap_pkt.ppi_packet_header.pph_dlt = htole32(DLT_BLUETOOTH_LE_LL); 401 402 pcap_pkt.ppi_fieldheader.pfh_type = htole16(PPI_BTLE); 403 pcap_pkt.ppi_fieldheader.pfh_datalen = htole16(sizeof(ppi_btle_t)); 404 405 pcap_pkt.le_ll_ppi_header.btle_version = h->btle_ppi_version; 406 pcap_pkt.le_ll_ppi_header.btle_channel = htole16(MHz); 407 pcap_pkt.le_ll_ppi_header.btle_clkn_high = clkn_high; 408 pcap_pkt.le_ll_ppi_header.btle_clk100ns = htole32(pkt->clk100ns); 409 pcap_pkt.le_ll_ppi_header.rssi_max = rssi_max; 410 pcap_pkt.le_ll_ppi_header.rssi_min = rssi_min; 411 pcap_pkt.le_ll_ppi_header.rssi_avg = rssi_avg; 412 pcap_pkt.le_ll_ppi_header.rssi_count = rssi_count; 413 (void) memcpy( &pcap_pkt.le_packet[0], &pkt->symbols[0], incl_len); 414 btbb_pcap_dump(h->pcap_file, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.ppi_packet_header); 415 return 0; 416 } 417 return -PCAP_INVALID_HANDLE; 418 } 419 420 int 421 lell_pcap_close(lell_pcap_handle *h) 422 { 423 if (h && h->pcap_file) { 424 fclose(h->pcap_file); 425 } 426 if (h) { 427 free(h); 428 return 0; 429 } 430 return -PCAP_INVALID_HANDLE; 431 } 432