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