Try to sync SDO, WIP

ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile

@@ -42,6 +42,7 @@ APPDB:=$(APP)/Db
 APPSRC:=$(APP)/src
 
 USR_CFLAGS   += -shared -fPIC -Wall -Wextra
+USR_CPPFLAGS += -std=c++11
 USR_LDFLAGS  += -lstdc++
 USR_INCLUDES += -I$(where_am_I)$(APPSRC)
 

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp

@@ -35,7 +35,8 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
   isMaster_           = false;
   pdoCounter_ = 0;
   sdoCounter_ = 0;
-
+  sdo1Busy_.test_and_set();   // make sure only one sdo is accessing the bus at the same time
+  sdo1Busy_.clear();
   for(int i = 0 ; i<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
     pdos_[i] = NULL;
   }

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h

@@ -13,6 +13,7 @@
 #define ECMC_CANOPEN_DEVICE_H_
 
 #include <stdexcept>
+#include <atomic>
 #include "ecmcDataItem.h"
 #include "ecmcAsynPortDriver.h"
 #include "ecmcSocketCANDefs.h"
@@ -23,6 +24,7 @@
 #include "ecmcSocketCANWriteBuffer.h"
 #include "epicsMutex.h"
 
+
 #include <linux/can.h>
 #include <linux/can/raw.h>
 
@@ -73,6 +75,7 @@ class ecmcCANOpenDevice {
   ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
   ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
   bool isMaster_;
+  std::atomic_flag sdo1Busy_;
 };
 
 #endif  /* ECMC_CANOPEN_DEVICE_H_ */

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp

@@ -41,6 +41,8 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   ODSize_             = ODSize;
   dbgMode_            = dbgMode;
   name_               = strdup(name);
+  errorCode_          = 0;
+  dataMutex_ = epicsMutexCreate();
 
   // convert to ODIndex_ to indiviual bytes struct
   memcpy(&ODIndexBytes_, &ODIndex, 2);
@@ -50,14 +52,14 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   exeSampleTimeMs_    = exeSampleTimeMs;
   rw_                 = rw;
   exeCounter_         = 0;
-  busy_               = 0;
   recivedBytes_       = 0;
   writtenBytes_       = 0; 
   readStates_         = READ_IDLE;
   writeStates_        = WRITE_IDLE;
   useTg1Frame_        = 1;
   dataBuffer_         = new uint8_t(ODSize_);
-
+  tempReadBuffer_     = new uint8_t(ODSize_);
+  busyCounter_        = 0;
   // Request data (send on slave RX)
   // w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00
   readReqTransferFrame_.can_id  = cobIdRx;
@@ -167,22 +169,48 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeConfReqFrameTg1_.data[4] = 0;
   writeConfReqFrameTg1_.data[5] = 0;
   writeConfReqFrameTg1_.data[6] = 0;
-  writeConfReqFrameTg1_.data[7] = 0;  
+  writeConfReqFrameTg1_.data[7] = 0;
+  busy_.clear();
 }
 
 ecmcCANOpenSDO::~ecmcCANOpenSDO() {
   delete[] dataBuffer_;
+  delete[] tempReadBuffer_;
   free(name_);
 }
 
 void ecmcCANOpenSDO::execute() {
+  
+  
+  if(busy_.test_and_set()) {
+   busyCounter_++;
+  } else {
+    busy_.clear();
+    busyCounter_ = 0;
+  }
+  
+  if(busyCounter_>ECMC_SDO_REPLY_TIMOUT_MS) {
+    // cancel read or write
+    printf("SDO BUSY timeout!!\n");
+    memset(tempReadBuffer_,0,ODSize_);
+    readStates_ = READ_IDLE;
+    writeStates_ = WRITE_IDLE;
+    exeCounter_  = 0;
+    busyCounter_ = 0;
+    errorCode_ = ECMC_CAN_ERROR_SDO_TIMEOUT;
+    busy_.clear();
+  }
 
-  exeCounter_++;
-  if(exeCounter_* exeSampleTimeMs_ >= readSampleTimeMs_ && ! busy_) {
-    exeCounter_ =0;
+  if(exeCounter_* exeSampleTimeMs_ < readSampleTimeMs_ && rw_ == DIR_READ) { // do not risk overflow
+    exeCounter_++;
+  } else { // Counter is higher, try to write
     if(rw_ == DIR_READ) {      
-      busy_ = 1;
-     
+      if(busy_.test_and_set()) {
+        // wait for busy to go down
+        return;
+      }      
+      
+      exeCounter_ =0; 
       readStates_ = READ_REQ_TRANSFER;
       if(dbgMode_) {
         printf("STATE = READ_REQ_TRANSFER\n");
@@ -205,7 +233,8 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
   // Wait for:
   // # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
   int errorCode = 0;
-  if(!busy_) {
+  if(!busy_.test_and_set()) {
+    busy_.clear();
     // Not waiting for any data..
     return;
   }
@@ -246,7 +275,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
       }
       
       if(bytesToRead + recivedBytes_ <= ODSize_) {
-        memcpy(dataBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead);
+        memcpy(tempReadBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead);
         recivedBytes_ += bytesToRead;
       }
       if(recivedBytes_ < ODSize_) {  // Ask for more data but must toggle so alternat the prepared frames
@@ -263,14 +292,20 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
       }
       
       if (recivedBytes_ >= ODSize_) {
-        readStates_ =READ_IDLE;
-        busy_ = 0;
+        readStates_ =READ_IDLE;        
         useTg1Frame_ = 0;
+        
+        epicsMutexLock(dataMutex_);
+        memcpy(dataBuffer_,tempReadBuffer_,ODSize_);
+        epicsMutexUnlock(dataMutex_);
         if(dbgMode_) {
           printf("STATE = READ_IDLE\n");
           printf("All data read from slave SDO.\n");
-          printBuffer();            
+          //copy complete data to dataBuffer_
+          printBuffer();
         }
+
+        busy_.clear();        
         return 0;
       }
       break;
@@ -312,13 +347,13 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
       // Check if write was done already or if more frames are needed!
       if(writtenBytes_ >= ODSize_) {
         writeStates_ = WRITE_IDLE;
-        useTg1Frame_ = 0;
-        busy_ = 0;
+        useTg1Frame_ = 0;        
         if(dbgMode_) {
           printf("STATE = WRITE_IDLE\n");
           printf("All data written to slave SDO.\n");
           printBuffer();          
         }
+        busy_.clear();
         return 0;
       }
 
@@ -415,17 +450,22 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
   if(bytesToCopy == 0) {
     return;
   }
+  epicsMutexLock(dataMutex_);
   memcpy(dataBuffer_, &data, ODSize_);
+  epicsMutexUnlock(dataMutex_);
 }
 
 int ecmcCANOpenSDO::writeValue() {
   // Busy right now!
   printf("WRITEVALUE!!\n");
-  if(busy_ || writeStates_ != WRITE_IDLE ) {
+  if(busy_.test_and_set()) {
     return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
   }
-  busy_ = 1;
-
+  
+  if(writeStates_ != WRITE_IDLE ) {
+    return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
+  }
+  
   writeStates_ = WRITE_REQ_TRANSFER;
   if(dbgMode_) {
     printf("STATE = WRITE_REQ_TRANSFER\n");

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h

@@ -26,6 +26,7 @@
 
 #define ECMC_CAN_ERROR_SDO_WRITE_BUSY 110
 
+#define ECMC_CAN_ERROR_SDO_TIMEOUT 111
 
 
 class ecmcCANOpenSDO {
@@ -40,7 +41,7 @@ class ecmcCANOpenSDO {
                  int readSampleTimeMs,
                  int exeSampleTimeMs,
                  const char *name,
-                 int dbgMode);
+                 int dbgMode);                
   ~ecmcCANOpenSDO();
   void execute();
   void newRxFrame(can_frame *frame);
@@ -78,8 +79,9 @@ class ecmcCANOpenSDO {
   can_frame writeConfReqFrameTg1_;
   
   int dbgMode_;
-  int busy_;
+  int errorCode_;
   uint8_t *dataBuffer_;
+  uint8_t *tempReadBuffer_;
   uint32_t recivedBytes_;
   int useTg1Frame_;
   ecmc_read_states readStates_;
@@ -87,6 +89,10 @@ class ecmcCANOpenSDO {
   void printBuffer();
   uint32_t writtenBytes_;
   char *name_;
+  epicsMutexId  dataMutex_;
+  int busyCounter_;
+  //std::atomic_flag *ptrSdo1Busy_;
+  std::atomic_flag busy_;
 };
 
 #endif  /* ECMC_CANOPEN_SDO_H_ */

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h

@@ -18,6 +18,9 @@
 #define ECMC_PLUGIN_IF_OPTION_CMD          "IF="
 #define ECMC_PLUGIN_CONNECT_OPTION_CMD     "CONNECT="
 
+#define ECMC_SDO_REPLY_TIMOUT_MS           200
+
+
 enum ecmc_can_direction {
     DIR_WRITE = 1,
     DIR_READ  = 2 };