1# Copyright 2022 Oticon A/S
2# SPDX-License-Identifier: Apache-2.0
3
4# Simple BabbleSim device that supports sending of raw packets on the 2G4 phy
5#
6# The device supports two actions right now (in addition to connecting and tearing down the device):
7# * wait() to allow the simulation time to advance without taking any action
8#          This will be called automatically by the EDTT BSim transport when needed
9# * tx() for transmitting a raw packet at the specified time
10#        Note that only one packet can be transmitting at any given time
11#
12# The public functions are non-blocking; They simply add to a command queue
13# An internally handled thread will pick up these commands and execute them in order
14
15import math
16import os
17import queue
18import struct
19import threading
20
21from components.bsim_lib import ( P2G4_MOD_BLE, P2G4_MOD_BLE2M, P2G4_MSG_TX, P2G4_MSG_TX_END, PB_MSG_DISCONNECT, PB_MSG_WAIT, PB_MSG_WAIT_END, TIME_NEVER,
22                                  ch_idx_to_2G4_freq, create_com_folder, create_fifo_if_not_there, test_and_create_lock_file )
23
24class BSimDevice:
25
26    device_nbr = -1
27    sim_id = ""
28    com_path = ""
29    lock_path = ""
30    ff_path_dtp = ""
31    ff_path_ptd = ""
32    ff_ptd = 0
33    ff_dtp = 0
34    connected = False
35    simulation_time = 0
36
37    command_queue = queue.Queue()
38    worker_thread = None
39
40    def __init__(self, device_nbr, sim_id, TraceClass):
41        self.Trace = TraceClass
42
43        self.device_nbr = device_nbr
44        self.sim_id = sim_id
45
46    def __process_commands(self):
47        while self.connected:
48            command, args = self.command_queue.get()
49            if (command == PB_MSG_DISCONNECT):
50                self.__device_disconnect()
51            elif (command == PB_MSG_WAIT):
52                self.__device_wait(args[0])
53            elif (command == P2G4_MSG_TX):
54                self.__device_tx(args[0], args[1], args[2], args[3], args[4])
55
56    def __read(self, fifo, nbr_bytes):
57        result = os.read(fifo, nbr_bytes)
58        if len(result) != nbr_bytes:
59            raise Exception("Low level communication with PHY failed; (tried to get %s got %s bytes)" % (nbr_bytes, len(result)))
60        return result
61
62    def connect(self):
63        self.com_path = create_com_folder(self.sim_id)
64
65        self.lock_path = test_and_create_lock_file(self.com_path, self.device_nbr, self.Trace)
66
67        self.ff_path_dtp = "%s/%s.d%i.dtp" % (self.com_path, "2G4", self.device_nbr)
68        self.ff_path_ptd = "%s/%s.d%i.ptd" % (self.com_path, "2G4", self.device_nbr)
69
70        try:
71            create_fifo_if_not_there(self.ff_path_dtp)
72            create_fifo_if_not_there(self.ff_path_ptd)
73
74            self.ff_ptd = os.open(self.ff_path_ptd, os.O_RDONLY)
75            self.ff_dtp = os.open(self.ff_path_dtp, os.O_WRONLY)
76
77            self.connected = True
78
79            # Start worker
80            self.worker_thread = threading.Thread(target=self.__process_commands, daemon=True)
81            self.worker_thread.start()
82        except:
83            self.cleanup()
84            raise
85
86    def cleanup(self):
87        if self.lock_path:
88            try:
89                os.remove(self.lock_path)
90            except:
91                pass
92            self.lock_path = ""
93
94        self.connected = False
95
96        if self.ff_path_dtp:
97            if self.ff_dtp:
98                os.close(self.ff_dtp)
99            self.ff_dtp = 0
100            try:
101                os.remove(self.ff_path_dtp)
102            except:
103                pass
104            self.ff_path_dtp = ""
105
106        if self.ff_path_ptd:
107            if self.ff_ptd:
108                os.close(self.ff_ptd)
109            self.ff_ptd = 0
110            try:
111                os.remove(self.ff_path_ptd)
112            except:
113                pass
114            self.ff_path_ptd = ""
115
116        if self.com_path:
117            try:
118                os.rmdir(self.com_path)
119            except:
120                pass
121            self.com_path = ""
122
123    def __device_disconnect(self):
124        if self.connected:
125            msg = struct.pack('=I', PB_MSG_DISCONNECT)
126            os.write(self.ff_dtp, msg)
127            self.connected = False
128
129    def disconnect(self):
130        if self.connected:
131            self.command_queue.put_nowait((PB_MSG_DISCONNECT, []))
132            self.worker_thread.join(1.0)
133        self.cleanup()
134
135    def __device_wait(self, end_of_wait):
136        if self.connected and end_of_wait > self.simulation_time:
137            msg = struct.pack("=IQ", PB_MSG_WAIT, end_of_wait)
138            os.write(self.ff_dtp, msg)
139
140            # wait for reply
141            raw_header = self.__read(self.ff_ptd, 4)
142
143            header, = struct.unpack("=I", raw_header)
144            if header == PB_MSG_DISCONNECT:
145                self.connected = False
146            elif header != PB_MSG_WAIT_END:
147                raise Exception("Low level communication with PHY failed; Received invalid response %s" % header)
148            self.simulation_time = end_of_wait
149
150    def wait(self, end_of_wait):
151        if self.connected:
152            self.command_queue.put_nowait((PB_MSG_WAIT, [end_of_wait]))
153
154    def __device_tx(self, ch_idx, phy, aa, transmit_time, packet_data):
155        if self.connected:
156            # Note: Packet air length is: pre-amble + AA + packetData
157            airtime = math.ceil(((2 if phy == '2M' else 1) + 4 + len(packet_data))*8/(2 if phy == '2M' else 1))
158            modulation = P2G4_MOD_BLE2M if phy == '2M' else P2G4_MOD_BLE
159            freq = ch_idx_to_2G4_freq(ch_idx)
160
161            # Header structure: start_time, end_time, abort_time, (abort_)recheck_time, phy_address, modulation, frequency, power, packet_size
162            msg = struct.pack("=IQQQQIHHHH", P2G4_MSG_TX, transmit_time, transmit_time + airtime, TIME_NEVER, TIME_NEVER, aa, modulation, freq, 0, len(packet_data))
163            os.write(self.ff_dtp, msg)
164            os.write(self.ff_dtp, packet_data)
165
166            # wait for reply
167            raw_header = self.__read(self.ff_ptd, 4)
168
169            header, = struct.unpack("=I", raw_header)
170            if header == PB_MSG_DISCONNECT:
171                self.connected = False
172            elif header != P2G4_MSG_TX_END:
173                raise Exception("Low level communication with PHY failed; Received invalid response %s" % header)
174
175            raw_end_time = self.__read(self.ff_ptd, 8)
176            end_time, = struct.unpack("=Q", raw_end_time)
177            self.simulation_time = end_time
178
179    def tx(self, ch_idx, phy, aa, transmit_time, packet_data):
180        # Note: packet_data is expected to include CRC
181        if self.connected:
182            self.command_queue.put_nowait((P2G4_MSG_TX, [ch_idx, phy, aa, transmit_time, packet_data]))
183