Implemented reboot from multicast message

3 files changed, 315 insertions, 273 deletions

diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.cpp b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
index 96630a58..da3f582a 100644
--- a/src/Networking/MulticastDiscovery/MulticastResponder.cpp
+++ b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
@@ -11,6 +11,8 @@
 
 #include <Platform/RepRap.h>
 #include <Platform/Platform.h>
+#include <Hardware/ExceptionHandlers.h>
+
 #include "fgmc_header.h"
 #include "fgmc_protocol.h"
 
@@ -29,6 +31,8 @@ static volatile uint16_t receivedPort;
 static uint16_t lastMessageReceivedPort;
 static unsigned int messagesProcessed = 0;
 static FGMCProtocol *fgmcHandler = nullptr;
+static uint32_t ticksToReboot = 0;
+static uint32_t whenRebootScheduled;
 
 static bool active = false;
 
@@ -55,21 +59,33 @@ void MulticastResponder::Init() noexcept
 // Do some work, returning true if we did anything significant
 void MulticastResponder::Spin() noexcept
 {
-	pbuf *rxPbuf = receivedPbuf;
-	if (rxPbuf != nullptr)
+	if (ticksToReboot != 0)
+	{
+		if (millis() - whenRebootScheduled > ticksToReboot)
+		{
+			reprap.EmergencyStop();									// this disables heaters and drives - Duet WiFi pre-production boards need drives disabled here
+			SoftwareReset(SoftwareResetReason::user);				// doesn't return
+		}
+
+	}
+	else
 	{
-		lastMessageReceivedPort = receivedPort;
-		debugPrintf("Rx UDP: addr %u.%u.%u.%u port %u data",
-			(unsigned int)(receivedIpAddr & 0xFF), (unsigned int)((receivedIpAddr >> 8) & 0xFF), (unsigned int)((receivedIpAddr >> 16) & 0xFF), (unsigned int)((receivedIpAddr >> 24) & 0xFF), lastMessageReceivedPort);
-		for (size_t i = 0; i < rxPbuf->len; ++i)
+		pbuf *rxPbuf = receivedPbuf;
+		if (rxPbuf != nullptr)
 		{
-			debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]);
+			lastMessageReceivedPort = receivedPort;
+			debugPrintf("Rx UDP: addr %u.%u.%u.%u port %u data",
+				(unsigned int)(receivedIpAddr & 0xFF), (unsigned int)((receivedIpAddr >> 8) & 0xFF), (unsigned int)((receivedIpAddr >> 16) & 0xFF), (unsigned int)((receivedIpAddr >> 24) & 0xFF), lastMessageReceivedPort);
+			for (size_t i = 0; i < rxPbuf->len; ++i)
+			{
+				debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]);
+			}
+			debugPrintf("\n");
+			receivedPbuf = nullptr;
+			fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len);
+			pbuf_free(rxPbuf);
+			++messagesProcessed;
 		}
-		debugPrintf("\n");
-		receivedPbuf = nullptr;
-		fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len);
-		pbuf_free(rxPbuf);
-		++messagesProcessed;
 	}
 }
 
@@ -134,6 +150,13 @@ void MulticastResponder::SendResponse(uint8_t *data, size_t length) noexcept
 	//TODO actually send it
 }
 
+// Schedule a reboot. We delay a little while to allow the response to be transmitted first.
+void MulticastResponder::ScheduleReboot() noexcept
+{
+	whenRebootScheduled = millis();
+	ticksToReboot = 1000;
+}
+
 #endif
 
 // End
diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.h b/src/Networking/MulticastDiscovery/MulticastResponder.h
index 25b717d7..173a0087 100644
--- a/src/Networking/MulticastDiscovery/MulticastResponder.h
+++ b/src/Networking/MulticastDiscovery/MulticastResponder.h
@@ -20,6 +20,7 @@ namespace MulticastResponder
 	void Start(TcpPort port) noexcept;
 	void Stop() noexcept;
 	void SendResponse(uint8_t *data, size_t length) noexcept;
+	void ScheduleReboot() noexcept;
 	void Diagnostics(MessageType mtype) noexcept;
 }
 
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
index cb936772..f1b99969 100644
--- a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
@@ -103,247 +103,262 @@ void FGMCProtocol::init() noexcept
 
 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;
+	// 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;
-      }
-    }
-  }
+	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* const 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(uint8_t* 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_);
-
-  MulticastResponder::SendResponse(tx_netbuf, length);
+	FGMC_GenericHeader* pOutGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(tx_netbuf);
+
+	//-----------------------------------------------------------------------------------
+	// Generic Multicast Header
+	//-----------------------------------------------------------------------------------
+	memcpy(pOutGenericHeader->fgmc_name_, FGMC_NAME, SIZE_FGMC_NAME);
+	pOutGenericHeader->fgmc_length_ = length;
+	pOutGenericHeader->fgmc_hw_type_id_ = fgmc_device_id_;
+	// device unique fgmc destination id
+	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_);
+
+	MulticastResponder::SendResponse(tx_netbuf, length);
 }
 
 void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
 {
-  FGMC_ResUploadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResUploadNetInfoHeader*>(tx_netbuf_);
-  (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResUploadNetInfoHeader));
+	FGMC_ResUploadNetInfoHeader* pOutCmdHeader = reinterpret_cast<FGMC_ResUploadNetInfoHeader*>(tx_netbuf_);
+	(void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResUploadNetInfoHeader));
 
-  //-----------------------------------------------------------------------------------
-  // The Upload Netinformation Structure
-  //-----------------------------------------------------------------------------------
+	//-----------------------------------------------------------------------------------
+	// 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;
-  }
+	// 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);
+	// 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);
+	// 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 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);
+	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;
+	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);
+	// 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);
+	// 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 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);
+	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;
+	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);
+	// NOC-CODE
+	(void)strncpy(&pOutCmdHeader->fgmc_noc_code_[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
 
-  // iTTL
-  pOutCmdHeader->fgmc_ttl_ = 255;
+	// iTTL
+	pOutCmdHeader->fgmc_ttl_ = 255;
 
-  // Connection State
-  pOutCmdHeader->fgmc_connection_state_ = 0;
+	// Connection State
+	pOutCmdHeader->fgmc_connection_state_ = 0;
 
-  // Device Type
-  (void)strncpy(&pOutCmdHeader->fgmc_device_type_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE);
+	// 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);
+	// 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 Type
+	pOutCmdHeader->fgmc_application_type_ = fgmc_application_type_;
 
-  // Application Version
-  pOutCmdHeader->fgmc_application_version_ = 0;
+	// Application Version
+	pOutCmdHeader->fgmc_application_version_ = 0;
 
-  // Application Version Revision
-  pOutCmdHeader->fgmc_application_version_revision_ = 0;
+	// Application Version Revision
+	pOutCmdHeader->fgmc_application_version_revision_ = 0;
 
-  // Generic Info
-  (void)memset(&pOutCmdHeader->fgmc_generic_info_[0], 0x00, SIZE_GENERIC_INFO);
+	// 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_));
+	// 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);
+	//-----------------------------------------------------------------------------------
+	// 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);
+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);
+	//-----------------------------------------------------------------------------------
+	// Generic Multicast Header
+	//-----------------------------------------------------------------------------------
+	sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_DNETINF, sizeof(FGMC_ResDownloadNetInfoHeader), inPacketId, 0, 1);
 }
 #endif
 
@@ -355,8 +370,7 @@ void FGMCProtocol::cmdReboot(uint32_t inPacketId) noexcept
 	// Generic Multicast Header
 	//-----------------------------------------------------------------------------------
 	sendGenericHeader(tx_netbuf_, FGMCCommand::MCD_COMMAND_REBOOT, sizeof(FGMC_GenericHeader), inPacketId, 0, 1);
-
-	//TODO schedule the reboot
+	MulticastResponder::ScheduleReboot();
 }
 
 void FGMCProtocol::cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacketId) noexcept
@@ -388,78 +402,82 @@ void FGMCProtocol::cmdIdentify(FGMC_ReqIdentify* pInCmdHeader, uint32_t inPacket
 
 void FGMCProtocol::cmdGetFirmwareVersion(uint32_t inPacketId) noexcept
 {
-  FGMC_ResGetFwVersion* pOutCmdHeader = reinterpret_cast<FGMC_ResGetFwVersion*>(tx_netbuf_);
-  (void)memset(pOutCmdHeader, 0x00, sizeof(FGMC_ResGetFwVersion));
+	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);
+	//-----------------------------------------------------------------------------------
+	// 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);
+	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);
+	//-----------------------------------------------------------------------------------
+	// 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;
+	uint32_t segmentIndex = 0;
+	uint32_t segmentCount = 0;
 
-  FGMCCommand supportedCommands[] =
-  {
-	  FGMCCommand::MCD_COMMAND_UNETINF,
+	FGMCCommand supportedCommands[] =
+	{
+		FGMCCommand::MCD_COMMAND_UNETINF,
 #if 0	// not supported for now
-	  FGMCCommand::MCD_COMMAND_DNETINF,
+		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);
-  }
+		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;
+	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;
-  }
+	if (isPacketInBuffer(packetId))
+	{
+		return;
+	}
+	packetIdBuffer[iface_id_][packetIdIndex[iface_id_]] = packetId;
+	packetIdIndex[iface_id_]++;
+	if (packetIdIndex[iface_id_] >= ringBufferSize)
+	{
+		packetIdIndex[iface_id_] = 0;
+	}
 }
 
 #endif