WiFi UDP & Multicast Functionality

libraries/WiFi/src/WiFiUdp.cpp

@@ -7,10 +7,14 @@ extern WiFiClass WiFi;
 #endif
 
 arduino::WiFiUDP::WiFiUDP() {
-    _packet_buffer = (uint8_t*)malloc(WIFI_UDP_BUFFER_SIZE);
+    _packet_buffer = new uint8_t[WIFI_UDP_BUFFER_SIZE];
     _current_packet = NULL;
     _current_packet_size = 0;
-    // if this malloc fails then ::begin will fail
+    // if this allocation fails then ::begin will fail
+}
+
+arduino::WiFiUDP::~WiFiUDP() {
+    delete[] _packet_buffer;
 }
 
 uint8_t arduino::WiFiUDP::begin(uint16_t port) {
@@ -21,6 +25,10 @@ uint8_t arduino::WiFiUDP::begin(uint16_t port) {
         return 0;
     }
 
+    if (_socket.bind(port) < 0) {
+        return 0; //Failed to bind UDP Socket to port
+    }
+
     if (!_packet_buffer) {
         return 0;
     }
@@ -31,56 +39,79 @@ uint8_t arduino::WiFiUDP::begin(uint16_t port) {
     return 1;
 }
 
-void arduino::WiFiUDP::stop() {
-    _socket.close();
-}
+uint8_t arduino::WiFiUDP::beginMulticast(IPAddress ip, uint16_t port) {
+    // success = 1, fail = 0
+    if(begin(port) != 1){
+        return 0;
+    }
 
-// we should get the octets out of the IPAddress... there's nothing for it right now
-// int arduino::WiFiUDP::beginPacket(IPAddress ip, uint16_t port) {
-// }
+    nsapi_addr_t multicastGroup = {NSAPI_IPv4, {ip[0], ip[1], ip[2], ip[3]}};       
 
-int arduino::WiFiUDP::beginPacket(const char *host, uint16_t port) {
-    _host = host;
-    _port = port;
+    if (_socket.join_multicast_group(SocketAddress(multicastGroup)) != NSAPI_ERROR_OK) {
+        printf("Error joining the multicast group\n");
+        return 0;
+    }
 
     return 1;
 }
 
+void arduino::WiFiUDP::stop() {
+    _socket.close();    
+}
+
+int arduino::WiFiUDP::beginPacket(IPAddress ip, uint16_t port) {
+    nsapi_addr_t convertedIP = {NSAPI_IPv4, {ip[0], ip[1], ip[2], ip[3]}};   
+    _host = SocketAddress(convertedIP, port);
+    //If IP is null and port is 0 the initialization failed
+    return (_host.get_ip_address() == nullptr && _host.get_port() == 0) ? 0 : 1;
+}
+
+int arduino::WiFiUDP::beginPacket(const char *host, uint16_t port) {     
+    _host = SocketAddress(host, port);
+    //If IP is null and port is 0 the initialization failed
+    return (_host.get_ip_address() == nullptr && _host.get_port() == 0) ? 0 : 1;
+}
+
 int arduino::WiFiUDP::endPacket() {
     return 1;
 }
 
 // Write a single byte into the packet
 size_t arduino::WiFiUDP::write(uint8_t byte) {
-    uint8_t buffer[1] = { byte };
-    SocketAddress addr(_host, _port);
-    return _socket.sendto(addr, buffer, 1);
+    uint8_t buffer[1] = { byte };    
+    return _socket.sendto(_host, buffer, 1);
 }
 
 // Write size bytes from buffer into the packet
-size_t arduino::WiFiUDP::write(const uint8_t *buffer, size_t size) {
-    SocketAddress addr(_host, _port);
-    return _socket.sendto(addr, buffer, size);
+size_t arduino::WiFiUDP::write(const uint8_t *buffer, size_t size) {    
+    return _socket.sendto(_host, buffer, size);
 }
 
 int arduino::WiFiUDP::parsePacket() {
-    nsapi_size_or_error_t ret = _socket.recvfrom(NULL, _packet_buffer, WIFI_UDP_BUFFER_SIZE);
+    nsapi_size_or_error_t ret = _socket.recvfrom(&_remoteHost, _packet_buffer, WIFI_UDP_BUFFER_SIZE);
 
     if (ret == NSAPI_ERROR_WOULD_BLOCK) {
         // no data
         return 0;
+    } else if(ret == NSAPI_ERROR_NO_SOCKET){
+        // socket was not created correctly.
+        return -1;
     }
     // error codes below zero are errors
     else if (ret <= 0) {
         // something else went wrong, need some tracing info...
-        return 0;
+        return -1;
     }
 
     // set current packet states
     _current_packet = _packet_buffer;
     _current_packet_size = ret;
 
-    return 1;
+    return _current_packet_size;
+}
+
+int arduino::WiFiUDP::available() {
+    return _current_packet_size;
 }
 
 // Read a single byte from the current packet
@@ -96,7 +127,7 @@ int arduino::WiFiUDP::read() {
     // check for overflow
     if (_current_packet > _packet_buffer + _current_packet_size) {
         // try reading the next packet...
-        if (parsePacket() == 1) {
+        if (parsePacket() > 0) {
             // if so, read first byte of next packet;
             return read();
         }
@@ -131,7 +162,7 @@ int arduino::WiFiUDP::read(unsigned char* buffer, size_t len) {
     // at the end of the packet?
     if (max_bytes == 0) {
         // try read next packet...
-        if (parsePacket() == 1) {
+        if (parsePacket() > 0) {
             return read(buffer, len);
         }
         else {
@@ -147,4 +178,25 @@ int arduino::WiFiUDP::read(unsigned char* buffer, size_t len) {
     _current_packet += len;
 
     return len;
+}
+
+IPAddress arduino::WiFiUDP::remoteIP() {
+    nsapi_addr_t address = _remoteHost.get_addr();
+    return IPAddress(address.bytes[0], address.bytes[1], address.bytes[2], address.bytes[3]);
+}
+
+uint16_t arduino::WiFiUDP::remotePort() {    
+    return _remoteHost.get_port();
+}
+
+void arduino::WiFiUDP::flush(){
+    // TODO: a real check to ensure transmission has been completed
+}
+
+int arduino::WiFiUDP::peek(){
+  if (_current_packet_size < 1){
+    return -1;
+  }
+
+  return _current_packet[0];
 }

libraries/WiFi/src/WiFiUdp.h

@@ -33,8 +33,8 @@ namespace arduino {
 class WiFiUDP : public UDP {
 private:
   UDPSocket _socket;  // Mbed OS socket
-  const char *_host;  // Host to be used to send data (todo: switch to SocketAddress)
-  uint16_t _port;     // Port to be used to send data (^)
+  SocketAddress _host;  // Host to be used to send data  
+  SocketAddress _remoteHost; // Remote host that sent incoming packets
 
   uint8_t* _packet_buffer;  // Raw packet buffer (contains data we got from the UDPSocket)
 
@@ -45,15 +45,16 @@ class WiFiUDP : public UDP {
 
 public:
   WiFiUDP();  // Constructor
+  ~WiFiUDP();
   virtual uint8_t begin(uint16_t);	// initialize, start listening on specified port. Returns 1 if successful, 0 if there are no sockets available to use
-  // virtual uint8_t beginMulticast(IPAddress, uint16_t);  // initialize, start listening on specified multicast IP address and port. Returns 1 if successful, 0 if there are no sockets available to use
+  virtual uint8_t beginMulticast(IPAddress, uint16_t);  // initialize, start listening on specified multicast IP address and port. Returns 1 if successful, 0 if there are no sockets available to use
   virtual void stop();  // Finish with the UDP socket
 
   // Sending UDP packets
 
   // Start building up a packet to send to the remote host specific in ip and port
   // Returns 1 if successful, 0 if there was a problem with the supplied IP address or port
-  // virtual int beginPacket(IPAddress ip, uint16_t port);
+  virtual int beginPacket(IPAddress ip, uint16_t port);
   // Start building up a packet to send to the remote host specific in host and port
   // Returns 1 if successful, 0 if there was a problem resolving the hostname or port
   virtual int beginPacket(const char *host, uint16_t port);
@@ -71,7 +72,7 @@ class WiFiUDP : public UDP {
   // Returns the size of the packet in bytes, or 0 if no packets are available
   virtual int parsePacket();
   // Number of bytes remaining in the current packet
-  // virtual int available();
+  virtual int available();
   // Read a single byte from the current packet
   virtual int read();
   // Read up to len bytes from the current packet and place them into buffer
@@ -81,13 +82,13 @@ class WiFiUDP : public UDP {
   // Returns the number of characters read, or 0 if none are available
   virtual int read(char* buffer, size_t len) { return read((unsigned char*)buffer, len); };
   // Return the next byte from the current packet without moving on to the next byte
-  // virtual int peek();
-  // virtual void flush();	// Finish reading the current packet
+  virtual int peek();
+  virtual void flush();	// Finish reading the current packet
 
   // Return the IP address of the host who sent the current incoming packet
-  // virtual IPAddress remoteIP();
+  virtual IPAddress remoteIP();
   // // Return the port of the host who sent the current incoming packet
-  // virtual uint16_t remotePort();
+  virtual uint16_t remotePort();
 };
 
 }