eive/fsfw
eive/fsfw
15
1
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests Releases 1 Wiki Activity

Merge pull request 'new tmtc unix udp bridge' (#141) from KSat/fsfw:mueller/feature/TmTcUnixBridge into master

Browse Source 
Reviewed-on: fsfw/fsfw#141
This commit is contained in:
taheran 2020-09-08 14:14:00 +02:00
commit fb36c55419
6 changed files with 523 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

191
globalfunctions/timevalOperations.cpp
View File

@ -1,92 +1,99 @@
#include "timevalOperations.h" #include "timevalOperations.h"
timeval& operator+=(timeval& lhs, const timeval& rhs) { timeval& operator+=(timeval& lhs, const timeval& rhs) {
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
sum += rhs.tv_sec * 1000000. + rhs.tv_usec; sum += rhs.tv_sec * 1000000. + rhs.tv_usec;
lhs.tv_sec = sum / 1000000; lhs.tv_sec = sum / 1000000;
lhs.tv_usec = sum - lhs.tv_sec * 1000000; lhs.tv_usec = sum - lhs.tv_sec * 1000000;
return lhs; return lhs;
} }
timeval operator+(timeval lhs, const timeval& rhs) { timeval operator+(timeval lhs, const timeval& rhs) {
lhs += rhs; lhs += rhs;
return lhs; return lhs;
} }
timeval& operator-=(timeval& lhs, const timeval& rhs) { timeval& operator-=(timeval& lhs, const timeval& rhs) {
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; sum -= rhs.tv_sec * 1000000. + rhs.tv_usec;
lhs.tv_sec = sum / 1000000; lhs.tv_sec = sum / 1000000;
lhs.tv_usec = sum - lhs.tv_sec * 1000000; lhs.tv_usec = sum - lhs.tv_sec * 1000000;
return lhs; return lhs;
} }
timeval operator-(timeval lhs, const timeval& rhs) { timeval operator-(timeval lhs, const timeval& rhs) {
lhs -= rhs; lhs -= rhs;
return lhs; return lhs;
} }
double operator/(const timeval& lhs, const timeval& rhs) { double operator/(const timeval& lhs, const timeval& rhs) {
double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
return lhs64 / rhs64; return lhs64 / rhs64;
} }
timeval& operator/=(timeval& lhs, double scalar) { timeval& operator/=(timeval& lhs, double scalar) {
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
product /= scalar; product /= scalar;
lhs.tv_sec = product / 1000000; lhs.tv_sec = product / 1000000;
lhs.tv_usec = product - lhs.tv_sec * 1000000; lhs.tv_usec = product - lhs.tv_sec * 1000000;
return lhs; return lhs;
} }
timeval operator/(timeval lhs, double scalar) { timeval operator/(timeval lhs, double scalar) {
lhs /= scalar; lhs /= scalar;
return lhs; return lhs;
} }
timeval& operator*=(timeval& lhs, double scalar) { timeval& operator*=(timeval& lhs, double scalar) {
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
product *= scalar; product *= scalar;
lhs.tv_sec = product / 1000000; lhs.tv_sec = product / 1000000;
lhs.tv_usec = product - lhs.tv_sec * 1000000; lhs.tv_usec = product - lhs.tv_sec * 1000000;
return lhs; return lhs;
} }
timeval operator*(timeval lhs, double scalar) { timeval operator*(timeval lhs, double scalar) {
lhs *= scalar; lhs *= scalar;
return lhs; return lhs;
} }
timeval operator*(double scalar, timeval rhs) { timeval operator*(double scalar, timeval rhs) {
rhs *= scalar; rhs *= scalar;
return rhs; return rhs;
} }
bool operator==(const timeval& lhs, const timeval& rhs) { bool operator==(const timeval& lhs, const timeval& rhs) {
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
return lhs64 == rhs64; return lhs64 == rhs64;
} }
bool operator!=(const timeval& lhs, const timeval& rhs) { bool operator!=(const timeval& lhs, const timeval& rhs) {
return !operator==(lhs, rhs); return !operator==(lhs, rhs);
} }
bool operator<(const timeval& lhs, const timeval& rhs) { bool operator<(const timeval& lhs, const timeval& rhs) {
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
return lhs64 < rhs64; return lhs64 < rhs64;
} }
bool operator>(const timeval& lhs, const timeval& rhs) { bool operator>(const timeval& lhs, const timeval& rhs) {
return operator<(rhs, lhs); return operator<(rhs, lhs);
} }
bool operator<=(const timeval& lhs, const timeval& rhs) { bool operator<=(const timeval& lhs, const timeval& rhs) {
return !operator>(lhs, rhs); return !operator>(lhs, rhs);
} }
bool operator>=(const timeval& lhs, const timeval& rhs) { bool operator>=(const timeval& lhs, const timeval& rhs) {
return !operator<(lhs, rhs); return !operator<(lhs, rhs);
} }
double timevalOperations::toDouble(const timeval timeval) { double timevalOperations::toDouble(const timeval timeval) {
double result = timeval.tv_sec * 1000000. + timeval.tv_usec; double result = timeval.tv_sec * 1000000. + timeval.tv_usec;
return result / 1000000.; return result / 1000000.;
} }
timeval timevalOperations::toTimeval(const double seconds) {
timeval tval;
tval.tv_sec = seconds;
tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6);
return tval;
}

1
globalfunctions/timevalOperations.h
View File

@ -41,6 +41,7 @@ namespace timevalOperations {
* @return seconds * @return seconds
*/ */
double toDouble(const timeval timeval); double toDouble(const timeval timeval);
timeval toTimeval(const double seconds);
} }
#endif /* TIMEVALOPERATIONS_H_ */ #endif /* TIMEVALOPERATIONS_H_ */

138
osal/linux/TcUnixUdpPollingTask.cpp Normal file
View File

@ -0,0 +1,138 @@
#include "TcUnixUdpPollingTask.h"
#include "../../globalfunctions/arrayprinter.h"
TcUnixUdpPollingTask::TcUnixUdpPollingTask(object_id_t objectId,
object_id_t tmtcUnixUdpBridge, size_t frameSize,
double timeoutSeconds): SystemObject(objectId),
tmtcBridgeId(tmtcUnixUdpBridge) {
if(frameSize > 0) {
this->frameSize = frameSize;
}
else {
this->frameSize = DEFAULT_MAX_FRAME_SIZE;
}
// Set up reception buffer with specified frame size.
// For now, it is assumed that only one frame is held in the buffer!
receptionBuffer.reserve(this->frameSize);
receptionBuffer.resize(this->frameSize);
if(timeoutSeconds == -1) {
receptionTimeout = DEFAULT_TIMEOUT;
}
else {
receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
}
}
TcUnixUdpPollingTask::~TcUnixUdpPollingTask() {}
ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
// Poll for new UDP datagrams in permanent loop.
while(1) {
//! Sender Address is cached here.
struct sockaddr_in senderAddress;
socklen_t senderSockLen = 0;
ssize_t bytesReceived = recvfrom(serverUdpSocket,
receptionBuffer.data(), frameSize, receptionFlags,
reinterpret_cast<sockaddr*>(&senderAddress), &senderSockLen);
if(bytesReceived < 0) {
// handle error
sif::error << "TcSocketPollingTask::performOperation: Reception"
"error." << std::endl;
handleReadError();
continue;
}
// sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived
// << " bytes received" << std::endl;
ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
if(result != HasReturnvaluesIF::RETURN_FAILED) {
}
tmtcBridge->registerCommConnect();
tmtcBridge->checkAndSetClientAddress(senderAddress);
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
store_address_t storeId;
ReturnValue_t result = tcStore->addData(&storeId,
receptionBuffer.data(), bytesRead);
// arrayprinter::print(receptionBuffer.data(), bytesRead);
if (result != HasReturnvaluesIF::RETURN_OK) {
sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data "
"storage failed" << std::endl;
sif::error << "Packet size: " << bytesRead << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
TmTcMessage message(storeId);
result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
if (result != HasReturnvaluesIF::RETURN_OK) {
sif::error << "Serial Polling: Sending message to queue failed"
<< std::endl;
tcStore->deleteData(storeId);
}
return result;
}
ReturnValue_t TcUnixUdpPollingTask::initialize() {
tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == nullptr) {
sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
<< std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
tmtcBridge = objectManager->get<TmTcUnixUdpBridge>(tmtcBridgeId);
if(tmtcBridge == nullptr) {
sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
" TMTC bridge object!" << std::endl;
return ObjectManagerIF::CHILD_INIT_FAILED;
}
serverUdpSocket = tmtcBridge->serverSocket;
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t TcUnixUdpPollingTask::initializeAfterTaskCreation() {
// Initialize the destination after task creation. This ensures
// that the destination will be set in the TMTC bridge.
targetTcDestination = tmtcBridge->getRequestQueue();
return HasReturnvaluesIF::RETURN_OK;
}
void TcUnixUdpPollingTask::setTimeout(double timeoutSeconds) {
timeval tval;
tval = timevalOperations::toTimeval(timeoutSeconds);
int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
&tval, sizeof(receptionTimeout));
if(result == -1) {
sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
"receive timeout failed with " << strerror(errno) << std::endl;
}
}
// TODO: sleep after error detection to prevent spam
void TcUnixUdpPollingTask::handleReadError() {
switch(errno) {
case(EAGAIN): {
// todo: When working in timeout mode, this will occur more often
// and is not an error.
sif::error << "TcUnixUdpPollingTask::handleReadError: Timeout."
<< std::endl;
break;
}
default: {
sif::error << "TcUnixUdpPollingTask::handleReadError: "
<< strerror(errno) << std::endl;
}
}
}

67
osal/linux/TcUnixUdpPollingTask.h Normal file
View File

@ -0,0 +1,67 @@
#ifndef FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
#define FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
#include "../../objectmanager/SystemObject.h"
#include "../../osal/linux/TmTcUnixUdpBridge.h"
#include "../../tasks/ExecutableObjectIF.h"
#include <sys/socket.h>
#include <vector>
/**
* @brief This class can be used to implement the polling of a Unix socket,
* using UDP for now.
* @details
* The task will be blocked while the specified number of bytes has not been
* received, so TC reception is handled inside a separate task.
* This class caches the IP address of the sender. It is assumed there
* is only one sender for now.
*/
class TcUnixUdpPollingTask: public SystemObject,
public ExecutableObjectIF {
friend class TmTcUnixUdpBridge;
public:
static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048;
//! 0.5 default milliseconds timeout for now.
static constexpr timeval DEFAULT_TIMEOUT = {.tv_sec = 0, .tv_usec = 500};
TcUnixUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
size_t frameSize = 0, double timeoutSeconds = -1);
virtual~ TcUnixUdpPollingTask();
/**
* Turn on optional timeout for UDP polling. In the default mode,
* the receive function will block until a packet is received.
* @param timeoutSeconds
*/
void setTimeout(double timeoutSeconds);
virtual ReturnValue_t performOperation(uint8_t opCode) override;
virtual ReturnValue_t initialize() override;
virtual ReturnValue_t initializeAfterTaskCreation() override;
protected:
StorageManagerIF* tcStore = nullptr;
private:
//! TMTC bridge is cached.
object_id_t tmtcBridgeId = objects::NO_OBJECT;
TmTcUnixUdpBridge* tmtcBridge = nullptr;
MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
//! Reception flags: https://linux.die.net/man/2/recvfrom.
int receptionFlags = 0;
//! Server socket, which is member of TMTC bridge and is assigned in
//! constructor
int serverUdpSocket = 0;
std::vector<uint8_t> receptionBuffer;
size_t frameSize = 0;
timeval receptionTimeout;
ReturnValue_t handleSuccessfullTcRead(size_t bytesRead);
void handleReadError();
};
#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */

170
osal/linux/TmTcUnixUdpBridge.cpp Normal file
View File

@ -0,0 +1,170 @@
#include "TmTcUnixUdpBridge.h"
#include "../../serviceinterface/ServiceInterfaceStream.h"
#include "../../ipc/MutexHelper.h"
#include <errno.h>
#include <arpa/inet.h>
TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
object_id_t tcDestination, object_id_t tmStoreId, object_id_t tcStoreId,
uint16_t serverPort, uint16_t clientPort):
TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
mutex = MutexFactory::instance()->createMutex();
uint16_t setServerPort = DEFAULT_UDP_SERVER_PORT;
if(serverPort != 0xFFFF) {
setServerPort = serverPort;
}
uint16_t setClientPort = DEFAULT_UDP_CLIENT_PORT;
if(clientPort != 0xFFFF) {
setClientPort = clientPort;
}
// Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html
//clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if(serverSocket < 0) {
sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open"
" UDP socket!" << std::endl;
handleSocketError();
return;
}
serverAddress.sin_family = AF_INET;
// Accept packets from any interface.
//serverAddress.sin_addr.s_addr = inet_addr("127.73.73.0");
serverAddress.sin_addr.s_addr = htonl(INADDR_ANY);
serverAddress.sin_port = htons(setServerPort);
serverAddressLen = sizeof(serverAddress);
setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, &serverSocketOptions,
sizeof(serverSocketOptions));
clientAddress.sin_family = AF_INET;
clientAddress.sin_addr.s_addr = htonl(INADDR_ANY);
clientAddress.sin_port = htons(setClientPort);
clientAddressLen = sizeof(clientAddress);
int result = bind(serverSocket,
reinterpret_cast<struct sockaddr*>(&serverAddress),
serverAddressLen);
if(result == -1) {
sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind "
"local port " << setServerPort << " to server socket!"
<< std::endl;
handleBindError();
return;
}
}
TmTcUnixUdpBridge::~TmTcUnixUdpBridge() {
}
ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
int flags = 0;
clientAddress.sin_addr.s_addr = htons(INADDR_ANY);
//clientAddress.sin_addr.s_addr = inet_addr("127.73.73.1");
clientAddressLen = sizeof(serverAddress);
// char ipAddress [15];
// sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
// &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
ssize_t bytesSent = sendto(serverSocket, data, dataLen, flags,
reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
if(bytesSent < 0) {
sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed."
<< std::endl;
handleSendError();
}
// sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
// " sent." << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in newAddress) {
MutexHelper lock(mutex, MutexIF::TimeoutType::WAITING, 10);
// char ipAddress [15];
// sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
// &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
// sif::debug << "IP Address Old: " << inet_ntop(AF_INET,
// &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
// Set new IP address if it has changed.
if(clientAddress.sin_addr.s_addr != newAddress.sin_addr.s_addr) {
clientAddress.sin_addr.s_addr = newAddress.sin_addr.s_addr;
clientAddressLen = sizeof(clientAddress);
}
}
void TmTcUnixUdpBridge::handleSocketError() {
// See: https://man7.org/linux/man-pages/man2/socket.2.html
switch(errno) {
case(EACCES):
case(EINVAL):
case(EMFILE):
case(ENFILE):
case(EAFNOSUPPORT):
case(ENOBUFS):
case(ENOMEM):
case(EPROTONOSUPPORT):
sif::error << "TmTcUnixBridge::handleSocketError: Socket creation failed"
<< " with " << strerror(errno) << std::endl;
break;
default:
sif::error << "TmTcUnixBridge::handleSocketError: Unknown error"
<< std::endl;
break;
}
}
void TmTcUnixUdpBridge::handleBindError() {
// See: https://man7.org/linux/man-pages/man2/bind.2.html
switch(errno) {
case(EACCES): {
/*
Ephermeral ports can be shown with following command:
sysctl -A | grep ip_local_port_range
*/
sif::error << "TmTcUnixBridge::handleBindError: Port access issue."
"Ports 1-1024 are reserved on UNIX systems and require root "
"rights while ephermeral ports should not be used as well."
<< std::endl;
}
break;
case(EADDRINUSE):
case(EBADF):
case(EINVAL):
case(ENOTSOCK):
case(EADDRNOTAVAIL):
case(EFAULT):
case(ELOOP):
case(ENAMETOOLONG):
case(ENOENT):
case(ENOMEM):
case(ENOTDIR):
case(EROFS): {
sif::error << "TmTcUnixBridge::handleBindError: Socket creation failed"
<< " with " << strerror(errno) << std::endl;
break;
}
default:
sif::error << "TmTcUnixBridge::handleBindError: Unknown error"
<< std::endl;
break;
}
}
void TmTcUnixUdpBridge::handleSendError() {
switch(errno) {
default:
sif::error << "TmTcUnixBridge::handleSendError: "
<< strerror(errno) << std::endl;
}
}

48
osal/linux/TmTcUnixUdpBridge.h Normal file
View File

@ -0,0 +1,48 @@
#ifndef FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
#define FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
#include "../../tmtcservices/AcceptsTelecommandsIF.h"
#include "../../tmtcservices/TmTcBridge.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/udp.h>
class TmTcUnixUdpBridge: public TmTcBridge {
friend class TcUnixUdpPollingTask;
public:
// The ports chosen here should not be used by any other process.
// List of used ports on Linux: /etc/services
static constexpr uint16_t DEFAULT_UDP_SERVER_PORT = 7301;
static constexpr uint16_t DEFAULT_UDP_CLIENT_PORT = 7302;
TmTcUnixUdpBridge(object_id_t objectId, object_id_t tcDestination,
object_id_t tmStoreId, object_id_t tcStoreId,
uint16_t serverPort = 0xFFFF,uint16_t clientPort = 0xFFFF);
virtual~ TmTcUnixUdpBridge();
void checkAndSetClientAddress(sockaddr_in clientAddress);
protected:
virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
private:
int serverSocket = 0;
const int serverSocketOptions = 0;
struct sockaddr_in clientAddress;
socklen_t clientAddressLen = 0;
struct sockaddr_in serverAddress;
socklen_t serverAddressLen = 0;
//! Access to the client address is mutex protected as it is set
//! by another task.
MutexIF* mutex;
void handleSocketError();
void handleBindError();
void handleSendError();
};
#endif /* FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_ */