Use memcpy() inside codec2_fifo

src/codec2_fifo.c

@@ -30,14 +30,26 @@
 */
 
 #include <assert.h>
-#include <stdlib.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifndef __STDC_NO_ATOMICS__
+#include <stdint.h>
+#include <stdatomic.h>
+#endif /* !__STDC_NO_ATOMICS__ */
+
 #include "codec2_fifo.h"
 
 struct FIFO {
     short *buf;
+#ifdef __STDC_NO_ATOMICS__
     short *pin;
     short *pout;
+#else
+    atomic_intptr_t pin;
+    atomic_intptr_t pout;
+#endif /* __STDC_NO_ATOMICS__ */
     int    nshort;
 };
 
@@ -56,8 +68,13 @@ struct FIFO *codec2_fifo_create_buf(int nshort, short* buf) {
     assert(fifo != NULL);
 
     fifo->buf = buf;
+#ifdef __STDC_NO_ATOMICS__
     fifo->pin = fifo->buf;
     fifo->pout = fifo->buf;
+#else
+    atomic_store(&fifo->pin, (intptr_t)fifo->buf);
+    atomic_store(&fifo->pout, (intptr_t)fifo->buf);
+#endif /* __STDC_NO_ATOMICS__ */
     fifo->nshort = nshort;
 
     return fifo;
@@ -69,82 +86,125 @@ void codec2_fifo_destroy(struct FIFO *fifo) {
     free(fifo);
 }
 
-int codec2_fifo_write(struct FIFO *fifo, short data[], int n) {
-    int            i;
-    short         *pdata;
-    short         *pin = fifo->pin;
+static int codec2_fifo_used_internal(short* pin, short* pout, int nshort)
+{
+    int used = 0;
+
+    if (pin >= pout)
+        used = pin - pout;
+    else
+        used = nshort + (unsigned int)(pin - pout);
 
+    return used;
+}
+
+static int codec2_fifo_free_internal(short* pin, short* pout, int nshort)
+{
+    // available storage is one less than nshort as prd == pwr
+    // is reserved for empty rather than full
+
+    return nshort - codec2_fifo_used_internal(pin, pout, nshort) - 1;
+}
+
+int codec2_fifo_write(struct FIFO *fifo, short data[], int n) {
     assert(fifo != NULL);
     assert(data != NULL);
 
-    if (n > codec2_fifo_free(fifo)) {
-	return -1;
+#ifdef __STDC_NO_ATOMICS__
+    short         *pin = fifo->pin;
+    short         *pin = fifo->pout;
+#else
+    short         *pin = (short*)atomic_load(&fifo->pin);
+    short         *pout = (short*)atomic_load(&fifo->pout);
+#endif /* __STDC_NO_ATOMICS__ */
+
+    int free = codec2_fifo_free_internal(pin, pout, fifo->nshort);
+    if (n > free) {
+        return -1;
     }
     else {
-
-	/* This could be made more efficient with block copies
-	   using memcpy */
-
-	pdata = data;
-	for(i=0; i<n; i++) {
-	    *pin++ = *pdata++;
-	    if (pin == (fifo->buf + fifo->nshort))
-		pin = fifo->buf;
-	}
-	fifo->pin = pin;
+        short         *pdata = data;
+        if ((pin + n) >= (fifo->buf + fifo->nshort))
+        {
+            int firstSamples = fifo->buf + fifo->nshort - pin;
+            memcpy(pin, pdata, firstSamples * sizeof(short));
+            n -= firstSamples;
+            pin = fifo->buf;
+            pdata += firstSamples;
+        }
+        memcpy(pin, pdata, n * sizeof(short));
+        pin += n;
+#ifdef __STDC_NO_ATOMICS__
+        fifo->pin = pin;
+#else
+        atomic_store(&fifo->pin, (intptr_t)pin);
+#endif /* __STDC_NO_ATOMICS__ */
     }
 
     return 0;
 }
 
 int codec2_fifo_read(struct FIFO *fifo, short data[], int n)
 {
-    int            i;
-    short         *pdata;
-    short         *pout = fifo->pout;
-
     assert(fifo != NULL);
     assert(data != NULL);
-
-    if (n > codec2_fifo_used(fifo)) {
-	return -1;
+
+#ifdef __STDC_NO_ATOMICS__
+    short         *pin = fifo->pin;
+    short         *pin = fifo->pout;
+#else
+    short         *pin = (short*)atomic_load(&fifo->pin);
+    short         *pout = (short*)atomic_load(&fifo->pout);
+#endif /* __STDC_NO_ATOMICS__ */
+
+    int used = codec2_fifo_used_internal(pin, pout, fifo->nshort);
+    if (n > used) {
+        return -1;
     }
     else {
-
-	/* This could be made more efficient with block copies
-	   using memcpy */
-
-	pdata = data;
-	for(i=0; i<n; i++) {
-	    *pdata++ = *pout++;
-	    if (pout == (fifo->buf + fifo->nshort))
-		pout = fifo->buf;
-	}
-	fifo->pout = pout;
+        short         *pdata = data;
+        if ((pout + n) >= (fifo->buf + fifo->nshort))
+        {
+            int firstSamples = fifo->buf + fifo->nshort - pout;
+            memcpy(pdata, pout, firstSamples * sizeof(short));
+            n -= firstSamples;
+            pout = fifo->buf;
+            pdata += firstSamples;
+        }
+        memcpy(pdata, pout, n * sizeof(short));
+        pout += n;
+#ifdef __STDC_NO_ATOMICS__
+        fifo->pout = pout;
+#else
+	atomic_store(&fifo->pout, (intptr_t)pout);
+#endif /* __STDC_NO_ATOMICS__ */
     }
 
     return 0;
 }
 
 int codec2_fifo_used(const struct FIFO * const fifo)
 {
+#ifdef __STDC_NO_ATOMICS__
     short         *pin = fifo->pin;
     short         *pout = fifo->pout;
-    unsigned int   used;
-
-    assert(fifo != NULL);
-    if (pin >= pout)
-        used = pin - pout;
-    else
-        used = fifo->nshort + (unsigned int)(pin - pout);
-
-    return used;
+#else
+    short         *pin = (short*)atomic_load(&fifo->pin);
+    short         *pout = (short*)atomic_load(&fifo->pout);
+#endif /* __STDC_NO_ATOMICS__ */
+
+    return codec2_fifo_used_internal(pin, pout, fifo->nshort);
 }
 
 int codec2_fifo_free(const struct FIFO * const fifo)
 {
-    // available storage is one less than nshort as prd == pwr
-    // is reserved for empty rather than full
-
-    return fifo->nshort - codec2_fifo_used(fifo) - 1;
+#ifdef __STDC_NO_ATOMICS__
+    short         *pin = fifo->pin;
+    short         *pout = fifo->pout;
+#else
+    short         *pin = (short*)atomic_load(&fifo->pin);
+    short         *pout = (short*)atomic_load(&fifo->pout);
+#endif /* __STDC_NO_ATOMICS__ */
+
+    return codec2_fifo_free_internal(pin, pout, fifo->nshort);
 }

unittest/tfifo.c

@@ -9,6 +9,7 @@
 #include <assert.h>
 #include <stdio.h>
 #include <pthread.h>
+#include <unistd.h>
 #include "codec2_fifo.h"
 
 #define FIFO_SZ  1024