extern "C" {
#include <rscanfd/rscanfd_uciaprcn/rscfd.h>
#include <rscanfd/rscanfd_uciaprcn/rscfd_p.h>
#include <rscanfd/rscanfd_uciaprcn/rscfd_s.h>
}
#include <canbusdevice.h>
#include <candriverhelper.h>
#include <canlog.h>
#include <rawcanmessageadapter.h>
#include <etl/unordered_multimap.h>
using namespace std;
namespace {
const uint8_t NUM_CHANNELS = 1 ;
const uint8_t CAN_CONTROLLER_ID = 0 ;
u16 lastGlobalError = 0 ;
u16 lastChannelError[ NUM_CHANNELS] = {0 };
struct RawHandlerCallback
{
CanBus:: CanBusDevice:: RawCanMessageHandler callback;
void * userData;
};
CanBus:: CanDriverHelper:: MessageFilterList filterList;
typedef etl:: unordered_multimap< uint32_t, RawHandlerCallback, CANBUS_MAX_RAW_HANDLERS> RawHandlersMap;
RawHandlersMap rawHandlers;
RawHandlerCallback globalHandler = {NULL, NULL};
}
static void int_globalError()
{
lastGlobalError = CanBus:: CanDriverHelper:: getError(CAN_CONTROLLER_ID, EE_RSCFD_GLOBAL);
}
static void int_channelError(unsigned int channelNo)
{
lastChannelError[ channelNo] = CanBus:: CanDriverHelper:: getError(CAN_CONTROLLER_ID, channelNo);
}
static void int_ch0Error()
{
int_channelError(0 );
}
static void printErrors()
{
if (lastGlobalError) {
LOG_CAN_ERROR("Global error %#x" , lastGlobalError);
lastGlobalError = 0 ;
}
for (int i = 0 ; i < NUM_CHANNELS; + + i) {
if (lastChannelError[ i] ) {
LOG_CAN_ERROR("Channel %d error %#x" , i, lastChannelError[ i] );
lastChannelError[ i] = 0 ;
}
}
}
static void handleCanFrame(ee_rscfd_message & rawFrame)
{
CanBus:: RawCanMessageAdapter frame(rawFrame);
pair< RawHandlersMap:: iterator, RawHandlersMap:: iterator> its = rawHandlers. equal_range(frame. id());
for (RawHandlersMap:: iterator it = its. first; it ! = its. second; + + it) {
const RawHandlerCallback & callback = it- > second;
callback. callback(frame. id(), frame. type(), frame. data(), frame. length(), callback. userData);
}
if (globalHandler. callback) {
globalHandler. callback(frame. id(), frame. type(), frame. data(), frame. length(), globalHandler. userData);
}
}
static bool doConfigure(bool canFD, uint32_t bitrate, uint32_t dataBitrate)
{
CanBus:: CanDriverHelper:: configure(canFD, bitrate, dataBitrate, NUM_CHANNELS);
if (! CanBus:: CanDriverHelper:: initCAN(CAN_CONTROLLER_ID, NUM_CHANNELS)) {
LOG_CAN_ERROR("Device: initCAN failed" );
return false ;
}
return true ;
}
static bool doConnect()
{
if (! CanBus:: CanDriverHelper:: setGlobalInterrupts(CAN_CONTROLLER_ID, int_globalError)) {
LOG_CAN_ERROR("Device: setGlobalInterrupts failed" );
return false ;
}
if (! CanBus:: CanDriverHelper:: setChannelInterrupts(CAN_CONTROLLER_ID, 0 , int_ch0Error)) {
LOG_CAN_ERROR("Device: setChannelInterrupts failed" );
return false ;
}
if (! CanBus:: CanDriverHelper:: setupReceieveRules(CAN_CONTROLLER_ID, 0 , filterList)) {
LOG_CAN_ERROR("Device: setupReceieveRules failed" );
return false ;
}
if (! CanBus:: CanDriverHelper:: startCAN(CAN_CONTROLLER_ID, 0 )) {
LOG_CAN_ERROR("Device: startCAN failed" );
return false ;
}
#ifdef CANBUS_USE_RX_FIFOS
if (! CanBus:: CanDriverHelper:: setupFIFOs(CAN_CONTROLLER_ID)) {
LOG_CAN_ERROR("Device: setupFIFOs failed" );
return false ;
}
#endif
return true ;
}
namespace CanBus {
CanBusDevice:: CanBusDevice()
{
status. setValue(UnconnectedState);
isCanFD. setValue(false );
}
void CanBusDevice:: connect()
{
disconnect();
LOG_CAN_INFO("Device: Connecting..." );
status. setValue(ConnectingState);
if (! doConfigure(isCanFD. value(), bitrate. value(), dataBitrate. value())) {
LOG_CAN_ERROR("Device: Configuration failed" );
error(ConfigurationError);
status. setValue(UnconnectedState);
return ;
}
if (! doConnect()) {
LOG_CAN_ERROR("Device: Connection failed" );
error(ConnectionError);
status. setValue(UnconnectedState);
return ;
}
LOG_CAN_INFO("Device: Connected" );
status. setValue(ConnectedState);
}
void CanBusDevice:: disconnect()
{
CanBusDeviceState currentState = status. value();
status. setValue(DisconnectingState);
if (! CanDriverHelper:: stopCAN(CAN_CONTROLLER_ID)) {
LOG_CAN_ERROR("Device: Failed to disconect" );
error(UnknownError);
status. setValue(currentState);
return ;
}
status. setValue(UnconnectedState);
}
void CanBusDevice:: sendRawMessage(uint32_t msgId,
CanFrameType type,
const uint8_t * payload,
uint8_t payloadLength,
bool isExtended,
bool isCanFD,
bool bitrateSwitch)
{
ee_rscfd_message rawMessage = {0 };
RawCanMessageAdapter message(rawMessage);
message. setId(msgId)
. setData(payload, payloadLength)
. setIsExtended(isExtended)
. setIsCanFD(isCanFD)
. setBitrateSwitch(bitrateSwitch);
if (! CanDriverHelper:: sendMessage(CAN_CONTROLLER_ID, 0 , rawMessage)) {
LOG_CAN_ERROR("Device: Failed to send message!" );
}
}
void CanBusDevice:: appendMessageFilter(uint32_t frameId, uint32_t frameIdMask, FormatFilter format, CanFrameType type)
{
CanDriverHelper:: MessageFiler filter;
filter. id = frameId;
filter. mask = frameIdMask;
if (format = = MatchBaseFormat | | format = = MatchExtendedFormat) {
filter. extenedFrame = format = = MatchExtendedFormat;
}
if (type = = DataFrame | | type = = RemoteRequestFrame) {
filter. remoteFrame = type = = RemoteRequestFrame;
}
filterList. push_back(filter);
}
bool CanBusDevice:: registerRawMessageHandler(uint32_t msgId, RawCanMessageHandler handler, void * userData)
{
if (! rawHandlers. full() | | handler = = NULL) {
RawHandlerCallback callback = {handler, userData};
rawHandlers. insert(std:: pair< uint32_t, RawHandlerCallback> (msgId, callback));
return true ;
}
return false ;
}
bool CanBusDevice:: registerGlobalMessageHandler(RawCanMessageHandler handler, void * userData)
{
if (globalHandler. callback = = NULL) {
globalHandler. callback = handler;
globalHandler. userData = userData;
return true ;
}
return false ;
}
void CanBusDevice:: processPendingData()
{
#ifndef NDEBUG
printErrors();
#ifdef CANBUS_USE_RX_FIFOS
CanDriverHelper:: printFIFOStatus(CAN_CONTROLLER_ID, false , true , true );
#endif
#endif
if (! CanDriverHelper:: receiveMessages(CAN_CONTROLLER_ID, handleCanFrame)) {
LOG_CAN_ERROR("Device: Error while receiving messages" );
error(ReadError);
}
}
}