native_ethernet_udp.cpp

Go to the documentation of this file.

 
#include <net/ethernet_udp.h>
#include <IPAddress.h>
#include "native_ethernet_udp.h"
 
namespace net {
 
EthernetUDP::EthernetUDP() : hal_(std::make_unique<NativeEthernetUDP>()) {
 
}
 
NativeEthernetUDP::NativeEthernetUDP() :
  _socket(INVALID_SOCKET),
  _localPort(0),
  _sendBuffer(nullptr),
  _sendBufferSize(1024),
  _sendBufferPos(0),
  _recvBuffer(nullptr),
  _recvBufferSize(1024),
  _recvBufferPos(0),
  _recvDataSize(0),
  _hasData(false)
{
#ifdef _WIN32
  if (!_wsaInitialized) {
      WSADATA wsaData;
      if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
          std::cerr << "WSAStartup failed" << std::endl;
          return;
      }
      _wsaInitialized = true;
  }
#endif
  _sendBuffer = new uint8_t[_sendBufferSize];
  _recvBuffer = new uint8_t[_recvBufferSize];
  memset(&_localAddr, 0, sizeof(_localAddr));
  memset(&_remoteAddr, 0, sizeof(_remoteAddr));
}
 
NativeEthernetUDP::~NativeEthernetUDP() {
  stop();
  delete[] _sendBuffer;
  delete[] _recvBuffer;
}
 
uint8_t NativeEthernetUDP::begin(uint16_t port) {
  if (_socket != INVALID_SOCKET) {
      closesocket(_socket);
  }
 
  _socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (_socket == INVALID_SOCKET) {
      return 0;
  }
 
  // Set socket to non-blocking mode
#ifdef _WIN32
  u_long mode = 1;
  if (ioctlsocket(_socket, FIONBIO, &mode) != 0) {
      closesocket(_socket);
      _socket = INVALID_SOCKET;
      return 0;
  }
#else
  int flags = fcntl(_socket, F_GETFL, 0);
  if (flags == -1 || fcntl(_socket, F_SETFL, flags | O_NONBLOCK) == -1) {
      closesocket(_socket);
      _socket = INVALID_SOCKET;
      return 0;
  }
#endif
 
  _localAddr.sin_family = AF_INET;
  _localAddr.sin_addr.s_addr = INADDR_ANY;
  _localAddr.sin_port = htons(port);
 
  if (bind(_socket, (struct sockaddr*)&_localAddr, sizeof(_localAddr)) == SOCKET_ERROR) {
      closesocket(_socket);
      _socket = INVALID_SOCKET;
      return 0;
  }
 
  _localPort = port;
  return 1;
}
 
uint8_t NativeEthernetUDP::beginMulticast(IPAddress ip, uint16_t port) {
  if (!begin(port)) {
      return 0;
  }
 
  // Join multicast group
  struct ip_mreq mreq;
  mreq.imr_multiaddr.s_addr = ip;
  mreq.imr_interface.s_addr = INADDR_ANY;
 
  if (setsockopt(_socket, IPPROTO_IP, IP_ADD_MEMBERSHIP,
                (const char*)&mreq, sizeof(mreq)) == SOCKET_ERROR) {
      stop();
      return 0;
  }
 
  return 1;
}
 
void NativeEthernetUDP::stop() {
  if (_socket != INVALID_SOCKET) {
      closesocket(_socket);
      _socket = INVALID_SOCKET;
  }
  _hasData = false;
  _recvDataSize = 0;
  _recvBufferPos = 0;
  _sendBufferPos = 0;
}
 
int NativeEthernetUDP::beginPacket(IPAddress ip, uint16_t port) {
  _remoteAddr.sin_family = AF_INET;
  _remoteAddr.sin_addr.s_addr = ip;
  _remoteAddr.sin_port = htons(port);
  _sendBufferPos = 0;
  return 1;
}
 
int NativeEthernetUDP::beginPacket(const char* host, uint16_t port) {
  struct hostent* he = gethostbyname(host);
  if (he == nullptr) {
      return 0;
  }
 
  IPAddress ip = *((uint32_t*)he->h_addr_list[0]);
  return beginPacket(ip, port);
}
 
int NativeEthernetUDP::endPacket() {
  if (_socket == INVALID_SOCKET || _sendBufferPos == 0) {
      return 0;
  }
 
  int result = sendto(_socket, (const char*)_sendBuffer, _sendBufferPos,
                     0, (struct sockaddr*)&_remoteAddr, sizeof(_remoteAddr));
 
  _sendBufferPos = 0;
  return (result == SOCKET_ERROR) ? 0 : 1;
}
 
size_t NativeEthernetUDP::write(uint8_t byte) {
  return write(&byte, 1);
}
 
size_t NativeEthernetUDP::write(const uint8_t* buffer, size_t size) {
  if (_sendBufferPos + size > _sendBufferSize) {
      // Resize buffer if needed
      size_t newSize = _sendBufferSize * 2;
      while (newSize < _sendBufferPos + size) {
          newSize *= 2;
      }
      uint8_t* newBuffer = new uint8_t[newSize];
      memcpy(newBuffer, _sendBuffer, _sendBufferPos);
      delete[] _sendBuffer;
      _sendBuffer = newBuffer;
      _sendBufferSize = newSize;
  }
 
  memcpy(_sendBuffer + _sendBufferPos, buffer, size);
  _sendBufferPos += size;
  return size;
}
 
int NativeEthernetUDP::parsePacket() {
  if (_socket == INVALID_SOCKET) {
      return 0;
  }
 
  socklen_t addrLen = sizeof(_remoteAddr);
  int result = recvfrom(_socket, (char*)_recvBuffer, _recvBufferSize,
                       0, (struct sockaddr*)&_remoteAddr, &addrLen);
 
  if (result == SOCKET_ERROR) {
#ifdef _WIN32
      int error = WSAGetLastError();
      if (error == WSAEWOULDBLOCK) {
          return 0;  // No data available
      }
#else
      if (errno == EAGAIN || errno == EWOULDBLOCK) {
          return 0;  // No data available
      }
#endif
      return 0;  // Error
  }
 
  _recvDataSize = result;
  _recvBufferPos = 0;
  _hasData = true;
  return result;
}
 
int NativeEthernetUDP::available() {
  if (!_hasData) {
    return 0;
  }
  return _recvDataSize - _recvBufferPos;
}
 
int NativeEthernetUDP::read() {
  if (!_hasData || _recvBufferPos >= _recvDataSize) {
    return -1;
  }
  return _recvBuffer[_recvBufferPos++];
}
 
int NativeEthernetUDP::read(unsigned char* buffer, size_t len) {
  return read((char*)buffer, len);
}
 
int NativeEthernetUDP::read(char* buffer, size_t len) {
  if (!_hasData) {
    return 0;
  }
 
  size_t available_bytes = _recvDataSize - _recvBufferPos;
  size_t bytes_to_read = (len < available_bytes) ? len : available_bytes;
 
  memcpy(buffer, _recvBuffer + _recvBufferPos, bytes_to_read);
  _recvBufferPos += bytes_to_read;
 
  return bytes_to_read;
}
 
int NativeEthernetUDP::peek() {
  if (!_hasData || _recvBufferPos >= _recvDataSize) {
      return -1;
  }
  return _recvBuffer[_recvBufferPos];
}
 
void NativeEthernetUDP::flush() {
  _hasData = false;
  _recvDataSize = 0;
  _recvBufferPos = 0;
}
 
IPAddress NativeEthernetUDP::remoteIP() {
  return _remoteAddr.sin_addr.s_addr;
}
 
uint16_t NativeEthernetUDP::remotePort() {
  return ntohs(_remoteAddr.sin_port);
}
 
}