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