eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

clean up op done
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
Details
EIVE/eive-obsw/pipeline/pr-develop Build started...
Details

Browse Source
This commit is contained in:
Robin Müller
2023-03-26 16:42:00 +02:00
parent e7eaaa9295
commit 63b50e6101
116 changed files with 630 additions and 601 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
linux/power/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
target_sources(${OBSW_NAME} PUBLIC CspComIF.cpp)

396
linux/power/CspComIF.cpp Normal file
View File

@ -0,0 +1,396 @@
#include <csp/drivers/can_socketcan.h>
#include <fsfw/serialize/SerializeAdapter.h>
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
#include <linux/power/CspComIF.h>
#include <mission/power/CspCookie.h>
#include <mission/power/gsDefs.h>
#include <p60acu.h>
#include <p60dock.h>
#include <p60pdu.h>
#include <param/param_string.h>
#include <param/rparam_client.h>
using namespace GOMSPACE;
CspComIF::CspComIF(object_id_t objectId, const char* routeTaskName, uint32_t routerRealTimePriority)
: SystemObject(objectId),
routerRealTimePriority(routerRealTimePriority),
routerTaskName(routeTaskName) {}
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()) {
sif::info << "Performing " << canInterface << " initialization.." << std::endl;
/* 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(cspOwnAddress) != CSP_ERR_NONE ||
csp_buffer_init(buf_count, buf_size) != CSP_ERR_NONE) {
sif::error << "Failed to init CSP\r\n" << std::endl;
return returnvalue::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 returnvalue::FAILED;
}
/* Start the route task */
result = startRouterTask();
if (result != returnvalue::OK) {
sif::error << "Failed to start csp route task" << std::endl;
return returnvalue::FAILED;
}
sif::info << canInterface << " initialized successfully" << std::endl;
}
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, ReplyInfo(maxReplyLength));
}
return returnvalue::OK;
}
ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
int result;
if (cookie == nullptr) {
return returnvalue::FAILED;
}
CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
if (cspCookie == nullptr) {
return returnvalue::FAILED;
}
uint8_t cspPort;
uint16_t querySize = 0;
if (cspCookie->getRequest() == GOMSPACE::SpecialRequestTypes::DEFAULT_COM_IF) {
/* Extract csp port and bytes to query from command buffer */
result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize);
if (result != returnvalue::OK) {
return result;
}
} else {
cspPort = cspCookie->getCspPort();
querySize = cspCookie->getReplyLen();
}
if (querySize > cspCookie->getMaxReplyLength()) {
sif::error << "Query size " << querySize << " is larger than maximum allowed "
<< cspCookie->getMaxReplyLength() << std::endl;
return returnvalue::FAILED;
}
uint8_t cspAddress = cspCookie->getCspAddress();
auto iter = cspDeviceMap.find(cspAddress);
if (iter == cspDeviceMap.end()) {
return returnvalue::FAILED;
}
switch (cspPort) {
case (CspPorts::CSP_PING): {
initiatePingRequest(cspAddress, querySize);
break;
}
case (CspPorts::CSP_REBOOT): {
csp_reboot(cspAddress);
break;
}
case (CspPorts::P60_PORT_GNDWDT_RESET_ENUM):
case (CspPorts::P60_PORT_RPARAM_ENUM): {
if (cspCookie->getRequest() != SpecialRequestTypes::DEFAULT_COM_IF) {
param_index_t requestStruct{};
requestStruct.physaddr = iter->second.replyBuf.data();
auto req = cspCookie->getRequest();
if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_HK) {
if (!p60pdu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_HK) {
if (!p60acu_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_HK) {
if (!p60dock_get_hk(&requestStruct, cspAddress, cspCookie->getTimeout())) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::GET_PDU_CONFIG) {
requestStruct.table = p60pdu_config;
requestStruct.mem_id = P60PDU_PARAM;
requestStruct.count = p60pdu_config_count;
requestStruct.size = P60PDU_PARAM_SIZE;
result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
requestStruct.mem_id, cspCookie->getTimeout());
if (result != 0) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::GET_ACU_CONFIG) {
requestStruct.table = p60acu_config;
requestStruct.mem_id = P60ACU_PARAM;
requestStruct.count = p60acu_config_count;
requestStruct.size = P60ACU_PARAM_SIZE;
result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
requestStruct.mem_id, cspCookie->getTimeout());
if (result != 0) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::GET_P60DOCK_CONFIG) {
requestStruct.table = p60dock_config;
requestStruct.mem_id = P60DOCK_PARAM;
requestStruct.count = p60dock_config_count;
requestStruct.size = P60DOCK_PARAM_SIZE;
result = rparam_get_full_table(&requestStruct, cspAddress, P60_PORT_RPARAM,
requestStruct.mem_id, cspCookie->getTimeout());
if (result != 0) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::SAVE_TABLE) {
if (sendLen < 2) {
return returnvalue::FAILED;
}
const TableInfo* tableInfo = reinterpret_cast<const TableInfo*>(sendData);
result = gs_rparam_save(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable,
tableInfo->targetTable);
if (result != 0) {
return returnvalue::FAILED;
}
} else if (req == GOMSPACE::SpecialRequestTypes::LOAD_TABLE) {
if (sendLen < 2) {
return returnvalue::FAILED;
}
const TableInfo* tableInfo = reinterpret_cast<const TableInfo*>(sendData);
result = gs_rparam_load(cspAddress, cspCookie->getTimeout(), tableInfo->sourceTable,
tableInfo->targetTable);
if (result != 0) {
return returnvalue::FAILED;
}
}
} 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 != returnvalue::OK) {
return returnvalue::FAILED;
}
}
iter->second.replyLen = querySize;
break;
}
default:
sif::error << "CspComIF: Invalid port specified" << std::endl;
break;
}
return returnvalue::OK;
}
ReturnValue_t CspComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }
ReturnValue_t CspComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
return returnvalue::OK;
}
ReturnValue_t CspComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) {
if (cookie == NULL) {
return returnvalue::FAILED;
}
CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
if (cspCookie == NULL) {
return returnvalue::FAILED;
}
uint8_t cspAddress = cspCookie->getCspAddress();
auto iter = cspDeviceMap.find(cspAddress);
if (iter == cspDeviceMap.end()) {
return returnvalue::FAILED;
}
*buffer = iter->second.replyBuf.data();
*size = iter->second.replyLen;
return returnvalue::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 = static_cast<int32_t>(querySize);
auto iter = cspDeviceMap.find(cspAddress);
if (iter == cspDeviceMap.end()) {
sif::error << "CSP device with address " << cspAddress << " no found in"
<< " device map" << std::endl;
return returnvalue::FAILED;
}
uint8_t* replyBuffer = iter->second.replyBuf.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 returnvalue::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 returnvalue::FAILED;
}
/* Return when no reply is expected */
if (expectedSize == 0) {
return returnvalue::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 returnvalue::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 returnvalue::FAILED;
}
if ((reply->length + bytesRead) > iter->second.replyBuf.size()) {
sif::error << "CspComIF::cspTransfer: Reply buffer to short" << std::endl;
csp_buffer_free(reply);
csp_close(conn);
return returnvalue::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;
sif::debug << "CspComIF::cspTransfer: Received bytes: " << bytesRead << std::endl;
csp_close(conn);
return returnvalue::FAILED;
}
csp_close(conn);
return returnvalue::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 != returnvalue::OK) {
sif::error << "CspComIF: Failed to deserialize CSP port from command "
<< "buffer" << std::endl;
return returnvalue::FAILED;
}
SerializeAdapter::deSerialize(querySize, sendData, sendLen, SerializeIF::Endianness::BIG);
if (result != returnvalue::OK) {
sif::error << "CspComIF: Failed to deserialize querySize from command "
<< "buffer" << std::endl;
return returnvalue::FAILED;
}
return returnvalue::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;
auto iter = cspDeviceMap.find(cspAddress);
if (iter == cspDeviceMap.end()) {
return;
}
/* Store reply time in reply buffer * */
uint8_t* replyBuffer = iter->second.replyBuf.data();
memcpy(replyBuffer, &replyTime, sizeof(replyTime));
iter->second.replyLen = sizeof(replyTime);
}
ReturnValue_t CspComIF::startRouterTask() {
pthread_attr_t attr;
int res = pthread_attr_init(&attr);
if (res) {
return returnvalue::FAILED;
}
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
res = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (res != 0) {
return returnvalue::FAILED;
}
// Set scheduling policy to SCHED_RR
res = pthread_attr_setschedpolicy(&attr, SCHED_RR);
if (res) {
pthread_attr_destroy(&attr);
return returnvalue::FAILED;
}
struct sched_param sched_param;
sched_param.sched_priority = routerRealTimePriority;
res = pthread_attr_setschedparam(&attr, &sched_param);
if (res) {
pthread_attr_destroy(&attr);
return returnvalue::FAILED;
}
res = pthread_create(&routerTaskHandle, &attr, routerWorkWrapper, NULL);
if (res) {
pthread_attr_destroy(&attr);
return returnvalue::FAILED;
}
res = pthread_setname_np(routerTaskHandle, routerTaskName);
if (res) {
pthread_attr_destroy(&attr);
return returnvalue::FAILED;
}
pthread_attr_destroy(&attr);
return returnvalue::OK;
}
void* CspComIF::routerWorkWrapper(void* args) {
/* Here there be routing */
while (1) {
csp_route_work(FIFO_TIMEOUT);
}
return nullptr;
}

93
linux/power/CspComIF.h Normal file
View File

@ -0,0 +1,93 @@
#ifndef LINUX_POWER_CSPCOMIF_H_
#define LINUX_POWER_CSPCOMIF_H_
#include <csp/csp.h>
#include <csp/csp_autoconfig.h>
#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
#include <fsfw/objectmanager/SystemObject.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <pthread.h>
#include <unordered_map>
#include <vector>
/**
* @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, const char *routeTaskName, uint32_t routerRealTimePriority);
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:
#ifdef CSP_USE_RDP
//! If RDP is enabled, the router needs to awake some times to check timeouts
static constexpr uint32_t FIFO_TIMEOUT = 100;
#else
//! If no RDP, the router can sleep untill data arrives
static constexpr uint32_t FIFO_TIMEOUT = CSP_MAX_DELAY;
#endif
/**
* @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);
typedef uint8_t node_t;
struct ReplyInfo {
ReplyInfo(size_t maxLen) : replyBuf(maxLen){};
std::vector<uint8_t> replyBuf;
size_t replyLen = 0;
};
using VectorBufferMap = std::unordered_map<node_t, ReplyInfo>;
/* In this map assigns reply buffers to a CSP device */
VectorBufferMap cspDeviceMap;
/* This is the CSP address of the OBC. */
node_t cspOwnAddress = 1;
pthread_t routerTaskHandle{};
uint32_t routerRealTimePriority = 0;
const char *routerTaskName;
/* 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);
ReturnValue_t startRouterTask();
static void *routerWorkWrapper(void *args);
};
#endif /* LINUX_POWER_CSPCOMIF_H_ */