From c20d4ff5cc71caace7720868e7e186598ae6708c Mon Sep 17 00:00:00 2001
From: Xavier Arteaga <xavier@softwareradiosystems.com>
Date: Wed, 16 Sep 2020 18:37:41 +0200
Subject: [PATCH] Added ringbuffer zerocopy a timeout

---
 lib/include/srslte/phy/utils/ringbuffer.h |  2 +-
 lib/src/phy/utils/ringbuffer.c            | 48 +++++++++++++++++++----
 2 files changed, 41 insertions(+), 9 deletions(-)

diff --git a/lib/include/srslte/phy/utils/ringbuffer.h b/lib/include/srslte/phy/utils/ringbuffer.h
index 15a1bbfef..73018f004 100644
--- a/lib/include/srslte/phy/utils/ringbuffer.h
+++ b/lib/include/srslte/phy/utils/ringbuffer.h
@@ -78,7 +78,7 @@ SRSLTE_API int srslte_ringbuffer_read_timed_block(srslte_ringbuffer_t* q, void*
 // read samples from the buffer, convert them from uint16_t to cplx float and get the conjugate
 SRSLTE_API int srslte_ringbuffer_read_convert_conj(srslte_ringbuffer_t* q, cf_t* dst_ptr, float norm, int nof_samples);
 
-SRSLTE_API int srslte_ringbuffer_read_block(srslte_ringbuffer_t* q, void** p, int nof_bytes);
+SRSLTE_API int srslte_ringbuffer_read_block(srslte_ringbuffer_t* q, void** p, int nof_bytes, int32_t timeout_ms);
 
 SRSLTE_API void srslte_ringbuffer_stop(srslte_ringbuffer_t* q);
 
diff --git a/lib/src/phy/utils/ringbuffer.c b/lib/src/phy/utils/ringbuffer.c
index 6971c75a1..b1aeb86a3 100644
--- a/lib/src/phy/utils/ringbuffer.c
+++ b/lib/src/phy/utils/ringbuffer.c
@@ -118,6 +118,11 @@ int srslte_ringbuffer_write_timed_block(srslte_ringbuffer_t* q, void* p, int nof
   struct timespec towait;
   struct timeval  now;
 
+  if (q == NULL || q->buffer == NULL) {
+    ERROR("Invalid inputs\n");
+    return SRSLTE_ERROR_INVALID_INPUTS;
+  }
+
   // Get current time and update timeout
   if (timeout_ms > 0) {
     gettimeofday(&now, NULL);
@@ -281,19 +286,39 @@ int srslte_ringbuffer_read_convert_conj(srslte_ringbuffer_t* q, cf_t* dst_ptr, f
 }
 
 /* For this function, the ring buffer capacity must be multiple of block size */
-int srslte_ringbuffer_read_block(srslte_ringbuffer_t* q, void** p, int nof_bytes)
+int srslte_ringbuffer_read_block(srslte_ringbuffer_t* q, void** p, int nof_bytes, int32_t timeout_ms)
 {
-  int ret = nof_bytes;
-  pthread_mutex_lock(&q->mutex);
+  int             ret    = SRSLTE_SUCCESS;
+  struct timespec towait = {};
 
-  /* Wait until enough data is in the buffer */
-  while (q->count < nof_bytes && q->active) {
-    pthread_cond_wait(&q->write_cvar, &q->mutex);
+  // Get current time and update timeout
+  if (timeout_ms > 0) {
+    struct timespec now = {};
+    timespec_get(&now, TIME_UTC);
+
+    // check nsec wrap-around
+    towait.tv_sec = now.tv_sec + timeout_ms / 1000L;
+    long nsec     = now.tv_nsec + ((timeout_ms % 1000U) * 1000UL);
+    towait.tv_sec += nsec / 1000000000L;
+    towait.tv_nsec = nsec % 1000000000L;
   }
 
-  if (!q->active) {
+  pthread_mutex_lock(&q->mutex);
+
+  // Wait for having enough samples
+  while (q->count < nof_bytes && q->active && ret == SRSLTE_SUCCESS) {
+    if (timeout_ms > 0) {
+      ret = pthread_cond_timedwait(&q->write_cvar, &q->mutex, &towait);
+    } else {
+      pthread_cond_wait(&q->write_cvar, &q->mutex);
+    }
+  }
+
+  if (ret == ETIMEDOUT) {
+    ret = SRSLTE_ERROR_TIMEOUT;
+  } else if (!q->active) {
     ret = 0;
-  } else {
+  } else if (ret == SRSLTE_SUCCESS) {
     *p = &q->buffer[q->rpm];
 
     q->count -= nof_bytes;
@@ -302,6 +327,13 @@ int srslte_ringbuffer_read_block(srslte_ringbuffer_t* q, void** p, int nof_bytes
     if (q->rpm >= q->capacity) {
       q->rpm -= q->capacity;
     }
+    ret = nof_bytes;
+  } else if (ret == EINVAL) {
+    fprintf(stderr, "Error: pthread_cond_timedwait() returned EINVAL, timeout value corrupted.\n");
+    ret = SRSLTE_ERROR;
+  } else {
+    printf("ret=%d %s\n", ret, strerror(ret));
+    ret = SRSLTE_ERROR;
   }
   pthread_cond_broadcast(&q->read_cvar);
   pthread_mutex_unlock(&q->mutex);