moved stuff to linux folder
This commit is contained in:
8
linux/csp/CMakeLists.txt
Normal file
8
linux/csp/CMakeLists.txt
Normal file
@ -0,0 +1,8 @@
|
||||
target_sources(${TARGET_NAME} PUBLIC
|
||||
CspComIF.cpp
|
||||
CspCookie.cpp
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
255
linux/csp/CspComIF.cpp
Normal file
255
linux/csp/CspComIF.cpp
Normal file
@ -0,0 +1,255 @@
|
||||
#include "CspComIF.h"
|
||||
#include "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;
|
||||
}
|
||||
|
||||
/* Perform CAN and CSP initialization only once */
|
||||
if(cspDeviceMap.empty()){
|
||||
/* 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;
|
||||
result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
uint8_t cspAddress = cspCookie->getCspAddress();
|
||||
switch(cspPort) {
|
||||
case(Ports::CSP_PING): {
|
||||
initiatePingRequest(cspAddress, querySize);
|
||||
break;
|
||||
}
|
||||
case(Ports::CSP_REBOOT): {
|
||||
csp_reboot(cspAddress);
|
||||
break;
|
||||
}
|
||||
case(Ports::P60_PORT_GNDWDT_RESET):
|
||||
case(Ports::P60_PORT_RPARAM): {
|
||||
/* 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;
|
||||
}
|
||||
replySize = querySize;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::error << "CspComIF: Invalid port specified" << std::endl;
|
||||
break;
|
||||
}
|
||||
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 = replySize;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CspComIF::cspTransfer(uint8_t cspAddress, uint8_t cspPort,
|
||||
const uint8_t* cmdBuffer, int cmdLen, uint16_t querySize) {
|
||||
|
||||
uint32_t timeout_ms = 1000;
|
||||
uint16_t bytesRead = 0;
|
||||
int32_t expectedSize = (int32_t)querySize;
|
||||
vectorBufferIter iter = cspDeviceMap.find(cspAddress);
|
||||
if(iter == cspDeviceMap.end()){
|
||||
sif::error << "CSP device with address " << cspAddress << " no found in"
|
||||
<< " device map" << std::endl;
|
||||
}
|
||||
uint8_t* replyBuffer = iter->second.data();
|
||||
|
||||
csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0,
|
||||
CSP_O_NONE);
|
||||
|
||||
csp_packet_t* commandPacket = (csp_packet_t*)csp_buffer_get(cmdLen);
|
||||
if (commandPacket == NULL) {
|
||||
sif::error << "CspComIF::cspTransfer: Failed to get memory for a csp packet from the csp "
|
||||
<< "stack" << std::endl;
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
memcpy(commandPacket->data, cmdBuffer, cmdLen);
|
||||
commandPacket->length = cmdLen;
|
||||
|
||||
if (!csp_send(conn, commandPacket, timeout_ms)) {
|
||||
csp_buffer_free(commandPacket);
|
||||
sif::error << "CspComIF::cspTransfer: Failed to send csp packet" << std::endl;
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
/* Return when no reply is expected */
|
||||
if (expectedSize == 0) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
csp_packet_t * reply;
|
||||
reply = csp_read(conn, timeout_ms);
|
||||
if (reply == NULL) {
|
||||
sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl;
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
memcpy(replyBuffer, reply->data, reply->length);
|
||||
expectedSize = expectedSize - reply->length;
|
||||
bytesRead += reply->length;
|
||||
csp_buffer_free(reply);
|
||||
while (expectedSize > 0) {
|
||||
reply = csp_read(conn, timeout_ms);
|
||||
if (reply == NULL) {
|
||||
sif::error << "CspComIF::cspTransfer: Failed to read csp packet" << std::endl;
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
if ((reply->length + bytesRead) > iter->second.size()) {
|
||||
sif::error << "CspComIF::cspTransfer: Reply buffer to short" << std::endl;
|
||||
csp_buffer_free(reply);
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
memcpy(replyBuffer + bytesRead, reply->data, reply->length);
|
||||
expectedSize = expectedSize - reply->length;
|
||||
bytesRead += reply->length;
|
||||
csp_buffer_free(reply);
|
||||
}
|
||||
|
||||
if(expectedSize != 0){
|
||||
sif::error << "CspComIF::cspTransfer: Received more bytes than requested" << std::endl;
|
||||
csp_close(conn);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
csp_close(conn);
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CspComIF::getPortAndQuerySize(const uint8_t** sendData,
|
||||
size_t* sendLen, uint8_t* cspPort, uint16_t* querySize) {
|
||||
ReturnValue_t result = SerializeAdapter::deSerialize(cspPort, sendData,
|
||||
sendLen, SerializeIF::Endianness::BIG);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK){
|
||||
sif::error << "CspComIF: Failed to deserialize CSP port from command "
|
||||
<< "buffer" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
SerializeAdapter::deSerialize(querySize, sendData, sendLen,
|
||||
SerializeIF::Endianness::BIG);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK){
|
||||
sif::error << "CspComIF: Failed to deserialize querySize from command "
|
||||
<< "buffer" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void CspComIF::initiatePingRequest(uint8_t cspAddress, uint16_t querySize){
|
||||
uint32_t timeout_ms = 500;
|
||||
uint32_t replyTime = csp_ping(cspAddress, timeout_ms, querySize,
|
||||
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));
|
||||
replySize = sizeof(replyTime);
|
||||
}
|
||||
89
linux/csp/CspComIF.h
Normal file
89
linux/csp/CspComIF.h
Normal file
@ -0,0 +1,89 @@
|
||||
#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 serves as the communication interface to devices
|
||||
* supporting the CSP protocol. As physical layer can0 is used
|
||||
* in this implementation.
|
||||
* @author J. 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 cmdLen 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 cmdLen, uint16_t querySize);
|
||||
|
||||
enum Ports {
|
||||
CSP_PING = 1,
|
||||
CSP_REBOOT = 4,
|
||||
P60_PORT_RPARAM = 7,
|
||||
P60_PORT_GNDWDT_RESET = 9
|
||||
};
|
||||
|
||||
|
||||
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 replySize = 0;
|
||||
|
||||
/* This is the CSP address of the OBC. */
|
||||
node_t cspClientAddress = 1;
|
||||
|
||||
/* Interface struct for csp protocol stack */
|
||||
csp_iface_t csp_if;
|
||||
|
||||
char canInterface[5] = "can0";
|
||||
int bitrate = 1000;
|
||||
|
||||
/**
|
||||
* @brief Function to extract the csp port and the query size from the
|
||||
* command buffer.
|
||||
*/
|
||||
ReturnValue_t getPortAndQuerySize(const uint8_t** sendData, size_t* sendLen,
|
||||
uint8_t* cspPort, uint16_t* querySize);
|
||||
|
||||
/**
|
||||
* @brief This function initiates the ping request.
|
||||
*/
|
||||
void initiatePingRequest(uint8_t cspAddress, uint16_t querySize);
|
||||
};
|
||||
|
||||
#endif /* BSP_LINUX_COMIF_COOKIES_CSPCOMIF_H_ */
|
||||
16
linux/csp/CspCookie.cpp
Normal file
16
linux/csp/CspCookie.cpp
Normal file
@ -0,0 +1,16 @@
|
||||
#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;
|
||||
}
|
||||
27
linux/csp/CspCookie.h
Normal file
27
linux/csp/CspCookie.h
Normal file
@ -0,0 +1,27 @@
|
||||
#ifndef BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_
|
||||
#define BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_
|
||||
|
||||
#include <fsfw/devicehandlers/CookieIF.h>
|
||||
#include <cstdint>
|
||||
|
||||
/**
|
||||
* @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();
|
||||
|
||||
private:
|
||||
|
||||
uint16_t maxReplyLength;
|
||||
uint8_t cspAddress;
|
||||
};
|
||||
|
||||
#endif /* BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_ */
|
||||
Reference in New Issue
Block a user