restructure UDP packet receive

Z21Throttle.cpp

@@ -1,8 +1,9 @@
 /*
  *  © 2023 Thierry Paris / Locoduino
+ *  © 2023 Harald Barth
  *  All rights reserved.
  *  
- *  This file is part of CommandStation-EX-Labox
+ *  This file is part of CommandStation-EX
  *
  *  This is free software: you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
@@ -61,49 +62,29 @@ void Z21Throttle::setup(IPAddress ip, int port) {
 	NetworkClientUDP::client.flush();
 }
 
-int readUdpPacket() {
-	byte udp[UDPBYTE_SIZE];
-	memset(udp, 0, UDPBYTE_SIZE);
-	int len = NetworkClientUDP::client.read(udp, UDPBYTE_SIZE);
-	if (len > 0) {
-		for (int clientId = 0; clientId < clientsUDP.size(); clientId++) {
-		if (clientsUDP[clientId].inUse)
-			if (clientsUDP[clientId].remoteIP == NetworkClientUDP::client.remoteIP() && clientsUDP[clientId].remotePort == NetworkClientUDP::client.remotePort()) {
-				clientsUDP[clientId].pudpBuffer->PushBytes(udp, len);
-			return clientId;
-			}
-		}
-	}
-
-	return -1;
-}
-
 void Z21Throttle::loop() {
   int clientId = 0;
-
+  byte networkPacket[MAX_MTU] = {0};
-	// loop over all clients and remove inactive
-	for (clientId = 0; clientId < clientsUDP.size(); clientId++) {
-		// check if client is there and alive
-		if (clientsUDP[clientId].inUse && !clientsUDP[clientId].connected) {
-			if (Diag::Z21THROTTLE) DIAG(F("Remove UDP client %d"), clientId);
-			clientsUDP[clientId].inUse = false;
-			printClientsUDP();
-		}
-	}
 
   int len = NetworkClientUDP::client.parsePacket();
+  if (len > MAX_MTU) {
+    DIAG(F("ERROR: len > MAX_MTU"));
+    return;
+  }
+  IPAddress remoteIP = NetworkClientUDP::client.remoteIP();
+  int remotePort =  NetworkClientUDP::client.remotePort();
   if (len > 0) {
-		int clientId = 0;
+    for (clientId = 0; clientId < clientsUDP.size(); clientId++) {
-		for (; clientId < clientsUDP.size(); clientId++) {
       if (clientsUDP[clientId].inUse) {
-				if (clientsUDP[clientId].remoteIP == NetworkClientUDP::client.remoteIP() && clientsUDP[clientId].remotePort == NetworkClientUDP::client.remotePort()) {
+	if (clientsUDP[clientId].remoteIP == remoteIP
+	    && clientsUDP[clientId].remotePort == remotePort) {
 	  //if (Diag::Z21THROTTLEVERBOSE) DIAG(F("UDP client %d : %s Already connected"), clientId, clientsUDP[clientId].remoteIP.toString().c_str());
 	  break;
 	}
       }
     }
     
-		if (clientId >= clientsUDP.size()) {
+    if (clientId >= clientsUDP.size()) { // not found, let's create it
       NetworkClientUDP nc;
       nc.remoteIP = NetworkClientUDP::client.remoteIP();
       nc.remotePort = NetworkClientUDP::client.remotePort();
@@ -117,23 +98,25 @@ void Z21Throttle::loop() {
       if (hmi::CurrentInterface != NULL) hmi::CurrentInterface->NewClient(clientId, nc.remoteIP, 0);
 #endif
       // Fleischmann/Roco Android app starts with Power on !
+      // XXX this is the wrong place to do this
       TrackManager::setMainPower(POWERMODE::ON);
     }
     
-		clientId = readUdpPacket();
+    // now clientId is on "current client", either new or old
+    
+    int l = NetworkClientUDP::client.read(networkPacket, len);
+    if (l != len) {
+      DIAG(F(" l %d = len %d"), l, len);
+      return;
+    }
     
-		if (clientId >= 0) {
-			if (clientsUDP[clientId].ok()) {        
     Z21Throttle* pThrottle = getOrAddThrottle(clientId); 
-
     if (pThrottle != NULL)
-					pThrottle->parse();
+      pThrottle->parse(networkPacket, len);
-			}
-		}
    }
 }
 
-/** Print the list of assigned locomotives. */
+// Print the list of assigned locomotives
 void Z21Throttle::printLocomotives(bool addTab) {
 	if (!Diag::Z21THROTTLE)
 		return;
@@ -144,7 +127,7 @@ void Z21Throttle::printLocomotives(bool addTab) {
 			DIAG(F("%s         %d : cab %d on throttle %c"), addTab ? "   ":"", loco, myLocos[loco].cab, myLocos[loco].throttle);
 }
 
-/** Print the list of assigned locomotives. */
+// Print the list of UDP clients
 void printClientsUDP() {
 	if (!Diag::Z21THROTTLE) return;
 
@@ -156,7 +139,7 @@ void printClientsUDP() {
 			DIAG(F("         %d unused"), clientId);
 }
 
-/** Print the list of assigned locomotives. */
+// Print the list of throttles
 void Z21Throttle::printThrottles(bool inPrintLocomotives) {
 	if (!Diag::Z21THROTTLE)	return;
 
@@ -215,10 +198,10 @@ bool Z21Throttle::areYouUsingThrottle(int cab) {
 // One instance of Z21Throttle per connected client, so we know what the locos are 
  
 Z21Throttle::Z21Throttle(int inClientId) {
+        clientid = inClientId;
 	if (Diag::Z21THROTTLE) DIAG(F("New Z21Throttle for client UDP %d"), clientid); 
 	nextThrottle=firstThrottle;
 	firstThrottle= this;
-	clientid = inClientId;
 	initSent=false; // prevent sending heartbeats before connection completed
 	turnoutListHash = -1;  // make sure turnout list is sent once
 	exRailSent=false;
@@ -617,25 +600,68 @@ void Z21Throttle::cvReadPom(byte inDB1, byte inDB2, byte inDB3, byte inDB4) {
 	DCC::readCV(cvAddress, ptr);
 }
 
-bool Z21Throttle::parse() {
+void diagPacket(byte *networkPacket, int len) {
+  DIAG(F("len=%d 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x  0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"),
+       len,
+       networkPacket[0],
+       networkPacket[1],
+       networkPacket[2],
+       networkPacket[3],
+       networkPacket[4],
+       networkPacket[5],
+       networkPacket[6],
+       networkPacket[7],
+       networkPacket[8],
+       networkPacket[9],
+       networkPacket[10],
+       networkPacket[11],
+       networkPacket[12],
+       networkPacket[13],
+       networkPacket[14]);
+}
+#define GETINT16(BUF) (int16_t((unsigned char)(*(BUF+1)) << 8 | (unsigned char)(*BUF)));
+bool Z21Throttle::parse(byte *networkPacket, int len) {
+
   bool done = false;
-	byte DB[100];
+  byte *DB;
-	CircularBuffer* pBuffer = clientsUDP[this->clientid].pudpBuffer;
+  byte *p = networkPacket;
+  int l = len;
 
-	if (pBuffer == NULL)
+  while (l > 0) {
-		return false;
-	if (pBuffer->isEmpty())
-		return false;
 
-	int lengthData = pBuffer->GetInt16() - 4;	// length of the data = total length - length of length (!) - length of header
+    int lengthData = GETINT16(p);
-	int header = pBuffer->GetInt16();
+    l -= lengthData;
+    if (p == networkPacket && lengthData != len) {
+      diagPacket(networkPacket, len);
+    }
+    if (l < 0) {
+      DIAG(F("ERROR: Xbus data exceeds UDP packet size: l < 0 pos=%d, l=%d"), p-networkPacket, l);
+      diagPacket(networkPacket, len);
+      return false;
+    }
+    if (l > 0 && lengthData < 4) {
+      DIAG(F("WARNING: Xbus data does not fill UDP packet size: l > 0 pos=%d, l=%d"), p-networkPacket, l);
+      diagPacket(networkPacket, len);
+      return true;
+    }
+    // length of the data = total length - length of length (!) - length of header
+    lengthData -= 4;
+    if (lengthData < 0) {
+      DIAG(F("ERROR: lengthData < 0 SHOULD NOT GET HERE"));
+      diagPacket(networkPacket, len);
+      return false;
+    }
+    p += 2;
+    int header = GETINT16(p);
     byte Xheader = 0;
+    p += 2;
+    DB = p;
     byte DB0 = 0;
     int nbLocos = CountLocos();
-
+    // set p for next round
-	if (lengthData > 0)	{
+    p += lengthData;
-		pBuffer->GetBytes(DB, lengthData);
+    if (Diag::Z21THROTTLEDATA && DB[1] != LAN_X_DB0_GET_STATUS)
-		if (Diag::Z21THROTTLEDATA && DB[1] != LAN_X_DB0_GET_STATUS) DIAG(F("%d <- len:%d  header:0x%02x  : 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"),
+      DIAG(F("%d <- lengthData:%d  header:0x%02x  : 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x"),
 	   this->clientid, lengthData, header,
 	   (lengthData > 0)?DB[0]:0,
 	   (lengthData > 1)?DB[1]:0,
@@ -647,7 +673,7 @@ bool Z21Throttle::parse() {
 	   (lengthData > 7)?DB[7]:0,
 	   (lengthData > 8)?DB[8]:0,
 	   (lengthData > 9)?DB[9]:0);
-	}
+    if (l > 0 && Diag::Z21THROTTLEDATA) DIAG(F("next packet follows"));
     
     switch (header)	{
     case HEADER_LAN_XPRESS_NET:
@@ -780,7 +806,7 @@ bool Z21Throttle::parse() {
       break;
       
     case HEADER_LAN_SET_BROADCASTFLAGS:
-			this->broadcastFlags = CircularBuffer::GetInt32(DB, 0);
+      this->broadcastFlags = int32_t(DB[3] << 24 | DB[2] << 16 | DB[1] << 8 | DB[0]);
       if (Diag::Z21THROTTLEDATA) DIAG(F("BROADCAST FLAGS %d : %s %s %s %s %s %s %s %s %s %s %s"), this->clientid,
 				      (this->broadcastFlags & BROADCAST_BASE)	? "BASE " : "" ,
 				      (this->broadcastFlags & BROADCAST_RBUS)	? "RBUS " : "" ,
@@ -836,11 +862,12 @@ bool Z21Throttle::parse() {
     
     if (!done) {
       if (Diag::Z21THROTTLE) DIAG(F("Z21 Throttle %d : not treated :  header:%x   Xheader:%x   DB0:%x"), this->clientid, header, Xheader, DB0);
-	}
+    } else {
-	else {
       int newNbLocos = CountLocos();
       if (nbLocos != newNbLocos)
 	printLocomotives();
     }
+  }
+  // if we get here, we did parse one or several xbus packets inside USB packets
   return true;
 }

Z21Throttle.h

@@ -1,5 +1,6 @@
 /*
  *  © 2023 Thierry Paris / Locoduino
+ *  © 2023 Harald Barth
  *  All rights reserved.
  *  
  *  This is free software: you can redistribute it and/or modify
@@ -18,9 +19,10 @@
 #ifndef Z21Throttle_h
 #define Z21Throttle_h
 
-#include "CircularBuffer.hpp"
+//#include "CircularBuffer.hpp"
 #include "WiFiClient.h"
 
+#define MAX_MTU 1460
 #define UDPBYTE_SIZE	1500
 #define UDP_BUFFERSIZE  2048
 
@@ -49,8 +51,6 @@ struct MYLOCOZ21 {
 class NetworkClientUDP {
 	public:
 	NetworkClientUDP() {
-		this->pudpBuffer = new CircularBuffer(UDP_BUFFERSIZE);
-		this->pudpBuffer->begin(true);
 	};
 	bool ok() {
 		return (inUse);
@@ -58,7 +58,6 @@ class NetworkClientUDP {
 
 	bool inUse = true;
 	bool connected = false;
-	CircularBuffer *pudpBuffer = NULL;
 	IPAddress remoteIP;
 	int remotePort;
 
@@ -77,7 +76,7 @@ class Z21Throttle {
 		void notifyCvNACK(int inCvAddress);
 		void notifyCvRead(int inCvAddress, int inValue);
 		
-		bool parse();
+  bool parse(byte *networkPacket, int len);
 
 		static Z21Throttle *readWriteThrottle;	// NULL if no throttle is reading or writing a CV...
 		static int cvAddress;