More work on multicast

14 files changed, 731 insertions, 26 deletions

diff --git a/src/CAN/CanInterface.cpp b/src/CAN/CanInterface.cpp
index f3d1de15..a5ad7a81 100644
--- a/src/CAN/CanInterface.cpp
+++ b/src/CAN/CanInterface.cpp
@@ -203,6 +203,50 @@ extern "C" [[noreturn]] void CanSenderLoop(void *) noexcept;
 extern "C" [[noreturn]] void CanClockLoop(void *) noexcept;
 extern "C" [[noreturn]] void CanReceiverLoop(void *) noexcept;
 
+// Status LED handling
+
+#if SUPPORT_MULTICAST_DISCOVERY
+
+// The STATUS LED is also used to identify one board among several visible to the user
+static volatile uint32_t identInitialClocks = 0;		// when we started identifying
+static volatile uint32_t identTotalClocks = 0;			// how many step clocks to identify for, zero means until cancelled
+static volatile bool identifying = false;
+
+void CanInterface::SetStatusLedIdentify(uint32_t seconds) noexcept
+{
+	identTotalClocks = seconds * StepClockRate;
+	identInitialClocks = StepTimer::GetTimerTicks();
+	identifying = true;
+}
+
+void CanInterface::SetStatusLedNormal() noexcept
+{
+	identifying = false;
+}
+
+#endif
+
+// This is called only from the CAN clock loop, so inline
+static inline void UpdateLed(uint32_t stepClocks) noexcept
+{
+	// Blink the LED at about 2Hz. Duet 3 expansion boards will blink in sync when they have established clock sync with us.
+	bool turnLedOn = (stepClocks & (1u << 19)) != 0;
+#if SUPPORT_MULTICAST_DISCOVERY
+	if (identifying)
+	{
+		if (identTotalClocks != 0 && stepClocks - identInitialClocks >= identTotalClocks)
+		{
+			identifying = 0;							// stop identifying
+		}
+		else if ((stepClocks & (1u << 17)) == 0)		// double flash instead of single
+		{
+			turnLedOn = false;
+		}
+	}
+#endif
+	digitalWrite(DiagPin, XNor(DiagOnPolarity, turnLedOn));
+}
+
 static void InitReceiveFilters() noexcept
 {
 	// Set up a filter to receive all request messages addressed to us in FIFO 0
@@ -570,8 +614,7 @@ extern "C" [[noreturn]] void CanClockLoop(void *) noexcept
 		SendCanMessage(TxBufferIndexTimeSync, 0, &buf);
 		++timeSyncMessagesSent;
 
-		// Blink the LED at about 2Hz. Duet 3 expansion boards will blink in sync when they have established clock sync with us.
-		digitalWrite(DiagPin, XNor(DiagOnPolarity, lastTimeSent & (1u << 19)) != 0);
+		UpdateLed(lastTimeSent);
 
 		// Delay until it is time again
 		vTaskDelayUntil(&lastWakeTime, CanClockIntervalMillis);
@@ -1490,15 +1533,14 @@ GCodeResult CanInterface::StartClosedLoopDataCollection(DriverId device, uint16_
 }
 
 #if DUAL_CAN
+
 CanId CanInterface::ODrive::ArbitrationId(DriverId const driver, uint8_t const cmd) noexcept {
 	const auto arbitration_id = (driver.boardAddress << 5) + cmd;
 	CanId canId;
 	canId.SetReceivedId(arbitration_id);
 	return canId;
 }
-#endif
 
-#if DUAL_CAN
 CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const driver, const StringRef& reply) noexcept
 {
 	// Detect any early return conditions
@@ -1523,17 +1565,12 @@ CanMessageBuffer * CanInterface::ODrive::PrepareSimpleMessage(DriverId const dri
 
 	return buf;
 }
-#endif
 
-#if DUAL_CAN
 void CanInterface::ODrive::FlushCanReceiveHardware() noexcept
 {
 	while (CanInterface::ReceivePlainMessage(nullptr, 0)) { }
 }
-#endif
 
-
-#if DUAL_CAN
 bool CanInterface::ODrive::GetExpectedSimpleMessage(CanMessageBuffer *buf, DriverId const driver, uint8_t const cmd, const StringRef& reply) noexcept
 {
 	CanId const expectedId = ArbitrationId(driver, cmd);
@@ -1554,7 +1591,7 @@ bool CanInterface::ODrive::GetExpectedSimpleMessage(CanMessageBuffer *buf, Drive
 
 	return ok;
 }
-#endif
+#endif	// DUAL_CAN
 
 #endif
 
diff --git a/src/CAN/CanInterface.h b/src/CAN/CanInterface.h
index ef322205..14b9faca 100644
--- a/src/CAN/CanInterface.h
+++ b/src/CAN/CanInterface.h
@@ -109,6 +109,11 @@ namespace CanInterface
 #endif
 	GCodeResult StartClosedLoopDataCollection(DriverId device, uint16_t filter, uint16_t numSamples, uint16_t rateRequested, uint8_t movementRequested, uint8_t mode, const GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeException);
 
+#if SUPPORT_MULTICAST_DISCOVERY
+	void SetStatusLedIdentify(uint32_t seconds) noexcept;
+	void SetStatusLedNormal() noexcept;
+#endif
+
 #if DUAL_CAN
 namespace ODrive {
 	CanId ArbitrationId(DriverId driver, uint8_t cmd) noexcept;
diff --git a/src/Networking/ESP8266WiFi/WiFiInterface.cpp b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
index bb7e0c29..ee284d91 100644
--- a/src/Networking/ESP8266WiFi/WiFiInterface.cpp
+++ b/src/Networking/ESP8266WiFi/WiFiInterface.cpp
@@ -1029,6 +1029,7 @@ void WiFiInterface::EspRequestsTransfer() noexcept
 void WiFiInterface::SetIPAddress(IPAddress p_ip, IPAddress p_netmask, IPAddress p_gateway) noexcept
 {
 	ipAddress = p_ip;
+	usingDhcp = ipAddress.IsNull();
 	netmask = p_netmask;
 	gateway = p_gateway;
 }
diff --git a/src/Networking/ESP8266WiFi/WiFiInterface.h b/src/Networking/ESP8266WiFi/WiFiInterface.h
index 29ba546d..18fac5bc 100644
--- a/src/Networking/ESP8266WiFi/WiFiInterface.h
+++ b/src/Networking/ESP8266WiFi/WiFiInterface.h
@@ -58,7 +58,11 @@ public:
 	bool IsWiFiInterface() const noexcept override { return true; }
 
 	void UpdateHostname(const char *hostname) noexcept override;
+
 	IPAddress GetIPAddress() const noexcept override { return ipAddress; }
+	IPAddress GetNetmask() const noexcept override { return netmask; }
+	IPAddress GetGateway() const noexcept override { return gateway; }
+	bool UsingDhcp() const noexcept override { return usingDhcp; }
 	void SetIPAddress(IPAddress p_ip, IPAddress p_netmask, IPAddress p_gateway) noexcept override;
 	GCodeResult SetMacAddress(const MacAddress& mac, const StringRef& reply) noexcept override;
 	const MacAddress& GetMacAddress() const noexcept override { return macAddress; }
@@ -138,7 +142,7 @@ private:
 	WiFiSocket *sockets[NumWiFiTcpSockets];
 	size_t currentSocket;
 
-	TcpPort portNumbers[NumProtocols];					// port number used for each protocol
+	TcpPort portNumbers[NumProtocols];				// port number used for each protocol
 	TcpPort ftpDataPort;
 	bool closeDataPort;
 	bool protocolEnabled[NumProtocols];				// whether each protocol is enabled
@@ -164,6 +168,8 @@ private:
 
 	char wiFiServerVersion[16];
 
+	bool usingDhcp = true;
+
 	// For processing debug messages from the WiFi module
 	bool serialRunning;
 	bool debugPrintPending;
diff --git a/src/Networking/LwipEthernet/LwipEthernetInterface.cpp b/src/Networking/LwipEthernet/LwipEthernetInterface.cpp
index 69049406..443a09f7 100644
--- a/src/Networking/LwipEthernet/LwipEthernetInterface.cpp
+++ b/src/Networking/LwipEthernet/LwipEthernetInterface.cpp
@@ -575,11 +575,6 @@ bool LwipEthernetInterface::ConnectionEstablished(tcp_pcb *pcb) noexcept
 	return false;
 }
 
-IPAddress LwipEthernetInterface::GetIPAddress() const noexcept
-{
-	return ipAddress;
-}
-
 void LwipEthernetInterface::SetIPAddress(IPAddress p_ipAddress, IPAddress p_netmask, IPAddress p_gateway) noexcept
 {
 	if (GetState() == NetworkState::obtainingIP || GetState() == NetworkState::active)
diff --git a/src/Networking/LwipEthernet/LwipEthernetInterface.h b/src/Networking/LwipEthernet/LwipEthernetInterface.h
index 12c2bc08..e1a34da5 100644
--- a/src/Networking/LwipEthernet/LwipEthernetInterface.h
+++ b/src/Networking/LwipEthernet/LwipEthernetInterface.h
@@ -48,8 +48,13 @@ public:
 	bool IsWiFiInterface() const noexcept override { return false; }
 
 	void UpdateHostname(const char *hostname) noexcept override;
-	IPAddress GetIPAddress() const noexcept override;
+
+	IPAddress GetIPAddress() const noexcept override { return ipAddress; }
+	IPAddress GetNetmask() const noexcept override { return netmask; }
+	IPAddress GetGateway() const noexcept override { return gateway; }
+	bool UsingDhcp() const noexcept override { return usingDhcp; }
 	void SetIPAddress(IPAddress p_ipAddress, IPAddress p_netmask, IPAddress p_gateway) noexcept override;
+
 	GCodeResult SetMacAddress(const MacAddress& mac, const StringRef& reply) noexcept override;
 	const MacAddress& GetMacAddress() const noexcept override { return macAddress; }
 
@@ -91,7 +96,7 @@ private:
 
 	bool activated;
 	bool initialised;
-	bool usingDhcp;
+	bool usingDhcp = true;
 
 	IPAddress ipAddress;
 	IPAddress netmask;
diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.cpp b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
index 16ac0ba3..7cd1d66b 100644
--- a/src/Networking/MulticastDiscovery/MulticastResponder.cpp
+++ b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
@@ -12,6 +12,7 @@
 #include <Platform/RepRap.h>
 #include <Platform/Platform.h>
 #include "fgmc_header.h"
+#include "fgmc_protocol.h"
 
 extern "C" {
 #include "LwipEthernet/Lwip/src/include/lwip/udp.h"
@@ -24,6 +25,7 @@ static pbuf * volatile receivedPbuf = nullptr;
 static volatile uint32_t receivedIpAddr;
 static volatile uint16_t receivedPort;
 static unsigned int messagesProcessed = 0;
+static FGMCProtocol *fgmcHandler = nullptr;
 
 static bool active = false;
 
@@ -62,8 +64,9 @@ void MulticastResponder::Spin() noexcept
 			debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]);
 		}
 		debugPrintf("\n");
-		pbuf_free(rxPbuf);
 		receivedPbuf = nullptr;
+		fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len);
+		pbuf_free(rxPbuf);
 		++messagesProcessed;
 	}
 }
@@ -75,6 +78,11 @@ void MulticastResponder::Diagnostics(MessageType mtype) noexcept
 
 void MulticastResponder::Start(TcpPort port) noexcept
 {
+	if (fgmcHandler == nullptr)
+	{
+		fgmcHandler = new FGMCProtocol;
+	}
+
 	if (ourPcb == nullptr)
 	{
 		ourPcb = udp_new_ip_type(IPADDR_TYPE_ANY);
diff --git a/src/Networking/MulticastDiscovery/fgmc_header.h b/src/Networking/MulticastDiscovery/fgmc_header.h
index beba63c7..2c708940 100644
--- a/src/Networking/MulticastDiscovery/fgmc_header.h
+++ b/src/Networking/MulticastDiscovery/fgmc_header.h
@@ -3,10 +3,8 @@
 #ifndef FGMC_HEADER_H
 #define FGMC_HEADER_H
 
-namespace engp {
-
 /// fgmc device id for platform family cmmt
-enum class FGMCHwTypeId : uint32_t { FGMC_DEVICE_ID_ZERO = 0, FGMC_DEVICE_ID_CMMT_AS = 23, FGMC_DEVICE_ID_CMMT_ST = 33 };
+enum class FGMCHwTypeId : uint32_t { FGMC_DEVICE_ID_ZERO = 0, FGMC_DEVICE_ID_CMMT_AS = 23, FGMC_DEVICE_ID_CMMT_ST = 33, FGMC_DEVICE_ID_DUET3 = 41 };
 
 /// fgmc error codes
 enum class FGMCErrorCode : int16_t
@@ -168,6 +166,4 @@ struct __attribute__((packed)) FGMC_ResGetSupportedCommands
 	uint16_t cmd_version_;
 };
 
-}  // namespace engp
-
 #endif // #ifndef FGMC_HEADER_H
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
new file mode 100644
index 00000000..832053bc
--- /dev/null
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
@@ -0,0 +1,475 @@
+// Implementation of MulticastDiscovery protocol
+
+#include "fgmc_protocol.h"
+
+#if SUPPORT_MULTICAST_DISCOVERY
+
+#include <Platform/RepRap.h>
+#include <Networking/Network.h>
+#include <CAN/CanInterface.h>
+#include <Version.h>
+#include <cstring>
+
+FGMCProtocol::FGMCProtocol() noexcept
+    : iface_id_(0),
+      eeprom_product_key_{0},
+      unique_id_{0},
+      eeprom_noc_code_{0},
+      fgmc_device_id_(FGMCHwTypeId::FGMC_DEVICE_ID_ZERO),
+      fgmc_application_type_(0),
+      fgmc_device_type_{0},
+      tx_netbuf_{0},
+#if 0
+      app_info_(nullptr),
+      led_manager_(nullptr),
+      engp_router_{nullptr, nullptr},
+      parameter_management_(nullptr),
+#endif
+      packetIdBuffer{0},
+      packetIdIndex{0}
+{
+}
+
+void FGMCProtocol::macToString(uint32_t interface) noexcept
+{
+  for (uint32_t i = 0; i < SIZE_FGMC_DEST_ID; i++)
+  {
+    unique_id_[interface][i] = '\0';
+  }
+
+  const MacAddress& macAddress = reprap.GetNetwork().GetMacAddress(interface);
+  for (uint32_t i = 0; i < 6; i++)
+  {
+    const uint8_t byte = macAddress.bytes[i];
+    uint8_t lowValue = (byte & 0x0Fu);
+    if (lowValue < 0xA)
+    {
+      lowValue = lowValue + 48;
+    }
+    else
+    {
+      lowValue = lowValue + 55;
+    }
+
+    uint8_t highValue = (byte >> 4u) & 0x0Fu;
+    if (highValue < 0xA)
+    {
+      highValue = highValue + 48;
+    }
+    else
+    {
+      highValue = highValue + 55;
+    }
+    unique_id_[interface][i * 2] = static_cast<char>(lowValue);
+    unique_id_[interface][(i * 2) + 1] = static_cast<char>(highValue);
+  }
+}
+
+void FGMCProtocol::init() noexcept
+{
+  char deviceName[SIZE_DEVICE_NAME] = {0};
+
+#if 0
+  // Read eeprom data
+  ret = deviceInfo.GetFestoNocCode(&eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
+  if (ret != base::ReturnCode::kRcSuccess) {
+    retTotal = ret;
+  }
+  ret = deviceInfo.GetFestoProductKey(&eeprom_product_key_[0], SIZE_EEPROM_PRODUCT_KEY);
+  if (ret != base::ReturnCode::kRcSuccess) {
+    retTotal = ret;
+  }
+  fgmc_application_type_ = deviceInfo.GetFestoPartNumber();
+  (void)strncpy(&deviceName[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
+
+  // check eeprom values available
+  if ((eeprom_noc_code_[0] == '\0')) {  // || (eeprom_product_key_[0] == '\0')
+    // EEPROMs not described
+    eeprom_product_key_[0] = '1';
+    eeprom_product_key_[1] = '\0';
+    (void)memcpy(&eeprom_noc_code_[0], &INVALID_EEPROM_DATA_STRING, INVALID_EEPROM_DATA_STRING_LEN);
+    (void)memcpy(&deviceName[0], &INVALID_EEPROM_DATA_STRING, INVALID_EEPROM_DATA_STRING_LEN);
+  }
+
+  for (uint32_t i = 0; i < IP_MAX_IFACES; i++) {
+    macToString(i);
+  }
+#endif
+
+  SafeStrncpy(deviceName, BOARD_SHORT_NAME, ARRAY_SIZE(deviceName));
+  fgmc_device_id_ = FGMCHwTypeId::FGMC_DEVICE_ID_DUET3;
+}
+
+void FGMCProtocol::handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept
+{
+  // backup sciopta connection handle
+  // if a null pointer received => plattform should execute a exception
+  this->iface_id_ = iFaceId;
+
+  if (rxLength >= static_cast<uint32_t>(sizeof(FGMC_GenericHeader))) {
+    FGMC_GenericHeader* pInGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(inputBufferAddress);
+
+    // read incoming packetid
+    uint32_t packetId = pInGenericHeader->fgmc_packet_id_;
+
+    if (isPacketInBuffer(packetId)) {
+      return;  // TODO: Filter out iFace 1, if iFace 0 is active (PACKET-ID) ?
+    }
+    insertPacketId(packetId);
+
+    switch (pInGenericHeader->fgmc_command_) {
+      case FGMCCommand::MCD_COMMAND_UNETINF: {
+        if (pInGenericHeader->fgmc_destination_id_[0] == '\0') {
+          // fgmc command: upload network information
+          cmdUnetinf(packetId);
+        }
+        break;
+      }
+#if 0	// not supported for now
+      case FGMCCommand::MCD_COMMAND_DNETINF: {
+        if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) {
+          // fgmc command: download network information
+          FGMC_ReqDownloadNetInfoHeader* pInCmdPointer = reinterpret_cast<FGMC_ReqDownloadNetInfoHeader*>(reinterpret_cast<uint32_t>(inputBufferAddress) +
+                                                                                                          static_cast<uint32_t>(sizeof(FGMC_GenericHeader)));
+          cmdDnetinf(pInCmdPointer, packetId);
+        }
+        break;
+      }
+#endif
+      case FGMCCommand::MCD_COMMAND_REBOOT: {
+        if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) {
+          // fgmc command: reboot
+          cmdReboot(packetId);
+        }
+        break;
+      }
+      case FGMCCommand::MCD_COMMAND_IDENTIFY: {
+        if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) {
+          // fgmc command: identify
+          FGMC_ReqIdentify* pInCmdPointer =
+              reinterpret_cast<FGMC_ReqIdentify*>(reinterpret_cast<uint32_t>(inputBufferAddress) + static_cast<uint32_t>(sizeof(FGMC_GenericHeader)));
+          cmdIdentify(pInCmdPointer, packetId);
+        }
+        break;
+      }
+      case FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION: {
+        if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) {
+          // fgmc command: get firmware version
+          cmdGetFirmwareVersion(packetId);
+        }
+        break;
+      }
+      case FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS: {
+        if (strncmp(pInGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID) == 0) {
+          // fgmc command: get supported commands
+          cmdGetSupportedCommands(packetId);
+        }
+        break;
+      }
+      default: {
+        // unknown command will be skipped
+        break;
+      }
+    }
+  }
+}
+
+void FGMCProtocol::sendGenericHeader(char* tx_netbuf, FGMCCommand cmd, uint32_t length, uint32_t packetId, uint32_t segmentIndex, uint32_t segmentCount) noexcept
+{
+  FGMC_GenericHeader* pOutGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(tx_netbuf);
+
+  //-----------------------------------------------------------------------------------
+  // Generic Multicast Header
+  //-----------------------------------------------------------------------------------
+  (void)memcpy(&pOutGenericHeader->fgmc_name_[0], &FGMC_NAME, SIZE_FGMC_NAME);
+  pOutGenericHeader->fgmc_length_ = length;
+  pOutGenericHeader->fgmc_hw_type_id_ = fgmc_device_id_;
+  // device unique fgmc destination id
+  (void)strncpy(&pOutGenericHeader->fgmc_destination_id_[0], unique_id_[iface_id_], SIZE_FGMC_DEST_ID);
+  pOutGenericHeader->fgmc_packet_id_ = packetId + 1;
+  pOutGenericHeader->fgmc_segment_index_ = segmentIndex;
+  pOutGenericHeader->fgmc_segment_count_ = segmentCount;
+  pOutGenericHeader->fgmc_command_ = cmd;
+  pOutGenericHeader->fgmc_command_version_ = FGMCCommandVersion::MCD_COMMAND_VERSION;
+  pOutGenericHeader->fgmc_error_code_ = FGMCErrorCode::FGMC_ERROR_CODE_NORMAL;
+
+  // Send Message
+  insertPacketId(pOutGenericHeader->fgmc_packet_id_);
+
+#if 1
+  //TODO
+#else
+  IP_PACKET* pPacket = nullptr;
+  pPacket = ::IP_UDP_AllocEx(iface_id_, static_cast<unsigned>(length));
+  if (pPacket != nullptr) {
+    (void)memcpy(pPacket->pData, tx_netbuf, static_cast<unsigned>(length));
+    ::IP_UDP_SendAndFree(iface_id_, htonl(FGMC_IP), FGMC_PORT, FGMC_PORT, pPacket);
+  }
+#endif
+}
+
+void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
+{
+  FGMC_ResUploadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResUploadNetInfoHeader*>(tx_netbuf_);
+  (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResUploadNetInfoHeader));
+
+  //-----------------------------------------------------------------------------------
+  // The Upload Netinformation Structure
+  //-----------------------------------------------------------------------------------
+
+  // ip address type
+  const bool dhcpEnable = reprap.GetNetwork().UsingDhcp(iface_id_);
+  if (dhcpEnable)
+  {
+    // dynamic
+    pOutCmdHeader->fgmc_ip_address_type_ = 1;
+  }
+  else
+  {
+    // static
+    pOutCmdHeader->fgmc_ip_address_type_ = 0;
+  }
+
+  // mac address
+  const MacAddress& macAddress = reprap.GetNetwork().GetMacAddress(iface_id_);
+  memcpy(pOutCmdHeader->fgmc_mac_address_, macAddress.bytes, 6);
+
+  // IPv4
+  const uint32_t ipaddress = reprap.GetNetwork().GetIPAddress(iface_id_).GetV4BigEndian();
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_address_[0], &ipaddress, SIZE_IP_V4);
+
+  const uint32_t subnetmask = reprap.GetNetwork().GetNetmask(iface_id_).GetV4BigEndian();
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_netmask_[0], &subnetmask, SIZE_IP_V4);
+
+  const uint32_t gateway = reprap.GetNetwork().GetGateway(iface_id_).GetV4BigEndian();
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_gateway_[0], &gateway, SIZE_IP_V4);
+
+  pOutCmdHeader->fgmc_ip_v4_static_dns_[0] = 0;
+  pOutCmdHeader->fgmc_ip_v4_static_dns_[1] = 0;
+  pOutCmdHeader->fgmc_ip_v4_static_dns_[2] = 0;
+  pOutCmdHeader->fgmc_ip_v4_static_dns_[3] = 0;
+
+  // IPv6
+  (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_6_addr_[0], 0x00, SIZE_IP_V6);
+  (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_netmask_[0], 0x00, SIZE_IP_V6);
+  (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_gateway_[0], 0x00, SIZE_IP_V6);
+  (void)memset(&pOutCmdHeader->fgmc_ip_v6_static_dns_[0], 0x00, SIZE_IP_V6);
+
+  // IPv4 active
+  const uint32_t ipaddressActive = ipaddress;
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_address_[0], &ipaddressActive, SIZE_IP_V4);
+
+  const uint32_t subnetmaskActive = subnetmask;
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_netmask_[0], &subnetmaskActive, SIZE_IP_V4);
+
+  const uint32_t gatewayActive = gateway;
+  (void)memcpy(&pOutCmdHeader->fgmc_ip_v4_gateway_[0], &gatewayActive, SIZE_IP_V4);
+
+  pOutCmdHeader->fgmc_ip_v4_dns_[0] = 0;
+  pOutCmdHeader->fgmc_ip_v4_dns_[1] = 0;
+  pOutCmdHeader->fgmc_ip_v4_dns_[2] = 0;
+  pOutCmdHeader->fgmc_ip_v4_dns_[3] = 0;
+
+  // NOC-CODE
+  (void)strncpy(&pOutCmdHeader->fgmc_noc_code_[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
+
+  // iTTL
+  pOutCmdHeader->fgmc_ttl_ = 255;
+
+  // Connection State
+  pOutCmdHeader->fgmc_connection_state_ = 0;
+
+  // Device Type
+  (void)strncpy(&pOutCmdHeader->fgmc_device_type_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE);
+
+  // Device Serial Number
+  (void)strncpy(&pOutCmdHeader->fgmc_serial_number_[0], &eeprom_product_key_[0], SIZE_SERIAL_NUMBER);
+
+  // Application Type
+  pOutCmdHeader->fgmc_application_type_ = fgmc_application_type_;
+
+  // Application Version
+  pOutCmdHeader->fgmc_application_version_ = 0;
+
+  // Application Version Revision
+  pOutCmdHeader->fgmc_application_version_revision_ = 0;
+
+  // Generic Info
+  (void)memset(&pOutCmdHeader->fgmc_generic_info_[0], 0x00, SIZE_GENERIC_INFO);
+
+  // Device Name
+  SafeStrncpy(pOutCmdHeader->fgmc_device_name_, BOARD_SHORT_NAME, ARRAY_SIZE(pOutCmdHeader->fgmc_device_name_));
+
+  //-----------------------------------------------------------------------------------
+  // Generic Multicast Header
+  //-----------------------------------------------------------------------------------
+  sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_UNETINF, sizeof(FGMC_ResUploadNetInfoHeader), inPacketId, 0, 1);
+}
+
+#if 0	// not supported for now
+void FGMCProtocol::cmdDnetinf(FGMC_ReqDownloadNetInfoHeader* pInCmdHeader, uint32_t inPacketId) {
+  FGMC_ResDownloadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResDownloadNetInfoHeader*>(tx_netbuf_);
+  (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResDownloadNetInfoHeader));
+
+  //-----------------------------------------------------------------------------------
+  // The Download Netinformation Structure
+  //-----------------------------------------------------------------------------------
+
+  bool dhcpEnable = false;
+  uint32_t ipaddress = 0;
+  uint32_t subnetmask = 0;
+  uint32_t gateway = 0;
+
+  // set new network settings
+  dhcpEnable = static_cast<bool>(pInCmdHeader->fgmc_ip_address_type_);
+  engp_router_[iface_id_]->SetDhcpenable(dhcpEnable);
+  (void)memcpy(&ipaddress, &pInCmdHeader->fgmc_ip_v4_static_address_[0], SIZE_IP_V4);
+  engp_router_[iface_id_]->SetIpaddress(ntohl(ipaddress));
+  (void)memcpy(&subnetmask, &pInCmdHeader->fgmc_ip_v4_static_netmask_[0], SIZE_IP_V4);
+  engp_router_[iface_id_]->SetSubnetmask(ntohl(subnetmask));
+  (void)memcpy(&gateway, &pInCmdHeader->fgmc_ip_v4_static_gateway_[0], SIZE_IP_V4);
+  engp_router_[iface_id_]->SetGateway(ntohl(gateway));
+
+  // set new device name
+  // filter out " (X19)  / (X18)
+  for (uint32_t i = 0; i < SIZE_DEVICE_NAME; i++) {
+    if (pInCmdHeader->fgmc_device_name_[i] == '\0') {
+      if (i > 6) {
+        if ((pInCmdHeader->fgmc_device_name_[i - 6] == ' ') && (pInCmdHeader->fgmc_device_name_[i - 5] == '(') &&
+            (pInCmdHeader->fgmc_device_name_[i - 4] == 'X') && (pInCmdHeader->fgmc_device_name_[i - 3] == '1') &&
+            (pInCmdHeader->fgmc_device_name_[i - 1] == ')')) {
+          for (uint32_t j = 1; j <= 6; j++) {
+            pInCmdHeader->fgmc_device_name_[i - j] = '\0';
+          }
+        }
+      }
+      break;
+    }
+  }
+  app_info_->setDeviceName(&pInCmdHeader->fgmc_device_name_[0], SIZE_DEVICE_NAME);
+
+  //-----------------------------------------------------------------------------------
+  // Generic Multicast Header
+  //-----------------------------------------------------------------------------------
+  sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_DNETINF, sizeof(FGMC_ResDownloadNetInfoHeader), inPacketId, 0, 1);
+}
+#endif
+
+void FGMCProtocol::cmdReboot(uint32_t inPacketId) noexcept
+{
+	(void)memset(tx_netbuf_, 0x00, sizeof(FGMC_GenericHeader));
+
+	//-----------------------------------------------------------------------------------
+	// Generic Multicast Header
+	//-----------------------------------------------------------------------------------
+	sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_REBOOT, sizeof(FGMC_GenericHeader), inPacketId, 0, 1);
+
+	//TODO schedule the reboot
+}
+
+void FGMCProtocol::cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacketId) noexcept
+{
+	FGMC_ResIdentify* pOutCmdHeader = reinterpret_cast<FGMC_ResIdentify*>(tx_netbuf_);
+	(void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResIdentify));
+
+	switch (pInCmdHeader->identification_type_)
+	{
+	case IDENTIFY_DEFAULT:
+		CanInterface::SetStatusLedIdentify(30);						// identify for about 30 seconds
+		break;
+
+	case IDENTIFY_ON:
+		CanInterface::SetStatusLedIdentify(0);						// identify until cancelled
+		break;
+
+	case IDENTIFY_OFF:
+	default:
+		CanInterface::SetStatusLedNormal();							// cancel identify
+		break;
+	}
+
+	//-----------------------------------------------------------------------------------
+	// Generic Multicast Header
+	//-----------------------------------------------------------------------------------
+	sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_IDENTIFY, sizeof(FGMC_ResIdentify), inPacketId, 0, 1);
+}
+
+void FGMCProtocol::cmdGetFirmwareVersion(uint32_t inPacketId) noexcept
+{
+  FGMC_ResGetFwVersion* pOutCmdHeader = reinterpret_cast<FGMC_ResGetFwVersion*>(tx_netbuf_);
+  (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetFwVersion));
+
+  //-----------------------------------------------------------------------------------
+  // Get Firmware Version
+  //-----------------------------------------------------------------------------------
+  (void)strncpy(&pOutCmdHeader->module_name_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE);
+
+  SafeSnprintf(pOutCmdHeader->module_version_, SIZE_MODULE_VERSION, "%s version %s", FIRMWARE_NAME, VERSION);
+
+  //-----------------------------------------------------------------------------------
+  // Generic Multicast Header
+  //-----------------------------------------------------------------------------------
+  sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION, sizeof(FGMC_ResGetFwVersion), inPacketId, 0, 1);
+}
+
+void FGMCProtocol::cmdGetSupportedCommands(uint32_t inPacketId) noexcept
+{
+  uint32_t segmentIndex = 0;
+  uint32_t segmentCount = 0;
+
+  FGMCCommand supportedCommands[] =
+  {
+	  FGMCCommand::MCD_COMMAND_UNETINF,
+#if 0	// not supported for now
+	  FGMCCommand::MCD_COMMAND_DNETINF,
+#endif
+	  FGMCCommand::MCD_COMMAND_IDENTIFY,
+	  FGMCCommand::MCD_COMMAND_GET_FIRMWARE_VERSION,
+	  FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS
+  };
+
+  for (uint8_t i = 0; i < (sizeof(supportedCommands) / sizeof(FGMCCommand)); i++) {
+    FGMC_ResGetSupportedCommands* pOutCmdHeader = reinterpret_cast<FGMC_ResGetSupportedCommands*>(tx_netbuf_);
+    (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetSupportedCommands));
+
+    //-----------------------------------------------------------------------------------
+    // Get Supported Commands
+    //-----------------------------------------------------------------------------------
+    pOutCmdHeader->cmd_ = static_cast<uint32_t>(supportedCommands[i]);
+    pOutCmdHeader->cmd_version_ = 0;
+
+    //-----------------------------------------------------------------------------------
+    // Generic Multicast Header
+    //-----------------------------------------------------------------------------------
+    segmentIndex = static_cast<uint32_t>(i);
+    segmentCount = static_cast<uint32_t>(sizeof(supportedCommands)) / sizeof(FGMCCommand);
+    sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_GET_SUPPORTED_COMMANDS, sizeof(FGMC_ResGetSupportedCommands), inPacketId, segmentIndex,
+                      segmentCount);
+  }
+}
+
+bool FGMCProtocol::isPacketInBuffer(uint32_t packetId) noexcept
+{
+  for (uint32_t i = 0; i < ringBufferSize; i++) {
+    if (packetId == packetIdBuffer[iface_id_][i]) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void FGMCProtocol::insertPacketId(uint32_t packetId) noexcept
+{
+  if (isPacketInBuffer(packetId)) {
+    return;
+  }
+  packetIdBuffer[iface_id_][packetIdIndex[iface_id_]] = packetId;
+  packetIdIndex[iface_id_]++;
+  if (packetIdIndex[iface_id_] >= ringBufferSize) {
+    packetIdIndex[iface_id_] = 0;
+  }
+}
+
+#endif
+
+// End
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.h b/src/Networking/MulticastDiscovery/fgmc_protocol.h
new file mode 100644
index 00000000..bc8719d5
--- /dev/null
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.h
@@ -0,0 +1,140 @@
+// FGMC protocol handler
+
+#ifndef FGMC_PROTOCOL_H
+#define FGMC_PROTOCOL_H
+
+#include <RepRapFirmware.h>
+
+#if SUPPORT_MULTICAST_DISCOVERY
+
+#include "fgmc_header.h"
+
+/// array size definitions
+#define SIZE_EEPROM_MAC_ADDRESS 6
+#define SIZE_EEPROM_PRODUCT_KEY 15
+#define SIZE_EEPROM_SERIAL_NUMBER 5
+#define SIZE_EEPROM_TEST_NUMBER 9
+#define SIZE_EEPROM_NOC_CODE 50
+
+/// fgmc port
+//#define FGMC_PORT 10002
+//#define FGMC_IP IP_BYTES2ADDR(239, 255, 2, 3)  // Broadcast-IP
+
+constexpr size_t IP_MAX_IFACES = 1;
+
+class FGMCProtocol
+{
+public:
+	/// constructor
+	FGMCProtocol() noexcept;
+
+#if 0
+	/// registers engp router component
+	/// \param router reference to engp router component
+	void registerEngpRouter(com::protocols::engp::ENGPRouter& router0, com::protocols::engp::ENGPRouter& router1)
+	{
+		this->engp_router_[router0.GetIFaceId()] = &router0;
+		this->engp_router_[router0.GetVirtIFaceId()] = &router0;
+		this->engp_router_[router1.GetIFaceId()] = &router1;
+		this->engp_router_[router1.GetVirtIFaceId()] = &router1;
+	}
+
+	/// registers interface self identification
+	/// \param ledManager reference to led manager component
+	void registerLedManager(hal::ISelfIdentification& ledManager) { this->led_manager_ = &ledManager; }
+
+	/// registers application information component
+	/// \param appInfo reference to led manager component
+	void registerApplikationInformation(bmc::services::ApplicationInformation& appInfo) { this->app_info_ = &appInfo; }
+
+	/// registers parameter management
+	/// \param parameterManagement reference to led parameter management component
+	void registerParameterManagement(interfaces::IParameterManagement& parameterManagement) { this->parameter_management_ = &parameterManagement; }
+#endif
+
+	/// this function initializes fgmc protocoll class
+	void init() noexcept;
+
+	/// this handles a fgmc frame
+	/// \param nConn connection
+	/// \param inputBufferAddress fgmc request frame
+	/// \param rxLength receive frame length
+	void handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept;
+
+	/// this functions sends fgmc frame
+	/// \param pOutPointer sciopta network buffer
+	/// \param cmd cmd
+	/// \param length length
+	/// \param packetId packedId
+	/// \param segmentIndex segmentIndex
+	/// \param segmentCount segmentCount
+	void sendGenericHeader(char* tx_netbuf, FGMCCommand cmd, uint32_t length, uint32_t packetId, uint32_t segmentIndex, uint32_t segmentCount) noexcept;
+
+	/// fgmc command "upload network informations"
+	/// \param inPacketId packedId
+	void cmdUnetinf(uint32_t inPacketId) noexcept;
+
+	/// fgmc command "download network information"
+	/// \param pInCmdHeader request header network information header
+	/// \param inPacketId packedId
+	void cmdDnetinf(FGMC_ReqDownloadNetInfoHeader* pInCmdHeader, uint32_t inPacketId) noexcept;
+
+	/// fgmc command "reboot"
+	/// \param inPacketId packedId
+	void cmdReboot(uint32_t inPacketId) noexcept;
+
+	/// fgmc command "identify"
+	/// \param pInCmdHeader request header identify
+	/// \param inPacketId packedId
+	void cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacketId) noexcept;
+
+	/// fgmc command "get firmware version"
+	/// \param inPacketId packedId
+	void cmdGetFirmwareVersion(uint32_t inPacketId) noexcept;
+
+	/// fgmc command "get supported commands"
+	/// \param inPacketId packedId
+	void cmdGetSupportedCommands(uint32_t inPacketId) noexcept;
+
+	/// insert used packet-id
+	/// \param packetId packedId
+	void insertPacketId(uint32_t packetId) noexcept;
+
+	/// check if packet id is already used
+	/// \param packetId packedId
+	bool isPacketInBuffer(uint32_t packetId) noexcept;
+
+	private:
+	void macToString(uint32_t interface) noexcept;
+
+	// connection data pointer
+	unsigned int iface_id_;
+
+	// product key from eeprom
+	char eeprom_product_key_[SIZE_EEPROM_PRODUCT_KEY];
+	char unique_id_[IP_MAX_IFACES][SIZE_FGMC_DEST_ID];
+	char eeprom_noc_code_[SIZE_EEPROM_NOC_CODE];
+
+	// variables will be initialized at init phase
+	FGMCHwTypeId fgmc_device_id_;
+	uint32_t fgmc_application_type_;
+	char fgmc_device_type_[SIZE_DEVICE_TYPE];
+
+	char tx_netbuf_[SIZE_FGMC_RES_MAX];
+
+	/// Object that holds communication related parameter.
+#if 0
+	bmc::services::ApplicationInformation* app_info_;
+	hal::ISelfIdentification* led_manager_;
+	com::protocols::engp::ENGPRouter* engp_router_[IP_MAX_IFACES];
+	interfaces::IParameterManagement* parameter_management_;
+#endif
+
+	static constexpr uint32_t ringBufferSize = 10;
+	uint32_t packetIdBuffer[IP_MAX_IFACES][ringBufferSize];
+	uint32_t packetIdIndex[IP_MAX_IFACES];
+};
+
+#endif	// SUPPORT_MULTICAST_DISCOVERY
+
+#endif // FGMC_PROTOCOL_H
diff --git a/src/Networking/Network.cpp b/src/Networking/Network.cpp
index 7a000675..c8fd283a 100644
--- a/src/Networking/Network.cpp
+++ b/src/Networking/Network.cpp
@@ -626,6 +626,33 @@ IPAddress Network::GetIPAddress(unsigned int interface) const noexcept
 					IPAddress();
 }
 
+IPAddress Network::GetNetmask(unsigned int interface) const noexcept
+{
+	return
+#if HAS_NETWORKING
+			(interface < NumNetworkInterfaces) ? interfaces[interface]->GetNetmask() :
+#endif
+					IPAddress();
+}
+
+IPAddress Network::GetGateway(unsigned int interface) const noexcept
+{
+	return
+#if HAS_NETWORKING
+			(interface < NumNetworkInterfaces) ? interfaces[interface]->GetGateway() :
+#endif
+					IPAddress();
+}
+
+bool Network::UsingDhcp(unsigned int interface) const noexcept
+{
+#if HAS_NETWORKING
+	return interface < NumNetworkInterfaces && interfaces[interface]->UsingDhcp();
+#else
+	return false;
+#endif
+}
+
 void Network::SetHostname(const char *name) noexcept
 {
 #if HAS_NETWORKING
diff --git a/src/Networking/Network.h b/src/Networking/Network.h
index c0ce6e73..2543443d 100644
--- a/src/Networking/Network.h
+++ b/src/Networking/Network.h
@@ -95,6 +95,9 @@ public:
 
 	void SetEthernetIPAddress(IPAddress p_ipAddress, IPAddress p_netmask, IPAddress p_gateway) noexcept;
 	IPAddress GetIPAddress(unsigned int interface) const noexcept;
+	IPAddress GetNetmask(unsigned int interface) const noexcept;
+	IPAddress GetGateway(unsigned int interface) const noexcept;
+	bool UsingDhcp(unsigned int interface) const noexcept;
 	const char *GetHostname() const noexcept { return hostname; }
 	void SetHostname(const char *name) noexcept;
 	GCodeResult SetMacAddress(unsigned int interface, const MacAddress& mac, const StringRef& reply) noexcept;
diff --git a/src/Networking/NetworkInterface.h b/src/Networking/NetworkInterface.h
index 9401827b..b6279eef 100644
--- a/src/Networking/NetworkInterface.h
+++ b/src/Networking/NetworkInterface.h
@@ -35,6 +35,9 @@ public:
 	virtual GCodeResult ReportProtocols(const StringRef& reply) const noexcept = 0;
 
 	virtual IPAddress GetIPAddress() const noexcept = 0;
+	virtual IPAddress GetNetmask() const noexcept = 0;
+	virtual IPAddress GetGateway() const noexcept = 0;
+	virtual bool UsingDhcp() const noexcept = 0;
 	virtual void SetIPAddress(IPAddress p_ipAddress, IPAddress p_netmask, IPAddress p_gateway) noexcept = 0;
 	virtual GCodeResult SetMacAddress(const MacAddress& mac, const StringRef& reply) noexcept = 0;
 	virtual const MacAddress& GetMacAddress() const noexcept = 0;
diff --git a/src/Networking/W5500Ethernet/W5500Interface.h b/src/Networking/W5500Ethernet/W5500Interface.h
index e0176891..66e03b89 100644
--- a/src/Networking/W5500Ethernet/W5500Interface.h
+++ b/src/Networking/W5500Ethernet/W5500Interface.h
@@ -50,7 +50,11 @@ public:
 	bool IsWiFiInterface() const noexcept override { return false; }
 
 	void UpdateHostname(const char *name) noexcept override;
+
 	IPAddress GetIPAddress() const noexcept override { return ipAddress; }
+	IPAddress GetNetmask() const noexcept override { return netmask; }
+	IPAddress GetGateway() const noexcept override { return gateway; }
+	bool UsingDhcp() const noexcept override { return usingDhcp; }
 	void SetIPAddress(IPAddress p_ipAddress, IPAddress p_netmask, IPAddress p_gateway) noexcept override;
 	GCodeResult SetMacAddress(const MacAddress& mac, const StringRef& reply) noexcept override;
 	const MacAddress& GetMacAddress() const noexcept override { return macAddress; }
@@ -81,11 +85,11 @@ private:
 	W5500Socket *mdnsSocket;
 	MdnsResponder *mdnsResponder;
 
-	TcpPort portNumbers[NumProtocols];					// port number used for each protocol
+	TcpPort portNumbers[NumProtocols];				// port number used for each protocol
 	bool protocolEnabled[NumProtocols];				// whether each protocol is enabled
 
 	bool activated;
-	bool usingDhcp;
+	bool usingDhcp = true;
 
 	IPAddress ipAddress;
 	IPAddress netmask;