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