lib: shared memory map

The shared memory map will allow passing SDU's between IPCPs
efficiently. It is a single block of managed memory. SDU's are stored
in a shm_du_buff datastructure, which is a list of blocks inside the
shared memory region. The mechanism for passing SDU's is not yet
implemented and could be either within the region or using another
local IPC mechanism.

The following things are configured at compile time:

SHM_FILE_NAME         : the name. Default is "ouroboros_du_map".
SHM_DU_BUFF_BLOCK_SIZE: size of a block inside the shared memory
                        region. Default is the pagesize of the system.
SHM_DU_MAP_SIZE       : the approximate size in bytes of the shared
                        memory map. The actual filesize may differ by
                        a small margin. Default is 4MB.

1 files changed, 493 insertions, 0 deletions

diff --git a/src/lib/shm_du_map.c b/src/lib/shm_du_map.c
new file mode 100644
index 00000000..052b8868
--- /dev/null
+++ b/src/lib/shm_du_map.c
@@ -0,0 +1,493 @@
+/*
+ * 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.
+ */
+
+#ifndef SHM_DU_MAP_C
+#define SHM_DU_MAP_C
+
+#include <ouroboros/shm_du_map.h>
+
+#include <pthread.h>
+#include <sys/mman.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <string.h>
+
+
+#define SHM_DU_BLOCK_DATA_SIZE (SHM_DU_BUFF_BLOCK_SIZE -                       \
+                                sizeof (struct shm_block))
+#define SHM_BLOCKS_IN_MAP (SHM_DU_MAP_SIZE / SHM_DU_BUFF_BLOCK_SIZE)
+#define SHM_BLOCKS_SIZE (SHM_DU_BUFF_BLOCK_SIZE * SHM_BLOCKS_IN_MAP)
+#define SHM_BUFFS_SIZE (SHM_BLOCKS_IN_MAP * sizeof (struct shm_du_buff))
+#define SHM_MGT_MAP_SIZE (SHM_BLOCKS_IN_MAP * sizeof (uint8_t))
+#define SHM_FILE_SIZE (SHM_BLOCKS_IN_MAP * (SHM_DU_BUFF_BLOCK_SIZE             \
+                                            + sizeof (struct shm_du_buff)      \
+                                            + sizeof (uint8_t))                \
+                       + sizeof(pthread_mutex_t))
+
+#define idx_to_block_ptr(dum, i) ((struct shm_block *)                        \
+                                  (dum->shm_base + i * SHM_DU_BUFF_BLOCK_SIZE))
+#define idx_to_du_buff_ptr(dum, i) (dum->ptr_du_buff + i)
+#define du_buff_ptr_to_idx(dum, sdb) ((sdb - dum->ptr_du_buff) / sizeof *sdb)
+#define block_ptr_to_idx(dum, sdb) (((uint8_t *)sdb - dum->shm_base)           \
+                                    / SHM_DU_BUFF_BLOCK_SIZE)
+
+struct shm_block {
+        size_t size;
+        long   next;
+        long   prev;
+};
+
+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 buffer blocks */
+        struct shm_du_buff * ptr_du_buff; /* start of du_buff structs */
+        uint8_t            * ptr_map;     /* start of management map */
+        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;
+        uint8_t           * ptr;
+        pthread_mutexattr_t attr;
+        int                 i;
+
+        dum = malloc(sizeof *dum);
+        if (dum == NULL) {
+                LOG_ERR("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_ERR("Failed creating shared memory map.");
+                free(dum);
+                return NULL;
+        }
+
+        if (lseek (shm_fd,SHM_FILE_SIZE - 1, SEEK_SET) < 0) {
+                LOG_ERR("Failed to extend shared memory map.");
+                free(dum);
+                return NULL;
+        }
+
+        if (write (shm_fd, "", 1) != 1) {
+                LOG_ERR("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_ERR("Failed to map shared memory.");
+                if (close(shm_fd) == -1)
+                        LOG_ERR("Failed to close invalid shm.");
+
+                if (shm_unlink(SHM_DU_MAP_FILENAME) == -1)
+                        LOG_ERR("Failed to remove invalid shm.");
+
+                free(dum);
+                return NULL;
+        }
+#ifdef CONFIG_OUROBOROS_DEBUG
+        memset(shm_base, 0, SHM_FILE_SIZE);
+#endif
+        dum->shm_base = shm_base;
+        dum->ptr_du_buff = (struct shm_du_buff *)
+                ((uint8_t *) dum->shm_base + SHM_BLOCKS_SIZE);
+        dum->ptr_map = (uint8_t *) dum->ptr_du_buff + SHM_BUFFS_SIZE;
+        dum->shm_mutex = (pthread_mutex_t *)
+                ((uint8_t *) dum->ptr_map + SHM_MGT_MAP_SIZE);
+
+        pthread_mutexattr_init(&attr);
+        pthread_mutexattr_setpshared(&attr, PTHREAD_PROCESS_SHARED);
+        pthread_mutex_init(dum->shm_mutex, &attr);
+
+        ptr = dum->ptr_map;
+        for (i = 0; i < SHM_BLOCKS_IN_MAP; ++i)
+                *(ptr++) = 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_ERR("Failed opening shared memory for du_buff.");
+                return NULL;
+        }
+
+        shm_base = mmap(NULL,
+                        SHM_FILE_SIZE,
+                        PROT_READ | PROT_WRITE,
+                        MAP_SHARED,
+                        shm_fd,
+                        0);
+        if (shm_base == MAP_FAILED) {
+                LOG_ERR("Failed to map shared memory.");
+
+                if (close(shm_fd) == -1)
+                        LOG_ERR("Failed to close invalid shm.");
+
+                if (shm_unlink(SHM_DU_MAP_FILENAME) == -1)
+                        LOG_ERR("Failed to unlink invalid shm.");
+
+                return NULL;
+        }
+
+        dum = malloc(sizeof *dum);
+        if (dum == NULL) {
+                LOG_ERR("Could not allocate struct.");
+                return NULL;
+        }
+
+        dum->shm_base = shm_base;
+        dum->ptr_du_buff = (struct shm_du_buff *)
+                ((uint8_t *) dum->shm_base + SHM_BLOCKS_SIZE);
+        dum->ptr_map = (uint8_t *) dum->ptr_du_buff + SHM_BUFFS_SIZE;
+        dum->shm_mutex = (pthread_mutex_t *)
+                ((uint8_t *) dum->ptr_map + SHM_MGT_MAP_SIZE);
+
+        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);
+}
+
+static struct shm_du_buff * alloc_du_buff(struct shm_du_map * dum)
+{
+        uint8_t * ptr = dum->ptr_map;
+
+        pthread_mutex_lock(dum->shm_mutex);
+
+        while (*ptr == 1 && ptr < (uint8_t *) dum->shm_mutex)
+                ++ptr;
+
+        if (ptr < (uint8_t *) dum->shm_mutex) {
+                *ptr = 1;
+        } else {
+                pthread_mutex_unlock(dum->shm_mutex);
+                return NULL;
+        }
+
+        pthread_mutex_unlock(dum->shm_mutex);
+
+        return dum->ptr_du_buff + (ptr - dum->ptr_map);
+}
+
+static struct shm_block * alloc_block(struct shm_du_map * dum)
+{
+
+        pthread_mutex_lock(dum->shm_mutex);
+
+        uint8_t * ptr = dum->ptr_map;
+
+        while (*ptr == 1 && ptr < (uint8_t *) dum->shm_mutex)
+                ++ptr;
+
+        if (ptr < (uint8_t *) dum->shm_mutex) {
+                *ptr = 1;
+        }
+        else {
+                pthread_mutex_unlock(dum->shm_mutex);
+                return NULL;
+        }
+
+        pthread_mutex_unlock(dum->shm_mutex);
+
+
+        return (struct shm_block *)
+                (dum->shm_base + ((ptr - dum->ptr_map)
+                                  * SHM_DU_BUFF_BLOCK_SIZE));
+}
+
+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                 index;
+        long                 prev_index = -1;
+        size_t               remaining = size;
+        size_t               ts = size - (headspace + len);
+        uint8_t            * read_pos = data;
+
+        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;
+        }
+
+        if (headspace > SHM_DU_BLOCK_DATA_SIZE || ts > SHM_DU_BLOCK_DATA_SIZE) {
+                LOG_ERR("Illegal du_buff: Cannot fit PCI in DU_BUFF_BLOCK.");
+                return NULL;
+        }
+
+        sdb = alloc_du_buff(dum);
+        if (sdb == NULL) {
+                LOG_DBGF("Allocation failed, Out of Memory.");
+                return NULL;
+        }
+
+        index = du_buff_ptr_to_idx(dum, sdb);
+        sdb->size = size;
+        sdb->du_head = headspace;
+        sdb->du_tail = sdb->du_head + len;
+
+        while (remaining > 0) {
+                struct shm_block * shm_buf;
+                unsigned long       bytes_to_copy = len;
+                uint8_t           * write_pos;
+
+                if (prev_index == -1)
+                        shm_buf = idx_to_block_ptr(dum, index);
+                else
+                        shm_buf = alloc_block(dum);
+
+                if (shm_buf == NULL) {
+                        shm_release_du_buff(dum, sdb);
+                        return NULL;
+                }
+
+                write_pos = (uint8_t *) shm_buf + sizeof *shm_buf;
+
+                index = block_ptr_to_idx(dum, shm_buf);
+
+                if (size > SHM_DU_BLOCK_DATA_SIZE
+                    && remaining - ts <=  SHM_DU_BLOCK_DATA_SIZE
+                    && remaining != ts) {
+                        shm_buf->size = remaining - ts;
+                        bytes_to_copy = shm_buf->size;
+                } else if (size > SHM_DU_BLOCK_DATA_SIZE && remaining == ts) {
+                        shm_buf->size = ts;
+                        bytes_to_copy = 0;
+                } else {
+                        shm_buf->size = remaining < SHM_DU_BLOCK_DATA_SIZE ?
+                                remaining : SHM_DU_BLOCK_DATA_SIZE;
+                        bytes_to_copy =shm_buf->size;
+                        if (remaining == shm_buf->size)
+                                bytes_to_copy -= ts;
+                }
+
+                remaining -= shm_buf->size;
+
+                if (prev_index == -1) {
+#ifdef CONFIG_OUROBOROS_DEBUG
+                        memset(write_pos, 0, sdb->du_head);
+#endif
+
+                        write_pos += sdb->du_head;
+                        bytes_to_copy -= sdb->du_head;
+                }
+
+                if (prev_index != -1)
+                        idx_to_block_ptr(dum, prev_index)->next = index;
+
+                memcpy(write_pos, read_pos, bytes_to_copy);
+
+                read_pos += bytes_to_copy;
+#ifdef CONFIG_OUROBOROS_DEBUG
+                if (remaining == 0) {
+                        write_pos + = bytes_to_copy;
+                        memset(write_pos, 0, ts);
+                }
+#endif
+                shm_buf->next = -1;
+                shm_buf->prev = prev_index;
+
+                prev_index = index;
+        }
+
+        return sdb;
+}
+
+void shm_release_du_buff(struct shm_du_map * dum, struct shm_du_buff * sdb)
+{
+        long index = 0;
+
+        if (sdb == NULL) {
+                LOG_DBGF("Bogus input, bugging out.");
+                return;
+        }
+
+        if (sdb < dum->ptr_du_buff || sdb > dum->ptr_du_buff + SHM_BUFFS_SIZE) {
+                LOG_DBGF("Refused to free sdb outside of region.");
+                return;
+        }
+
+        if ((sdb - dum->ptr_du_buff) % sizeof *sdb != 0) {
+                LOG_DBGF("Refused to free sdb at incorrect offset.");
+                return;
+        }
+
+        index = du_buff_ptr_to_idx(dum, sdb);
+
+        pthread_mutex_lock(dum->shm_mutex);
+
+        if (dum->ptr_map[index] == 0) {
+                LOG_DBGF("Attempt to free unused sdb. Nothing to do.");
+                pthread_mutex_unlock(dum->shm_mutex);
+                return;
+        }
+
+        /* release all blocks in the structure */
+        dum->ptr_map[index] = 0;
+
+        index = idx_to_block_ptr(dum, index)->next;
+        while (index != -1) {
+                dum->ptr_map[index] = 0;
+                index = idx_to_block_ptr(dum, index)->next;
+        }
+
+        pthread_mutex_unlock(dum->shm_mutex);
+
+}
+
+uint8_t * shm_du_buff_head_alloc(struct shm_du_map * dum,
+                                 struct shm_du_buff * sdb,
+                                 size_t size)
+{
+        uint8_t * ret;
+
+        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;
+
+        ret = (uint8_t *) idx_to_block_ptr(dum, du_buff_ptr_to_idx(dum,sdb));
+
+        return ret + sizeof(struct shm_block) + sdb->du_head;
+}
+
+uint8_t * shm_du_buff_tail_alloc(struct shm_du_map * dum,
+                                 struct shm_du_buff * sdb,
+                                 size_t size)
+{
+        uint8_t * ret;
+
+        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;
+
+        ret = (uint8_t *) idx_to_block_ptr(dum, du_buff_ptr_to_idx(dum,sdb));
+
+        return ret + sizeof(struct shm_block) + 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;
+}
+
+#endif