248 lines
8.0 KiB
C++
248 lines
8.0 KiB
C++
#include "CspComIF.h"
|
|
|
|
#include <csp/drivers/can_socketcan.h>
|
|
#include <fsfw/serialize/SerializeAdapter.h>
|
|
#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
|
|
|
|
#include "CspCookie.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()) {
|
|
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 */
|
|
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 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, vectorBuffer(maxReplyLength));
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t CspComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) {
|
|
int result;
|
|
if (cookie == NULL) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
CspCookie* cspCookie = dynamic_cast<CspCookie*>(cookie);
|
|
if (cspCookie == NULL) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
/* Extract csp port and bytes to query from command buffer */
|
|
uint8_t cspPort;
|
|
uint16_t querySize = 0;
|
|
result = getPortAndQuerySize(&sendData, &sendLen, &cspPort, &querySize);
|
|
if (result != returnvalue::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 != returnvalue::OK) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
replySize = 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();
|
|
|
|
*buffer = cspDeviceMap[cspAddress].data();
|
|
*size = replySize;
|
|
|
|
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);
|
|
vectorBufferIter 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.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.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;
|
|
/* Store reply time in reply buffer * */
|
|
uint8_t* replyBuffer = cspDeviceMap[cspAddress].data();
|
|
memcpy(replyBuffer, &replyTime, sizeof(replyTime));
|
|
replySize = sizeof(replyTime);
|
|
}
|