intermediate: udp reader

2025-12-16 07:23:07 +01:00 · 2024-11-08 21:00:25 +01:00
parent b0d5e27b5a
commit 82f5e17987
10 changed files with 406 additions and 24 deletions
--- a/lib/socketserver/GwUdpReader.cpp
+++ b/lib/socketserver/GwUdpReader.cpp
@@ -0,0 +1,158 @@
+#include "GwUdpReader.h"
+#include <ESPmDNS.h>
+#include <errno.h>
+#include "GwBuffer.h"
+#include "GwSocketConnection.h"
+#include "GwSocketHelper.h"
+#include "GWWifi.h"
+
+
+GwUdpReader::GwUdpReader(const GwConfigHandler *config, GwLog *logger, int minId)
+{
+    this->config = config;
+    this->logger = logger;
+    this->minId = minId;
+    port=config->getInt(GwConfigDefinitions::udprPort);
+    buffer= new GwBuffer(logger,GwBuffer::RX_BUFFER_SIZE,"udprd");
+}
+
+void GwUdpReader::createAndBind(){
+    if (fd >= 0){
+        ::close(fd);
+    }
+    if (currentStationIp.isEmpty() && (type == T_STA || type == T_MCSTA)) return;
+    fd=socket(AF_INET,SOCK_DGRAM,IPPROTO_IP);
+    if (fd < 0){
+        LOG_ERROR("UDPR: unable to create udp socket: %d",errno);
+        return;
+    }
+    int enable = 1;
+    setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(int));
+    if (type == T_STA)
+    {
+        if (inet_pton(AF_INET, currentStationIp.c_str(), &listenA.sin_addr) != 1)
+        {
+            LOG_ERROR("UDPR: invalid station ip address %s", currentStationIp.c_str());
+            close(fd);
+            fd = -1;
+            return;
+        }
+    }
+    if (bind(fd,(struct sockaddr *)&listenA,sizeof(listenA)) < 0){
+        LOG_ERROR("UDPR: unable to bind: %d",errno);
+        close(fd);
+        fd=-1;
+        return;
+    }
+    LOG_INFO("UDPR: socket created and bound");
+    if (type != T_MCALL && type != T_MCAP && type != T_MCSTA) {
+        return;
+    }
+    struct ip_mreq mc;
+    String mcAddr=config->getString(GwConfigDefinitions::udprMC);
+    if (inet_pton(AF_INET,mcAddr.c_str(),&mc.imr_multiaddr) != 1){
+        LOG_ERROR("UDPR: invalid multicast addr %s",mcAddr.c_str());
+        ::close(fd);
+        fd=-1;
+        return;
+    }
+    if (type == T_MCALL || type == T_MCAP){
+        mc.imr_interface=apAddr;
+        int res=setsockopt(fd,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mc,sizeof(mc));
+        if (res != 0){
+            LOG_ERROR("UDPR: unable to add MC membership for AP:%d",errno);
+        }
+        else{
+            LOG_INFO("UDPR: membership for %s for AP",mcAddr.c_str());
+        }
+    }
+    if (!currentStationIp.isEmpty() && (type == T_MCALL || type == T_MCSTA))
+    {
+        mc.imr_interface = staAddr;
+        int res = setsockopt(fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mc, sizeof(mc));
+        if (res != 0)
+        {
+            LOG_ERROR("UDPR: unable to add MC membership for STA:%d", errno);
+        }
+        else{
+            LOG_INFO("UDPR: membership for %s for STA %s",mcAddr.c_str(),currentStationIp.c_str());
+        }
+    }
+}
+
+void GwUdpReader::begin()
+{
+    if (type != T_UNKNOWN) return; //already started
+    type=(UType)(config->getInt(GwConfigDefinitions::udprType));
+    LOG_INFO("UDPR begin, mode=%d",(int)type);
+    port=config->getInt(GwConfigDefinitions::udprPort);
+    listenA.sin_family=AF_INET;
+    listenA.sin_port=htons(port);
+    if (type != T_STA){
+        listenA.sin_addr.s_addr=htonl(INADDR_ANY);
+    }
+    String ap=WiFi.softAPIP().toString();
+    if (inet_pton(AF_INET, ap.c_str(), &apAddr) != 1)
+    {
+        LOG_ERROR("UDPR: invalid ap ip address %s", ap.c_str());
+        return;
+    }
+    String sta;
+    if (WiFi.isConnected()) sta=WiFi.localIP().toString();
+    setStationAdd(sta);
+    createAndBind();
+}
+
+bool GwUdpReader::setStationAdd(const String &sta){
+    if (sta == currentStationIp) return false;
+    currentStationIp=sta;
+    if (inet_pton(AF_INET, currentStationIp.c_str(), &staAddr) != 1){
+            LOG_ERROR("UDPR: invalid station ip address %s", currentStationIp.c_str());
+            return false;
+    }
+    LOG_INFO("UDPR: new station IP %s",currentStationIp.c_str());
+    return true;
+}
+void GwUdpReader::loop(bool handleRead, bool handleWrite)
+{
+    if (handleRead){
+        if (type == T_STA || type == T_MCALL || type == T_MCSTA){
+            //only change anything if we considered the station IP
+            String nextStationIp;
+            if (WiFi.isConnected()){
+                String nextStationIp=WiFi.localIP().toString();
+            }
+            if (setStationAdd(nextStationIp)){
+                LOG_INFO("UDPR: wifi client IP changed, restart");
+                createAndBind();
+            }
+        }
+    }
+    
+}
+
+void GwUdpReader::readMessages(GwMessageFetcher *writer)
+{
+    if (fd < 0) return;
+    //we expect one NMEA message in one UDP packet
+    buffer->reset();
+    struct sockaddr_in from;
+    socklen_t fromLen=sizeof(from);
+    ssize_t res=recvfrom(fd,buffer->getWp(),buffer->continousSpace(),MSG_DONTWAIT,
+        (struct sockaddr*)&from,&fromLen);
+    if (res <= 0) return;
+    if (GwSocketHelper::equals(from.sin_addr,apAddr)) return;
+    if (!currentStationIp.isEmpty() && (GwSocketHelper::equals(from.sin_addr,staAddr))) return;
+    buffer->moveWp(res);
+    LOG_DEBUG(GwLog::DEBUG,"UDPR: received %d bytes",res);
+    writer->handleBuffer(buffer);   
+}
+size_t GwUdpReader::sendToClients(const char *buf, int source,bool partial)
+{ 
+    return 0;
+}
+
+
+GwUdpReader::~GwUdpReader()
+{
+}