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 400, 55 PCAP_TSTAMP_PRECISION_NANO); 56 #else 57 handle->pcap = pcap_open_dead(DLT_BLUETOOTH_BREDR_BB, 400); 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[400]; 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 uint8_t payload_bytes[400]; 151 uint32_t caplen = (uint32_t) btbb_get_payload_packed( pkt, (char *) &payload_bytes[0] ); 152 pcap_bredr_packet pcap_pkt; 153 assemble_pcapng_bredr_packet( &pcap_pkt, 154 0, 155 ns, 156 caplen, 157 btbb_packet_get_channel(pkt), 158 sigdbm, 159 noisedbm, 160 btbb_packet_get_ac_errors(pkt), 161 BREDR_TRANSPORT_ANY, 162 BREDR_GFSK, /* currently only supported */ 163 0, /* TODO: corrected header bits */ 164 0, /* TODO: corrected payload bits */ 165 btbb_packet_get_lap(pkt), 166 reflap, 167 refuap, 168 btbb_packet_get_header_packed(pkt), 169 flags, 170 payload_bytes ); 171 pcap_dump((u_char *)h->dumper, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.bredr_bb_header); 172 return 0; 173 } 174 return -PCAP_INVALID_HANDLE; 175 } 176 177 int 178 btbb_pcap_close(btbb_pcap_handle * h) 179 { 180 if (h && h->dumper) { 181 pcap_dump_close(h->dumper); 182 } 183 if (h && h->pcap) { 184 pcap_close(h->pcap); 185 } 186 if (h) { 187 free(h); 188 return 0; 189 } 190 return -PCAP_INVALID_HANDLE; 191 } 192 193 /* BTLE support */ 194 195 typedef struct lell_pcap_handle { 196 pcap_t * pcap; 197 pcap_dumper_t * dumper; 198 int dlt; 199 uint8_t btle_ppi_version; 200 } lell_pcap_handle; 201 202 static int 203 lell_pcap_create_file_dlt(const char *filename, int dlt, lell_pcap_handle ** ph) 204 { 205 int retval = 0; 206 lell_pcap_handle * handle = malloc( sizeof(lell_pcap_handle) ); 207 if (handle) { 208 memset(handle, 0, sizeof(*handle)); 209 #ifdef PCAP_TSTAMP_PRECISION_NANO 210 handle->pcap = pcap_open_dead_with_tstamp_precision(dlt, 211 400, 212 PCAP_TSTAMP_PRECISION_NANO); 213 #else 214 handle->pcap = pcap_open_dead(dlt, 400); 215 #endif 216 if (handle->pcap) { 217 handle->dumper = pcap_dump_open(handle->pcap, filename); 218 if (handle->dumper) { 219 handle->dlt = dlt; 220 *ph = handle; 221 } 222 else { 223 retval = -PCAP_FILE_NOT_ALLOWED; 224 goto fail; 225 } 226 } 227 else { 228 retval = -PCAP_INVALID_HANDLE; 229 goto fail; 230 } 231 } 232 else { 233 retval = -PCAP_NO_MEMORY; 234 goto fail; 235 } 236 return retval; 237 fail: 238 (void) lell_pcap_close( handle ); 239 return retval; 240 } 241 242 int 243 lell_pcap_create_file(const char *filename, lell_pcap_handle ** ph) 244 { 245 return lell_pcap_create_file_dlt(filename, DLT_BLUETOOTH_LE_LL_WITH_PHDR, ph); 246 } 247 248 int 249 lell_pcap_ppi_create_file(const char *filename, int btle_ppi_version, 250 lell_pcap_handle ** ph) 251 { 252 int retval = lell_pcap_create_file_dlt(filename, DLT_PPI, ph); 253 if (!retval) { 254 (*ph)->btle_ppi_version = btle_ppi_version; 255 } 256 return retval; 257 } 258 259 typedef struct { 260 struct pcap_pkthdr pcap_header; 261 pcap_bluetooth_le_ll_header le_ll_header; 262 uint8_t le_packet[48]; 263 } pcap_le_packet; 264 265 static void 266 assemble_pcapng_le_packet( pcap_le_packet * pkt, 267 const uint32_t interface_id, 268 const uint64_t ns, 269 const uint32_t caplen, 270 const uint8_t rf_channel, 271 const int8_t signal_power, 272 const int8_t noise_power, 273 const uint8_t access_address_offenses, 274 const uint32_t ref_access_address, 275 const uint16_t flags, 276 const uint8_t * lepkt ) 277 { 278 uint32_t pcap_caplen = sizeof(pcap_bluetooth_le_ll_header)+caplen; 279 280 pkt->pcap_header.ts.tv_sec = ns / 1000000000ull; 281 pkt->pcap_header.ts.tv_usec = ns % 1000000000ull; 282 pkt->pcap_header.caplen = pcap_caplen; 283 pkt->pcap_header.len = pcap_caplen; 284 285 pkt->le_ll_header.rf_channel = rf_channel; 286 pkt->le_ll_header.signal_power = signal_power; 287 pkt->le_ll_header.noise_power = noise_power; 288 pkt->le_ll_header.access_address_offenses = access_address_offenses; 289 pkt->le_ll_header.ref_access_address = htole32( ref_access_address ); 290 pkt->le_ll_header.flags = htole16( flags ); 291 (void) memcpy( &pkt->le_packet[0], lepkt, caplen ); 292 } 293 294 int 295 lell_pcap_append_packet(lell_pcap_handle * h, const uint64_t ns, 296 const int8_t sigdbm, const int8_t noisedbm, 297 const uint32_t refAA, const lell_packet *pkt) 298 { 299 if (h && h->dumper && 300 (h->dlt == DLT_BLUETOOTH_LE_LL_WITH_PHDR)) { 301 uint16_t flags = LE_DEWHITENED | LE_AA_OFFENSES_VALID | 302 LE_SIGPOWER_VALID | 303 ((noisedbm < sigdbm) ? LE_NOISEPOWER_VALID : 0) | 304 (lell_packet_is_data(pkt) ? 0 : LE_REF_AA_VALID); 305 pcap_le_packet pcap_pkt; 306 assemble_pcapng_le_packet( &pcap_pkt, 307 0, 308 ns, 309 9+pkt->length, 310 pkt->channel_k, 311 sigdbm, 312 noisedbm, 313 pkt->access_address_offenses, 314 refAA, 315 flags, 316 &pkt->symbols[0] ); 317 pcap_dump((u_char *)h->dumper, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.le_ll_header); 318 return 0; 319 } 320 return -PCAP_INVALID_HANDLE; 321 } 322 323 #define PPI_BTLE 30006 324 325 typedef struct __attribute__((packed)) { 326 uint8_t pph_version; 327 uint8_t pph_flags; 328 uint16_t pph_len; 329 uint32_t pph_dlt; 330 } ppi_packet_header_t; 331 332 typedef struct __attribute__((packed)) { 333 uint16_t pfh_type; 334 uint16_t pfh_datalen; 335 } ppi_fieldheader_t; 336 337 typedef struct __attribute__((packed)) { 338 uint8_t btle_version; 339 uint16_t btle_channel; 340 uint8_t btle_clkn_high; 341 uint32_t btle_clk100ns; 342 int8_t rssi_max; 343 int8_t rssi_min; 344 int8_t rssi_avg; 345 uint8_t rssi_count; 346 } ppi_btle_t; 347 348 typedef struct __attribute__((packed)) { 349 struct pcap_pkthdr pcap_header; 350 ppi_packet_header_t ppi_packet_header; 351 ppi_fieldheader_t ppi_fieldheader; 352 ppi_btle_t le_ll_ppi_header; 353 uint8_t le_packet[48]; 354 } pcap_ppi_le_packet; 355 356 int 357 lell_pcap_append_ppi_packet(lell_pcap_handle * h, const uint64_t ns, 358 const uint8_t clkn_high, 359 const int8_t rssi_min, const int8_t rssi_max, 360 const int8_t rssi_avg, const uint8_t rssi_count, 361 const lell_packet *pkt) 362 { 363 const ppi_packet_header_sz = sizeof(ppi_packet_header_t); 364 const ppi_fieldheader_sz = sizeof(ppi_fieldheader_t); 365 const le_ll_ppi_header_sz = sizeof(ppi_btle_t); 366 367 if (h && h->dumper && 368 (h->dlt == DLT_PPI)) { 369 pcap_ppi_le_packet pcap_pkt; 370 uint32_t pcap_caplen = 371 ppi_packet_header_sz+ppi_fieldheader_sz+le_ll_ppi_header_sz+pkt->length+9; 372 uint16_t MHz = 2402 + 2*lell_get_channel_k(pkt); 373 374 pcap_pkt.pcap_header.ts.tv_sec = ns / 1000000000ull; 375 pcap_pkt.pcap_header.ts.tv_usec = ns % 1000000000ull; 376 pcap_pkt.pcap_header.caplen = pcap_caplen; 377 pcap_pkt.pcap_header.len = pcap_caplen; 378 379 pcap_pkt.ppi_packet_header.pph_version = 0; 380 pcap_pkt.ppi_packet_header.pph_flags = 0; 381 pcap_pkt.ppi_packet_header.pph_len = htole16(ppi_packet_header_sz+ppi_fieldheader_sz+le_ll_ppi_header_sz); 382 pcap_pkt.ppi_packet_header.pph_dlt = htole32(DLT_USER0); 383 384 pcap_pkt.ppi_fieldheader.pfh_type = htole16(PPI_BTLE); 385 pcap_pkt.ppi_fieldheader.pfh_datalen = htole16(le_ll_ppi_header_sz); 386 387 pcap_pkt.le_ll_ppi_header.btle_version = h->btle_ppi_version; 388 pcap_pkt.le_ll_ppi_header.btle_channel = htole16(MHz); 389 pcap_pkt.le_ll_ppi_header.btle_clkn_high = clkn_high; 390 pcap_pkt.le_ll_ppi_header.btle_clk100ns = htole32(pkt->clk100ns); 391 pcap_pkt.le_ll_ppi_header.rssi_max = rssi_max; 392 pcap_pkt.le_ll_ppi_header.rssi_min = rssi_min; 393 pcap_pkt.le_ll_ppi_header.rssi_avg = rssi_avg; 394 pcap_pkt.le_ll_ppi_header.rssi_count = rssi_count; 395 (void) memcpy( &pcap_pkt.le_packet[0], &pkt->symbols[0], pkt->length + 9 ); // FIXME where does the 9 come from? 396 pcap_dump((u_char *)h->dumper, &pcap_pkt.pcap_header, (u_char *)&pcap_pkt.ppi_packet_header); 397 pcap_dump_flush(h->dumper); 398 return 0; 399 } 400 return -PCAP_INVALID_HANDLE; 401 } 402 403 int 404 lell_pcap_close(lell_pcap_handle *h) 405 { 406 if (h && h->dumper) { 407 pcap_dump_close(h->dumper); 408 } 409 if (h && h->pcap) { 410 pcap_close(h->pcap); 411 } 412 if (h) { 413 free(h); 414 return 0; 415 } 416 return -PCAP_INVALID_HANDLE; 417 } 418 419 #endif /* USE_PCAP */ 420