From 60a4d1569f61d956f4acecb043bbc7eb351b94f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Fri, 24 Jan 2025 14:54:48 +0100
Subject: [PATCH] [ARP] Sniffing class EthernetSniffer and changes to support
it
Now the sniffing is done by the EthernetSniffing class
This only receives from the port that EthernetBase use as destination port
To allow this a new class with its own thread should be done.
Also, EthernetSniffing needed a shared Wiz pointer from the EthernetBase class
Lyra-gs-entry: Needed to be changed to the new interface which requires the sniffing boolean parameter
---
.../Common/Ports/EthernetBase.cpp | 45 ++++----
src/Groundstation/Common/Ports/EthernetBase.h | 15 +--
.../Common/Ports/EthernetSniffer.cpp | 101 ++++++++++++++++++
.../Common/Ports/EthernetSniffer.h | 71 ++++++++++++
src/Groundstation/LyraGS/Ports/Ethernet.cpp | 4 +-
src/Groundstation/LyraGS/lyra-gs-entry.cpp | 6 +-
6 files changed, 206 insertions(+), 36 deletions(-)
create mode 100644 src/Groundstation/Common/Ports/EthernetSniffer.cpp
create mode 100644 src/Groundstation/Common/Ports/EthernetSniffer.h
diff --git a/src/Groundstation/Common/Ports/EthernetBase.cpp b/src/Groundstation/Common/Ports/EthernetBase.cpp
index 52b2e982a..e66be196a 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/Groundstation/Common/Ports/EthernetBase.cpp
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Davide Mor
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -65,7 +65,7 @@ Boardcore::Wiz5500::PhyState EthernetBase::getState()
return wiz5500->getPhyState();
}
-bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
+bool EthernetBase::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
{
this->wiz5500 = std::move(wiz5500);
@@ -103,16 +103,10 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
return false;
}
- if (sniffOtherGs)
+ if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255}, RECV_PORT,
+ 500))
{
- TRACE("[info] starting second UDP socket (inverse direction)\n");
- if (!this->wiz5500->openUdp(1, SEND_PORT, {255, 255, 255, 255},
- RECV_PORT, 500))
-
- {
- TRACE("[error] starting second UDP socket\n");
- return false;
- }
+ return false;
}
auto mav_handler = [this](EthernetMavDriver* channel,
@@ -122,7 +116,17 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
if (!mav_driver->start())
return false;
+ TRACE("[info] mavlink driver started correctly\n");
+ // Create and start a second mavlink driver to sniff the ethernet port
+ if (sniffOtherGs)
+ {
+ getModule<EthernetSniffer>()->init(1, RECV_PORT, SEND_PORT);
+ if (!getModule<EthernetSniffer>()->start(wiz5500))
+ return false;
+ }
+
+ TRACE("[info] Ethernet sniffing started correctly\n");
return true;
}
@@ -134,26 +138,13 @@ void EthernetBase::handleMsg(const mavlink_message_t& msg)
ssize_t EthernetBase::receive(uint8_t* pkt, size_t max_len)
{
- ssize_t size = 0;
WizIp dst_ip;
uint16_t dst_port;
- if (!sniffOtherGs)
- size = wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
- else
- // In case of sniffing, there is a maximum waiting time for messages
- // from the groundstation software to switch between one and other port
- size = wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port,
- RECEIVE_PORT_TIMEOUT_MS);
- if (size <= 0 && sniffOtherGs)
- {
- size = wiz5500->recvfrom(1, pkt, max_len, dst_ip, dst_port,
- RECEIVE_PORT_TIMEOUT_MS);
- }
- return size;
+ return wiz5500->recvfrom(0, pkt, max_len, dst_ip, dst_port);
}
bool EthernetBase::send(uint8_t* pkt, size_t len)
{
- return wiz5500->send(0, pkt, len, 100);
+ return wiz5500->send(0, pkt, len, 1000);
// return true;
}
diff --git a/src/Groundstation/Common/Ports/EthernetBase.h b/src/Groundstation/Common/Ports/EthernetBase.h
index 82adc3c1a..57e6622e5 100644
--- a/src/Groundstation/Common/Ports/EthernetBase.h
+++ b/src/Groundstation/Common/Ports/EthernetBase.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2023 Skyward Experimental Rocketry
- * Author: Davide Mor
+/* Copyright (c) 2023-2024 Skyward Experimental Rocketry
+ * Authors: Davide Mor, Nicolò Caruso
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -24,6 +24,7 @@
#include <ActiveObject.h>
#include <Groundstation/Common/HubBase.h>
+#include <Groundstation/Common/Ports/EthernetSniffer.h>
#include <common/MavlinkLyra.h>
#include <drivers/WIZ5500/WIZ5500.h>
#include <radio/MavlinkDriver/MavlinkDriver.h>
@@ -43,8 +44,9 @@ Boardcore::WizMac genNewRandomMac();
using EthernetMavDriver =
Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
-class EthernetBase : public Boardcore::Transceiver,
- public Boardcore::InjectableWithDeps<HubBase>
+class EthernetBase
+ : public Boardcore::Transceiver,
+ public Boardcore::InjectableWithDeps<HubBase, EthernetSniffer>
{
public:
EthernetBase() {};
@@ -58,8 +60,8 @@ public:
Boardcore::Wiz5500::PhyState getState();
protected:
- bool start(std::unique_ptr<Boardcore::Wiz5500> wiz5500);
- std::unique_ptr<Boardcore::Wiz5500> wiz5500;
+ bool start(std::shared_ptr<Boardcore::Wiz5500> wiz5500);
+ std::shared_ptr<Boardcore::Wiz5500> wiz5500;
private:
/**
@@ -76,6 +78,7 @@ private:
bool randomIp = true;
uint8_t ipOffset = 0;
bool sniffOtherGs = false;
+ bool firstPort = true;
};
} // namespace Groundstation
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.cpp b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
new file mode 100644
index 000000000..8b6be2b7f
--- /dev/null
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.cpp
@@ -0,0 +1,101 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Nicolò Caruso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <Groundstation/Common/Config/EthernetConfig.h>
+#include <Groundstation/Common/HubBase.h>
+
+#include <random>
+
+#include "EthernetBase.h"
+
+using namespace Groundstation;
+using namespace Boardcore;
+using namespace miosix;
+
+void EthernetSniffer::handleINTn()
+{
+ if (wiz5500)
+ wiz5500->handleINTn();
+}
+
+bool EthernetSniffer::send(uint8_t* pkt, size_t len)
+{
+ // Send is not needed in sniffing, therefore not implemented
+ return false;
+};
+
+Boardcore::Wiz5500::PhyState EthernetSniffer::getState()
+{
+ return wiz5500->getPhyState();
+}
+
+void EthernetSniffer::init(uint16_t portNumber, uint16_t srcPort,
+ uint16_t dstPort)
+{
+ portNr = portNumber;
+ srcPort = srcPort;
+ dstPort = dstPort;
+}
+
+bool EthernetSniffer::start(std::shared_ptr<Boardcore::Wiz5500> wiz5500)
+{
+ TRACE("Movin\n");
+ this->wiz5500 = std::move(wiz5500);
+
+ TRACE("Opening\n");
+ // We open the port for sniffing using the port we specified
+ // if (!this->wiz5500->openUdp(portNr, srcPort, {255, 255, 255, 255},
+ // dstPort,
+ // 500))
+ // {
+ // return false;
+ // }
+
+ TRACE("Mavlinker\n");
+
+ auto mav_handler = [this](EthernetMavDriver* channel,
+ const mavlink_message_t& msg) { handleMsg(msg); };
+
+ mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 1, 10);
+
+ TRACE("Starting\n");
+
+ if (!mav_driver->start())
+ return false;
+
+ TRACE("Start ok\n");
+
+ return true;
+}
+
+void EthernetSniffer::handleMsg(const mavlink_message_t& msg)
+{
+ // Dispatch the message through the hub.
+ getModule<HubBase>()->dispatchOutgoingMsg(msg);
+}
+
+ssize_t EthernetSniffer::receive(uint8_t* pkt, size_t max_len)
+{
+ WizIp dst_ip;
+ uint16_t dst_port;
+ return wiz5500->recvfrom(portNr, pkt, max_len, dst_ip, dst_port);
+}
diff --git a/src/Groundstation/Common/Ports/EthernetSniffer.h b/src/Groundstation/Common/Ports/EthernetSniffer.h
new file mode 100644
index 000000000..7f4ea7a74
--- /dev/null
+++ b/src/Groundstation/Common/Ports/EthernetSniffer.h
@@ -0,0 +1,71 @@
+/* Copyright (c) 2025 Skyward Experimental Rocketry
+ * Author: Nicolò Caruso
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <ActiveObject.h>
+#include <Groundstation/Common/HubBase.h>
+#include <common/MavlinkLyra.h>
+#include <drivers/WIZ5500/WIZ5500.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <utils/DependencyManager/DependencyManager.h>
+
+#include <memory>
+
+namespace Groundstation
+{
+
+using EthernetMavDriver =
+ Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
+
+class EthernetSniffer : public Boardcore::Transceiver,
+ public Boardcore::InjectableWithDeps<HubBase>
+{
+public:
+ void handleINTn();
+
+ Boardcore::Wiz5500::PhyState getState();
+
+ bool start(std::shared_ptr<Boardcore::Wiz5500> wiz5500);
+
+ void init(uint16_t portNumber, uint16_t srcPort, uint16_t dstPort);
+
+private:
+ std::shared_ptr<Boardcore::Wiz5500> wiz5500;
+
+ /**
+ * @brief Called internally when a message is received.
+ */
+ void handleMsg(const mavlink_message_t& msg);
+
+ ssize_t receive(uint8_t* pkt, size_t max_len) override;
+
+ bool send(uint8_t* pkt, size_t len) override;
+
+ bool started = false;
+ std::unique_ptr<EthernetMavDriver> mav_driver;
+ uint16_t portNr;
+ uint16_t srcPort;
+ uint16_t dstPort;
+};
+
+} // namespace Groundstation
diff --git a/src/Groundstation/LyraGS/Ports/Ethernet.cpp b/src/Groundstation/LyraGS/Ports/Ethernet.cpp
index 6b3e3270f..6013c835a 100644
--- a/src/Groundstation/LyraGS/Ports/Ethernet.cpp
+++ b/src/Groundstation/LyraGS/Ports/Ethernet.cpp
@@ -45,7 +45,7 @@ namespace LyraGS
bool EthernetGS::start()
{
- std::unique_ptr<Wiz5500> wiz5500 = std::make_unique<Wiz5500>(
+ std::shared_ptr<Wiz5500> wiz5500 = std::make_shared<Wiz5500>(
getModule<Buses>()->ethernet_bus, miosix::ethernet::cs::getPin(),
miosix::ethernet::intr::getPin(), SPI::ClockDivider::DIV_64);
@@ -55,7 +55,7 @@ bool EthernetGS::start()
if (!present)
return false;
- if (!EthernetBase::start(std::move(wiz5500)))
+ if (!EthernetBase::start(wiz5500))
return false;
ethernetGSGlobal = this;
diff --git a/src/Groundstation/LyraGS/lyra-gs-entry.cpp b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
index 26ab6211c..29a0fdef2 100644
--- a/src/Groundstation/LyraGS/lyra-gs-entry.cpp
+++ b/src/Groundstation/LyraGS/lyra-gs-entry.cpp
@@ -93,6 +93,8 @@ void errorLoop()
}
}
+static bool constexpr ethernetSniffing = true;
+
/**
* @brief Lyra GS entrypoint.
* This entrypoint performs the following operations:
@@ -141,7 +143,8 @@ int main()
new LyraGS::RadioMain(dipRead.mainHasBackup, dipRead.mainTXenable);
LyraGS::BoardStatus* board_status = new LyraGS::BoardStatus(dipRead.isARP);
LyraGS::EthernetGS* ethernet =
- new LyraGS::EthernetGS(false, dipRead.ipConfig, true);
+ new LyraGS::EthernetGS(false, dipRead.ipConfig, ethernetSniffing);
+ EthernetSniffer* ethernetSniffer = new EthernetSniffer();
LyraGS::RadioPayload* radio_payload = new LyraGS::RadioPayload(
dipRead.payloadHasBackup, dipRead.payloadTXenable);
@@ -160,6 +163,7 @@ int main()
ok &= manager.insert(serial);
ok &= manager.insert<LyraGS::RadioMain>(radio_main);
ok &= manager.insert<LyraGS::EthernetGS>(ethernet);
+ ok &= manager.insert<EthernetSniffer>(ethernetSniffer);
ok &= manager.insert<LyraGS::RadioPayload>(radio_payload);
ok &= manager.insert(board_status);
--
GitLab