starting fsfw

bsp_z7/objects/communication/ServoCommInterface.cpp

@@ -0,0 +1,138 @@
+#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;
+}