eive-obsw/bsp_linux/comIF/CspComIF.cpp
2020-12-20 13:31:44 +01:00

224 lines
6.9 KiB
C++

#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;
// 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;
// }
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 cmdBufferLen, uint16_t querySize) {
uint32_t timeout_ms = 500;
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();
uint8_t tmpCmdBuffer[cmdBufferLen];
memcpy(tmpCmdBuffer, cmdBuffer, cmdBufferLen);
csp_conn_t * conn = csp_connect(CSP_PRIO_HIGH, cspAddress, cspPort, 0,
CSP_O_NONE);
int result = csp_transaction_persistent(conn, timeout_ms,
tmpCmdBuffer, cmdBufferLen, replyBuffer, querySize);
if(querySize != 0){
if(result != querySize){
sif::error << "CSP transfer failed to receive all requested bytes "
<< std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
} else {
if(result != 1){
sif::error << "CSP transfer failed" << std::endl;
return HasReturnvaluesIF::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);
}