From 236ad1b85b600acecc741127faa19c72925542fc Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Wed, 8 Jul 2020 15:06:46 +0200
Subject: [PATCH] a lot of debug output added

---
 osal/linux/TcUnixUdpPollingTask.cpp   |  1 +
 osal/linux/TmTcUnixUdpBridge.cpp      | 54 ++++++++++++++++++++-------
 osal/linux/TmTcUnixUdpBridge.h        | 11 ++++--
 tmtcservices/PusServiceBase.cpp       | 20 +++++-----
 tmtcservices/TmTcBridge.cpp           | 32 ++++++++++------
 tmtcservices/TmTcBridge.h             | 17 ++++++---
 tmtcservices/VerificationReporter.cpp |  8 ++--
 7 files changed, 93 insertions(+), 50 deletions(-)

diff --git a/osal/linux/TcUnixUdpPollingTask.cpp b/osal/linux/TcUnixUdpPollingTask.cpp
index 97fbd044..122301da 100644
--- a/osal/linux/TcUnixUdpPollingTask.cpp
+++ b/osal/linux/TcUnixUdpPollingTask.cpp
@@ -53,6 +53,7 @@ ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
 		if(result != HasReturnvaluesIF::RETURN_FAILED) {
 
 		}
+		tmtcBridge->registerCommConnect();
 		tmtcBridge->checkAndSetClientAddress(senderAddress);
 	}
 	return HasReturnvaluesIF::RETURN_OK;
diff --git a/osal/linux/TmTcUnixUdpBridge.cpp b/osal/linux/TmTcUnixUdpBridge.cpp
index 4dc61c50..40e96561 100644
--- a/osal/linux/TmTcUnixUdpBridge.cpp
+++ b/osal/linux/TmTcUnixUdpBridge.cpp
@@ -3,11 +3,12 @@
 #include <framework/ipc/MutexHelper.h>
 
 #include <errno.h>
+#include <arpa/inet.h>
 
 TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
-		object_id_t ccsdsPacketDistributor, uint16_t serverPort,
-		uint16_t clientPort):
-		TmTcBridge(objectId, ccsdsPacketDistributor) {
+		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;
@@ -15,13 +16,14 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
 		setServerPort = serverPort;
 	}
 
-//	uint16_t setClientPort = DEFAULT_UDP_CLIENT_PORT;
-//	if(clientPort != 0xFFFF) {
-//		setClientPort = clientPort;
-//	}
+	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
-	serverSocket = socket(AF_INET, SOCK_DGRAM, 0);
+	//clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
+	serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 	if(socket < 0) {
 		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open"
 				" UDP socket!" << std::endl;
@@ -36,10 +38,14 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
 	setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, &serverSocketOptions,
 			sizeof(serverSocketOptions));
 
-	serverSocketLen = sizeof(serverAddress);
+	clientAddress.sin_family = AF_INET;
+	clientAddress.sin_addr.s_addr = htonl(INADDR_ANY);
+	clientAddress.sin_port = htons(setClientPort);
+
+	serverAddressLen = sizeof(serverAddress);
 	int result = bind(serverSocket,
 			reinterpret_cast<struct sockaddr*>(&serverAddress),
-			serverSocketLen);
+			serverAddressLen);
 	if(result == -1) {
 		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind "
 				"local port " << setServerPort << " to server socket!"
@@ -54,20 +60,35 @@ TmTcUnixUdpBridge::~TmTcUnixUdpBridge() {
 
 ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
 	int flags = 0;
-	ssize_t result = send(serverSocket, data, dataLen, flags);
-	if(result < 0) {
+	sif::debug << "Client Port: "<<ntohs(clientAddress.sin_port) << std::endl;
+	ssize_t bytesSent = sendto(serverSocket, data, dataLen, flags,
+			reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
+	if(bytesSent < 0) {
 		// todo: handle errors
-		sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed "
-				"with error " << strerror(errno) << std::endl;
+		sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed."
+				<< std::endl;
+		sif::error << "Error: " << strerror(errno) << 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, 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) {
+		sif::info << "setting new address" << std::endl;
 		clientAddress.sin_addr.s_addr = newAddress.sin_addr.s_addr;
+		clientAddressLen = sizeof(clientAddress);
 	}
 }
 
@@ -130,3 +151,8 @@ void TmTcUnixUdpBridge::handleBindError() {
 	}
 }
 
+void TmTcUnixUdpBridge::handleSendError() {
+
+
+}
+
diff --git a/osal/linux/TmTcUnixUdpBridge.h b/osal/linux/TmTcUnixUdpBridge.h
index 538664d8..dbddc6c3 100644
--- a/osal/linux/TmTcUnixUdpBridge.h
+++ b/osal/linux/TmTcUnixUdpBridge.h
@@ -15,8 +15,9 @@ public:
 	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);
+	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);
@@ -26,13 +27,14 @@ protected:
 
 private:
 	int serverSocket = 0;
+
 	const int serverSocketOptions = 0;
 
 	struct sockaddr_in clientAddress;
-	socklen_t clientSocketLen = 0;
+	socklen_t clientAddressLen = 0;
 
 	struct sockaddr_in serverAddress;
-	socklen_t serverSocketLen = 0;
+	socklen_t serverAddressLen = 0;
 
 	//! Access to the client address is mutex protected as it is set
 	//! by another task.
@@ -40,6 +42,7 @@ private:
 
 	void handleSocketError();
 	void handleBindError();
+	void handleSendError();
 };
 
 #endif /* FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_ */
diff --git a/tmtcservices/PusServiceBase.cpp b/tmtcservices/PusServiceBase.cpp
index cb1e633f..4385609b 100644
--- a/tmtcservices/PusServiceBase.cpp
+++ b/tmtcservices/PusServiceBase.cpp
@@ -103,19 +103,17 @@ ReturnValue_t PusServiceBase::initialize() {
 			packetDestination);
 	PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
 			packetSource);
-	if ((destService != nullptr) && (distributor != nullptr)) {
-		this->requestQueue->setDefaultDestination(
-				destService->getReportReceptionQueue());
-		distributor->registerService(this);
-		return RETURN_OK;
-	}
-	else {
+	if (destService == nullptr or distributor == nullptr) {
 		sif::error << "PusServiceBase::PusServiceBase: Service "
-				<< (uint32_t) this->serviceId << ": Configuration error."
-				<< " Make sure packetSource and packetDestination are defined "
-			       "correctly" << std::endl;
-		return RETURN_FAILED;
+				<< this->serviceId << ": Configuration error. Make sure "
+				<<	"packetSource and packetDestination are defined correctly"
+				<< std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
+	this->requestQueue->setDefaultDestination(
+			destService->getReportReceptionQueue());
+	distributor->registerService(this);
+	return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t PusServiceBase::initializeAfterTaskCreation() {
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index 9c3e5b94..8c44f7de 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -5,9 +5,11 @@
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 #include <framework/globalfunctions/arrayprinter.h>
 
-TmTcBridge::TmTcBridge(object_id_t objectId,
-        object_id_t ccsdsPacketDistributor): SystemObject(objectId),
-        ccsdsPacketDistributor(ccsdsPacketDistributor)
+TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination,
+		object_id_t tmStoreId, object_id_t tcStoreId):
+		SystemObject(objectId),tmStoreId(tmStoreId), tcStoreId(tcStoreId),
+		tcDestination(tcDestination)
+
 {
     tmTcReceptionQueue = QueueFactory::instance()->
             createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
@@ -42,18 +44,24 @@ ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(
 }
 
 ReturnValue_t TmTcBridge::initialize() {
-	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
-	if (tcStore == NULL) {
-		return RETURN_FAILED;
+	tcStore = objectManager->get<StorageManagerIF>(tcStoreId);
+	if (tcStore == nullptr) {
+		sif::error << "TmTcBridge::initialize: TC store invalid. Make sure"
+				"it is created and set up properly." << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
-	tmStore = objectManager->get<StorageManagerIF>(objects::TM_STORE);
-	if (tmStore == NULL) {
-		return RETURN_FAILED;
+	tmStore = objectManager->get<StorageManagerIF>(tmStoreId);
+	if (tmStore == nullptr) {
+		sif::error << "TmTcBridge::initialize: TM store invalid. Make sure"
+				"it is created and set up properly." << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	AcceptsTelecommandsIF* tcDistributor =
-			objectManager->get<AcceptsTelecommandsIF>(ccsdsPacketDistributor);
-	if (tcDistributor == NULL) {
-		return RETURN_FAILED;
+			objectManager->get<AcceptsTelecommandsIF>(tcDestination);
+	if (tcDistributor == nullptr) {
+		sif::error << "TmTcBridge::initialize: TC Distributor invalid"
+				<< std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	tmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
diff --git a/tmtcservices/TmTcBridge.h b/tmtcservices/TmTcBridge.h
index 3fcbf84c..3e3d1b71 100644
--- a/tmtcservices/TmTcBridge.h
+++ b/tmtcservices/TmTcBridge.h
@@ -24,7 +24,8 @@ public:
 	static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5;
 	static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;
 
-	TmTcBridge(object_id_t objectId, object_id_t ccsdsPacketDistributor);
+	TmTcBridge(object_id_t objectId, object_id_t tcDestination,
+			object_id_t tmStoreId, object_id_t tcStoreId);
 	virtual ~TmTcBridge();
 
 	/**
@@ -69,14 +70,20 @@ public:
 	virtual MessageQueueId_t getRequestQueue() override;
 
 protected:
+	//! Cached for initialize function.
+	object_id_t tmStoreId = objects::NO_OBJECT;
+	object_id_t tcStoreId = objects::NO_OBJECT;
+	object_id_t tcDestination = objects::NO_OBJECT;
+
 	//! Used to send and receive TMTC messages.
 	//! TmTcMessage is used to transport messages between tasks.
 	MessageQueueIF* tmTcReceptionQueue = nullptr;
-	StorageManagerIF* tcStore = nullptr;
+
 	StorageManagerIF* tmStore = nullptr;
-	object_id_t ccsdsPacketDistributor = 0;
-	//! Used to specify whether communication link is up
-	bool communicationLinkUp = false;
+	StorageManagerIF* tcStore = nullptr;
+
+	//! Used to specify whether communication link is up by default.
+	bool communicationLinkUp = true;
 	bool tmStored = false;
 
 	/**
diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp
index 40f7e57f..79e650f1 100644
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@@ -16,7 +16,7 @@ VerificationReporter::~VerificationReporter() {}
 
 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 		TcPacketBase* current_packet, uint8_t set_step) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(set_report_id,
@@ -35,7 +35,7 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 		uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
 		uint8_t set_step) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE)  {
 		this->initialize();
 	}
 	PusVerificationMessage message(set_report_id, ackFlags, tcPacketId,
@@ -52,7 +52,7 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 void VerificationReporter::sendFailureReport(uint8_t report_id,
 		TcPacketBase* current_packet, ReturnValue_t error_code, uint8_t step,
 		uint32_t parameter1, uint32_t parameter2) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(report_id,
@@ -73,7 +73,7 @@ void VerificationReporter::sendFailureReport(uint8_t report_id,
 		uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
 		ReturnValue_t error_code, uint8_t step, uint32_t parameter1,
 		uint32_t parameter2) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(report_id, ackFlags, tcPacketId,