Merged in dstaesse/ouroboros/be-shm (pull request #37)

lib: shared memory map

4 files changed, 563 insertions, 0 deletions

diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt
index e81f3475..7ce98bf2 100644
--- a/src/lib/CMakeLists.txt
+++ b/src/lib/CMakeLists.txt
@@ -4,6 +4,16 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR})
 include_directories(${CMAKE_SOURCE_DIR}/include)
 include_directories(${CMAKE_BINARY_DIR}/include)
 
+find_library(LIBRT_LIBRARIES rt)
+if(NOT LIBRT_LIBRARIES)
+       message(FATAL_ERROR "librt not found")
+endif()
+
+find_library(LIBPTHREAD_LIBRARIES pthread)
+if(NOT LIBPTHREAD_LIBRARIES)
+       message(FATAL_ERROR "libpthread not found")
+endif()
+
 set(SOURCE_FILES
         # Add source files here
         bitmap.c
@@ -15,11 +25,13 @@ set(SOURCE_FILES
         irm.c
         list.c
         rina_name.c
+        shm_du_map.c
         sockets.c
         utils.c
 )
 
 add_library(ouroboros SHARED ${SOURCE_FILES})
+target_link_libraries(ouroboros rt pthread)
 
 include(MacroAddCompileFlags)
 if (CMAKE_BUILD_TYPE MATCHES Debug)
diff --git a/src/lib/shm_du_map.c b/src/lib/shm_du_map.c
new file mode 100644
index 00000000..dc73077f
--- /dev/null
+++ b/src/lib/shm_du_map.c
@@ -0,0 +1,366 @@
+/*
+ * Ouroboros - Copyright (C) 2016
+ *
+ * Shared memory map for data units
+ *
+ *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
+ *    Sander Vrijders   <sander.vrijders@intec.ugent.be>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <ouroboros/shm_du_map.h>
+#include <pthread.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <string.h>
+
+#define SHM_BLOCKS_SIZE (SHM_BLOCKS_IN_MAP * SHM_DU_BUFF_BLOCK_SIZE)
+#define SHM_FILE_SIZE (SHM_BLOCKS_SIZE + 2 * sizeof (size_t)                   \
+                       + sizeof(pthread_mutex_t))
+
+#define get_head_ptr(dum)                                                      \
+((struct shm_du_buff *)(dum->shm_base + (*dum->ptr_head *                      \
+                                         SHM_DU_BUFF_BLOCK_SIZE)))
+
+#define get_tail_ptr(dum)                                                      \
+((struct shm_du_buff *)(dum->shm_base + (*dum->ptr_tail *                      \
+                                         SHM_DU_BUFF_BLOCK_SIZE)))
+
+#define block_ptr_to_idx(dum, sdb)                                             \
+        (((uint8_t *)sdb - dum->shm_base) / SHM_DU_BUFF_BLOCK_SIZE)
+
+#define shm_map_used(dum)((*dum->ptr_head + SHM_BLOCKS_IN_MAP - *dum->ptr_tail)\
+                          & (SHM_BLOCKS_IN_MAP - 1))
+#define shm_map_free(dum, i)(shm_map_used(dum) + i < SHM_BLOCKS_IN_MAP)
+
+#define MIN(a,b)(a < b ? a : b)
+
+struct shm_du_buff {
+        size_t            size;
+        size_t            du_head;
+        size_t            du_tail;
+};
+
+struct shm_du_map {
+        uint8_t            * shm_base;    /* start of blocks */
+        size_t             * ptr_head;    /* start of ringbuffer head */
+        size_t             * ptr_tail;    /* start of ringbuffer tail */
+        pthread_mutex_t    * shm_mutex;   /* lock all free space in shm */
+        int                  fd;
+};
+
+struct shm_du_map * shm_du_map_create()
+{
+        struct shm_du_map * dum;
+        int                 shm_fd;
+        uint8_t           * shm_base;
+        pthread_mutexattr_t attr;
+
+        dum = malloc(sizeof *dum);
+        if (dum == NULL) {
+                LOG_DBGF("Could not allocate struct.");
+                return NULL;
+        }
+
+        shm_fd = shm_open(SHM_DU_MAP_FILENAME, O_CREAT | O_EXCL | O_RDWR, 0666);
+        if (shm_fd == -1) {
+                LOG_DBGF("Failed creating shared memory map.");
+                free(dum);
+                return NULL;
+        }
+
+        if (lseek(shm_fd, SHM_FILE_SIZE - 1, SEEK_SET) < 0) {
+                LOG_DBGF("Failed to extend shared memory map.");
+                free(dum);
+                return NULL;
+        }
+
+        if (write(shm_fd, "", 1) != 1) {
+                LOG_DBGF("Failed to finalise extension of shared memory map.");
+                free(dum);
+                return NULL;
+        }
+
+        shm_base = mmap(NULL,
+                        SHM_FILE_SIZE,
+                        PROT_READ | PROT_WRITE,
+                        MAP_SHARED,
+                        shm_fd,
+                        0);
+
+        if (shm_base == MAP_FAILED) {
+                LOG_DBGF("Failed to map shared memory.");
+
+                if (shm_unlink(SHM_DU_MAP_FILENAME) == -1)
+                        LOG_DBGF("Failed to remove invalid shm.");
+
+                free(dum);
+                return NULL;
+        }
+
+        dum->shm_base = shm_base;
+        dum->ptr_head = (size_t *)
+                ((uint8_t *) dum->shm_base + SHM_BLOCKS_SIZE);
+        dum->ptr_tail = (size_t *)
+                ((uint8_t *) dum->ptr_head + sizeof(size_t));
+        dum->shm_mutex = (pthread_mutex_t *)
+                ((uint8_t *) dum->ptr_tail + sizeof(size_t));
+
+        pthread_mutexattr_init(&attr);
+        pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+        pthread_mutex_init(dum->shm_mutex, &attr);
+
+        *dum->ptr_head = 0;
+        *dum->ptr_tail = 0;
+
+        dum->fd = shm_fd;
+
+        return dum;
+}
+
+struct shm_du_map * shm_du_map_open()
+{
+        struct shm_du_map * dum;
+        int                 shm_fd;
+        uint8_t           * shm_base;
+
+        shm_fd = shm_open(SHM_DU_MAP_FILENAME, O_RDWR, 0666);
+        if (shm_fd == -1) {
+                LOG_DBGF("Failed opening shared memory.");
+                return NULL;
+        }
+
+        shm_base = mmap(NULL,
+                        SHM_FILE_SIZE,
+                        PROT_READ | PROT_WRITE,
+                        MAP_SHARED,
+                        shm_fd,
+                        0);
+        if (shm_base == MAP_FAILED) {
+                LOG_DBGF("Failed to map shared memory.");
+                if (close(shm_fd) == -1)
+                        LOG_DBGF("Failed to close invalid shm.");
+
+                if (shm_unlink(SHM_DU_MAP_FILENAME) == -1)
+                        LOG_DBGF("Failed to unlink invalid shm.");
+
+                return NULL;
+        }
+
+        dum = malloc(sizeof *dum);
+        if (dum == NULL) {
+                LOG_DBGF("Could not allocate struct.");
+                return NULL;
+        }
+
+        dum->shm_base = shm_base;
+        dum->ptr_head = (size_t *)
+                ((uint8_t *) dum->shm_base + SHM_BLOCKS_SIZE);
+        dum->ptr_tail = (size_t *)
+                ((uint8_t *) dum->ptr_head + sizeof(size_t));
+        dum->shm_mutex = (pthread_mutex_t *)
+                ((uint8_t *) dum->ptr_tail + sizeof(size_t));
+
+        dum->fd = shm_fd;
+
+        return dum;
+}
+
+void shm_du_map_close(struct shm_du_map * dum)
+{
+        if (dum == NULL) {
+                LOG_DBGF("Bogus input. Bugging out.");
+                return;
+        }
+
+        if (munmap(dum->shm_base, SHM_FILE_SIZE) == -1)
+                LOG_DBGF("Couldn't unmap shared memory.");
+
+        if (shm_unlink(SHM_DU_MAP_FILENAME) == -1)
+                LOG_DBGF("Failed to unlink shm.");
+
+        free(dum);
+}
+
+struct shm_du_buff * shm_create_du_buff(struct shm_du_map * dum,
+                                        size_t              size,
+                                        size_t              headspace,
+                                        uint8_t           * data,
+                                        size_t              len)
+{
+        struct shm_du_buff * sdb;
+        long                 blocks = 0;
+        int                  sz = size + sizeof *sdb;
+        int                  sz2 = headspace + len + sizeof *sdb;
+        uint8_t            * write_pos;
+        size_t               copy_len;
+
+        if (dum == NULL || data == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return NULL;
+        }
+
+        if (headspace >= size) {
+                LOG_DBGF("Index out of bounds.");
+                return NULL;
+        }
+
+        if (headspace + len > size) {
+                LOG_DBGF("Buffer too small for data.");
+                return NULL;
+        }
+
+        pthread_mutex_lock(dum->shm_mutex);
+
+        while (sz > 0) {
+                sz -= SHM_DU_BUFF_BLOCK_SIZE;
+                sz2 -= SHM_DU_BUFF_BLOCK_SIZE;
+                if (sz2 < 0 && sz > 0) {
+                        pthread_mutex_unlock(dum->shm_mutex);
+                        LOG_DBG("Can't handle this packet now");
+                        return NULL;
+                }
+                ++blocks;
+        }
+
+        if (!shm_map_free(dum, blocks)) {
+                pthread_mutex_unlock(dum->shm_mutex);
+                LOG_DBGF("Allocation failed, Out of Memory.");
+                return NULL;
+        }
+
+        sdb = get_head_ptr(dum);
+
+        sdb->size = size;
+        sdb->du_head = headspace;
+        sdb->du_tail = sdb->du_head + len;
+
+        copy_len = MIN(len, SHM_DU_BUFF_BLOCK_SIZE - headspace - sizeof *sdb);
+        write_pos = ((uint8_t *) sdb) + sizeof *sdb + headspace;
+
+        while (blocks > 0) {
+                memcpy(write_pos, data, copy_len);
+                *(dum->ptr_head) = (*dum->ptr_head + 1)
+                        & (SHM_BLOCKS_IN_MAP - 1);
+                len -= copy_len;
+                copy_len = MIN(len, SHM_DU_BUFF_BLOCK_SIZE);
+                write_pos = (uint8_t *) get_head_ptr(dum);
+                --blocks;
+        }
+
+        pthread_mutex_unlock(dum->shm_mutex);
+
+        return sdb;
+}
+
+int shm_release_du_buff(struct shm_du_map * dum)
+{
+        long sz;
+        long blocks = 0;
+
+        pthread_mutex_lock(dum->shm_mutex);
+
+        if (*dum->ptr_head == *dum->ptr_tail) {
+                LOG_DBGF("Attempt to free empty ringbuffer. Nothing to do.");
+                pthread_mutex_unlock(dum->shm_mutex);
+                return -1;
+        }
+
+        sz = get_tail_ptr(dum)->size;
+
+        while (sz + (long) sizeof (struct shm_du_buff) > 0) {
+                sz -= SHM_DU_BUFF_BLOCK_SIZE;
+                ++blocks;
+        }
+
+        *(dum->ptr_tail) = (*dum->ptr_tail + blocks) & (SHM_BLOCKS_IN_MAP - 1);
+        pthread_mutex_unlock(dum->shm_mutex);
+
+        return 0;
+}
+
+uint8_t * shm_du_buff_head_alloc(struct shm_du_buff * sdb,
+                                 size_t size)
+{
+        if (sdb == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return NULL;
+        }
+
+        if ((long) (sdb->du_head - size) < 0) {
+                LOG_DBGF("Failed to allocate PCI headspace.");
+                return NULL;
+        }
+
+        sdb->du_head -= size;
+
+        return (uint8_t *) sdb + sizeof *sdb + sdb->du_head;
+}
+
+uint8_t * shm_du_buff_tail_alloc(struct shm_du_buff * sdb,
+                                 size_t size)
+{
+        if (sdb == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return NULL;
+        }
+
+        if (sdb->du_tail + size >= sdb->size) {
+                LOG_DBGF("Failed to allocate PCI tailspace.");
+                return NULL;
+        }
+
+        sdb->du_tail += size;
+
+        return (uint8_t *) sdb + sizeof *sdb + sdb->du_tail;
+}
+
+int shm_du_buff_head_release(struct shm_du_buff * sdb,
+                             size_t size)
+{
+        if (sdb == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return -EINVAL;
+        }
+
+        if (size > sdb->du_tail - sdb->du_head) {
+                LOG_DBGF("Tried to release beyond sdu boundary.");
+                return -EOVERFLOW;
+        }
+
+        sdb->du_head += size;
+
+        return sdb->du_head;
+}
+
+int shm_du_buff_tail_release(struct shm_du_buff * sdb,
+                             size_t size)
+{
+        if (sdb == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return -EINVAL;
+        }
+
+        if (size > sdb->du_tail - sdb->du_head) {
+                LOG_DBGF("Tried to release beyond sdu boundary.");
+                return -EOVERFLOW;
+        }
+
+        sdb->du_tail -= size;
+
+        return sdb->du_tail;
+}
diff --git a/src/lib/tests/CMakeLists.txt b/src/lib/tests/CMakeLists.txt
index 99df7232..80834df8 100644
--- a/src/lib/tests/CMakeLists.txt
+++ b/src/lib/tests/CMakeLists.txt
@@ -5,6 +5,7 @@ create_test_sourcelist(${src_folder}_tests test_suite.c
                        # Add new tests here
                        bitmap_test.c
                        du_buff_test.c
+                       shm_du_map_test.c
 )
 
 add_executable(${src_folder}_test EXCLUDE_FROM_ALL ${${src_folder}_tests})
diff --git a/src/lib/tests/shm_du_map_test.c b/src/lib/tests/shm_du_map_test.c
new file mode 100644
index 00000000..d9b44732
--- /dev/null
+++ b/src/lib/tests/shm_du_map_test.c
@@ -0,0 +1,184 @@
+/*
+ * Ouroboros - Copyright (C) 2016
+ *
+ * Test of the Shared Memory Map
+ *
+ *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <ouroboros/shm_du_map.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <time.h>
+#include <sys/mman.h>
+#include <pthread.h>
+
+#define SIZE_OF_DU_BUFF 24
+#define TEST_BUFF_SIZE (SHM_DU_BUFF_BLOCK_SIZE - SIZE_OF_DU_BUFF)
+
+#define MAX(a,b) (a > b ? a : b)
+#define MIN(a,b) (a < b ? a : b)
+
+int sync;
+
+void * produce()
+{
+        struct shm_du_map * dum;
+        long                test_buf_size = 0;
+        uint8_t           * test_values;
+        int                 headspace;
+        int                 tailspace;
+        long                i;
+        long                bytes_written = 0;
+        struct timespec     starttime;
+        struct timespec     stoptime;
+        double              elapsed;
+
+        dum = shm_du_map_open();
+        if (dum == NULL) {
+                LOG_ERR("Could not open shm.");
+                return (void *)-1;
+        }
+
+        srand(time(NULL));
+
+        test_values = malloc (sizeof *test_values * TEST_BUFF_SIZE);
+        for (i = 0; i < TEST_BUFF_SIZE; i++)
+                test_values[i] = 170;
+
+        clock_gettime(CLOCK_MONOTONIC, &starttime);
+        for (i = 1; i < SHM_BLOCKS_IN_MAP; i++) {
+                struct shm_du_buff * sdb;
+                size_t               len;
+
+                test_buf_size = TEST_BUFF_SIZE;
+
+                headspace     = 32;
+                tailspace     = 8;
+
+                len = test_buf_size - (headspace + tailspace);
+
+                sdb = shm_create_du_buff(dum,
+                                         test_buf_size,
+                                         headspace,
+                                         test_values,
+                                         len);
+
+                if (sdb != NULL) {
+                        bytes_written += len;
+                }
+                else {
+                        sync = -2;
+                        break;
+                }
+        }
+
+        clock_gettime(CLOCK_MONOTONIC, &stoptime);
+        elapsed =(stoptime.tv_sec + stoptime.tv_nsec / 1000000000.0) -
+                (starttime.tv_sec + starttime.tv_nsec / 1000000000.0);
+        LOG_INFO("%ld bytes written in %.1lf ms = %lf Gb/s",
+                 bytes_written,
+                 elapsed * 1000.0,
+                 bytes_written * 8 / (elapsed * 1000000000));
+
+        free(test_values);
+
+        sync = -1;
+
+        return 0;
+}
+
+void * consume()
+{
+        struct shm_du_map * dum;
+
+        struct timespec     ts;
+
+        ts.tv_sec = 0;
+        ts.tv_nsec = 1000000;
+
+        dum = shm_du_map_open();
+
+        if (dum == NULL) {
+                LOG_ERR("Could not open shm.");
+                return (void *)-1;
+        }
+
+        while (!sync) {
+                while (!shm_release_du_buff(dum));
+                nanosleep(&ts, NULL);
+        }
+
+        return 0;
+}
+
+int shm_du_map_test(int argc, char ** argv)
+{
+        struct shm_du_map * dum;
+        int                 res1;
+        pthread_t           producer;
+        pthread_t           consumer;
+
+        /* test 1 */
+
+        LOG_INFO("starting create/close test.");
+
+        dum = shm_du_map_create();
+
+        if (dum == NULL) {
+                LOG_ERR("Could not open shm (dum).");
+                return -1;
+        }
+
+        shm_du_map_close(dum);
+
+        LOG_INFO("done.");
+
+        /* test 2 */
+
+        LOG_INFO("starting sequential test.");
+
+        dum = shm_du_map_create();
+
+        res1 = (int) pthread_create(&producer, NULL, produce, NULL);
+        pthread_join(producer, NULL);
+
+        pthread_create(&consumer, NULL, consume, NULL);
+        pthread_join(consumer, NULL);
+
+        shm_du_map_close(dum);
+
+        LOG_INFO("done.");
+
+        /* test 3 */
+
+        LOG_INFO("starting concurrency test.");
+
+        dum = shm_du_map_create();
+
+        res1 = (int) pthread_create(&producer, NULL, produce, NULL);
+        pthread_create(&consumer, NULL, consume, NULL);
+
+        pthread_join(producer, NULL);
+        pthread_join(consumer, NULL);
+
+        shm_du_map_close(dum);
+
+        LOG_INFO("done.");
+
+        return res1;
+}