wiliot-certificate 4.4.3__py3-none-any.whl → 4.5.0a1__py3-none-any.whl

{brg_certificate → certificate}/cert_gw_sim.py

@@ -4,21 +4,36 @@ import re
 import paho.mqtt.client as mqtt
 import serial
 import serial.tools.list_ports
-from brg_certificate.cert_mqtt import *
-from brg_certificate.cert_defines import *
-from brg_certificate.cert_prints import *
-import brg_certificate.cert_common as cert_common
-from brg_certificate.cert_data_sim import PIXEL_SIM_INDICATOR, write_to_data_sim_log_file
-import brg_certificate.cert_utils as cert_utils
+from certificate.cert_mqtt import *
+from certificate.cert_defines import *
+from certificate.cert_prints import *
+import certificate.cert_common as cert_common
+from certificate.cert_data_sim import PIXEL_SIM_INDICATOR, write_to_data_sim_log_file
+import certificate.cert_utils as cert_utils
 
 # Generic Defines
-SERIAL_TIMEOUT =                                    0.1 # TODO decide about the right value
+SERIAL_TIMEOUT =                                    0.05 # TODO decide about the right value
 STOP_ADVERTISING =                                  '!stop_advertising'
 RESET_GW =                                          '!reset'
 DEDUPLICATION_PKTS =                                '!deduplication_pkts'
+GW_CERT_TESTER =                                    '!gw_cert_tester'
 SET_RX_CHANNEL =                                    '!set_rx_channel'
 VERSION =                                           '!version'
 CONNECTIVITY_STATUS =                               '!connectivity_status'
+BLE_SIM_INIT =                                      '!ble_sim_init'
+BLE_SIM =                                           '!ble_sim'
+BLE_SIM_EXT_ADV =                                   '!ble_sim_ext_adv'
+
+# Received RX uarts
+RX_NORDIC_RECOVER_RESET =                           "NORDIC_RECOVER 1"
+RX_NORDIC_RECOVER_NORESET =                         "NORDIC_RECOVER 0"
+RX_RESET =                                          "reset"
+
+GW_SIM_RESET_TS = None
+
+# Slip related
+SLIP_END =                                          0xC0
+UART_PKT_PREFIX_BYTE =                              0x70
 
 # Interference Analysis Defines
 DEFAULT_LOOKOUT_TIME =                              2
@@ -34,6 +49,7 @@ CNTRS_KEYS =                                        [NON_WLT_RX, WLT_RX, BAD_CRC
 
 GW_STATUS_MESSAGES = []
 
+
 ##############################################
 # UART PKT TYPES
 ##############################################
@@ -41,9 +57,9 @@ class UplinkPkt(): # p6
     def __init__(self, gw, seq_id, raw):
         self.gw = gw
         self.seq_id = seq_id
-        self.alias_brg_id = raw[0:12]
-        self.payload = raw[12:74]
-        self.rssi = int(raw[74:76], 16)
+        self.alias_brg_id = raw[2:14]
+        self.payload = raw[14:76]
+        self.rssi = int(raw[0:2], 16)
     def dump(self):
         return {
             GW_ID: self.gw, TIMESTAMP: time.time()*1000,
@@ -58,9 +74,26 @@ class UplinkExtendedPkt(): # p7
     def __init__(self, gw, seq_id, raw):
         self.gw = gw
         self.seq_id = seq_id
-        self.alias_brg_id = raw[0:12]
-        self.payload = raw[12:96] # 39 payload + 3 side info
-        self.rssi = int(raw[96:98], 16)
+        self.alias_brg_id = raw[2:14]
+        self.payload = raw[14:98] # 39 payload + 3 side info
+        self.rssi = int(raw[0:2], 16)
+    def dump(self):
+        return {
+            GW_ID: self.gw, TIMESTAMP: time.time()*1000,
+            "packets": [{ALIAS_BRIDGE_ID: self.alias_brg_id,
+                         TIMESTAMP: time.time()*1000,
+                         SEQUENCE_ID: self.seq_id,
+                         RSSI: self.rssi,
+                         PAYLOAD: self.payload}]
+        }
+
+class UplinkAggregatedPkt(): # p8
+    def __init__(self, gw, seq_id, raw):
+        self.gw = gw
+        self.seq_id = seq_id
+        self.alias_brg_id = raw[2:14]
+        self.payload = raw[14:]
+        self.rssi = int(raw[0:2], 16)
     def dump(self):
         return {
             GW_ID: self.gw, TIMESTAMP: time.time()*1000,
@@ -74,20 +107,30 @@ class UplinkExtendedPkt(): # p7
 ##############################################
 # UT HELPER FUNCTIONS
 ##############################################
-def prep_gw(args, mqttc, start_time):
-    # Check GW is online and configure to defaults
-    utPrint(SEP)
-    utPrint("Checking UART response and configure internal brg to defaults", "BLUE")
-    gw = args.gw
-    protobuf = False
-    internal_brg_mac_addr = os.getenv(GW_SIM_BLE_MAC_ADDRESS)
-    internal_brg_ble_ver = os.getenv(GW_APP_VERSION_HEADER)
-    if not internal_brg_mac_addr:
-        cert_utils.handle_error(f"ERROR: Didn't receive {GW_SIM_BLE_MAC_ADDRESS} response!", start_time)
-    internal_brg = cert_utils.ut_prep_brg(args, mqttc, start_time, gw, internal_brg_mac_addr, "prod", protobuf)
-    if internal_brg.api_version != ag.API_VERSION_LATEST:
-        cert_utils.handle_error(f"ERROR: Certificate FW api_version={internal_brg.api_version} instead of api_version={ag.API_VERSION_LATEST}! Please upgrade the FW!", start_time)
-    return gw, internal_brg, "prod", {BLE_VERSION:internal_brg_ble_ver, WIFI_VERSION:"0.0.0"}, protobuf
+
+def slip_decode(data: bytes) -> bytes:
+    SLIP_ESC = 0xDB
+    SLIP_ESC_END = 0xDC
+    SLIP_ESC_ESC = 0xDD
+
+    decoded = bytearray()
+    i = 0
+    while i < len(data):
+        byte = data[i]
+        if byte == SLIP_END:
+            i += 1
+            continue
+        elif byte == SLIP_ESC:
+            i += 1
+            if i < len(data):
+                if data[i] == SLIP_ESC_END:
+                    decoded.append(SLIP_END)
+                elif data[i] == SLIP_ESC_ESC:
+                    decoded.append(SLIP_ESC)
+        else:
+            decoded.append(byte)
+        i += 1
+    return bytes(decoded)
 
 ##############################################
 # UART FUNCTIONS
@@ -97,11 +140,55 @@ def write_to_ble(ble_serial, txt, print_enable=True, sleep=0):
     #     print('\n' + txt)
     ble_serial.write(bytes(txt, encoding='utf-8') + b'\r\n')
     if sleep:
-        cert_common.wait_time_n_print(sleep)
+        wait_time_n_print(sleep)
+
+def read_frame(ble_serial: serial.Serial, frame_max_read_attempts: int = 5):
+    """
+    Read either [0xC0][data][0xC0] uart messages, or [data] message (for !version reply)
+
+    :return str/None: ascii string if read successfully. None otherwise
+    """
+    opening_delim_received = False
+    closing_delim_received = False
+    read_attempts = 0
+
+    input = ble_serial.read_until(expected=bytes([SLIP_END]))
+    # We got the opening SLIP_END delimiter, read the message.
+    # It's in a while loop to skip empty frames.
+    while len(input) == 1 and input[0] == SLIP_END:
+        opening_delim_received = True
+        if read_attempts > frame_max_read_attempts:
+            return None
+        input = ble_serial.read_until(expected=bytes([SLIP_END]))
+        read_attempts += 1
+    
+    while opening_delim_received and not closing_delim_received:
+        # Keep reading until we get the closing delimiter
+        if input[-1] == SLIP_END:
+            closing_delim_received = True
+            break
+        if read_attempts > frame_max_read_attempts:
+            print(f'WARNING: Dropped pkt with partial frame: {input}')
+            return None
+        chunk = ble_serial.read_until(expected=bytes([SLIP_END]))
+        if not chunk:
+            read_attempts += 1
+            continue
+        input += chunk
+    
+    return input
 
-def read_from_ble(ble_serial):
-    ble_serial_bytes = ble_serial.readline()
-    input = ble_serial_bytes.decode("utf-8", "ignore").strip()
+def read_from_ble(ble_serial: serial.Serial):
+    input = read_frame(ble_serial)
+    if input == None or len(input) == 0:
+        return None
+    
+    input = slip_decode(input)
+    input_len = len(input)
+    if input_len > 3 and input[0] == UART_PKT_PREFIX_BYTE:
+        input = input[0:3].decode("utf-8", "ignore") + input[3:].hex().upper()
+    else:
+        input = input.decode("utf-8", "ignore").strip()
     # if input:
     #     print(input)
     return input
@@ -155,7 +242,7 @@ def interference_analysis(ble_serial):
         print('\n' + '#' * 30 + f'\nAnalyzing channel {channel[0]}\n' + '#' * 30)
         # Send the sniffer a command to retrieve the counters and convert them to dict
         start_cntrs = get_pkts_cntrs(ble_serial, channel[0], set_rx_ch=True)
-        cert_common.wait_time_n_print(CNTRS_LISTEN_TIME_SEC)
+        wait_time_n_print(CNTRS_LISTEN_TIME_SEC)
         end_cntrs = get_pkts_cntrs(ble_serial, channel[0])
 
         if start_cntrs is None or end_cntrs is None:
@@ -193,10 +280,6 @@ def on_unsubscribe(mqttc, userdata, mid):
 def on_message(client, userdata, message):
     data = json.loads(message.payload.decode("utf-8"))
     print_enable = True if not PIXEL_SIM_INDICATOR in str(message.payload.decode("utf-8")) else False
-    # if print_enable:
-    #     print("##########\n// Message received at {}, topic={}:\n{}\n".format(datetime.datetime.now().strftime("%d/%m/%Y, %H:%M:%S"), message.topic, str(message.payload.decode("utf-8"))))
-    #     #TODO: logging print
-    #     # print("##########\n// Message received at {}, topic={}:\n{}\n".format(datetime.datetime.now().strftime("%d/%m/%Y, %H:%M:%S"), message.topic, str(message.payload.decode("utf-8"))))
     # Send packet to UART
     if TX_PKT in data:
         # Downlink packet
@@ -215,36 +298,54 @@ def on_message(client, userdata, message):
 # GW SIMULATOR
 ##############################################
 def parse_uart_pkts(input, mqttc, custom_broker, gw_id, seq_id):
-    # 3 for p6, 12 for alias_brg_id, 62 for payload, 2 for rssi
+    # 3 for p6, 2 for rssi, 12 for alias_brg_id, 62 for payload
     if input.startswith("p6 ") and len(input) == (3 + 12 + 62 + 2):
-        # p6 1234567898761E16C6FC0000EE02093E3C71BF6DFA3C006648001CB8003A730160000E010031
+        # p6 301234567898761E16C6FC0000EE02093E3C71BF6DFA3C006648001CB8003A730160000E0100
         pkt = UplinkPkt(gw_id, seq_id, input.split()[1])
         mqttc.publish(custom_broker[CUSTOM_BROKER_DATA_TOPIC], payload=json.dumps(pkt.dump(), indent=4))
         return True
-    # 3 for p7, 12 for alias_brg_id, 78 for payload, 6 for side info, 2 for rssi
+    # 3 for p7, 2 for rssi, 12 for alias_brg_id, 78 for payload, 6 for side info
     elif input.startswith("p7 ") and len(input) == (3 + 12 + 78 + 6 + 2):
-        # p7 1234567898762616C6FC05000002093E3C71BF6DFA3C006648001CB8003A730160000E0100112233445566778831
+        # p7 301234567898762616C6FC05000002093E3C71BF6DFA3C006648001CB8003A730160000E01001122334455667788
         pkt = UplinkExtendedPkt(gw_id, seq_id, input.split()[1])
         mqttc.publish(custom_broker[CUSTOM_BROKER_DATA_TOPIC], payload=json.dumps(pkt.dump(), indent=4))
         return True
+    # 3 for p8, 2 for rssi, 12 for alias_brg_id, the rest for payload of multiple packets
+    elif input.startswith("p8 "):
+        # p8 301234567898762616C6FC05000002093E3C71BF6DFA3C006648001CB8003A730160000E01001122334455667788....
+        pkt = UplinkAggregatedPkt(gw_id, seq_id, input.split()[1])
+        mqttc.publish(custom_broker[CUSTOM_BROKER_DATA_TOPIC], payload=json.dumps(pkt.dump(), indent=4))
+        return True
     elif GW_STATUS_MESSAGES:
         pkt = GW_STATUS_MESSAGES.pop(0)
         mqttc.publish(custom_broker[CUSTOM_BROKER_STATUS_TOPIC], payload=json.dumps(pkt, indent=4))
     return False
 
-def gw_sim_run(port, gw_id, analyze_interference=False):
+def handle_cmds(input, ble_serial):
+    if input.startswith(RX_NORDIC_RECOVER_RESET):
+        utPrint(f"Simulator received reboot packet", "CYAN")
+        GW_SIM_RESET_TS = datetime.datetime.now()
+        write_to_ble(ble_serial, RESET_GW, sleep=0)
+    if input.startswith(RX_NORDIC_RECOVER_NORESET):
+        utPrint(f"Simulator recovered", "CYAN")
+        write_to_ble(ble_serial, f"{CONNECTIVITY_STATUS} 1 1")
 
-    print(f"###>>> GW SIM STARTED WITH PORT {port}")
+def gw_sim_run(port, gw_id, custom_broker, analyze_interference=False):
 
     # Init serial side
-    print("\nAvailable ports:")
-    for port, desc, hwid in sorted(serial.tools.list_ports.comports()):
-        print("{}: {} [{}]".format(port, desc, hwid))
-    ble_serial = serial.Serial(port=port, baudrate=921600, timeout=SERIAL_TIMEOUT)
+    if not port:
+        print("\nNo COM port given. Scanning for available ports:")
+        for port, desc, hwid in sorted(serial.tools.list_ports.comports()):
+            print("{}: {} [{}]".format(port, desc, hwid))
+    if not port:
+        print("\nNo available COM port found!")
+        sys.exit(-1)
+    print(f"###>>> GW SIM STARTED WITH PORT {port}")
+    ble_serial = serial.serial_for_url(url=port, baudrate=921600, timeout=SERIAL_TIMEOUT)
     ble_serial.flushInput()
 
     # Init mqtt side
-    custom_broker = load_custom_broker(gw_id)
+    custom_broker = load_custom_broker(custom_broker, gw_id)
     client_id = '{}-republish2'.format(gw_id)
     userdata = {'serial': ble_serial}
     mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1, client_id, userdata=userdata)
@@ -282,10 +383,11 @@ def gw_sim_run(port, gw_id, analyze_interference=False):
     seq_id = 100
     while True:
         input = read_from_ble(ble_serial)
-        if input and input[0] == "p" and input[2] == " ":
+        if input and len(input) >= 3 and input[0] == "p" and input[2] == " ":
             seq_id += 1
         # input = ""
-        if not parse_uart_pkts(input, mqttc, custom_broker, gw_id, seq_id):
+        if input and not parse_uart_pkts(input, mqttc, custom_broker, gw_id, seq_id):
+            handle_cmds(input, ble_serial)
             # if input:
             if 0:
                 print(f"###>>> IGNORED: {input}")

{brg_certificate → certificate}/cert_mqtt.py

@@ -8,28 +8,40 @@ import datetime
 import base64
 import copy
 import traceback
-import brg_certificate.wltPb_pb2 as wpb
+import common.wltPb_pb2 as wpb
 from google.protobuf.message import DecodeError
 from google.protobuf.json_format import MessageToDict
 
 # Local imports
-from brg_certificate.wlt_types import *
-from brg_certificate.cert_data_sim import PIXEL_SIM_INDICATOR, TAG_ID_OFFSET
-from brg_certificate.cert_prints import *
-from brg_certificate.cert_defines import *
+from certificate.wlt_types import *
+from certificate.cert_data_sim import PIXEL_SIM_INDICATOR, TAG_ID_OFFSET
+from certificate.cert_prints import *
+from certificate.cert_defines import *
 
 
 SENSORS_DATA_SI_DUP = 6
-
+# Custom MQTT brokers' configuration jsons' paths
+EMQX = os.path.join(BASE_DIR, "config/emqx.json")
+WILIOT_DEV = os.path.join(BASE_DIR, "config/wiliot-dev.json")
 class WltMqttPkt:
     def __init__(self, body, topic, userdata):
         self.body = body
         self.mqtt_topic = topic
-        self.mqtt_timestamp = datetime.datetime.now()
+        self.mqtt_timestamp = int(time.time() * 1000)
         self.body_ex = {}
         if "data" in self.mqtt_topic:
             self.body_ex = copy.deepcopy(body)
             self.body_ex['undecrypted'] = 0
+
+            # Split aggregated packets into multiple packets
+            split_pkts = []
+            for pkt in self.body_ex[PACKETS]:
+                if len(pkt[PAYLOAD])/2 > (int(pkt[PAYLOAD][0:2], 16) + 1): # Aggregated packet since the length is larger than the header length
+                    split_pkts += split_aggregated_pkt(pkt)
+                else: # Not an aggregated packet
+                    split_pkts += [pkt]
+            self.body_ex[PACKETS] = split_pkts
+
             for pkt in self.body_ex[PACKETS]:
                 # if packet is not a mgmt packet or a side info packet it is a data packet
                 wlt_pkt = WltPkt(pkt[PAYLOAD])
@@ -45,7 +57,12 @@ class WltMqttPkt:
                     elif wlt_pkt.hdr.group_id == ag.GROUP_ID_SIDE_INFO_SENSOR:
                         pkt[SIDE_INFO_SENSOR_PKT] = copy.deepcopy(wlt_pkt)
                     elif wlt_pkt.hdr.uuid_lsb == ag.HDR_DEFAULT_BRG_SENSOR_UUID_LSB and wlt_pkt.hdr.uuid_msb == ag.HDR_DEFAULT_BRG_SENSOR_UUID_MSB:
-                        pkt[SENSOR_PKT] = copy.deepcopy(wlt_pkt)
+                        if isinstance(wlt_pkt.pkt, ag.UnifiedSensorPkt):
+                            pkt[UNIFIED_SENSOR_PKT] = copy.deepcopy(wlt_pkt)
+                            if (wlt_pkt.hdr.group_id == ag.GROUP_ID_SIGNAL_INDICATOR):
+                                pkt[UNIFIED_SENSOR_PKT].pkt.signal_indicator_payload = ag.SignalIndicatorDataV1(pkt[PAYLOAD][8:62])
+                        else:
+                            pkt[SENSOR_PKT] = copy.deepcopy(wlt_pkt)
                         pkt[DECODED_DATA] = {TAG_ID: None, PACKET_CNTR: None, PACKET_TYPE: None}
                 else:
                     if userdata["data"] != DATA_SIMULATION:
@@ -91,7 +108,7 @@ class WltMqttPkts:
         self.status = []
 
 def local_resolve(p):
-    import brg_certificate.ut.ut_resolve as ut_resolve
+    import certificate.ut.ut_resolve as ut_resolve
     irresolvable_payloads = ["1E16C6FC0000EE", "1E16AFFD0000EC", "1E16C6FC0000EC"]
     try:
         if any(substring in p for substring in irresolvable_payloads):
@@ -115,6 +132,39 @@ def local_resolve(p):
         print(e)
         print(f"Failed in local_resolve with packet: {p}\n")
         return None
+    
+def split_aggregated_pkt(pkt):
+    hex_str = pkt[PAYLOAD]
+    idx = 0
+    fragments = []
+
+    # walk through the hex string
+    while idx < len(hex_str):
+        # parse length byte
+        length = int(hex_str[idx:idx+2], 16)
+        if length < ag.HDR_DEFAULT_PKT_SIZE:
+            # The sensor packet itself is built from small segments, the smallest payload is of size 31 bytes
+            length = ag.HDR_DEFAULT_PKT_SIZE
+        end = idx + 2 + length*2
+        if end > len(hex_str):
+            raise ValueError(
+                f"Packet at position {idx} says {length} bytes, "
+                f"but only {(len(hex_str)-idx-2)//2} are available. pkt : {pkt[PAYLOAD]}"
+            )
+        # slice out this packet (including the length byte itself)
+        fragment = hex_str[idx:end]
+
+        # build new packet dict
+        fragments.append({
+            PAYLOAD: fragment,
+            TIMESTAMP: pkt[TIMESTAMP],
+            SEQUENCE_ID: pkt[SEQUENCE_ID],
+            RSSI: pkt[RSSI],
+            ALIAS_BRIDGE_ID: pkt[ALIAS_BRIDGE_ID],
+            AGGREGATED_PAYLOAD: pkt[PAYLOAD]
+        })
+        idx = end
+    return fragments
 
 def is_json(msg):
     is_utf = True
@@ -165,7 +215,7 @@ def on_message_protobuf(mqttc, userdata, message):
         pb_msg.ParseFromString(message.payload)
         pb_decoded = True
     except DecodeError as e: 
-        print(f'ERROR: failed decoding {message.topic} message: \n{e}\n')
+        print(f'ERROR: failed decoding {message.topic} message: {e}\npayload: {message.payload}')
 
     if pb_decoded is True:
         pb_msg_dict = MessageToDict(pb_msg)
@@ -178,6 +228,7 @@ def on_message_protobuf(mqttc, userdata, message):
                 for idx, id in enumerate(ids_list):
                     ids_list[idx] = base64.b64decode(id).hex().upper()
         if 'data' in message.topic and PACKETS in pb_msg_dict.keys():
+            pb_msg_dict[TIMESTAMP] = (int(pb_msg_dict[TIMESTAMP]))
             for idx, pkt in enumerate(pb_msg_dict[PACKETS]):
                 pb_msg_dict[PACKETS][idx][PAYLOAD] = base64.b64decode(pkt[PAYLOAD]).hex().upper()
                 pb_msg_dict[PACKETS][idx][TIMESTAMP] = int(pkt[TIMESTAMP])
@@ -196,37 +247,36 @@ def on_message_protobuf(mqttc, userdata, message):
 def pkts_to_log(pkts):
     text = ""
     for p in pkts:
-        text += "// {} topic={}".format(p.mqtt_timestamp.strftime("%d/%m/%Y, %H:%M:%S"), p.mqtt_topic)
+        dt = datetime.datetime.fromtimestamp(p.mqtt_timestamp / 1000)
+        text += "// {} topic={}".format(dt.strftime("%d/%m/%Y, %H:%M:%S"), p.mqtt_topic)
         text += "\n"+json.dumps(p.body, indent=4)+"\n"
     return text
 
-def write_to_log_file(file_name, pkts):
-    f = open(os.path.join(BASE_DIR, file_name), "w")
-    f.write(pkts_to_log(pkts))
-    f.close()
+def write_pkts_to_log_file(file, pkts):
+    write_to_log_file(file, pkts_to_log(pkts), "w")
 
 def write_to_mqtt_log_file(txt):
-    f = open(os.path.join(BASE_DIR, CERT_MQTT_LOG_FILE), "a")
-    f.write(txt)
-    f.close()
-
-def load_custom_broker(gw, rel_path="."):
-    f = open(os.path.join(BASE_DIR, rel_path, "config", "eclipse.json"))
-    # f = open(os.path.join(BASE_DIR, rel_path, "config", "hivemq.json"))
-    # f = open(os.path.join(BASE_DIR, rel_path, "config", "mosquitto.json"))
-    # f = open(os.path.join(BASE_DIR, rel_path, "config", "wiliot-dev.json"))
+    write_to_log_file(CERT_MQTT_LOG_FILE, txt, "a")
+
+def load_custom_broker(broker_filepath, gw):
+    try:
+        f = open(str(broker_filepath).strip("'"))
+    except FileNotFoundError:
+        raise FileNotFoundError(f"No such file: {broker_filepath}")
     data = json.load(f)
     data[CUSTOM_BROKER_UPDATE_TOPIC] += gw
     data[CUSTOM_BROKER_STATUS_TOPIC] += gw
     data[CUSTOM_BROKER_DATA_TOPIC] += gw
     return data
 
-def mqttc_init(gw_id, userdata={PKTS: WltMqttPkts()}, rel_path=".", data=DATA_REAL_TAGS):
-
-    custom_broker = load_custom_broker(gw=gw_id, rel_path=rel_path)
+def mqttc_init(gw_id, custom_broker, data=DATA_REAL_TAGS):
+    # Ensure each MQTT client gets its own userdata for logging and getting pkts
+    userdata = {PKTS: WltMqttPkts()}  
+    custom_broker = load_custom_broker(broker_filepath=custom_broker, gw=gw_id)
     client_id = '{}-republish'.format(gw_id)
     userdata["data"] = data
     mqttc = mqtt.Client(mqtt.CallbackAPIVersion.VERSION1, client_id, userdata=userdata)
+    print(f"Connecting to MQTT broker at {custom_broker[CUSTOM_BROKER_BROKER_URL]}:{custom_broker[CUSTOM_BROKER_PORT]} with client ID: {client_id}")
     mqttc.username_pw_set(custom_broker[CUSTOM_BROKER_USERNAME], custom_broker[CUSTOM_BROKER_PASSWORD])
     mqttc.on_message = on_message
     mqttc.on_connect = on_connect
@@ -240,10 +290,13 @@ def mqttc_init(gw_id, userdata={PKTS: WltMqttPkts()}, rel_path=".", data=DATA_RE
 
     mqttc.update_topic = custom_broker[CUSTOM_BROKER_UPDATE_TOPIC]
     mqttc.subscribe(mqttc.update_topic)
+    print(f"Subscribed to update topic: {mqttc.update_topic}")
     mqttc.data_topic = custom_broker[CUSTOM_BROKER_DATA_TOPIC]
     mqttc.subscribe(mqttc.data_topic)
+    print(f"Subscribed to data topic: {mqttc.data_topic}")
     mqttc.status_topic = custom_broker[CUSTOM_BROKER_STATUS_TOPIC]
     mqttc.subscribe(mqttc.status_topic)
+    print(f"Subscribed to status topic: {mqttc.status_topic}")
 
     mqttc.flush_pkts = mqttc._userdata[PKTS].flush
     mqttc.flush_data_pkts = mqttc._userdata[PKTS].flush_data
@@ -252,33 +305,65 @@ def mqttc_init(gw_id, userdata={PKTS: WltMqttPkts()}, rel_path=".", data=DATA_RE
 
     return mqttc
 
-def dump_pkts(test, log):
-    write_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_all.json"), test.mqttc._userdata[PKTS].all)
-    write_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_data.json"), test.mqttc._userdata[PKTS].data)
-    write_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_status.json"), test.mqttc._userdata[PKTS].status)
-    write_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_update.json"), test.mqttc._userdata[PKTS].update)
+def dump_pkts(test, log, target=DUT):
+    mqttc = test.get_mqttc_by_target(target)
+    write_pkts_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_all.json"), mqttc._userdata[PKTS].all)
+    write_pkts_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_data.json"), mqttc._userdata[PKTS].data)
+    write_pkts_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_status.json"), mqttc._userdata[PKTS].status)
+    write_pkts_to_log_file(os.path.join(test.dir, MQTT_LOG_PRE_STR+log+"_update.json"), mqttc._userdata[PKTS].update)
+
+def generate_log_file(test, val, target=DUT):
+    if type(val) == int or type(val) == str:
+        log_string = str(val)
+    else:
+        log_string = str(val[0])
+        for i in range(1,len(val)):
+            log_string += ","+ str(val[i])
+    if test.internal_brg:
+        log_string += "_internal_brg"
+    dump_pkts(test, log_string, target)
+
+def mqtt_scan_n_create_log_file(test, duration, phase, target=DUT):
+    test.flush_all_mqtt_packets()
+    mqtt_scan_wait(test, duration=duration, target=target)
+    generate_log_file(test, phase, DUT)
+    generate_log_file(test, phase + "_tester", TESTER)
 
 # Get data/tags functions
-def get_all_data_pkts(mqttc):
+def get_all_data_pkts(mqttc, indicator=None):
     data_pkts = []
+    gw_id = mqttc.update_topic.split("/")[-1]
     for p in mqttc._userdata[PKTS].data:
-        gw_id = p.body_ex[GW_ID] if GW_ID in p.body_ex else ""
         if PACKETS in p.body_ex:
             for pkt in p.body_ex[PACKETS]:
+                if indicator and indicator not in pkt[PAYLOAD]: continue
                 pkt[GW_ID] = gw_id
                 data_pkts += [pkt]
     return data_pkts
 
-def get_all_sim_data_pkts(mqttc):
-    data_pkts = []
+def get_all_aggregated_data_pkts(mqttc, indicator =""):
+    aggregated_data_pkts = []
+    seen_aggregated_payloads = set()
+    gw_id = mqttc.update_topic.split("/")[-1]
     for p in mqttc._userdata[PKTS].data:
-        gw_id = p.body_ex[GW_ID] if GW_ID in p.body_ex else ""
         if PACKETS in p.body_ex:
             for pkt in p.body_ex[PACKETS]:
-                if PIXEL_SIM_INDICATOR in pkt[PAYLOAD]:
-                    pkt[GW_ID] = gw_id
-                    data_pkts += [pkt]
-    return data_pkts
+                if indicator and indicator not in pkt[PAYLOAD]: continue
+                if AGGREGATED_PAYLOAD in pkt:
+                    # Deduplicate based on AGGREGATED_PAYLOAD (since for each packet in the aggregated packet, we have another element with the same AGGREGATED_PAYLOAD)
+                    aggregated_payload = pkt[AGGREGATED_PAYLOAD]
+                    if aggregated_payload not in seen_aggregated_payloads:
+                        seen_aggregated_payloads.add(aggregated_payload)
+                        pkt[GW_ID] = gw_id
+                        aggregated_data_pkts += [pkt]
+    return aggregated_data_pkts
+
+def get_all_status_pkts(mqttc, status_type=None):
+    status_pkts = []
+    for p in mqttc._userdata[PKTS].status:
+        if status_type and status_type not in p.body: continue
+        status_pkts += [p.body]
+    return status_pkts
 
 def get_undecrypted_data_pkts_count(mqttc):
     undecrypted = 0
@@ -287,13 +372,30 @@ def get_undecrypted_data_pkts_count(mqttc):
             undecrypted += p.body_ex['undecrypted']
     return undecrypted
 
-def get_all_sensor_pkts(test, is_embedded=False):
-    all_sensor_pkts = couple_sensor_data_si_coupling(test)
-    all_sensor_pkts = [p for p in all_sensor_pkts if test.active_brg.id_str == p[BRIDGE_ID] and (p[IS_EMBEDDED] == is_embedded)]
+def get_embedded_sensor_pkts(sensor_pkts):
+    embedded_sensor_pkts = []
+    embedded_sensor_uuid_list = [ag.SENSOR_SERVICE_ID_BATTERY_SENSOR, ag.SENSOR_SERVICE_ID_LIS2DW12, ag.SENSOR_SERVICE_ID_POF_DATA]
+    for uuid in embedded_sensor_uuid_list:
+        for p in sensor_pkts:
+            _uuid = (f"{p[UNIFIED_SENSOR_PKT].pkt.sensor_ad_type:02X}"
+                     f"{p[UNIFIED_SENSOR_PKT].pkt.sensor_uuid_msb:02X}"
+                     f"{p[UNIFIED_SENSOR_PKT].pkt.sensor_uuid_lsb:02X}")
+            if UNIFIED_SENSOR_PKT in p and (f"{uuid:06X}" == f"{_uuid}"):
+                embedded_sensor_pkts += [p]
+    return embedded_sensor_pkts
+
+def get_all_sensor_pkts(test, is_unified=False, remove_embedded=False, target=DUT):
+    all_pkts = get_all_data_pkts(test.get_mqttc_by_target(target))
+    all_unified_sensor_pkts = [p for p in all_pkts if UNIFIED_SENSOR_PKT in p and test.active_brg.id_alias == p[ALIAS_BRIDGE_ID]
+                               and is_unified]
+    all_coupled_sensor_pkts = couple_sensor_data_si_coupling(test, all_pkts)
+    all_sensor_pkts = [p for p in all_coupled_sensor_pkts if test.active_brg.id_str == p[BRIDGE_ID]] + all_unified_sensor_pkts
+    if remove_embedded:
+        embedded_sensor_pkts = get_embedded_sensor_pkts(all_unified_sensor_pkts)
+        all_sensor_pkts = [p for p in all_sensor_pkts if p not in embedded_sensor_pkts]
     return all_sensor_pkts
 
-def couple_sensor_data_si_coupling(test):
-    all_pkts = get_all_data_pkts(test.mqttc)
+def couple_sensor_data_si_coupling(test, all_pkts):
     all_sensor_data_pkts = [p for p in all_pkts if SENSOR_PKT in p]
     all_sensor_side_info_pkts = [p for p in all_pkts if SIDE_INFO_SENSOR_PKT in p]
     # Couple data and side info
@@ -342,27 +444,29 @@ def count_pkt_id_duplications(test, all_sensor_data_pkts, all_sensor_side_info_p
             #TODO: logging print(debug)
             # test.reason = f'Warning: {coupled_pkt[PKT_ID_CTR]} sensor data and si with pkt id 0x{_pkt_id:08X}'
 
-def get_all_brg1_ext_sensor_pkts(test=None):
+def get_all_brg1_ext_sensor_pkts(test=None, is_unified=False, target=DUT):
+    original_active_brg = test.active_brg
     test.active_brg = test.brg1
-    pkts = get_all_sensor_pkts(test)
-    test.active_brg = test.brg0
+    pkts = get_all_sensor_pkts(test, is_unified, target=target)
+    test.active_brg = original_active_brg
     return pkts
 
 def get_all_custom_pkts(test=None):
-    return get_all_sensor_pkts(test, is_embedded=True)
+    return get_all_sensor_pkts(test, is_unified=True)
 
-def get_all_mgmt_pkts(mqttc):
-    all_data_pkts = get_all_data_pkts(mqttc)
+def get_all_mgmt_pkts(mqttc, indicator=""):
+    all_data_pkts = get_all_data_pkts(mqttc, indicator)
     return [p for p in all_data_pkts if MGMT_PKT in p]
 
-def get_brg2gw_mgmt_pkts(mqttc, test=None, mgmt_types=[]):
+def get_brg2gw_mgmt_pkts(mqttc, brg, mgmt_types=[]):
     brg2gw_mgmt_pkts = [p for p in get_all_mgmt_pkts(mqttc) if ((p[MGMT_PKT].hdr.group_id == ag.GROUP_ID_BRG2GW) and
                                                                 (not mgmt_types or type(p[MGMT_PKT].pkt) in mgmt_types))]
-    pkts = [p for p in brg2gw_mgmt_pkts if test.active_brg.id_str in p[PAYLOAD]]
+    pkts = [p for p in brg2gw_mgmt_pkts if brg.id_str in p[PAYLOAD]]
     return pkts
 
-def get_unified_data_pkts(test, only_active_brg=True):
-    all_unified_pkts, pkts = [], get_all_sim_data_pkts(test.mqttc) if test.data == DATA_SIMULATION else get_all_data_pkts(test.mqttc)
+def get_unified_data_pkts(test, only_active_brg=True, target=DUT, indicator=PIXEL_SIM_INDICATOR):
+    mqttc = test.get_mqttc_by_target(target)
+    all_unified_pkts, pkts = [], get_all_data_pkts(mqttc, indicator) if test.data == DATA_SIMULATION else get_all_data_pkts(mqttc)
     for p in pkts:
         if UNIFIED_PKT in p:
             all_unified_pkts += [p]
@@ -372,11 +476,12 @@ def get_unified_data_pkts(test, only_active_brg=True):
     print(f"\nCollected {len(pkts)} unified data pkts")
     return pkts
 
-def get_internal_brg_unified_data_pkts(test):
-    all_unified_pkts, pkts = [], get_all_sim_data_pkts(test.mqttc) if test.data == DATA_SIMULATION else get_all_data_pkts(test.mqttc)
+def get_internal_brg_unified_data_pkts(test, target=DUT):
+    mqttc = test.get_mqttc_by_target(target)
+    all_unified_pkts, pkts = [], get_all_data_pkts(mqttc, indicator=PIXEL_SIM_INDICATOR) if test.data == DATA_SIMULATION else get_all_data_pkts(mqttc)
     for p in pkts:
         if UNIFIED_PKT in p:
             all_unified_pkts += [p]
     pkts = [p for p in all_unified_pkts if p[ALIAS_BRIDGE_ID] == test.internal_id_alias()]
     print(f"\nCollected {len(pkts)} unified gw_tag_pkts")
-    return pkts
+    return pkts