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