forked from ROMEO/obsw
138 lines
4.2 KiB
C++
138 lines
4.2 KiB
C++
#include "ServoCommInterface.h"
|
|
|
|
#include <arpa/inet.h>
|
|
#include <errno.h>
|
|
#include <fcntl.h>
|
|
#include <netinet/in.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/types.h> // POSIX.1-2001 does not require the inclusion of <sys/types.h>, and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it.
|
|
#include <unistd.h>
|
|
#include <utility>
|
|
|
|
#include "SerialTCPCookie.h"
|
|
|
|
ServoCommInterface::ServoCommInterface(object_id_t setObjectId) : SystemObject(setObjectId) {}
|
|
|
|
ReturnValue_t ServoCommInterface::initializeInterface(CookieIF *cookie) {
|
|
SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
|
|
if (tcpCookie != nullptr) {
|
|
return initializeInterface(tcpCookie);
|
|
}
|
|
return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::sendMessage(CookieIF *cookie, const uint8_t *sendData,
|
|
size_t sendLen) {
|
|
SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
|
|
if (tcpCookie != nullptr) {
|
|
return sendMessage(tcpCookie, sendData, sendLen);
|
|
}
|
|
return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::sendMessage(SerialTCPCookie *cookie, const uint8_t *sendData,
|
|
size_t sendLen) {
|
|
if (cookie->socket == -1) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
ssize_t result = send(cookie->socket, sendData, sendLen, 0);
|
|
|
|
if (result < 0){
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
if (std::cmp_not_equal(result, sendLen)) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::getSendSuccess(CookieIF *cookie) { return returnvalue::OK; }
|
|
|
|
ReturnValue_t ServoCommInterface::requestReceiveMessage(CookieIF *cookie, size_t requestLen) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
|
|
size_t *size) {
|
|
SerialTCPCookie *tcpCookie = dynamic_cast<SerialTCPCookie *>(cookie);
|
|
if (tcpCookie != nullptr) {
|
|
return readReceivedMessage(tcpCookie, buffer, size);
|
|
}
|
|
return DeviceCommunicationIF::INVALID_COOKIE_TYPE;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::readReceivedMessage(SerialTCPCookie *cookie, uint8_t **buffer,
|
|
size_t *size) {
|
|
if (cookie->socket == -1) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
ssize_t result = recv(cookie->socket, cookie->buffer, cookie->bufferSize, 0);
|
|
|
|
if (result <= -1) {
|
|
*size = 0;
|
|
if (errno == EAGAIN or errno == EWOULDBLOCK) {
|
|
//no data
|
|
return returnvalue::OK;
|
|
}
|
|
// TODO wrap errno
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
if (result == 0) {
|
|
// peer shut down
|
|
close(cookie->socket);
|
|
cookie->socket = -1;
|
|
return returnvalue::FAILED; // TODO connection closed
|
|
}
|
|
|
|
*size = result;
|
|
*buffer = cookie->buffer;
|
|
|
|
|
|
if (std::cmp_greater_equal(result, cookie->bufferSize) and cookie->flush) {
|
|
// we do not know if there is more data in the socket, so we read until we get no more
|
|
uint8_t ignore[10];
|
|
while (recv(cookie->socket, ignore, sizeof(ignore), 0) > 0) {
|
|
}
|
|
//ignore any fault here as previous read was ok. next call will catch it
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t ServoCommInterface::initializeInterface(SerialTCPCookie *cookie) {
|
|
cookie->socket = socket(AF_INET6, SOCK_STREAM, 0);
|
|
if (cookie->socket == -1) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
sockaddr_in6 serverAddr;
|
|
serverAddr.sin6_family = AF_INET6;
|
|
serverAddr.sin6_port = htons(cookie->port);
|
|
|
|
int retval =inet_pton(AF_INET6, cookie->host, &serverAddr.sin6_addr);
|
|
if (retval == 0) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "IpCommInterface::initializeInterface: Invalid IP!" << std::endl;
|
|
#endif
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
retval = connect(cookie->socket, (struct sockaddr *)&serverAddr, sizeof(serverAddr));
|
|
|
|
if (retval == -1) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "IpCommInterface::initializeInterface: connection failed with " << errno
|
|
<< std::endl;
|
|
#endif
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
fcntl(cookie->socket, F_SETFL, O_NONBLOCK);
|
|
|
|
return returnvalue::OK;
|
|
} |