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