More work on multicast responder

3 files changed, 64 insertions, 79 deletions

diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.cpp b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
index da3f582a..c2227c33 100644
--- a/src/Networking/MulticastDiscovery/MulticastResponder.cpp
+++ b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
@@ -74,6 +74,7 @@ void MulticastResponder::Spin() noexcept
 		if (rxPbuf != nullptr)
 		{
 			lastMessageReceivedPort = receivedPort;
+#if 0
 			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)
@@ -81,6 +82,7 @@ void MulticastResponder::Spin() noexcept
 				debugPrintf(" %02x", ((const uint8_t*)(rxPbuf->payload))[i]);
 			}
 			debugPrintf("\n");
+#endif
 			receivedPbuf = nullptr;
 			fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len);
 			pbuf_free(rxPbuf);
@@ -99,6 +101,7 @@ void MulticastResponder::Start(TcpPort port) noexcept
 	if (fgmcHandler == nullptr)
 	{
 		fgmcHandler = new FGMCProtocol;
+		fgmcHandler->init();
 	}
 
 	if (ourPcb == nullptr)
@@ -141,13 +144,29 @@ void MulticastResponder::Stop() noexcept
 
 void MulticastResponder::SendResponse(uint8_t *data, size_t length) noexcept
 {
+#if 0
 	debugPrintf("Tx UDP: port %u data", lastMessageReceivedPort);
 	for (size_t i = 0; i < length; ++i)
 	{
 		debugPrintf(" %02x", data[i]);
 	}
 	debugPrintf("\n");
-	//TODO actually send it
+#endif
+
+	pbuf * const pb = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_RAM);
+#if 0
+	if (pbuf_take(pb, data, length) != ERR_OK)
+	{
+		debugPrintf("pbuf_take returned error\n");
+	}
+	if (udp_sendto_if(ourPcb, pb, &ourGroup, lastMessageReceivedPort, &gs_net_if) != ERR_OK)
+	{
+		debugPrintf("UDP send failed\n");
+	}
+#else
+	(void)pbuf_take(pb, data, length);
+	(void)udp_sendto_if(ourPcb, pb, &ourGroup, lastMessageReceivedPort, &gs_net_if);
+#endif
 }
 
 // Schedule a reboot. We delay a little while to allow the response to be transmitted first.
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
index f1b99969..06406267 100644
--- a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
@@ -11,6 +11,10 @@
 #include <Version.h>
 #include <cstring>
 
+// Disable gcc warning about strncpy not being able to include a terminating null.
+// We don't need a terminating null when filling in response fields (if we did then we would use SafeStrcpy instead).
+#pragma GCC diagnostic ignored "-Wstringop-truncation"
+
 FGMCProtocol::FGMCProtocol() noexcept
     : iface_id_(0),
       eeprom_product_key_{0},
@@ -18,19 +22,13 @@ FGMCProtocol::FGMCProtocol() noexcept
       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}
 {
 }
 
+// Build a unique ID from the MAC address
 void FGMCProtocol::macToString(uint32_t interface) noexcept
 {
   for (uint32_t i = 0; i < SIZE_FGMC_DEST_ID; i++)
@@ -68,37 +66,38 @@ void FGMCProtocol::macToString(uint32_t interface) noexcept
 
 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);
-  }
+	char deviceName[SIZE_DEVICE_NAME] = {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;
+	for (uint32_t i = 0; i < IP_MAX_IFACES; i++)
+	{
+		macToString(i);
+	}
+
+	fgmc_device_id_ = FGMCHwTypeId::FGMC_DEVICE_ID_DUET3;
 }
 
 void FGMCProtocol::handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept
@@ -195,7 +194,7 @@ void FGMCProtocol::sendGenericHeader(uint8_t* tx_netbuf, FGMCCommand cmd, uint32
 	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);
+	strncpy(pOutGenericHeader->fgmc_destination_id_, unique_id_[iface_id_], SIZE_FGMC_DEST_ID);
 	pOutGenericHeader->fgmc_packet_id_ = packetId + 1;
 	pOutGenericHeader->fgmc_segment_index_ = segmentIndex;
 	pOutGenericHeader->fgmc_segment_count_ = segmentCount;
@@ -236,13 +235,13 @@ void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
 	memcpy(pOutCmdHeader->fgmc_mac_address_, macAddress.bytes, 6);
 
 	// IPv4
-	const uint32_t ipaddress = reprap.GetNetwork().GetIPAddress(iface_id_).GetV4BigEndian();
+	const uint32_t ipaddress = reprap.GetNetwork().GetIPAddress(iface_id_).GetV4LittleEndian();
 	(void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_address_[0], &ipaddress, SIZE_IP_V4);
 
-	const uint32_t subnetmask = reprap.GetNetwork().GetNetmask(iface_id_).GetV4BigEndian();
+	const uint32_t subnetmask = reprap.GetNetwork().GetNetmask(iface_id_).GetV4LittleEndian();
 	(void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_netmask_[0], &subnetmask, SIZE_IP_V4);
 
-	const uint32_t gateway = reprap.GetNetwork().GetGateway(iface_id_).GetV4BigEndian();
+	const uint32_t gateway = reprap.GetNetwork().GetGateway(iface_id_).GetV4LittleEndian();
 	(void)memcpy(&pOutCmdHeader->fgmc_ip_v4_static_gateway_[0], &gateway, SIZE_IP_V4);
 
 	pOutCmdHeader->fgmc_ip_v4_static_dns_[0] = 0;
@@ -272,7 +271,7 @@ void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
 	pOutCmdHeader->fgmc_ip_v4_dns_[3] = 0;
 
 	// NOC-CODE
-	(void)strncpy(&pOutCmdHeader->fgmc_noc_code_[0], &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
+//	(void)strncpy(pOutCmdHeader->fgmc_noc_code_, &eeprom_noc_code_[0], SIZE_EEPROM_NOC_CODE);
 
 	// iTTL
 	pOutCmdHeader->fgmc_ttl_ = 255;
@@ -281,10 +280,10 @@ void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
 	pOutCmdHeader->fgmc_connection_state_ = 0;
 
 	// Device Type
-	(void)strncpy(&pOutCmdHeader->fgmc_device_type_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE);
+	(void)strncpy(pOutCmdHeader->fgmc_device_type_, BOARD_NAME, SIZE_DEVICE_TYPE);
 
 	// Device Serial Number
-	(void)strncpy(&pOutCmdHeader->fgmc_serial_number_[0], &eeprom_product_key_[0], SIZE_SERIAL_NUMBER);
+	(void)strncpy(pOutCmdHeader->fgmc_serial_number_, &eeprom_product_key_[0], SIZE_SERIAL_NUMBER);
 
 	// Application Type
 	pOutCmdHeader->fgmc_application_type_ = fgmc_application_type_;
@@ -296,10 +295,10 @@ void FGMCProtocol::cmdUnetinf(uint32_t inPacketId) noexcept
 	pOutCmdHeader->fgmc_application_version_revision_ = 0;
 
 	// Generic Info
-	(void)memset(&pOutCmdHeader->fgmc_generic_info_[0], 0x00, SIZE_GENERIC_INFO);
+	(void)memset(pOutCmdHeader->fgmc_generic_info_, 0x00, SIZE_GENERIC_INFO);
 
 	// Device Name
-	SafeStrncpy(pOutCmdHeader->fgmc_device_name_, BOARD_SHORT_NAME, ARRAY_SIZE(pOutCmdHeader->fgmc_device_name_));
+	strncpy(pOutCmdHeader->fgmc_device_name_, reprap.GetName(), ARRAY_SIZE(pOutCmdHeader->fgmc_device_name_));
 
 	//-----------------------------------------------------------------------------------
 	// Generic Multicast Header
@@ -408,7 +407,7 @@ void FGMCProtocol::cmdGetFirmwareVersion(uint32_t inPacketId) noexcept
 	//-----------------------------------------------------------------------------------
 	// Get Firmware Version
 	//-----------------------------------------------------------------------------------
-	(void)strncpy(&pOutCmdHeader->module_name_[0], &fgmc_device_type_[0], SIZE_DEVICE_TYPE);
+	(void)strncpy(pOutCmdHeader->module_name_, BOARD_NAME, SIZE_DEVICE_TYPE);
 
 	SafeSnprintf(pOutCmdHeader->module_version_, SIZE_MODULE_VERSION, "%s version %s", FIRMWARE_NAME, VERSION);
 
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.h b/src/Networking/MulticastDiscovery/fgmc_protocol.h
index 2155fa32..31e443da 100644
--- a/src/Networking/MulticastDiscovery/fgmc_protocol.h
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.h
@@ -28,30 +28,6 @@ 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;
 
@@ -61,6 +37,7 @@ public:
 	/// \param rxLength receive frame length
 	void handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept;
 
+private:
 	/// this functions sends fgmc frame
 	/// \param pOutPointer sciopta network buffer
 	/// \param cmd cmd
@@ -104,7 +81,6 @@ public:
 	/// \param packetId packedId
 	bool isPacketInBuffer(uint32_t packetId) noexcept;
 
-	private:
 	void macToString(uint32_t interface) noexcept;
 
 	// connection data pointer
@@ -118,18 +94,9 @@ public:
 	// variables will be initialized at init phase
 	FGMCHwTypeId fgmc_device_id_;
 	uint32_t fgmc_application_type_;
-	char fgmc_device_type_[SIZE_DEVICE_TYPE];
 
 	uint8_t 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];