wip CspComIF

bsp_linux/comIF/CspComIF.cpp

@@ -0,0 +1,167 @@
+#include "CspComIF.h"
+#include <bsp_linux/comIF/cookies/CspCookie.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <csp/drivers/can_socketcan.h>
+#include <fsfw/serialize/SerializeAdapter.h>
+
+CspComIF::CspComIF(object_id_t objectId) :
+		SystemObject(objectId) {
+
+}
+
+CspComIF::~CspComIF() {
+}
+
+ReturnValue_t CspComIF::initializeInterface(CookieIF *cookie)  {
+	if(cookie == nullptr) {
+		return NULLPOINTER;
+	}
+
+	CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
+	if(cspCookie == nullptr) {
+		return NULLPOINTER;
+	}
+	char* canInterface = cspCookie->getCanIf();
+	int bitrate = cspCookie->getBitrate();
+	/* Define the memory to allocate for the CSP stack */
+	int buf_count = 10;
+	int buf_size = 300;
+	/* Init CSP and CSP buffer system */
+	if (csp_init(cspClientAddress) != CSP_ERR_NONE
+			|| csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
+		sif::error << "Failed to init CSP\r\n" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	int promisc = 0; // Set filter mode on
+	csp_iface_t *csp_if_ptr = &csp_if;
+	csp_if_ptr = csp_can_socketcan_init(canInterface, bitrate, promisc);
+
+	/* Set default route and start router */
+	uint8_t address = CSP_DEFAULT_ROUTE;
+	uint8_t netmask = 0;
+	uint8_t mac = CSP_NODE_MAC;
+	int result = csp_rtable_set(address, netmask, csp_if_ptr, mac);
+	if(result != CSP_ERR_NONE){
+		sif::error << "Failed to add can interface to router table"
+				<< std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	/* Start the route task */
+	unsigned int task_stack_size = 500;
+	unsigned int priority = 0;
+	result = csp_route_start_task(task_stack_size, priority);
+	if(result != CSP_ERR_NONE){
+		sif::error << "Failed to start csp route task" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	uint8_t cspAddress = cspCookie->getCspAddress();
+	uint16_t maxReplyLength = cspCookie->getMaxReplyLength();
+	if(cspDeviceMap.find(cspAddress) != cspDeviceMap.end()){
+		/* Insert device information in CSP map */
+		cspDeviceMap.emplace(cspAddress, vectorBuffer(maxReplyLength));
+	}
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::sendMessage(CookieIF *cookie,
+		const uint8_t * sendData, size_t sendLen) {
+	int result;
+	if(cookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
+	if(cspCookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	/* Extract csp port and bytes to query from command buffer */
+	uint8_t cspPort;
+	uint16_t querySize;
+	SerializeAdapter::deSerialize(&cspPort, &sendData, &sendLen,
+			SerializeIF::Endianness::BIG);
+	SerializeAdapter::deSerialize(&querySize, &sendData, &sendLen,
+			SerializeIF::Endianness::BIG);
+	uint8_t cspAddress = cspCookie->getCspAddress();
+
+	if(cspPort == csp_reserved_ports_e::CSP_PING){
+		uint32_t timeout = 1000; // ms
+		unsigned int pingSize = 100; // 100 bytes
+		uint32_t replyTime = csp_ping(cspAddress, timeout, pingSize,
+				CSP_O_NONE);
+		sif::info << "Ping address: " << cspAddress << ", reply after "
+				<< replyTime << " ms" << std::endl;
+		/* Store reply time in reply buffer * */
+		uint8_t* replyBuffer = cspDeviceMap[cspAddress].data();
+		memcpy(replyBuffer, &replyTime, sizeof(replyTime));
+	}
+	else if(cspPort == csp_reserved_ports_e::CSP_REBOOT){
+		csp_reboot(cspCookie->getCspAddress());
+	}
+	else{
+		/* No CSP fixed port was selected. Send data to the specified port and
+		 * wait for querySize number of bytes */
+		result = cspTransfer(cspAddress, cspPort, sendData, sendLen,
+				querySize);
+		if(result != HasReturnvaluesIF::RETURN_OK){
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		rememberQuerySize = querySize;
+	}
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::getSendSuccess(CookieIF *cookie) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::requestReceiveMessage(CookieIF *cookie,
+		size_t requestLen) {
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::readReceivedMessage(CookieIF *cookie,
+		uint8_t** buffer, size_t* size) {
+	if(cookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	CspCookie* cspCookie = dynamic_cast<CspCookie*> (cookie);
+	if(cspCookie == NULL){
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	uint8_t cspAddress = cspCookie->getCspAddress();
+
+	*buffer = cspDeviceMap[cspAddress].data();
+	*size = rememberQuerySize;
+
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort,
+		const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize) {
+
+	uint32_t timeout_ms = 1000;
+	uint8_t* replyBuffer = cspDeviceMap[cspAddress].data();
+	uint8_t tmpCmdBuffer[cmdBufferLen];
+	memcpy(tmpCmdBuffer, cmdBuffer, cmdBufferLen);
+
+	csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0,
+			CSP_O_NONE);
+
+	querySize = 12;
+	int receivedBytes = csp_transaction_persistent(conn, timeout_ms,
+			tmpCmdBuffer, cmdBufferLen, replyBuffer, querySize);
+	if(receivedBytes != querySize){
+		sif::error << "CSP transfer failed to receive all requested bytes "
+				   << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	csp_close(conn);
+
+	return HasReturnvaluesIF::RETURN_OK;
+}

bsp_linux/comIF/CspComIF.h

@@ -0,0 +1,66 @@
+#ifndef BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_
+#define BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <csp/csp.h>
+
+#include <vector>
+#include <unordered_map>
+
+/**
+ * @brief	This class is serves as the communication interface to devices
+ * 			supporting the CSP protocol. For now as physical interface only
+ * 			CAN is supported by this CSP implementation.
+ * @author	Jakob Meier
+ */
+class CspComIF: public DeviceCommunicationIF, public SystemObject {
+public:
+	CspComIF(object_id_t objectId);
+	virtual ~CspComIF();
+
+	ReturnValue_t initializeInterface(CookieIF * cookie) override;
+	ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t * sendData,
+			size_t sendLen) override;
+	ReturnValue_t getSendSuccess(CookieIF *cookie) override;
+	ReturnValue_t requestReceiveMessage(CookieIF *cookie,
+			size_t requestLen) override;
+	ReturnValue_t readReceivedMessage(CookieIF *cookie,
+			uint8_t **readData, size_t *readLen) override;
+
+private:
+
+	/**
+	 * @brief	This function initiates the CSP transfer.
+	 *
+	 * @param cspAddress	The CSP address of the target device.
+	 * @param cspPort		The port of the target device.
+	 * @param timeout		The timeout to wait for csp_send and csp_read
+	 * 						functions. Specifies how long the functions wait
+	 * 						for a successful operation.
+	 * @param cmdBuffer		The data to send.
+	 * @param cmpBuffer		The number of bytes to send.
+	 * @param querySize		The size of the requested message.
+	 */
+	ReturnValue_t cspTransfer(uint8_t cspAddress, uint8_t cspPort,
+			const uint8_t* cmdBuffer, int cmdBufferLen, uint16_t querySize);
+
+	typedef uint8_t node_t;
+	using vectorBuffer = std::vector<uint8_t>;
+	using VectorBufferMap = std::unordered_map<node_t, vectorBuffer>;
+	using vectorBufferIter = VectorBufferMap::iterator;
+
+	/* In this map assigns reply buffers to a CSP device */
+	VectorBufferMap cspDeviceMap;
+
+	uint16_t rememberQuerySize = 0;
+
+	/* This is the CSP address of the OBC. */
+	node_t cspClientAddress = 1;
+
+	/* Interface struct for csp protocol stack */
+	csp_iface_t csp_if;
+};
+
+#endif /* BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_ */

bsp_linux/comIF/cookies/CspCookie.cpp

@@ -0,0 +1,24 @@
+#include "CspCookie.h"
+
+CspCookie::CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_) :
+		maxReplyLength(maxReplyLength_), cspAddress(cspAddress_) {
+}
+
+CspCookie::~CspCookie() {
+}
+
+uint16_t CspCookie::getMaxReplyLength(){
+	return maxReplyLength;
+}
+
+uint8_t CspCookie::getCspAddress(){
+	return cspAddress;
+}
+
+char* CspCookie::getCanIf(){
+	return canInterface;
+}
+
+int CspCookie::getBitrate(){
+	return bitrate;
+}

bsp_linux/comIF/cookies/CspCookie.h

@@ -0,0 +1,31 @@
+#ifndef BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_
+#define BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_
+
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <stdint.h>
+
+/**
+ * @brief	This is the cookie for devices supporting the CSP (CubeSat Space
+ * 			Protocol).
+ * @author	J. Meier
+ */
+class CspCookie: public CookieIF {
+public:
+
+	CspCookie(uint16_t maxReplyLength_, uint8_t cspAddress_);
+	virtual ~CspCookie();
+
+	uint16_t getMaxReplyLength();
+	uint8_t getCspAddress();
+	char* getCanIf();
+	int getBitrate();
+
+private:
+
+	uint16_t maxReplyLength;
+	char canInterface[5] = "can0";
+	uint8_t cspAddress;
+	int bitrate = 1000;
+};
+
+#endif /* BSP_LINUX_COMIF_COOKIES_CSPCOOKIE_H_ */