diff --git a/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan.Makefile index 78f17d2..3df5337 100644 --- a/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan.Makefile @@ -50,6 +50,7 @@ TEMPLATES += $(wildcard $(APPDB)/*.template) SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp +SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile index 78f17d2..3df5337 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile @@ -50,6 +50,7 @@ TEMPLATES += $(wildcard $(APPDB)/*.template) SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp +SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c index cabf6d3..a176ffd 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c @@ -163,10 +163,6 @@ double can_connected() { return (double)getSocketCANConnectd(); } -double can_write_busy() { - return (double)getWriteBusy(); -} - // trigger all writes added to buffer double can_trigg_writes() { return (double)triggWrites(); @@ -319,29 +315,6 @@ struct ecmcPluginData pluginDataDef = { .funcGenericObj = NULL, }, .funcs[4] = - { /*----can_write_busy----*/ - // Function name (this is the name you use in ecmc plc-code) - .funcName = "can_write_busy", - // Function description - .funcDesc = "double can_write_busy() : get can busy writing added frames.", - /** - * 7 different prototypes allowed (only doubles since reg in plc). - * Only funcArg${argCount} func shall be assigned the rest set to NULL. - **/ - .funcArg0 = can_write_busy, - .funcArg1 = NULL, - .funcArg2 = NULL, - .funcArg3 = NULL, - .funcArg4 = NULL, - .funcArg5 = NULL, - .funcArg6 = NULL, - .funcArg7 = NULL, - .funcArg8 = NULL, - .funcArg9 = NULL, - .funcArg10 = NULL, - .funcGenericObj = NULL, - }, - .funcs[5] = { /*----can_last_writes_error----*/ // Function name (this is the name you use in ecmc plc-code) .funcName = "can_last_writes_error", @@ -364,7 +337,7 @@ struct ecmcPluginData pluginDataDef = { .funcArg10 = NULL, .funcGenericObj = NULL, }, - .funcs[6] = {0}, // last element set all to zero.. + .funcs[5] = {0}, // last element set all to zero.. // PLC consts .consts[0] = {0}, // last element set all to zero.. }; diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 77b50e4..5706b85 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -50,17 +50,6 @@ void f_worker_connect(void *obj) { canObj->doConnectWorker(); } -// Start worker for socket read() -void f_worker_write(void *obj) { - if(!obj) { - printf("%s/%s:%d: Error: Worker write thread ecmcSocketCAN object NULL..\n", - __FILE__, __FUNCTION__, __LINE__); - return; - } - ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj; - canObj->doWriteWorker(); -} - /** ecmc ecmcSocketCAN class * This object can throw: * - bad_alloc @@ -91,13 +80,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, destructs_ = 0; socketId_ = -1; connected_ = 0; - writeCmdCounter_ = 0; - writeBusy_ = 0; - lastWriteSumError_ = 0; + writeBuffer_ = NULL; + memset(&ifr_,0,sizeof(struct ifreq)); memset(&rxmsg_,0,sizeof(struct can_frame)); - //memset(&txmsg_,0,sizeof(struct can_frame)); - memset(&txmsgBuffer_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS); memset(&addr_,0,sizeof(struct sockaddr_can)); parseConfigStr(configStr); // Assigns all configs @@ -118,16 +104,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, throw std::runtime_error("Error: Failed create worker thread for connect()."); } - // Create worker thread for writing socket - threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write"; - if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) { - throw std::runtime_error("Error: Failed create worker thread for write()."); - } - if(cfgAutoConnect_) { connectPrivate(); } - + writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_); initAsyn(); } @@ -255,108 +235,49 @@ void ecmcSocketCAN::doConnectWorker() { } } -// Write socket worker -void ecmcSocketCAN::doWriteWorker() { - int errorCode = 0; - while(true) { - if(destructs_) { - return; - } - - doWriteEvent_.wait(); - if(destructs_) { - return; - } - for(int i=0; i=ECMC_CAN_MAX_WRITE_CMDS) { - return ECMC_CAN_ERROR_WRITE_FULL; - } - - txmsgBuffer_[writeCmdCounter_].can_id = canId; - txmsgBuffer_[writeCmdCounter_].can_dlc = len; - txmsgBuffer_[writeCmdCounter_].data[0] = data0; - txmsgBuffer_[writeCmdCounter_].data[1] = data1; - txmsgBuffer_[writeCmdCounter_].data[2] = data2; - txmsgBuffer_[writeCmdCounter_].data[3] = data3; - txmsgBuffer_[writeCmdCounter_].data[4] = data4; - txmsgBuffer_[writeCmdCounter_].data[5] = data5; - txmsgBuffer_[writeCmdCounter_].data[6] = data6; - txmsgBuffer_[writeCmdCounter_].data[7] = data7; - - writeCmdCounter_++; + writeBuffer_->triggWrites(); return 0; } -int ecmcSocketCAN::getWriteBusy() { - return writeBusy_; -} - int ecmcSocketCAN::getlastWritesError() { - return lastWriteSumError_; -} - -// Trigger all writes -int ecmcSocketCAN::triggWrites() { - if(writeBusy_ || writeCmdCounter_ == 0) { - return ECMC_CAN_ERROR_WRITE_BUSY; + if(!writeBuffer_) { + return ECMC_CAN_ERROR_WRITE_BUFFER_NULL; } - writeBusy_= true; - lastWriteSumError_ = 0; - doWriteEvent_.signal(); // let worker start - return 0; + return writeBuffer_->getlastWritesError(); } -// Test can write -int ecmcSocketCAN::writeCAN(can_frame *frame){ +int ecmcSocketCAN::addWriteCAN(uint32_t canId, + uint8_t len, + uint8_t data0, + uint8_t data1, + uint8_t data2, + uint8_t data3, + uint8_t data4, + uint8_t data5, + uint8_t data6, + uint8_t data7) { - if(!frame) { - return ECMC_CAN_ERROR_WRITE_NO_DATA; + if(!writeBuffer_) { + return ECMC_CAN_ERROR_WRITE_BUFFER_NULL; } - // Maybe need to add the size to write here.. if struct is not full, hmm?! - int nbytes = write(socketId_, frame, sizeof(struct can_frame)); - if (nbytes!= sizeof(struct can_frame)) { - return ECMC_CAN_ERROR_WRITE_INCOMPLETE; - } - - if(cfgDbgMode_) { - // Simulate candump printout - printf("w 0x%03X", frame->can_id); - printf(" [%d]", frame->can_dlc); - for(int i=0; ican_dlc; i++ ) { - printf(" 0x%02X", frame->data[i]); - } - printf("\n"); - } + writeBuffer_->addWriteCAN(canId, + len, + data0, + data1, + data2, + data3, + data4, + data5, + data6, + data7); return 0; } - + void ecmcSocketCAN::initAsyn() { // Add enable "plugin.fft%d.enable" diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h index c4d89ca..d8e28e8 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -9,13 +9,14 @@ * Author: anderssandstrom * \*************************************************************************/ -#ifndef ECMC_FFT_H_ -#define ECMC_FFT_H_ +#ifndef ECMC_SOCKETCAN_H_ +#define ECMC_SOCKETCAN_H_ #include #include "ecmcDataItem.h" #include "ecmcAsynPortDriver.h" #include "ecmcSocketCANDefs.h" +#include "ecmcSocketCANWriteBuffer.h" #include "inttypes.h" #include @@ -37,6 +38,7 @@ #define ECMC_CAN_ERROR_WRITE_BUSY 11 #define ECMC_CAN_ERROR_WRITE_NO_DATA 12 #define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13 +#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14 class ecmcSocketCAN : public asynPortDriver { public: @@ -74,7 +76,6 @@ class ecmcSocketCAN : public asynPortDriver { uint8_t data6, uint8_t data7); int triggWrites(); - int getWriteBusy(); int getlastWritesError(); private: @@ -91,7 +92,6 @@ class ecmcSocketCAN : public asynPortDriver { epicsEvent doConnectEvent_; epicsEvent doWriteEvent_; struct can_frame rxmsg_; - //struct can_frame txmsg_; struct ifreq ifr_; int socketId_; struct sockaddr_can addr_; @@ -99,6 +99,8 @@ class ecmcSocketCAN : public asynPortDriver { int writeCmdCounter_; int writeBusy_; int lastWriteSumError_; + + ecmcSocketCANWriteBuffer *writeBuffer_; }; -#endif /* ECMC_FFT_H_ */ +#endif /* ECMC_SOCKETCAN_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp index 5943d92..37914e4 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp @@ -62,6 +62,7 @@ int connectSocketCAN() { else { return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE; } + return 0; } int getSocketCANConnectd() { @@ -77,19 +78,6 @@ int getSocketCANConnectd() { return 0; } -int getWriteBusy() { - if(can){ - try { - return can->getWriteBusy(); - } - catch(std::exception& e) { - printf("Exception: %s.\n",e.what()); - return 1; - } - } - return 1; -} - int getlastWritesError() { if(can){ try { diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp new file mode 100644 index 0000000..4ca9a74 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp @@ -0,0 +1,209 @@ +/*************************************************************************\ +* Copyright (c) 2019 European Spallation Source ERIC +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcSocketCANWriteBuffer.cpp +* +* Created on: Mar 22, 2020 +* Author: anderssandstrom +* Credits to https://github.com/sgreg/dynamic-loading +* +\*************************************************************************/ + +// Needed to get headers in ecmc right... +#define ECMC_IS_PLUGIN + +#include +#include "ecmcSocketCANWriteBuffer.h" +#include "epicsThread.h" + +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can" + + +// Start worker for socket read() +void f_worker_write(void *obj) { + if(!obj) { + printf("%s/%s:%d: Error: Worker write thread ecmcSocketCANWriteBuffer object NULL..\n", + __FILE__, __FUNCTION__, __LINE__); + return; + } + ecmcSocketCANWriteBuffer * canObj = (ecmcSocketCANWriteBuffer*)obj; + canObj->doWriteWorker(); +} + +/** ecmc ecmcSocketCANWriteBuffer class +*/ +ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode) { + memset(&txmsgBuffer1_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS); + memset(&txmsgBuffer2_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS); + bufferIdAddFrames_ = 1; // start to add frames to buffer 1 + writeCmdCounter1_ = 0; + writeCmdCounter2_ = 0; + writeBusy_ = 0; + socketId_ = socketId; + cfgDbgMode_ = cfgDbgMode; + destructs_ = 0; + + + // Create worker thread for writing socket + std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write"; + if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) { + throw std::runtime_error("Error: Failed create worker thread for write()."); + } + +} + +ecmcSocketCANWriteBuffer::~ecmcSocketCANWriteBuffer() { + // kill worker + destructs_ = 1; // maybe need todo in other way.. + doWriteEvent_.signal(); +} + +// Write socket worker thread (switch between two buffers) +void ecmcSocketCANWriteBuffer::doWriteWorker() { + int errorCode = 0; + while(true) { + if(destructs_) { + return; + } + + doWriteEvent_.wait(); + if(destructs_) { + return; + } + if(bufferIdAddFrames_ == 1) { + // Write buffer 2 since addFrames to buffer 1 + for(int i=0; i= ECMC_CAN_MAX_WRITE_CMDS){ + return ECMC_CAN_ERROR_WRITE_FULL; + } + if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){ + return ECMC_CAN_ERROR_WRITE_FULL; + } + } else { // switch buffer if full + if(bufferIdAddFrames_ == 1 && writeCmdCounter1_ >= ECMC_CAN_MAX_WRITE_CMDS){ + triggWrites(); // will also switch buffer id + } + if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){ + triggWrites(); // will also switch buffer id + } + } + + if(bufferIdAddFrames_ == 1){ + txmsgBuffer1_[writeCmdCounter1_].can_id = canId; + txmsgBuffer1_[writeCmdCounter1_].can_dlc = len; + txmsgBuffer1_[writeCmdCounter1_].data[0] = data0; + txmsgBuffer1_[writeCmdCounter1_].data[1] = data1; + txmsgBuffer1_[writeCmdCounter1_].data[2] = data2; + txmsgBuffer1_[writeCmdCounter1_].data[3] = data3; + txmsgBuffer1_[writeCmdCounter1_].data[4] = data4; + txmsgBuffer1_[writeCmdCounter1_].data[5] = data5; + txmsgBuffer1_[writeCmdCounter1_].data[6] = data6; + txmsgBuffer1_[writeCmdCounter1_].data[7] = data7; + writeCmdCounter1_++; + } + else { + txmsgBuffer2_[writeCmdCounter2_].can_id = canId; + txmsgBuffer2_[writeCmdCounter2_].can_dlc = len; + txmsgBuffer2_[writeCmdCounter2_].data[0] = data0; + txmsgBuffer2_[writeCmdCounter2_].data[1] = data1; + txmsgBuffer2_[writeCmdCounter2_].data[2] = data2; + txmsgBuffer2_[writeCmdCounter2_].data[3] = data3; + txmsgBuffer2_[writeCmdCounter2_].data[4] = data4; + txmsgBuffer2_[writeCmdCounter2_].data[5] = data5; + txmsgBuffer2_[writeCmdCounter2_].data[6] = data6; + txmsgBuffer2_[writeCmdCounter2_].data[7] = data7; + writeCmdCounter2_++; + } + return 0; +} + +int ecmcSocketCANWriteBuffer::getlastWritesError() { + return lastWriteSumError_; +} + +// Trigger all writes +int ecmcSocketCANWriteBuffer::triggWrites() { + + if(writeBusy_) { + return ECMC_CAN_ERROR_WRITE_BUSY; + } + + if(bufferIdAddFrames_ == 1) { + bufferIdAddFrames_ = 2; + } else { + bufferIdAddFrames_ = 1; + } + writeBusy_ = 1; + lastWriteSumError_ = 0; + doWriteEvent_.signal(); // let worker start + return 0; +} + +// Write to socket +int ecmcSocketCANWriteBuffer::writeCAN(can_frame *frame){ + + if(!frame) { + return ECMC_CAN_ERROR_WRITE_NO_DATA; + } + + // Maybe need to add the size to write here.. if struct is not full, hmm?! + int nbytes = write(socketId_, frame, sizeof(struct can_frame)); + if (nbytes!= sizeof(struct can_frame)) { + return ECMC_CAN_ERROR_WRITE_INCOMPLETE; + } + + if(cfgDbgMode_) { + // Simulate candump printout + printf("w 0x%03X", frame->can_id); + printf(" [%d]", frame->can_dlc); + for(int i=0; ican_dlc; i++ ) { + printf(" 0x%02X", frame->data[i]); + } + printf("\n"); + } + return 0; +} + +// Avoid issues with std:to_string() +std::string ecmcSocketCANWriteBuffer::to_string(int value) { + std::ostringstream os; + os << value; + return os.str(); +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h new file mode 100644 index 0000000..24ddfc2 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h @@ -0,0 +1,85 @@ +/*************************************************************************\ +* Copyright (c) 2019 European Spallation Source ERIC +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcSocketCANWriteBuffer.h +* +* Created on: Mar 22, 2020 +* Author: anderssandstrom +* +\*************************************************************************/ +#ifndef ECMC_SOCKETCAN_BUFFER_WRITE_H_ +#define ECMC_SOCKETCAN_BUFFER_WRITE_H_ + +#include "ecmcAsynPortDriver.h" + +#include +#include "inttypes.h" +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#define ECMC_CAN_MAX_WRITE_CMDS 128 +#define ECMC_CAN_ERROR_WRITE_FULL 10 +#define ECMC_CAN_ERROR_WRITE_BUSY 11 +#define ECMC_CAN_ERROR_WRITE_NO_DATA 12 +#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13 + +class ecmcSocketCANWriteBuffer { + public: + + /** ecmc ecmcSocketCANWriteBuffer class + * This object can throw: + * - bad_alloc + * - invalid_argument + * - runtime_error + * - out_of_range + */ + ecmcSocketCANWriteBuffer(int socketId, int dbgMode); + ~ecmcSocketCANWriteBuffer(); + + + void doWriteWorker(); + + int addWriteCAN(uint32_t canId, + uint8_t len, + uint8_t data0, + uint8_t data1, + uint8_t data2, + uint8_t data3, + uint8_t data4, + uint8_t data5, + uint8_t data6, + uint8_t data7); + int triggWrites(); + int getlastWritesError(); + + private: + static std::string to_string(int value); + int writeCAN(can_frame *frame); + int destructs_; + int cfgDbgMode_; + epicsEvent doWriteEvent_; + int socketId_; + struct can_frame txmsgBuffer1_[ECMC_CAN_MAX_WRITE_CMDS]; + struct can_frame txmsgBuffer2_[ECMC_CAN_MAX_WRITE_CMDS]; + int writeCmdCounter1_; + int writeCmdCounter2_; + int writeBusy_; + int lastWriteSumError_; + int bufferIdAddFrames_; +}; + +#endif /* ECMC_SOCKETCAN_BUFFER_WRITE_H_ */