1#!/usr/bin/env python3
2# Copyright 2019 Oticon A/S
3# SPDX-License-Identifier: Apache-2.0
4
5import time
6import argparse
7import struct
8from csv_common import *
9
10# Wireshark constants
11WS_FLAGS_PHY_1M = 0b00<<14
12WS_FLAGS_PHY_2M = 0b01<<14
13WS_FLAGS_PHY_CODED = 0b10<<14
14WS_FLAGS_SIGNAL_POWER = 0b1<<1
15
16# BabbleSim constants (see ext_2G4_libPhyComv1/src/bs_pc_2G4_modulations.h)
17P2G4_MOD_BLE = 0x10
18P2G4_MOD_BLE2M = 0x20
19P2G4_MOD_BLE_CODED = 0x50
20
21# default max packet length (phdr + access address + pdu + crc)
22SNAPLEN = 512
23
24def get_data_and_len(row):
25	pdu_crc = bytes.fromhex("00")
26
27	orig_len = int(row['packet_size'], 10)
28
29	if orig_len != 0:
30		try:
31			pdu_crc = bytes.fromhex(row['packet'])
32		except: #In case the packet is broken mid byte
33			pdu_crc = bytes.fromhex("00")
34
35		if len(pdu_crc) != orig_len:  # Let's handle this somehow gracefully
36			print("Truncated input file (partial packet), writing partial packet in output")
37			orig_len = len(pdu_crc)
38		orig_len += 14; # 10 bytes phdr + 4 bytes access address
39
40	return (orig_len, pdu_crc)
41
42def is_coded_FEC2(row):
43	if not row:
44		return False
45	if int(row['modulation'], 10) != P2G4_MOD_BLE_CODED:
46		return False
47	if int(row['packet_size'], 10) < 5: # It must at least have a header and CRC (2+3 bytes)
48		return False
49	return True
50
51def write(outfile, *inputs, snaplen = SNAPLEN, basetime=None):
52	#For information on the pcap format see https://wiki.wireshark.org/Development/LibpcapFileFormat
53
54	buf = bytearray(30)
55
56	if basetime == None:
57		basetime = int(time.time() * 1000000)
58
59	# write pcap header
60	struct.pack_into('<IHHiIII', buf, 0,
61			0xa1b2c3d4, # magic_number
62			2,          # version_major
63			4,          # version_minor
64			0,          # thiszone
65			0,          # sigfigs
66			snaplen,    # snaplen
67			256)        # network, 256 = BLUETOOTH_LE_LL_WITH_PHDR
68	outfile.write(buf[:24])
69
70	inputs = [ CSVFile(f) for f in inputs ]
71	rows = [ next(cf, None) for cf in inputs ]
72
73	while True:
74		min_ts = None
75		min_idx = None
76		for idx, row in enumerate(rows):
77			if not row:
78				continue
79			ts = row['start_time']
80			if min_ts == None or ts < min_ts:
81				min_ts = ts
82				min_idx = idx
83
84		if min_idx == None:
85			break
86
87		row = rows[min_idx]
88		rows[min_idx] = next(inputs[min_idx], None)
89
90		(orig_len, pdu_crc) = get_data_and_len(row)
91
92		freq = float(row['center_freq'])
93		if freq >= 1.0 and freq < 81.0:
94			rf_channel = int((freq - 1.0) / 2)
95		elif freq >= 2401.0 and freq < 2481.0:
96			rf_channel = int((freq - 2401.0) / 2)
97		else:
98			raise ValueError
99
100		access_address = int(row['phy_address'], 16)
101
102		flags = 0
103		modulation = int(row['modulation'], 10)
104		if modulation == P2G4_MOD_BLE:
105			flags |= WS_FLAGS_PHY_1M
106		elif modulation == P2G4_MOD_BLE2M:
107			flags |= WS_FLAGS_PHY_2M
108		elif modulation == P2G4_MOD_BLE_CODED:
109			flags |= WS_FLAGS_PHY_CODED
110			if (int(row['packet_size'], 10) != 1): # If this is a FEC1, it has only the CI byte
111				continue
112
113			# The next row likely contains this packet FEC2:
114			row = rows[min_idx]
115			if not is_coded_FEC2(row):
116				# Otherwise the coded phy packet was aborted before the FEC2, so we just ignore it
117				continue
118
119			rows[min_idx] = next(inputs[min_idx], None)
120
121			(orig_len, pdu_crc_FEC2) = get_data_and_len(row)
122
123			pdu_crc = bytes(pdu_crc) + pdu_crc_FEC2
124			orig_len += 1
125
126		# Transmission power (dBm)
127		flags |= WS_FLAGS_SIGNAL_POWER
128		signal_power = int(float(row['power_level']))
129
130		incl_len = min(orig_len, snaplen)
131
132		ts = basetime + min_ts
133		ts_sec = ts // 1000000
134		ts_usec = ts % 1000000
135
136		struct.pack_into('<IIIIBbbBIHI', buf, 0,
137				# pcap record header, 16 bytes
138				ts_sec,
139				ts_usec,
140				incl_len,
141				orig_len,
142				# packet data, incl_len bytes
143				# - phdr, 10 bytes
144				rf_channel,
145				signal_power,
146				0, # noise power
147				0, # access address offenses
148				0, # reference access address
149				flags,
150				# - le packet (access address + pdu + crc, no preamble)
151				access_address)
152		outfile.write(buf)
153		outfile.write(pdu_crc[:(incl_len - 14)])
154
155def parse_args():
156	parser = argparse.ArgumentParser(description='Convert BabbleSim Phy csv files to pcap')
157	parser.add_argument(
158			'-er', '--epoch_real',
159			action='store_true',
160			dest='epoch_real',
161			required=False,
162			default=True,
163			help='If set, the pcap timestamps will be offset based on the host time when the simulation was run. Otherwise they are directly the simulation timestamps (default)',
164			)
165	parser.add_argument(
166			'-es', '--epoch_simu',
167			action='store_false',
168			dest='epoch_real',
169			required=False,
170			help='If set, pcap timestamp are directly the simulation timestamps',
171			)
172	parser.add_argument('-o', '--output',
173			dest='output',
174			metavar='OUTFILE',
175			help='Write to this pcap file (required)',
176			required=True,
177			type=argparse.FileType(mode='wb'))
178	parser.add_argument(
179			dest='inputs',
180			metavar='INFILE',
181			help='Input csv file(s) (at least one is required)',
182			nargs='+',
183			type=open_input)
184	return parser.parse_args()
185
186args = parse_args()
187
188if (args.epoch_real == True):
189	basetime = int(min([ p[0] for p in args.inputs ]) * 1000000)
190else:
191	basetime = 0
192
193write(args.output, *[ p[1] for p in args.inputs ], basetime=basetime)
194
195# vim: set ts=4 sw=4 noet:
196