Fixed loss of pbufs in MulticastResponder

5 files changed, 47 insertions, 16 deletions

diff --git a/src/Networking/LwipEthernet/Lwip/lwipopts.h b/src/Networking/LwipEthernet/Lwip/lwipopts.h
index 559aea5c..2f75ab8e 100644
--- a/src/Networking/LwipEthernet/Lwip/lwipopts.h
+++ b/src/Networking/LwipEthernet/Lwip/lwipopts.h
@@ -179,7 +179,12 @@
 /**
  * PBUF_POOL_SIZE: the number of buffers in the pbuf pool. Needs to be enough for IP packet reassembly.
  */
-#define PBUF_POOL_SIZE                  (GMAC_RX_BUFFERS + GMAC_TX_BUFFERS + 12)
+#if defined(__SAME70Q20B__) || defined(__SAME70Q21B__) || defined(__SAMV71Q20B__) || defined(__SAMV71Q21B__)
+// We may as well use the remainder of the non-cached RAM block for additional pbufs
+# define PBUF_POOL_SIZE                  (GMAC_RX_BUFFERS + GMAC_TX_BUFFERS + 15)
+#else
+# define PBUF_POOL_SIZE                  (GMAC_RX_BUFFERS + GMAC_TX_BUFFERS + 12)
+#endif
 
 /**
  * PBUF_POOL_BUFSIZE: the size of each pbuf in the pbuf pool.
diff --git a/src/Networking/MulticastDiscovery/MulticastResponder.cpp b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
index 2d0b2ee7..60a6369e 100644
--- a/src/Networking/MulticastDiscovery/MulticastResponder.cpp
+++ b/src/Networking/MulticastDiscovery/MulticastResponder.cpp
@@ -28,10 +28,12 @@ static constexpr ip_addr_t ourGroupIpAddr = IPADDR4_INIT_BYTES(239, 255, 2, 3);
 
 static udp_pcb *ourPcb = nullptr;
 static pbuf * volatile receivedPbuf = nullptr;
+static pbuf *pbufToFree = nullptr;
 static volatile uint32_t receivedIpAddr;
 static volatile uint16_t receivedPort;
 static uint16_t lastMessageReceivedPort;
 static unsigned int messagesProcessed = 0;
+static unsigned int responsesSent = 0;
 static FGMCProtocol *fgmcHandler = nullptr;
 static uint32_t ticksToReboot = 0;
 static uint32_t whenRebootScheduled;
@@ -45,7 +47,7 @@ extern "C" void rcvFunc(void *arg, struct udp_pcb *pcb, struct pbuf *p, const ip
 	{
 		receivedIpAddr = addr->addr;
 		receivedPort = port;
-		receivedPbuf = p;
+		receivedPbuf = p;				// store this one last
 	}
 	else
 	{
@@ -86,19 +88,26 @@ void MulticastResponder::Spin() noexcept
 			debugPrintf("\n");
 #endif
 			receivedPbuf = nullptr;
+			pbufToFree = rxPbuf;
 
-			fgmcHandler->handleStream(0, (uint8_t *)rxPbuf->payload, rxPbuf->len);
+			fgmcHandler->handleStream(0, (const uint8_t *)rxPbuf->payload, rxPbuf->len);
 			++messagesProcessed;
 
-			MutexLocker lock(lwipMutex);
-			pbuf_free(rxPbuf);
+			// If processing the message didn't free the pbuf, free it now
+			if (pbufToFree != nullptr)
+			{
+				MutexLocker lock(lwipMutex);
+				pbuf_free(pbufToFree);
+				pbufToFree = nullptr;
+			}
 		}
 	}
 }
 
 void MulticastResponder::Diagnostics(MessageType mtype) noexcept
 {
-	reprap.GetPlatform().MessageF(mtype, "=== Multicast handler ===\nResponder is %s, messages processed %u\n", (active) ? "active" : "inactive", messagesProcessed);
+	reprap.GetPlatform().MessageF(mtype, "=== Multicast handler ===\nResponder is %s, messages received %u, responses %u\n",
+									(active) ? "active" : "inactive", messagesProcessed, responsesSent);
 }
 
 void MulticastResponder::Start(TcpPort port) noexcept
@@ -115,6 +124,7 @@ void MulticastResponder::Start(TcpPort port) noexcept
 		ourPcb = udp_new_ip_type(IPADDR_TYPE_ANY);
 		if (ourPcb == nullptr)
 		{
+			lock.Release();
 			reprap.GetPlatform().Message(ErrorMessage, "unable to allocate a pcb\n");
 		}
 		else
@@ -137,7 +147,7 @@ void MulticastResponder::Start(TcpPort port) noexcept
 		}
 	}
 	active = true;
-	messagesProcessed = 0;
+	messagesProcessed = responsesSent = 0;
 }
 
 void MulticastResponder::Stop() noexcept
@@ -163,14 +173,30 @@ void MulticastResponder::SendResponse(uint8_t *data, size_t length) noexcept
 #endif
 
 	MutexLocker lock(lwipMutex);
+
+	// Release the pbuf that the message arrived in
+	if (pbufToFree != nullptr)
+	{
+		pbuf_free(pbufToFree);
+		pbufToFree = nullptr;
+	}
+
 	pbuf * const pb = pbuf_alloc(PBUF_TRANSPORT, length, PBUF_RAM);
 	if (pb != nullptr)
 	{
 		if (pbuf_take(pb, data, length) == ERR_OK)
 		{
-			if (udp_sendto(ourPcb, pb, &ourGroupIpAddr, lastMessageReceivedPort) != ERR_OK && reprap.Debug(moduleNetwork))
+			const err_t err = udp_sendto(ourPcb, pb, &ourGroupIpAddr, lastMessageReceivedPort);
+			if (err == ERR_OK)
+			{
+				++responsesSent;
+			}
+			else
 			{
-				debugPrintf("UDP send failed\n");
+				if (reprap.Debug(moduleNetwork))
+				{
+					debugPrintf("UDP send failed, err=%u\n", err);
+				}
 			}
 		}
 		else
@@ -179,12 +205,12 @@ void MulticastResponder::SendResponse(uint8_t *data, size_t length) noexcept
 			{
 				debugPrintf("pbuf_take returned error, length %u\n", length);
 			}
-			pbuf_free(pb);
 		}
+		pbuf_free(pb);
 	}
 	else if (reprap.Debug(moduleNetwork))
 	{
-		debugPrintf("pbug_alloc failed,length=%u\n", length);
+		debugPrintf("pbuf_alloc failed,length=%u\n", length);
 	}
 }
 
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
index e7b98322..6ffa13f1 100644
--- a/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.cpp
@@ -57,15 +57,15 @@ void FGMCProtocol::init() noexcept
 	fgmc_device_id_ = FGMCHwTypeId::FGMC_DEVICE_ID_DUET3;
 }
 
-void FGMCProtocol::handleStream(unsigned int iFaceId, uint8_t* inputBufferAddress, uint32_t rxLength) noexcept
+void FGMCProtocol::handleStream(unsigned int iFaceId, const uint8_t* inputBufferAddress, uint32_t rxLength) noexcept
 {
 	// backup sciopta connection handle
-	// if a null pointer received => plattform should execute a exception
+	// if a null pointer received => platform should execute a exception
 	this->iface_id_ = iFaceId;
 
 	if (rxLength >= static_cast<uint32_t>(sizeof(FGMC_GenericHeader)))
 	{
-		FGMC_GenericHeader* const pInGenericHeader = reinterpret_cast<FGMC_GenericHeader*>(inputBufferAddress);
+		const FGMC_GenericHeader* const pInGenericHeader = reinterpret_cast<const FGMC_GenericHeader*>(inputBufferAddress);
 
 		// read incoming packetid
 		const uint32_t packetId = pInGenericHeader->fgmc_packet_id_;
diff --git a/src/Networking/MulticastDiscovery/fgmc_protocol.h b/src/Networking/MulticastDiscovery/fgmc_protocol.h
index 436ddd00..77e7907c 100644
--- a/src/Networking/MulticastDiscovery/fgmc_protocol.h
+++ b/src/Networking/MulticastDiscovery/fgmc_protocol.h
@@ -36,7 +36,7 @@ public:
 	/// \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;
+	void handleStream(unsigned int iFaceId, const uint8_t* inputBufferAddress, uint32_t rxLength) noexcept;
 
 private:
 	/// this functions sends fgmc frame
diff --git a/src/Networking/TelnetResponder.cpp b/src/Networking/TelnetResponder.cpp
index 4fcb7835..e2443e7d 100644
--- a/src/Networking/TelnetResponder.cpp
+++ b/src/Networking/TelnetResponder.cpp
@@ -386,7 +386,7 @@ void TelnetResponder::ProcessLine() noexcept
 
 void TelnetResponder::Diagnostics(MessageType mt) const noexcept
 {
-	GetPlatform().MessageF(mt, " Telnet(%d), %u sessions", (int)responderState, numSessions);
+	GetPlatform().MessageF(mt, " Telnet(%d)", (int)responderState);
 }
 
 unsigned int TelnetResponder::numSessions = 0;