1#!/usr/bin/env python3
2# Copyright 2019 Oticon A/S
3# Copyright 2023 Nordic Semiconductor ASA
4# SPDX-License-Identifier: Apache-2.0
5
6# For information on the pcapng format see https://pcapng.com/ & https://www.ietf.org/staging/draft-tuexen-opsawg-pcapng-02.html
7# You may also want to check
8#   https://tshark.dev/
9#   https://wiki.wireshark.org/Development/PcapNg
10
11import time
12import argparse
13import struct
14from csv_common import *
15
16# default max packet length (phdr + access address + pdu + crc)
17SNAPLEN = 512
18
19def write(outfile, *inputs, snaplen = SNAPLEN, basetime=None):
20	buf = bytearray(64)
21	zeroes = bytearray(4)
22
23	# Write pcapng SHB (section header block) (We create just 1 for the whole file)
24	Block_len = 28
25	struct.pack_into('<IIIHHQI', buf, 0,
26			0x0a0d0d0a, # Block Type: SHB
27			Block_len,  # Block Length
28			0x1a2b3c4d, # Byte-Order Magic
29			1,          # version_major
30			0,          # version_minor
31			0xffffffffffffffff, #Section Length
32			# Options:
33				# No options
34			Block_len   # Block Length (repeat)
35			)
36	outfile.write(buf[:Block_len])
37
38	# Interface drescription block for BLE #0
39	Block_len = 32
40	struct.pack_into('<IIHHIHHIHHI', buf, 0,
41			1,         # Block Type : IDB
42			Block_len, # Block Total Length
43			256,       # Link Type: 256 = BLUETOOTH_LE_LL_WITH_PHDR
44			0,         # Reserved (0x0)
45			snaplen,   # Snap length
46			#Options
47			0x9, # if_tsresol
48				0x1, # (1 byte of if_tsresol)
49				0x6, # microsecond resolution
50				#Note 3 bytes of padding just added by storing 0x6 as 32bits
51			0x0, # opt_endofopt
52			0x0, # padding
53			Block_len)
54	outfile.write(buf[:Block_len])
55
56	# Interface drescription block for 15.4 #1
57	Block_len = 32
58	struct.pack_into('<IIHHIHHIHHI', buf, 0,
59			1,         # Block Type : IDB
60			Block_len, # Block Total Length
61			215,       # Link Type: 215 = DLT_IEEE802_15_4_NONASK_PHY ; https://www.tcpdump.org/linktypes.html
62			0,         # Reserved (0x0)
63			snaplen,   # Snap length
64			#Options
65			0x9, # if_tsresol
66				0x1, # (1 byte of if_tsresol)
67				0x6, # microsecond resolution
68				#Note 3 bytes of padding just added by storing 0x6 as 32bits
69			0x0, # opt_endofopt
70			0x0, # padding
71			Block_len)
72	outfile.write(buf[:Block_len])
73
74	# Actual packets:
75
76	if basetime == None:
77		basetime = int(time.time() * 1000000)
78
79	inputs = [ CSVFile(f) for f in inputs ]
80	rows = [ next(cf, None) for cf in inputs ]
81
82	while True:
83		min_ts = None
84		min_idx = None
85		for idx, row in enumerate(rows):
86			if not row:
87				continue
88			ts = row['start_time']
89			if min_ts == None or ts < min_ts:
90				min_ts = ts
91				min_idx = idx
92
93		if min_idx == None:
94			break
95
96		row = rows[min_idx]
97		rows[min_idx] = next(inputs[min_idx], None)
98
99		orig_len = int(row['packet_size'], 10)
100		if orig_len == 0:
101			continue
102
103		modulation = int(row['modulation'], 10)
104		if modulation == 256: # IEEE 802.15.4-2006 DSS 250kbps O-QPSK PHY
105			is_ble = False
106		else: # Otherwise we just assume it is BLE
107			is_ble = True
108
109		freq = float(row['center_freq'])
110		if freq >= 1.0 and freq < 81.0:
111			rf_channel = int((freq - 1.0) / 2)
112		elif freq >= 2401.0 and freq < 2481.0:
113			rf_channel = int((freq - 2401.0) / 2)
114		else:
115			raise ValueError
116
117		try:
118			pdu_crc = bytes.fromhex(row['packet'])
119		except: #In case the packet is broken mid byte
120			pdu_crc = bytes.fromhex("00")
121
122		if len(pdu_crc) != orig_len: # Let's handle this somehow gracefully
123			print("Truncated input file (partial packet), writing partial packet in output")
124			orig_len = len(pdu_crc)
125
126		if is_ble:
127			orig_len += 14; # 10 bytes phdr + 4 bytes access address
128		else:
129			orig_len += 5; # 4 bytes preamble + 1 bytes address/SFD
130		incl_len = min(orig_len, snaplen)
131		packet_padding = (4 - incl_len % 4) % 4
132
133		access_address = int(row['phy_address'], 16)
134
135		ts = basetime + min_ts
136
137		ts_upper = ts // 2**32;
138		ts_lower = ts % 2**32;
139
140		# Enhanced Packet Block (EPB) (header)
141		EPB_header = 28
142		EPB_tail = 4
143		Block_len = EPB_header + incl_len + packet_padding + EPB_tail
144
145		struct.pack_into('<IIIIIII', buf, 0,
146				6,         # Block Type : EPB
147				Block_len, # Block Total Length
148				0 if is_ble else 1, #Interface ID
149				ts_upper,  # Timestamp Upper 4 bytes
150				ts_lower,  # Timestamp Lower 4 bytes
151				incl_len,  # Captured Packet Length
152				orig_len  # Original Packet Length
153		)
154		outfile.write(buf[:EPB_header])
155
156		# Packet data
157		if is_ble:
158			struct.pack_into('<BbbBIHI', buf, 0,
159				# packet data, incl_len bytes
160				# - phdr, 10 bytes
161				rf_channel,
162				0, # signal power #TODO: Take it from the csv file
163				0, # noise power
164				0, # access address offenses
165				0, # reference access address
166				0, # flags
167				# - le packet (access address + pdu + crc, no preamble)
168				access_address)
169			outfile.write(buf[0:14])
170			outfile.write(pdu_crc[:(incl_len - 14)])
171		else: # 15.4
172			struct.pack_into('<IB', buf, 0,
173				#Header
174				0, # preamble (0)
175				access_address) #SFD
176			outfile.write(buf[:5])
177			#The packet starting from the lenght byte
178			outfile.write(pdu_crc[:(incl_len - 5)])
179
180		outfile.write(zeroes[:packet_padding])
181
182		struct.pack_into('<I', buf, 0,
183				#No options
184				Block_len)
185		outfile.write(buf[:EPB_tail])
186
187def parse_args():
188	parser = argparse.ArgumentParser(description='Convert BabbleSim Phy csv files to pcapng')
189	parser.add_argument(
190			'-er', '--epoch_real',
191			action='store_true',
192			dest='epoch_real',
193			required=False,
194			help='If set, the pcapng timestamps will be offset based on the host time when the simulation was run. Otherwise they are directly the simulation timestamps',
195			)
196	parser.add_argument(
197			'-es', '--epoch_simu',
198			action='store_false',
199			dest='epoch_real',
200			required=False,
201			help='If set, pcapng timestamp are directly the simulation timestamps (default)',
202			)
203	parser.add_argument('-o', '--output',
204			dest='output',
205			metavar='OUTFILE',
206			help='Write to this pcapng file (required)',
207			required=True,
208			type=argparse.FileType(mode='wb'))
209	parser.add_argument(
210			dest='inputs',
211			metavar='INFILE',
212			help='Input csv file(s) (at least one is required)',
213			nargs='+',
214			type=open_input)
215	return parser.parse_args()
216
217args = parse_args()
218
219if (args.epoch_real == True):
220	basetime = int(min([ p[0] for p in args.inputs ]) * 1000000)
221else:
222	basetime = 0
223
224write(args.output, *[ p[1] for p in args.inputs ], basetime=basetime)
225
226# vim: set ts=4 sw=4 noet:
227