From ebec074655d40dd60c4553c8b94ce3018d6f7358 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 5 Jul 2020 23:53:13 +0200 Subject: [PATCH] Split up FIFO into StaticFIFO and normale FIFO --- container/FIFO.h | 88 +++++---------------------- container/FIFOBase.h | 59 ++++++++++++++++++ container/FIFOBase.tpp | 76 +++++++++++++++++++++++ container/StaticFIFO.h | 23 +++++++ globalfunctions/timevalOperations.cpp | 7 +++ globalfunctions/timevalOperations.h | 1 + osal/linux/TcUnixUdpPollingTask.cpp | 66 +++++++++++++++++++- osal/linux/TcUnixUdpPollingTask.h | 30 +++++++-- osal/linux/TmTcUnixUdpBridge.cpp | 19 +++++- osal/linux/TmTcUnixUdpBridge.h | 16 ++++- tmtcservices/CommandingServiceBase.h | 4 +- tmtcservices/TmTcBridge.h | 4 +- 12 files changed, 309 insertions(+), 84 deletions(-) create mode 100644 container/FIFOBase.h create mode 100644 container/FIFOBase.tpp create mode 100644 container/StaticFIFO.h diff --git a/container/FIFO.h b/container/FIFO.h index f70c78b0..217ddbca 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -1,82 +1,26 @@ -#ifndef FIFO_H_ -#define FIFO_H_ +#ifndef FRAMEWORK_CONTAINER_FIFO_H_ +#define FRAMEWORK_CONTAINER_FIFO_H_ -#include +#include +#include /** - * @brief Simple First-In-First-Out data structure + * @brief Simple First-In-First-Out data structure. The maximum size + * can be set in the constructor. THe public interface of + * FIFOBase exposes the user interface for the FIFO. * @tparam T Entry Type * @tparam capacity Maximum capacity */ -template -class FIFO { -private: - uint8_t readIndex, writeIndex, currentSize; - T data[capacity]; - - uint8_t next(uint8_t current) { - ++current; - if (current == capacity) { - current = 0; - } - return current; - } +template +class FIFO: public FIFOBase { public: - FIFO() : - readIndex(0), writeIndex(0), currentSize(0) { - } + FIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity) { + values.reserve(maxCapacity); + values.resize(maxCapacity); + }; - bool empty() { - return (currentSize == 0); - } - - bool full() { - return (currentSize == capacity); - } - - uint8_t size(){ - return currentSize; - } - - ReturnValue_t insert(T value) { - if (full()) { - return FULL; - } else { - data[writeIndex] = value; - writeIndex = next(writeIndex); - ++currentSize; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t retrieve(T *value) { - if (empty()) { - return EMPTY; - } else { - *value = data[readIndex]; - readIndex = next(readIndex); - --currentSize; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t peek(T * value) { - if(empty()) { - return EMPTY; - } else { - *value = data[readIndex]; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t pop() { - T value; - return this->retrieve(&value); - } - - static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS; - static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); - static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); +private: + std::vector values; }; -#endif /* FIFO_H_ */ +#endif /* FRAMEWORK_CONTAINER_FIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h new file mode 100644 index 00000000..c6a74d2f --- /dev/null +++ b/container/FIFOBase.h @@ -0,0 +1,59 @@ +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ +#define FRAMEWORK_CONTAINER_FIFOBASE_H_ + +#include +#include + +template +class FIFOBase { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS; + static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); + static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); + + + /** Default ctor, no input arguments required. */ + FIFOBase(T* values, const size_t maxCapacity); + + /** + * Insert value into FIFO + * @param value + * @return + */ + ReturnValue_t insert(T value); + /** + * Retrieve item from FIFO. This removes the item from the FIFO. + * @param value + * @return + */ + ReturnValue_t retrieve(T *value); + /** + * Retrieve item from FIFO without removing it from FIFO. + * @param value + * @return + */ + ReturnValue_t peek(T * value); + /** + * Remove item from FIFO. + * @return + */ + ReturnValue_t pop(); + + bool empty(); + bool full(); + size_t size(); + +private: + T* values; + size_t maxCapacity; + + size_t readIndex = 0; + size_t writeIndex = 0; + size_t currentSize = 0; + + size_t next(size_t current); +}; + +#include + +#endif /* FRAMEWORK_CONTAINER_FIFOBASE_H_ */ diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp new file mode 100644 index 00000000..011ce6de --- /dev/null +++ b/container/FIFOBase.tpp @@ -0,0 +1,76 @@ +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_TPP_ +#define FRAMEWORK_CONTAINER_FIFOBASE_TPP_ + +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ +#error Include FIFOBase.h before FIFOBase.tpp! +#endif + +template +inline FIFOBase::FIFOBase(T* values, const size_t maxCapacity): + values(values), maxCapacity(maxCapacity) {}; + +template +inline ReturnValue_t FIFOBase::insert(T value) { + if (full()) { + return FULL; + } else { + values[writeIndex] = value; + writeIndex = next(writeIndex); + ++currentSize; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::retrieve(T* value) { + if (empty()) { + return EMPTY; + } else { + *value = values[readIndex]; + readIndex = next(readIndex); + --currentSize; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::peek(T* value) { + if(empty()) { + return EMPTY; + } else { + *value = values[readIndex]; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::pop() { + T value; + return this->retrieve(&value); +}; + +template +inline bool FIFOBase::empty() { + return (currentSize == 0); +}; + +template +inline bool FIFOBase::full() { + return (currentSize == maxCapacity); +} + +template +inline size_t FIFOBase::size() { + return currentSize; +} + +template +inline size_t FIFOBase::next(size_t current) { + ++current; + if (current == maxCapacity) { + current = 0; + } + return current; +} + +#endif diff --git a/container/StaticFIFO.h b/container/StaticFIFO.h new file mode 100644 index 00000000..651c4989 --- /dev/null +++ b/container/StaticFIFO.h @@ -0,0 +1,23 @@ +#ifndef FRAMEWORK_CONTAINER_STATICFIFO_H_ +#define FRAMEWORK_CONTAINER_STATICFIFO_H_ + +#include +#include + +/** + * @brief Simple First-In-First-Out data structure with size fixed at + * compile time. The public interface of FIFOBase exposes + * the user interface for the FIFO. + * @tparam T Entry Type + * @tparam capacity Maximum capacity + */ +template +class StaticFIFO: public FIFOBase { +public: + StaticFIFO(): FIFOBase(values.data(), capacity) {}; + +private: + std::array values; +}; + +#endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ diff --git a/globalfunctions/timevalOperations.cpp b/globalfunctions/timevalOperations.cpp index 253b6eb7..1da45d7b 100644 --- a/globalfunctions/timevalOperations.cpp +++ b/globalfunctions/timevalOperations.cpp @@ -90,3 +90,10 @@ double timevalOperations::toDouble(const timeval timeval) { double result = timeval.tv_sec * 1000000. + timeval.tv_usec; 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; +} diff --git a/globalfunctions/timevalOperations.h b/globalfunctions/timevalOperations.h index a553e60a..3977d5d9 100644 --- a/globalfunctions/timevalOperations.h +++ b/globalfunctions/timevalOperations.h @@ -41,6 +41,7 @@ namespace timevalOperations { * @return seconds */ double toDouble(const timeval timeval); +timeval toTimeval(const double seconds); } #endif /* TIMEVALOPERATIONS_H_ */ diff --git a/osal/linux/TcUnixUdpPollingTask.cpp b/osal/linux/TcUnixUdpPollingTask.cpp index 21ad1d9a..79dbdce2 100644 --- a/osal/linux/TcUnixUdpPollingTask.cpp +++ b/osal/linux/TcUnixUdpPollingTask.cpp @@ -1,16 +1,80 @@ #include TcSocketPollingTask::TcSocketPollingTask(object_id_t objectId, - object_id_t tmtcUnixUdpBridge): SystemObject(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); + } + + // Set receive timeout. + int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO, + &receptionTimeout, sizeof(receptionTimeout)); + if(result == -1) { + sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting receive" + "timeout failed with " << strerror(errno) << std::endl; + return; + } } TcSocketPollingTask::~TcSocketPollingTask() { } ReturnValue_t TcSocketPollingTask::performOperation(uint8_t opCode) { + // Poll for new data permanently. The call will block until the specified + // length of bytes has been received or a timeout occured. + 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(&senderAddress), &senderSockLen); + if(bytesReceived < 0) { + //handle error + sif::error << "TcSocketPollingTask::performOperation: recvfrom " + "failed with " << strerror(errno) << std::endl; + continue; + } + sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived + << " bytes received" << std::endl; + + ReturnValue_t result = handleSuccessfullTcRead(); + tmtcBridge->checkAndSetClientAddress(senderAddress); + } return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t TcSocketPollingTask::initialize() { + tmtcBridge = objectManager->get(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 TcSocketPollingTask::handleSuccessfullTcRead() { + return HasReturnvaluesIF::RETURN_OK; } diff --git a/osal/linux/TcUnixUdpPollingTask.h b/osal/linux/TcUnixUdpPollingTask.h index eda7fe5d..09ee6c99 100644 --- a/osal/linux/TcUnixUdpPollingTask.h +++ b/osal/linux/TcUnixUdpPollingTask.h @@ -1,8 +1,11 @@ #ifndef FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ #define FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ #include +#include #include + #include +#include /** * @brief This class can be used to implement the polling of a Unix socket, @@ -17,15 +20,34 @@ class TcSocketPollingTask: public SystemObject, public ExecutableObjectIF { friend class TmTcUnixUdpBridge; public: - TcSocketPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge); + 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}; + + TcSocketPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge, + size_t frameSize = 0, double timeoutSeconds = -1); virtual~ TcSocketPollingTask(); virtual ReturnValue_t performOperation(uint8_t opCode) override; virtual ReturnValue_t initialize() override; + private: - //! Sender Address is cached here. - const struct sockaddr senderAddress; -}; + //! TMTC bridge is cached. + object_id_t tmtcBridgeId = objects::NO_OBJECT; + TmTcUnixUdpBridge* tmtcBridge = nullptr; + //! 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 receptionBuffer; + + size_t frameSize = 0; + timeval receptionTimeout; + + ReturnValue_t handleSuccessfullTcRead(); diff --git a/osal/linux/TmTcUnixUdpBridge.cpp b/osal/linux/TmTcUnixUdpBridge.cpp index ff8b6588..710761b4 100644 --- a/osal/linux/TmTcUnixUdpBridge.cpp +++ b/osal/linux/TmTcUnixUdpBridge.cpp @@ -1,11 +1,14 @@ #include #include #include +#include TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId, object_id_t ccsdsPacketDistributor, uint16_t serverPort, uint16_t clientPort): TmTcBridge(objectId, ccsdsPacketDistributor) { + mutex = MutexFactory::instance()->createMutex(); + uint16_t setServerPort = DEFAULT_UDP_SERVER_PORT; if(serverPort != 0xFFFF) { setServerPort = serverPort; @@ -16,6 +19,7 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId, setClientPort = clientPort; } + // Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html serverSocket = socket(AF_INET, SOCK_DGRAM, 0); if(socket < 0) { sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open" @@ -26,14 +30,16 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId, } serverAddress.sin_family = AF_INET; + // Accept packets from any interface. serverAddress.sin_addr.s_addr = htonl(INADDR_ANY); serverAddress.sin_port = htons(setServerPort); setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, &serverSocketOptions, sizeof(serverSocketOptions)); + serverSocketLen = sizeof(serverAddress); int result = bind(serverSocket, reinterpret_cast(&serverAddress), - sizeof(serverAddress)); + serverSocketLen); if(result == -1) { sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind " "local port " << setServerPort << " to server socket!" @@ -81,6 +87,17 @@ void TmTcUnixUdpBridge::handleSocketError() { } } +void TmTcUnixUdpBridge::setTimeout(float timeoutSeconds) { +} + +void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in newAddress) { + MutexHelper lock(mutex, 10); + // 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; + } +} + void TmTcUnixUdpBridge::handleBindError() { // See: https://man7.org/linux/man-pages/man2/bind.2.html switch(errno) { diff --git a/osal/linux/TmTcUnixUdpBridge.h b/osal/linux/TmTcUnixUdpBridge.h index a969083d..15d256cd 100644 --- a/osal/linux/TmTcUnixUdpBridge.h +++ b/osal/linux/TmTcUnixUdpBridge.h @@ -7,16 +7,20 @@ #include class TmTcUnixUdpBridge: public TmTcBridge { + friend class TcSocketPollingTask; public: // The ports chosen here should not be used by any other process. // List of used ports on Linux: /etc/services - static constexpr int DEFAULT_UDP_SERVER_PORT = 7301; - static constexpr int DEFAULT_UDP_CLIENT_PORT = 7302; + 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 ccsdsPacketDistributor, uint16_t serverPort = 0xFFFF,uint16_t clientPort = 0xFFFF); virtual~ TmTcUnixUdpBridge(); + void setTimeout(float timeoutSeconds); + void checkAndSetClientAddress(sockaddr_in clientAddress); + protected: virtual ReturnValue_t receiveTc(uint8_t ** recvBuffer, size_t * size) override; @@ -24,8 +28,16 @@ protected: private: int serverSocket = 0; const int serverSocketOptions = 0; + struct sockaddr_in clientAddress; + socklen_t clientSocketLen = 0; + struct sockaddr_in serverAddress; + socklen_t serverSocketLen = 0; + + //! Access to the client address is mutex protected as it is set + //! by another task. + MutexIF* mutex; void handleSocketError(); void handleBindError(); diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index 700503c6..169eebe1 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include class TcPacketStored; @@ -199,7 +199,7 @@ protected: uint32_t state; Command_t command; object_id_t objectId; - FIFO fifo; + StaticFIFO fifo; }; using CommandMapIter = FixedMap #include -#include +#include class TmTcBridge : public AcceptsTelemetryIF, public ExecutableObjectIF, @@ -143,7 +143,7 @@ protected: * This fifo can be used to store downlink data * which can not be sent at the moment. */ - FIFO tmFifo; + StaticFIFO tmFifo; uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE; uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED; };