dani/CommandStation-EX
1
0
Fork 0
mirror of https://github.com/DCC-EX/CommandStation-EX.git synced 2024-11-23 16:16:13 +01:00
Code Releases Activity

restructure UDP packet receive

Browse Source
This commit is contained in:
Harald Barth 2023-06-08 22:55:45 +02:00
parent fb22bcd99d
commit 800972a57c
2 changed files with 321 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
Z21Throttle.cpp
View File

@ -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;
// 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();
}
}
byte networkPacket[MAX_MTU] = {0};
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 < clientsUDP.size(); clientId++) {
for (clientId = 0; 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();
@ -113,27 +94,29 @@ void Z21Throttle::loop() {
clientsUDP.push_back(nc);
if (Diag::Z21THROTTLE) DIAG(F("New UDP client %d, %s"), clientId, nc.remoteIP.toString().c_str());
printClientsUDP();
#ifdef USE_HMI
#ifdef USE_HMI
if (hmi::CurrentInterface != NULL) hmi::CurrentInterface->NewClient(clientId, nc.remoteIP, 0);
#endif
#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];
CircularBuffer* pBuffer = clientsUDP[this->clientid].pudpBuffer;
byte *DB;
byte *p = networkPacket;
int l = len;
if (pBuffer == NULL)
return false;
if (pBuffer->isEmpty())
return false;
while (l > 0) {
int lengthData = pBuffer->GetInt16() - 4; // length of the data = total length - length of length (!) - length of header
int header = pBuffer->GetInt16();
int lengthData = GETINT16(p);
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();
if (lengthData > 0) {
pBuffer->GetBytes(DB, lengthData);
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"),
// set p for next round
p += lengthData;
if (Diag::Z21THROTTLEDATA && DB[1] != LAN_X_DB0_GET_STATUS)
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;
}

9
Z21Throttle.h
View File

@ -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;