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