Replaced sem_post/sem_wait with pthread_cond_signal_timedwait

src/threads.h

@@ -20,6 +20,7 @@
 
 #include "util.h"
 
+#include <assert.h>
 #include <pthread.h>
 #include <functional>
 #include <sys/types.h>
@@ -129,4 +130,57 @@ struct Lock
     }
 };
 
+struct Semaphore
+{
+    Semaphore(const char *name)
+    : name_(name)
+    {
+        pthread_cond_init(&condition_, NULL);
+        pthread_mutex_init(&mutex_, NULL);
+    }
+
+    ~Semaphore()
+    {
+        pthread_mutex_destroy(&mutex_);
+        pthread_cond_destroy(&condition_);
+    }
+
+    bool wait()
+    {
+        pthread_mutex_lock(&mutex_);
+        struct timespec max_wait = {100, 0};
+        int rc = 0;
+        for (;;)
+        {
+            rc = pthread_cond_timedwait(&condition_, &mutex_, &max_wait);
+            if (!rc) break;
+            if (rc == EINTR) continue;
+            if (rc == ETIMEDOUT) break;
+            fprintf(stderr, "GURKA %d %d %d %d\n", rc, errno, EINTR, ETIMEDOUT);
+            assert(0);
+            error("(thread) pthread cond timedwait ERROR\n");
+        }
+
+        pthread_mutex_unlock(&mutex_);
+
+        // Return true if proper wait.
+        // Return false if timeout!!!!
+        return rc != ETIMEDOUT;
+    }
+
+    void notify()
+    {
+        int rc = pthread_cond_signal(&condition_);
+        if (rc)
+        {
+            error("(thread) pthread cond signal ERROR\n");
+        }
+    }
+
+private:
+    const char *name_;
+    pthread_mutex_t mutex_;
+    pthread_cond_t condition_;
+};
+
 #endif

src/wmbus.cc

@@ -3285,12 +3285,12 @@ WMBusCommonImplementation::WMBusCommonImplementation(WMBusDeviceType t,
     : manager_(manager),
       is_working_(true),
       type_(t),
-      serial_(serial)
+      serial_(serial),
+      command_wait_("command_wait")
 {
     // Initialize timeout from now.
     last_received_ = time(NULL);
     last_reset_ = time(NULL);
-    sem_init(&command_wait_, 0, 0);
     manager_->listenTo(this->serial(),call(this,processSerialData));
     manager_->onDisappear(this->serial(),call(this,disconnectedFromDevice));
 }
@@ -3524,9 +3524,9 @@ bool WMBusCommonImplementation::waitForResponse()
     while (manager_->isRunning())
     {
         // 5 second timeout.
-        const struct timespec timeout { 5, 0 };
         trace("[IM871A] waitForResponse sem_wait command_wait_\n");
-        int rc = sem_timedwait(&command_wait_, &timeout);
+        command_wait_.wait();
+        int rc = 0;
         trace("[IM871A] waitForResponse waited command_wait_\n");
         if (rc==0) break;
         if (rc==-1) {

src/wmbus_amb8465.cc

@@ -23,7 +23,6 @@
 
 #include<assert.h>
 #include<pthread.h>
-#include<semaphore.h>
 #include<errno.h>
 #include<unistd.h>
 #include<sys/time.h>
@@ -474,7 +473,7 @@ void WMBusAmber::handleMessage(int msgid, vector<uchar> &frame)
         received_payload_.clear();
         received_payload_.insert(received_payload_.end(), frame.begin(), frame.end());
         debugPayload("(amb8465) set link mode response", received_payload_);
-        sem_post(&command_wait_);
+        command_wait_.notify();
         break;
     }
     case (0x80|CMD_GET_REQ):
@@ -484,7 +483,7 @@ void WMBusAmber::handleMessage(int msgid, vector<uchar> &frame)
         received_payload_.clear();
         received_payload_.insert(received_payload_.end(), frame.begin(), frame.end());
         debugPayload("(amb8465) get config response", received_payload_);
-        sem_post(&command_wait_);
+        command_wait_.notify();
         break;
     }
     case (0x80|CMD_SERIALNO_REQ):
@@ -494,7 +493,7 @@ void WMBusAmber::handleMessage(int msgid, vector<uchar> &frame)
         received_payload_.clear();
         received_payload_.insert(received_payload_.end(), frame.begin(), frame.end());
         debugPayload("(amb8465) get device id response", received_payload_);
-        sem_post(&command_wait_);
+        command_wait_.notify();
         break;
     }
     default:

src/wmbus_cul.cc

@@ -224,7 +224,7 @@ void WMBusCUL::processSerialData()
                 if (r != "")
                 {
                     received_response_ = r;
-                    sem_post(&command_wait_);
+                    command_wait_.notify();
                 }
             }
             read_buffer_.clear();

src/wmbus_im871a.cc

@@ -582,28 +582,28 @@ void WMBusIM871A::handleDevMgmt(int msgid, vector<uchar> &payload)
         case DEVMGMT_MSG_PING_RSP: // 0x02
             verbose("(im871a) pong\n");
             received_command_ = msgid;
-            sem_post(&command_wait_);
+            command_wait_.notify();
             break;
         case DEVMGMT_MSG_SET_CONFIG_RSP: // 0x04
             verbose("(im871a) set config completed\n");
             received_command_ = msgid;
             received_payload_.clear();
             received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
-            sem_post(&command_wait_);
+            command_wait_.notify();
             break;
         case DEVMGMT_MSG_GET_CONFIG_RSP: // 0x06
             verbose("(im871a) get config completed\n");
             received_command_ = msgid;
             received_payload_.clear();
             received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
-            sem_post(&command_wait_);
+            command_wait_.notify();
             break;
         case DEVMGMT_MSG_GET_DEVICEINFO_RSP: // 0x10
             verbose("(im871a) device info completed\n");
             received_command_ = msgid;
             received_payload_.clear();
             received_payload_.insert(received_payload_.end(), payload.begin(), payload.end());
-            sem_post(&command_wait_);
+            command_wait_.notify();
             break;
     default:
         verbose("(im871a) Unhandled device management message %d\n", msgid);

src/wmbus_utils.h

@@ -19,10 +19,9 @@
 #define WMBUS_UTILS_H
 
 #include "util.h"
+#include "threads.h"
 #include "wmbus.h"
 
-#include<semaphore.h>
-
 bool decrypt_ELL_AES_CTR(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey);
 bool decrypt_TPL_AES_CBC_IV(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey);
 bool decrypt_TPL_AES_CBC_NO_IV(Telegram *t, vector<uchar> &frame, vector<uchar>::iterator &pos, vector<uchar> &aeskey);
@@ -79,7 +78,7 @@ struct WMBusCommonImplementation : public virtual WMBus
 
 protected:
 
-    sem_t command_wait_;
+    Semaphore command_wait_;
 };
 
 #endif