Bidirectional udp

src/Driver.cpp

@@ -203,7 +203,7 @@ void Driver::openURI(std::string const& uri)
             extended_info = boost::lexical_cast<int>(device.substr(second_marker + 1));
             device = device.substr(0, second_marker);
 
-            return openUDP(device, extended_info);
+            return openUDPBidirectional(device, extended_info, additional_info);
         }
 
         return openUDP(device, additional_info);
@@ -259,6 +259,73 @@ void Driver::openIPServer(int port, addrinfo const& hints)
     setMainStream(new UDPServerStream(sfd,true));
 }
 
+void find_addr(const char *hostname, const char *port, addrinfo const& hints, struct sockaddr *addr, size_t *addr_len)
+{
+    struct addrinfo *result;
+    int ret = getaddrinfo(hostname, port, &hints, &result);
+    if (ret != 0)
+        throw UnixError("cannot resolve client port " + string(port));
+
+    int sfd = -1;
+    struct addrinfo *rp;
+    for (rp = result; rp != NULL; rp = rp->ai_next) {
+        sfd = socket(rp->ai_family, rp->ai_socktype,
+                rp->ai_protocol);
+        if (sfd == -1)
+            continue;
+
+        if (connect(sfd, rp->ai_addr, rp->ai_addrlen) == 0)
+            break;                  /* Success */
+
+        ::close(sfd);
+    }
+
+    if (rp == NULL)
+    {
+        freeaddrinfo(result);
+        throw UnixError("cannot open client socket on port " + string(port));
+    }
+
+    memcpy(addr, rp->ai_addr, rp->ai_addrlen);
+    *addr_len = rp->ai_addrlen;
+
+    freeaddrinfo(result);
+    close(sfd);
+}
+
+void Driver::openIPBidirectional(std::string const& hostname, int out_port, addrinfo const& out_hints, int in_port, addrinfo const& in_hints)
+{
+    struct addrinfo *result;
+    string in_port_as_string = boost::lexical_cast<string>(in_port);
+    int ret = getaddrinfo(NULL, in_port_as_string.c_str(), &in_hints, &result);
+    if (ret != 0)
+        throw UnixError("cannot resolve server port " + in_port_as_string);
+
+    int sfd = -1;
+    struct addrinfo *rp;
+    for (rp = result; rp != NULL; rp = rp->ai_next) {
+        sfd = socket(rp->ai_family, rp->ai_socktype,
+                rp->ai_protocol);
+        if (sfd == -1)
+            continue;
+
+        if (::bind(sfd, rp->ai_addr, rp->ai_addrlen) == 0)
+            break;                  /* Success */
+
+        ::close(sfd);
+    }
+    freeaddrinfo(result);
+
+    if (rp == NULL)
+        throw UnixError("cannot open server socket on port " + in_port_as_string);
+
+    struct sockaddr peer;
+    size_t peer_len;
+
+    find_addr(hostname.c_str(), boost::lexical_cast<string>(out_port).c_str(), out_hints, &peer, &peer_len);
+    setMainStream(new UDPServerStream(sfd, true, &peer, &peer_len));
+}
+
 void Driver::openIPClient(std::string const& hostname, int port, addrinfo const& hints)
 {
     struct addrinfo *result;
@@ -328,6 +395,22 @@ void Driver::openUDP(std::string const& hostname, int port)
     }
 }
 
+void Driver::openUDPBidirectional(std::string const& hostname, int out_port, int in_port)
+{
+    struct addrinfo in_hints;
+    memset(&in_hints, 0, sizeof(struct addrinfo));
+    in_hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
+    in_hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+    in_hints.ai_flags = AI_PASSIVE;    /* For wildcard IP address */
+
+    struct addrinfo out_hints;
+    memset(&out_hints, 0, sizeof(struct addrinfo));
+    out_hints.ai_family = AF_UNSPEC;    /* Allow IPv4 or IPv6 */
+    out_hints.ai_socktype = SOCK_DGRAM; /* Datagram socket */
+
+    openIPBidirectional(hostname, out_port, out_hints, in_port, in_hints);
+}
+
 int Driver::openSerialIO(std::string const& port, int baud_rate)
 {
     int fd = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_SYNC | O_NONBLOCK );

src/Driver.hpp

@@ -157,6 +157,7 @@ class Driver
 
     void openIPServer(int port, addrinfo const& hints);
     void openIPClient(std::string const& hostname, int port, addrinfo const& hints);
+    void openIPBidirectional(std::string const& hostname, int out_port, addrinfo const& out_hints, int in_port, addrinfo const& in_hints);
 
 public:
     /** Creates an Driver class for a packet-based protocol
@@ -253,6 +254,8 @@ class Driver
     * The read_port port can be 0 if the local port does not need to be fixed.
     */
     void openUDP(std::string const& hostname, int remote_port);
+    void openUDPBidirectional(std::string const& hostname, int out_port, int in_port);
+
 
     /** Opens a serial port and sets it up to a sane configuration.  Use
      * then setSerialBaudrate() to change the actual baudrate of the

src/IOStream.cpp

@@ -99,11 +99,26 @@ UDPServerStream::UDPServerStream(int fd, bool auto_close)
   : FDStream(fd,auto_close)
 {
   m_s_len = sizeof(m_si_other);
+  m_si_other_dynamic = true;
 }
-
+
+UDPServerStream::UDPServerStream(int fd, bool auto_close, struct sockaddr *si_other, size_t *s_len)
+  : FDStream(fd,auto_close)
+{
+    m_si_other = *si_other;
+    m_s_len = *s_len;
+    m_si_other_dynamic = false;
+}
+
 size_t UDPServerStream::read(uint8_t* buffer, size_t buffer_size)
 {
-  ssize_t ret = recvfrom(m_fd, buffer, buffer_size, 0, &m_si_other, &m_s_len);
+  ssize_t ret;
+
+  if (m_si_other_dynamic)
+    ret = recvfrom(m_fd, buffer, buffer_size, 0, &m_si_other, &m_s_len);
+  else
+    ret = recvfrom(m_fd, buffer, buffer_size, 0, NULL, NULL);
+
   if (ret >= 0){
      return ret;
   }

src/IOStream.hpp

@@ -65,10 +65,12 @@ namespace iodrivers_base
     {
     public: 
       UDPServerStream(int fd, bool auto_close);
+      UDPServerStream(int fd, bool auto_close, struct sockaddr *si_other, size_t *s_len);
       virtual size_t read(uint8_t* buffer, size_t buffer_size);
       virtual size_t write(uint8_t const* buffer, size_t buffer_size);    
     protected:
       struct sockaddr m_si_other;
+      bool m_si_other_dynamic;
       unsigned int m_s_len;
     };
 }

test/test.cpp

@@ -334,13 +334,15 @@ BOOST_AUTO_TEST_CASE(test_recv_from_bidirectional_udp)
 {
     DriverTest test;
     uint8_t buffer[100];
+    int count;
     boost::thread send_thread;
 
     BOOST_REQUIRE_NO_THROW(test.openURI("udp://127.0.0.1:3135:4145"));
 
     send_thread = boost::thread(recv_test);
 
-    BOOST_REQUIRE_NO_THROW(test.readPacket(buffer, 100, 200));
+    BOOST_REQUIRE_NO_THROW((count = test.readPacket(buffer, 100, 200)));
+    BOOST_REQUIRE_EQUAL(count, 4);
 
     test.close();
 }
@@ -349,17 +351,19 @@ void send_test(bool *got)
 {
     DriverTest test;
     uint8_t buffer[100];
+    int count = 0;
 
     test.openURI("udpserver://5155");
 
     try {
-        test.readPacket(buffer, 100, 200);
+        count = test.readPacket(buffer, 100, 200);
     }
     catch (...) {
         *got = false;
     }
 
-    *got = true;
+    *got = (count == 4);
+    cout << "Got: " << count << endl;
     test.close();
 }