From 6bf428552f941b0a969492f3f6fedbe9412799df Mon Sep 17 00:00:00 2001
From: Anders Sandstrom <anders.sandstrom@esss.se>
Date: Tue, 2 Mar 2021 14:10:26 +0100
Subject: [PATCH] Add connect worker thread

---
 .../src/ecmcSocketCAN.cpp                     | 49 +++++++++++++++++--
 .../src/ecmcSocketCAN.h                       |  7 ++-
 .../src/ecmcSocketCANWrap.cpp                 |  2 +-
 3 files changed, 50 insertions(+), 8 deletions(-)

diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
index 41749a9..2062dec 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
@@ -39,6 +39,17 @@ void f_worker_read(void *obj) {
   canObj->doReadWorker();
 }
 
+// Start worker for socket connect()
+void f_worker_connect(void *obj) {
+  if(!obj) {
+    printf("%s/%s:%d: Error: Worker connect thread ecmcSocketCAN object NULL..\n",
+            __FILE__, __FUNCTION__, __LINE__);
+    return;
+  }
+  ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
+  canObj->doConnectWorker();
+}
+
 /** ecmc ecmcSocketCAN class
  * This object can throw: 
  *    - bad_alloc
@@ -81,13 +92,19 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   }
 
   // Create worker thread for reading socket
-  std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX;
+  std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".read";
   if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_read, this) == NULL) {
-    throw std::runtime_error("Error: Failed create worker thread.");
+    throw std::runtime_error("Error: Failed create worker thread for read().");
+  }
+
+  // Create worker thread for connecting socket
+  threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".connect";
+  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_connect, this) == NULL) {
+    throw std::runtime_error("Error: Failed create worker thread for connect().");
   }
 
   if(cfgAutoConnect_) {
-    connect();
+    connectPrivate();
   }
 
   initAsyn();
@@ -141,7 +158,14 @@ void ecmcSocketCAN::parseConfigStr(char *configStr) {
   }
 }
 
-void ecmcSocketCAN::connect() {
+// For connect commands over asyn or plc. let worker connect
+void ecmcSocketCAN::connectExternal() {
+  if(!connected_) {
+    doConnectEvent_.signal(); // let worker start
+  }
+}
+
+void ecmcSocketCAN::connectPrivate() {
 
 	if((socketId_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) {
     throw std::runtime_error( "Error while opening socket.");		
@@ -176,7 +200,9 @@ void ecmcSocketCAN::doReadWorker() {
       break;
     }
 
-    // Wait for new CAN frame   
+    // Wait for new CAN frame 
+
+    // TODO MUST CHECK RETRUN VALUE OF READ!!!!!  
     read(socketId_, &rxmsg_, sizeof(rxmsg_));
 
     if(cfgDbgMode_) {
@@ -190,6 +216,19 @@ void ecmcSocketCAN::doReadWorker() {
   }
 }
 
+// Read socket worker
+void ecmcSocketCAN::doConnectWorker() {
+
+  while(true) {
+    
+    if(destructs_) {
+      break;
+    }
+    doConnectEvent_.wait();
+    connectPrivate();
+  }
+}
+
 // Test can write function
 int ecmcSocketCAN::writeCAN() {
   struct can_frame frame;
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
index ddbef73..381aebf 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
@@ -45,26 +45,29 @@ class ecmcSocketCAN : public asynPortDriver {
   ecmcSocketCAN(char* configStr,
             char* portName);
   ~ecmcSocketCAN();
+
   void doReadWorker();
+  void doConnectWorker();
 
   virtual asynStatus    writeInt32(asynUser *pasynUser, epicsInt32 value);
   virtual asynStatus    readInt32(asynUser *pasynUser, epicsInt32 *value);
   virtual asynStatus    readInt8Array(asynUser *pasynUser, epicsInt8 *value, 
                                       size_t nElements, size_t *nIn);
   virtual asynStatus    readFloat64(asynUser *pasynUser, epicsFloat64 *value);
-
-  void                  connect();
+  void                  connectExternal();  
   int                   getConnected();
   int                   writeCAN();   // Add args later
  private:
   void                  parseConfigStr(char *configStr);
   void                  initAsyn();
   static std::string    to_string(int value);
+  void                  connectPrivate();
   char*                 cfgCanIFStr_;   // Config: can interface can0, vcan0..
   int                   cfgDbgMode_;
   int                   cfgAutoConnect_;
   int                   destructs_;
   int                   connected_;
+  epicsEvent            doConnectEvent_;
   struct can_frame      rxmsg_;
   struct can_frame      txmsg_;
   struct ifreq          ifr_;
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
index 258dd54..f2506a2 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
@@ -52,7 +52,7 @@ int createSocketCAN(char* configStr) {
 int connectSocketCAN() {
   if(can){
       try {       
-        can->connect();
+        can->connectExternal();
     }
     catch(std::exception& e) {
       printf("Exception: %s.\n",e.what());