diff --git a/include/ur_modern_driver/ur/stream.h b/include/ur_modern_driver/ur/stream.h new file mode 100644 index 0000000..88a5ed6 --- /dev/null +++ b/include/ur_modern_driver/ur/stream.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include +#include +#include +#include + +/// Encapsulates a TCP socket +class URStream { +private: + int _socket_fd = -1; + std::string _host; + int _port; + + std::atomic _initialized; + std::atomic _stopping; + +public: + URStream(std::string &host, int port) + : _host(host), + _port(port), + _initialized(false), + _stopping(false) {} + + bool connect(); + void disconnect(); + + ssize_t send(uint8_t *buf, size_t buf_len); + ssize_t receive(uint8_t *buf, size_t buf_len); +}; \ No newline at end of file diff --git a/src/ur/stream.cpp b/src/ur/stream.cpp new file mode 100644 index 0000000..a7c53a8 --- /dev/null +++ b/src/ur/stream.cpp @@ -0,0 +1,93 @@ +#include +#include +#include + +#include "ur_modern_driver/ur/stream.h" +#include "ur_modern_driver/log.h" + +bool URStream::connect() { + if(_initialized) + return false; + + LOG_INFO("Connecting to UR @ %s:%d\n", _host.c_str(), _port); + + //gethostbyname() is deprecated so use getadderinfo() as described in: + //http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo + + std::string service = std::to_string(_port); + struct addrinfo hints, *result; + std::memset(&hints, 0, sizeof(hints)); + + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_PASSIVE; + + if(getaddrinfo(_host.c_str(), service.c_str(), &hints, &result) != 0) { + LOG_ERROR("Failed to get host name\n"); + return false; + } + + //loop through the list of addresses untill we find one that's connectable + for(struct addrinfo *p = result; p != nullptr; p = p->ai_next) { + _socket_fd = socket(p->ai_family, p->ai_socktype, p->ai_protocol); + + if(_socket_fd == -1) //socket error? + continue; + + if(::connect(_socket_fd, p->ai_addr, p->ai_addrlen) != 0) { + if(_stopping) + break; + else + continue; //try next addrinfo if connect fails + } + + //disable Nagle's algorithm to ensure we sent packets as fast as possible + int flag = 1; + setsockopt(_socket_fd, IPPROTO_TCP, TCP_NODELAY, &flag, sizeof(flag)); + _initialized = true; + LOG_INFO("Connection successfully established\n"); + break; + } + + freeaddrinfo(result); + if(!_initialized) + LOG_ERROR("Connection failed\n"); + + return _initialized; +} + +void URStream::disconnect() { + if(!_initialized || _stopping) + return; + + _stopping = true; + close(_socket_fd); + _initialized = false; +} + +ssize_t URStream::send(uint8_t *buf, size_t buf_len) { + if(!_initialized) + return -1; + if(_stopping) + return 0; + + size_t total = 0; + size_t remaining = buf_len; + + //TODO: handle reconnect? + //handle partial sends + while(total < buf_len) { + ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0); + if(sent == -1) + return _stopping ? 0 : -1; + total += sent; + remaining -= sent; + } + + return total; +} + +ssize_t URStream::receive(uint8_t *buf, size_t buf_len) { + //TODO: handle reconnect? + return recv(_socket_fd, buf, buf_len, 0); +} \ No newline at end of file