diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
index 7f6a993..04cc924 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
@@ -35,8 +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();
+  sdo1Lock_.test_and_set();   // make sure only one sdo is accessing the bus at the same time
+  sdo1Lock_.clear();
   for(int i = 0 ; i<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
     pdos_[i] = NULL;
   }
@@ -159,6 +159,7 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx,    // 0x580 + CobId
                                          readSampleTimeMs,
                                          exeSampleTimeMs_,
                                          name,
+                                         &sdo1Lock_,
                                          dbgMode_);
   sdoCounter_++;                                       
   return 0;
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
index 2f1f8cd..607d93c 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
@@ -75,7 +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_;
+  std::atomic_flag sdo1Lock_;
 };
 
 #endif  /* ECMC_CANOPEN_DEVICE_H_ */
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
index 5e1906f..5693ed2 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
@@ -31,6 +31,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
                                int readSampleTimeMs, 
                                int exeSampleTimeMs,
                                const char *name,
+                               std::atomic_flag *ptrSdo1Lock,
                                int dbgMode) {
 
   writeBuffer_        = writeBuffer;
@@ -42,7 +43,9 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   dbgMode_            = dbgMode;
   name_               = strdup(name);
   errorCode_          = 0;
-  dataMutex_ = epicsMutexCreate();
+  ptrSdo1Lock_        = ptrSdo1Lock;
+  dataMutex_          = epicsMutexCreate();
+  getLockMutex_       = epicsMutexCreate();
 
   // convert to ODIndex_ to indiviual bytes struct
   memcpy(&ODIndexBytes_, &ODIndex, 2);
@@ -170,7 +173,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeConfReqFrameTg1_.data[5] = 0;
   writeConfReqFrameTg1_.data[6] = 0;
   writeConfReqFrameTg1_.data[7] = 0;
-  busy_.clear();
+  busy_ = false;
 }
 
 ecmcCANOpenSDO::~ecmcCANOpenSDO() {
@@ -181,11 +184,9 @@ ecmcCANOpenSDO::~ecmcCANOpenSDO() {
 
 void ecmcCANOpenSDO::execute() {
   
-  
-  if(busy_.test_and_set()) {
+  if(busy_) {
    busyCounter_++;
   } else {
-    busy_.clear();
     busyCounter_ = 0;
   }
   
@@ -198,18 +199,19 @@ void ecmcCANOpenSDO::execute() {
     exeCounter_  = 0;
     busyCounter_ = 0;
     errorCode_ = ECMC_CAN_ERROR_SDO_TIMEOUT;
-    busy_.clear();
+    tryUnlock();
   }
 
   if(exeCounter_* exeSampleTimeMs_ < readSampleTimeMs_ && rw_ == DIR_READ) { // do not risk overflow
     exeCounter_++;
   } else { // Counter is higher, try to write
-    if(rw_ == DIR_READ) {      
-      if(busy_.test_and_set()) {
-        // wait for busy to go down
-        return;
-      }      
+    if(rw_ == DIR_READ) {
       
+      if(!tryLock()) {
+        // wait for busy to go down
+       return;
+      }      
+
       exeCounter_ =0; 
       readStates_ = READ_REQ_TRANSFER;
       if(dbgMode_) {
@@ -233,8 +235,7 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
   // Wait for:
   // # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
   int errorCode = 0;
-  if(!busy_.test_and_set()) {
-    busy_.clear();
+  if(!busy_) {    
     // Not waiting for any data..
     return;
   }
@@ -304,8 +305,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
           //copy complete data to dataBuffer_
           printBuffer();
         }
-
-        busy_.clear();        
+        tryUnlock();
         return 0;
       }
       break;
@@ -353,7 +353,7 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
           printf("All data written to slave SDO.\n");
           printBuffer();          
         }
-        busy_.clear();
+        tryUnlock();
         return 0;
       }
 
@@ -458,9 +458,15 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
 int ecmcCANOpenSDO::writeValue() {
   // Busy right now!
   printf("WRITEVALUE!!\n");
-  if(busy_.test_and_set()) {
+  
+  if(busy_) {
     return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
   }
+
+  if(!tryLock()) {
+    // wait for busy to go down
+   return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
+  }
   
   if(writeStates_ != WRITE_IDLE ) {
     return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
@@ -481,6 +487,33 @@ int ecmcCANOpenSDO::writeValue() {
   // State machine is now in rx frame()
 }
 
+int ecmcCANOpenSDO::tryLock() {
+  epicsMutexLock(getLockMutex_);
+  if(busy_) {
+    return 0;
+  }
+
+  bool gotLock = ptrSdo1Lock_->test_and_set();
+  if(!gotLock) {
+    // wait for busy to go down
+   return 0;
+  }
+
+  busy_ = gotLock;
+  epicsMutexUnlock(getLockMutex_);
+  return gotLock;
+}
+
+int ecmcCANOpenSDO::tryUnlock() {
+  epicsMutexLock(getLockMutex_);
+  if(busy_) {
+    ptrSdo1Lock_->clear();
+    busy_ = false;
+  }
+  epicsMutexUnlock(getLockMutex_);
+  return 0;
+}
+
 //# w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00
 //# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
 //# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
index faefb30..8e88fa6 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
@@ -41,6 +41,7 @@ class ecmcCANOpenSDO {
                  int readSampleTimeMs,
                  int exeSampleTimeMs,
                  const char *name,
+                 std::atomic_flag *ptrSdo1Lock,
                  int dbgMode);                
   ~ecmcCANOpenSDO();
   void execute();
@@ -54,7 +55,8 @@ class ecmcCANOpenSDO {
   int writeDataStateMachine(can_frame *frame);
   int writeNextDataToSlave(int useToggle);
   int writeWaitForDataConfFrame(int useToggle, can_frame *frame);
-
+  int tryLock();
+  int tryUnlock();
   ecmcSocketCANWriteBuffer *writeBuffer_;
   uint32_t cobIdRx_;   // with cobid
   uint32_t cobIdTx_;   // with cobid
@@ -90,9 +92,12 @@ class ecmcCANOpenSDO {
   uint32_t writtenBytes_;
   char *name_;
   epicsMutexId  dataMutex_;
+  epicsMutexId  getLockMutex_;
   int busyCounter_;
   //std::atomic_flag *ptrSdo1Busy_;
-  std::atomic_flag busy_;
+  //std::atomic_flag busy_;
+  std::atomic_flag *ptrSdo1Lock_;
+  bool busy_;
 };
 
 #endif  /* ECMC_CANOPEN_SDO_H_ */
diff --git a/iocsh/pvs.log b/iocsh/pvs.log
index 3c912bc..40a302e 100644
--- a/iocsh/pvs.log
+++ b/iocsh/pvs.log
@@ -1,12 +1,12 @@
-REQMOD:mcag-trgt-muts--20872:MODULES
-REQMOD:mcag-trgt-muts--20872:VERSIONS
-REQMOD:mcag-trgt-muts--20872:MOD_VER
-REQMOD:mcag-trgt-muts--20872:exit
-REQMOD:mcag-trgt-muts--20872:BaseVersion
-REQMOD:mcag-trgt-muts--20872:require_VER
-REQMOD:mcag-trgt-muts--20872:ecmccfg_VER
-REQMOD:mcag-trgt-muts--20872:asyn_VER
-REQMOD:mcag-trgt-muts--20872:exprtk_VER
-REQMOD:mcag-trgt-muts--20872:motor_VER
-REQMOD:mcag-trgt-muts--20872:ecmc_VER
-REQMOD:mcag-trgt-muts--20872:ecmc_plugin_socketcan_VER
+REQMOD:mcag-trgt-muts--16711:MODULES
+REQMOD:mcag-trgt-muts--16711:VERSIONS
+REQMOD:mcag-trgt-muts--16711:MOD_VER
+REQMOD:mcag-trgt-muts--16711:exit
+REQMOD:mcag-trgt-muts--16711:BaseVersion
+REQMOD:mcag-trgt-muts--16711:require_VER
+REQMOD:mcag-trgt-muts--16711:ecmccfg_VER
+REQMOD:mcag-trgt-muts--16711:asyn_VER
+REQMOD:mcag-trgt-muts--16711:exprtk_VER
+REQMOD:mcag-trgt-muts--16711:motor_VER
+REQMOD:mcag-trgt-muts--16711:ecmc_VER
+REQMOD:mcag-trgt-muts--16711:ecmc_plugin_socketcan_VER