From 1c210d6b421012bfceb54c64e080726cba0b4b05 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Tue, 2 Mar 2021 17:17:21 +0100 Subject: [PATCH] Add thread for writes! --- .../src/ecmcPluginSocketCAN.c | 132 ++++++++++++--- .../src/ecmcSocketCAN.cpp | 150 ++++++++++++++---- .../src/ecmcSocketCAN.h | 38 +++-- .../src/ecmcSocketCANWrap.cpp | 80 +++++++--- .../src/ecmcSocketCANWrap.h | 37 +++-- 5 files changed, 337 insertions(+), 100 deletions(-) diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c index 45aeff5..cabf6d3 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c @@ -163,26 +163,41 @@ double can_connected() { return (double)getSocketCANConnectd(); } -double can_write(double canId, - double len, - double data0, - double data1, - double data2, - double data3, - double data4, - double data5, - double data6, - double data7) { - return (double)writeSocketCAN(canId, - len, - data0, - data1, - data2, - data3, - data4, - data5, - data6, - data7); +double can_write_busy() { + return (double)getWriteBusy(); +} + +// trigger all writes added to buffer +double can_trigg_writes() { + return (double)triggWrites(); +} + +// trigger all writes added to buffer +double can_last_writes_error() { + return (double)getlastWritesError(); +} + +// Add frrame to output buffer +double can_add_write(double canId, + double len, + double data0, + double data1, + double data2, + double data3, + double data4, + double data5, + double data6, + double data7) { + return (double)addWriteSocketCAN(canId, + len, + data0, + data1, + data2, + data3, + data4, + data5, + data6, + data7); } // Register data for plugin so ecmc know what to use @@ -260,9 +275,9 @@ struct ecmcPluginData pluginDataDef = { .funcs[2] = { /*----can_connected----*/ // Function name (this is the name you use in ecmc plc-code) - .funcName = "can_write", + .funcName = "can_add_write", // Function description - .funcDesc = "double can_write(canId,len,data0..data7) : Write to can interface.", + .funcDesc = "double can_add_write(canId,len,data0..data7) : Add write frame to can interface output buffer.", /** * 7 different prototypes allowed (only doubles since reg in plc). * Only funcArg${argCount} func shall be assigned the rest set to NULL. @@ -277,10 +292,79 @@ struct ecmcPluginData pluginDataDef = { .funcArg7 = NULL, .funcArg8 = NULL, .funcArg9 = NULL, - .funcArg10 = can_write, + .funcArg10 = can_add_write, .funcGenericObj = NULL, }, - .funcs[3] = {0}, // last element set all to zero.. + .funcs[3] = + { /*----can_trigg_writes----*/ + // Function name (this is the name you use in ecmc plc-code) + .funcName = "can_trigg_writes", + // Function description + .funcDesc = "double can_trigg_writes() : Trigger write of all 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_trigg_writes, + .funcArg1 = NULL, + .funcArg2 = NULL, + .funcArg3 = NULL, + .funcArg4 = NULL, + .funcArg5 = NULL, + .funcArg6 = NULL, + .funcArg7 = NULL, + .funcArg8 = NULL, + .funcArg9 = NULL, + .funcArg10 = NULL, + .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", + // Function description + .funcDesc = "double can_last_writes_error() : get error from last writes.", + /** + * 7 different prototypes allowed (only doubles since reg in plc). + * Only funcArg${argCount} func shall be assigned the rest set to NULL. + **/ + .funcArg0 = can_last_writes_error, + .funcArg1 = NULL, + .funcArg2 = NULL, + .funcArg3 = NULL, + .funcArg4 = NULL, + .funcArg5 = NULL, + .funcArg6 = NULL, + .funcArg7 = NULL, + .funcArg8 = NULL, + .funcArg9 = NULL, + .funcArg10 = NULL, + .funcGenericObj = NULL, + }, + .funcs[6] = {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 e130196..77b50e4 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -50,6 +50,17 @@ 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 @@ -80,11 +91,15 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, destructs_ = 0; socketId_ = -1; connected_ = 0; + writeCmdCounter_ = 0; + writeBusy_ = 0; + lastWriteSumError_ = 0; memset(&ifr_,0,sizeof(struct ifreq)); memset(&rxmsg_,0,sizeof(struct can_frame)); - memset(&txmsg_,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 // Check valid nfft if(!cfgCanIFStr_ ) { @@ -103,6 +118,12 @@ 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(); } @@ -113,7 +134,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, ecmcSocketCAN::~ecmcSocketCAN() { // kill worker destructs_ = 1; // maybe need todo in other way.. - //doCalcEvent_.signal(); + doWriteEvent_.signal(); + doConnectEvent_.signal(); } void ecmcSocketCAN::parseConfigStr(char *configStr) { @@ -217,56 +239,122 @@ void ecmcSocketCAN::doReadWorker() { } } -// Read socket worker +// Connect socket worker void ecmcSocketCAN::doConnectWorker() { while(true) { if(destructs_) { - break; + return; } doConnectEvent_.wait(); + if(destructs_) { + return; + } connectPrivate(); } } +// 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_++; + 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; + } + writeBusy_= true; + lastWriteSumError_ = 0; + doWriteEvent_.signal(); // let worker start + return 0; +} + +// Test can write +int ecmcSocketCAN::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_, &txmsg_, sizeof(struct can_frame)); + int nbytes = write(socketId_, frame, sizeof(struct can_frame)); if (nbytes!= sizeof(struct can_frame)) { - throw std::runtime_error( "Error in write."); + return ECMC_CAN_ERROR_WRITE_INCOMPLETE; } if(cfgDbgMode_) { // Simulate candump printout - printf("w 0x%03X", txmsg_.can_id); - printf(" [%d]", txmsg_.can_dlc); - for(int i=0; ican_id); + printf(" [%d]", frame->can_dlc); + for(int i=0; ican_dlc; i++ ) { + printf(" 0x%02X", frame->data[i]); } printf("\n"); } + return 0; } void ecmcSocketCAN::initAsyn() { diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h index e3b3812..c4d89ca 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -32,6 +32,12 @@ #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 ecmcSocketCAN : public asynPortDriver { public: @@ -47,6 +53,7 @@ class ecmcSocketCAN : public asynPortDriver { ~ecmcSocketCAN(); void doReadWorker(); + void doWriteWorker(); void doConnectWorker(); virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value); @@ -56,33 +63,42 @@ class ecmcSocketCAN : public asynPortDriver { virtual asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value); void connectExternal(); int getConnected(); - void writeCAN( 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 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 getWriteBusy(); + int getlastWritesError(); private: void parseConfigStr(char *configStr); void initAsyn(); static std::string to_string(int value); void connectPrivate(); + int writeCAN(can_frame *frame); char* cfgCanIFStr_; // Config: can interface can0, vcan0.. int cfgDbgMode_; int cfgAutoConnect_; int destructs_; int connected_; epicsEvent doConnectEvent_; + epicsEvent doWriteEvent_; struct can_frame rxmsg_; - struct can_frame txmsg_; + //struct can_frame txmsg_; struct ifreq ifr_; int socketId_; struct sockaddr_can addr_; + struct can_frame txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS]; + int writeCmdCounter_; + int writeBusy_; + int lastWriteSumError_; }; #endif /* ECMC_FFT_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp index 822c7ec..5943d92 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp @@ -77,29 +77,67 @@ int getSocketCANConnectd() { return 0; } -int writeSocketCAN( double canId, - double len, - double data0, - double data1, - double data2, - double data3, - double data4, - double data5, - double data6, - double data7) { +int getWriteBusy() { if(can){ try { - can->writeCAN((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); - return 0; + return can->getWriteBusy(); + } + catch(std::exception& e) { + printf("Exception: %s.\n",e.what()); + return 1; + } + } + return 1; +} + +int getlastWritesError() { + if(can){ + try { + return can->getlastWritesError(); + } + catch(std::exception& e) { + printf("Exception: %s.\n",e.what()); + return 1; + } + } + return 1; +} + +int triggWrites() { + if(can){ + try { + return can->triggWrites(); + } + catch(std::exception& e) { + printf("Exception: %s.\n",e.what()); + return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE; + } + } + return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE; +} + +int addWriteSocketCAN( double canId, + double len, + double data0, + double data1, + double data2, + double data3, + double data4, + double data5, + double data6, + double data7) { + if(can){ + try { + return can->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); } catch(std::exception& e) { printf("Exception: %s.\n",e.what()); diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h index c34fbe4..f11706b 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h @@ -36,22 +36,33 @@ int connectSocketCAN(); /** \brief Connected to can interface\n */ - int getSocketCANConnectd(); - -/** \brief Write CAN +/** \brief Write all buffered writes to CAN */ -int writeSocketCAN( double canId, - double len, - double data0, - double data1, - double data2, - double data3, - double data4, - double data5, - double data6, - double data7); +int triggWrites(); + +/** \brief Busy writing\n + */ +int getWriteBusy(); + +/** \brief Get last error from writes\n + */ +int getlastWritesError(); + + +/** \brief add CAN frame to write buffer + */ +int addWriteSocketCAN( double canId, + double len, + double data0, + double data1, + double data2, + double data3, + double data4, + double data5, + double data6, + double data7); /** \brief Delete SocketCAN object\n *