Port FFS tools over from Building Block repository.

1 files changed, 662 insertions, 0 deletions

diff --git a/clib/src/vector.c b/clib/src/vector.c
new file mode 100644
index 0000000..54f9981
--- /dev/null
+++ b/clib/src/vector.c
@@ -0,0 +1,662 @@
+/*
+ * Copyright (c) International Business Machines Corp., 2014
+ *
+ * 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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ *   File: vector.c
+ * Author: Shaun Wetzstein <shaun@us.ibm.com>
+ *  Descr: dynamic vector
+ *   Note:
+ *   Date: 08/29/10
+ */
+
+#include <unistd.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <malloc.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <limits.h>
+
+#include "libclib.h"
+
+#include "vector.h"
+#include "tree.h"
+#include "tree_iter.h"
+#include "mq.h"
+
+/* ======================================================================= */
+
+/*! @cond */
+#define VECTOR_NODE_MAGIC	"VCND"
+
+#define VECTOR_NODE_MAGIC_CHECK(m) ({                           \
+    bool rc = (((m)[0] != VECTOR_NODE_MAGIC[0]) ||              \
+               ((m)[1] != VECTOR_NODE_MAGIC[1]) ||              \
+               ((m)[2] != VECTOR_NODE_MAGIC[2]) ||              \
+               ((m)[3] != VECTOR_NODE_MAGIC[3]));               \
+    rc;                                                         \
+                                    })
+
+typedef struct vector_node vector_node_t;
+
+struct vector_node {
+	uint8_t magic[4];
+
+	uint32_t address;
+	tree_node_t node;
+
+	uint8_t data[];
+};
+
+#define VECTOR_PAGE_MAX		UINT16_MAX
+#define VECTOR_PAGE_DIVISOR	32
+
+#define __index_to_page(i,s)                                    \
+({                                                              \
+    typeof(i) _p = ((i) / (s));                                 \
+    _p;                                      			\
+})
+
+#define __index_to_page_hashed(i,s)                             \
+({                                                              \
+    typeof(i) _h = int32_hash1(__index_to_page((i),(s)));       \
+    _h;                                                         \
+})
+/*! @endcond */
+
+/* ======================================================================= */
+
+static vector_node_t *__vector_find_page(vector_t * self, size_t idx)
+{
+	const void *hash;
+	hash = (const void *)__index_to_page_hashed(idx, self->hdr.elem_num);
+
+	tree_node_t *node = tree_find(&self->tree, hash);
+	if (unlikely(node == NULL)) {
+		UNEXPECTED("'%d' index out of range", idx);
+		return NULL;
+	}
+
+	return container_of(node, vector_node_t, node);
+}
+
+static int __vector_shrink(vector_t * self)
+{
+	assert(self != NULL);
+
+	vector_node_t *node = __vector_find_page(self,
+						 vector_capacity(self) - 1);
+	assert(node != NULL);
+
+	int rc = splay_remove(&self->tree, &node->node);
+
+	free(node);
+	self->hdr.page_count--;
+
+	return rc;
+}
+
+static vector_node_t *__vector_grow(vector_t * self)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	vector_node_t *node = NULL;
+	int rc = posix_memalign((void **)&node, sizeof(void *),
+				self->hdr.page_size);
+	if (rc != 0) {
+		ERRNO(errno);
+		return NULL;
+	}
+
+	memset(node, 0, self->hdr.page_size);
+
+	node->magic[0] = VECTOR_NODE_MAGIC[0];
+	node->magic[1] = VECTOR_NODE_MAGIC[1];
+	node->magic[2] = VECTOR_NODE_MAGIC[2];
+	node->magic[3] = VECTOR_NODE_MAGIC[3];
+
+	node->address = (uint32_t) node;
+
+	size_t hash = __index_to_page_hashed(vector_capacity(self),
+					     self->hdr.elem_num);
+
+	tree_node_init(&node->node, (const void *)hash);
+	if (splay_insert(&self->tree, &node->node) < 0) {
+		free(node);
+		return NULL;
+	}
+	self->hdr.page_count++;
+
+	return node;
+}
+
+static int __vector_compare(const int i1, const int i2)
+{
+	return i1 - i2;
+}
+
+/* ======================================================================= */
+
+int vector_init3(vector_t * self, const char *name, size_t elem_size)
+{
+	size_t page_size = max(sysconf(_SC_PAGESIZE),
+			       __round_pow2(elem_size * VECTOR_PAGE_DIVISOR));
+	return vector_init4(self, name, elem_size, page_size);
+}
+
+int vector_init4(vector_t * self, const char *name, size_t elem_size,
+		 size_t page_size)
+{
+	assert(self != NULL);
+
+	if (unlikely(MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC)) == false)
+		vector_delete(self);
+
+	if (elem_size < VECTOR_ELEM_MIN || VECTOR_ELEM_MAX < elem_size) {
+		UNEXPECTED("'%d' elem_size out of range [%d..%d]",
+			   elem_size, VECTOR_ELEM_MIN, VECTOR_ELEM_MAX);
+		return -1;
+	}
+
+	page_size = __round_pow2(page_size);
+	if (page_size / elem_size < VECTOR_PAGE_DIVISOR) {
+		UNEXPECTED("'%d' page_size out of range [%d..%d]",
+			   page_size, elem_size * VECTOR_PAGE_DIVISOR,
+			   VECTOR_PAGE_MAX);
+		return -1;
+	}
+
+	memset(self, 0, sizeof *self);
+
+	self->hdr.id[IDENT_MAGIC_0] = VECTOR_MAGIC[IDENT_MAGIC_0];
+	self->hdr.id[IDENT_MAGIC_1] = VECTOR_MAGIC[IDENT_MAGIC_1];
+	self->hdr.id[IDENT_MAGIC_2] = VECTOR_MAGIC[IDENT_MAGIC_2];
+	self->hdr.id[IDENT_MAGIC_3] = VECTOR_MAGIC[IDENT_MAGIC_3];
+
+	self->hdr.id[IDENT_MAJOR] = CLIB_MAJOR;
+	self->hdr.id[IDENT_MINOR] = CLIB_MINOR;
+	self->hdr.id[IDENT_PATCH] = CLIB_PATCH;
+
+	if (__BYTE_ORDER == __LITTLE_ENDIAN)
+		self->hdr.id[IDENT_FLAGS] |= VECTOR_FLAG_LSB;
+	if (__BYTE_ORDER == __BIG_ENDIAN)
+		self->hdr.id[IDENT_FLAGS] |= VECTOR_FLAG_MSB;
+
+	self->hdr.page_size = page_size;
+	self->hdr.elem_size = elem_size;
+	self->hdr.elem_num = (self->hdr.page_size - sizeof(vector_node_t)) /
+	    self->hdr.elem_size;
+
+	if (name != NULL && *name != '\0')
+		strncpy(self->hdr.name, name, sizeof(self->hdr.name));
+
+	tree_init(&self->tree, (compare_f) __vector_compare);
+
+	return 0;
+}
+
+int vector_delete(vector_t * self)
+{
+	if (unlikely(self == NULL))
+		return 0;
+
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	tree_iter_t it;
+	tree_iter_init(&it, &self->tree, TI_FLAG_FWD);
+
+	vector_node_t *node;
+	tree_for_each(&it, node, node) {
+		if (VECTOR_NODE_MAGIC_CHECK(node->magic)) {
+			UNEXPECTED("'%s' invalid or corrupt vector_node"
+				   "object => '%.4s'", self->hdr.name,
+				   node->magic);
+			return -1;
+		}
+
+		if (splay_remove(&self->tree, &node->node) < 0)
+			return -1;
+
+		memset(node, 0, sizeof(*node));
+		free(node);
+	}
+
+	self->hdr.page_count = self->hdr.size = 0;
+
+	return 0;
+}
+
+const void *vector_at(vector_t * self, size_t idx)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+	assert(idx < self->hdr.size);
+
+	vector_node_t *node = __vector_find_page(self, idx);
+	return node->data + (self->hdr.elem_size * (idx % self->hdr.elem_num));
+}
+
+int vector_get3(vector_t * self, size_t idx, void *ptr)
+{
+	return vector_get4(self, idx, ptr, 1);
+}
+
+int vector_get4(vector_t * self, size_t idx, void *ptr, size_t count)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	while (0 < count) {
+		memcpy(ptr, vector_at(self, idx), self->hdr.elem_size);
+
+		idx++;
+		count--;
+
+		ptr += self->hdr.elem_size;
+	}
+
+	return 0;
+}
+
+static inline int __vector_put(vector_t * self, size_t idx, const void *ptr)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	vector_node_t *node = __vector_find_page(self, idx);
+	assert(node != NULL);
+
+	if (VECTOR_NODE_MAGIC_CHECK(node->magic)) {
+		UNEXPECTED("'%s' invalid or corrupt vector_node object => "
+			   "'%.4s'", self->hdr.name, node->magic);
+		return -1;
+	}
+
+	memcpy(node->data + (self->hdr.elem_size * (idx % self->hdr.elem_num)),
+	       ptr, self->hdr.elem_size);
+
+	return 0;
+}
+
+int vector_put3(vector_t * self, size_t idx, const void *ptr)
+{
+	return vector_put4(self, idx, ptr, 1);
+}
+
+int vector_put4(vector_t * self, size_t idx, const void *ptr, size_t count)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+	assert(idx < self->hdr.size);
+
+	while (0 < count) {
+		if (__vector_put(self, idx, ptr) < 0)
+			return -1;
+
+		idx++;
+		count--;
+
+		ptr += self->hdr.elem_size;
+	}
+
+	return 0;
+}
+
+size_t vector_size1(vector_t * self)
+{
+	assert(self != NULL);
+	return self->hdr.size;
+}
+
+int vector_size2(vector_t * self, size_t size)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	size_t pages = __index_to_page(size, self->hdr.elem_num) + 1;
+
+	if (vector_pages(self) < pages) {
+		while (vector_pages(self) < pages)
+			(void)__vector_grow(self);
+	} else if (pages < vector_pages(self)) {
+		if (size <= 0)
+			vector_delete(self);
+		else
+			while (pages < vector_pages(self))
+				if (__vector_shrink(self) < 0)
+					return -1;
+	}
+
+	return self->hdr.size = size;
+}
+
+size_t vector_pages(vector_t * self)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+	return self->hdr.page_count;
+}
+
+size_t vector_capacity(vector_t * self)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+	return self->hdr.page_count * self->hdr.elem_num;
+}
+
+size_t vector_elem_size(vector_t * self)
+{
+	assert(self != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+	return self->hdr.elem_size;
+}
+
+ssize_t vector_save(vector_t * self, FILE * out)
+{
+	assert(self != NULL);
+	assert(out != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	int header_swap(vector_header_t * hdr) {
+		assert(hdr != NULL);
+
+		if (hdr->id[IDENT_FLAGS] & VECTOR_FLAG_MSB) {
+			hdr->page_size = htobe32(hdr->page_size);
+			hdr->elem_size = htobe16(hdr->elem_size);
+			hdr->elem_num = htobe16(hdr->elem_num);
+			hdr->size = htobe32(hdr->size);
+			hdr->page_count = htobe32(hdr->page_count);
+		} else if (hdr->id[IDENT_FLAGS] & VECTOR_FLAG_LSB) {
+			hdr->page_size = htole32(hdr->page_size);
+			hdr->elem_size = htole16(hdr->elem_size);
+			hdr->elem_num = htole16(hdr->elem_num);
+			hdr->size = htole32(hdr->size);
+			hdr->page_count = htole32(hdr->page_count);
+		} else {
+			UNEXPECTED("'%s' invalid or corrupt flash object => "
+				   "'%x'", hdr->name, hdr->id[IDENT_FLAGS]);
+			return -1;
+		}
+
+		return 0;
+	}
+
+	ssize_t save(vector_t * self, FILE * out) {
+		tree_iter_t it;
+		tree_iter_init(&it, &self->tree, TI_FLAG_FWD);
+
+		ssize_t len = 0;
+
+		vector_node_t *node;
+		tree_for_each(&it, node, node) {
+			if (VECTOR_NODE_MAGIC_CHECK(node->magic)) {
+				UNEXPECTED("'%s' invalid or corrupt vector_node"
+					   "object => '%.4s'", self->hdr.name,
+					   node->magic);
+				return -1;
+			}
+
+			size_t rc;
+
+			vector_node_t copy = *node;
+
+			copy.address = 0;
+			copy.node.left = copy.node.right = NULL;
+			copy.node.parent = NULL;
+
+			rc = fwrite((char *)&copy, 1, sizeof(copy), out);
+			if (rc != sizeof(copy)) {
+				if (ferror(out)) {
+					ERRNO(errno);
+					return -1;
+				}
+			}
+			len += rc;
+
+			rc = fwrite((char *)node->data, 1,
+				    self->hdr.page_size - sizeof(*node), out);
+			if (rc != self->hdr.page_size - sizeof(*node)) {
+				if (ferror(out)) {
+					ERRNO(errno);
+					return -1;
+				}
+			}
+			len += rc;
+		}
+
+		return len;
+	}
+
+	ssize_t total = 0;
+
+	vector_header_t hdr = self->hdr;
+	if (header_swap(&hdr) < 0)
+		return -1;
+
+	clearerr(out);
+	total = fwrite(&hdr, 1, sizeof(hdr), out);
+	if (total != sizeof(hdr)) {
+		if (ferror(out)) {
+			ERRNO(errno);
+			return -1;
+		}
+	}
+
+	total += save(self, out);
+
+	return total;
+}
+
+ssize_t vector_load(vector_t * self, FILE * in)
+{
+	assert(self != NULL);
+	assert(in != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	int header_swap(vector_header_t * hdr) {
+		assert(hdr != NULL);
+
+		if (hdr->id[IDENT_FLAGS] & VECTOR_FLAG_MSB) {
+			hdr->page_size = be32toh(hdr->page_size);
+			hdr->elem_size = be16toh(hdr->elem_size);
+			hdr->elem_num = be16toh(hdr->elem_num);
+			hdr->size = be32toh(hdr->size);
+			hdr->page_count = be32toh(hdr->page_count);
+		} else if (hdr->id[IDENT_FLAGS] & VECTOR_FLAG_LSB) {
+			hdr->page_size = le32toh(hdr->page_size);
+			hdr->elem_size = le16toh(hdr->elem_size);
+			hdr->elem_num = le16toh(hdr->elem_num);
+			hdr->size = le32toh(hdr->size);
+			hdr->page_count = le32toh(hdr->page_count);
+		} else {
+			UNEXPECTED("'%s' invalid or corrupt flash object => "
+				   "'%x'", hdr->name, hdr->id[IDENT_FLAGS]);
+			return -1;
+		}
+
+		return 0;
+	}
+
+	vector_delete(self);
+
+	clearerr(in);
+	ssize_t len = fread(&self->hdr, 1, sizeof(self->hdr), in);
+	if (len != sizeof(self->hdr)) {
+		if (feof(in)) {
+			UNEXPECTED("'%s' end-of-file encountered",
+				   self->hdr.name);
+			return -1;
+		}
+		if (ferror(in)) {
+			ERRNO(errno);
+			return -1;
+		}
+	}
+
+	if (header_swap(&self->hdr) < 0)
+		return -1;
+
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	tree_init(&self->tree, (compare_f) __vector_compare);
+	vector_node_t *node = NULL;
+
+	for (size_t i = 0; i < vector_pages(self); i++) {
+		size_t rc = posix_memalign((void **)&node, sizeof(void *),
+					   self->hdr.page_size);
+		if (rc != 0) {
+			ERRNO(errno);
+			return -1;
+		}
+		memset(node, 0, self->hdr.page_size);
+
+		rc = fread((void *)node, 1, self->hdr.page_size, in);
+		if (rc != self->hdr.page_size) {
+			if (feof(in)) {
+				UNEXPECTED("'%s' end-of-file encountered",
+					   self->hdr.name);
+				return -1;
+			}
+			if (ferror(in)) {
+				ERRNO(errno);
+				return -1;
+			}
+		}
+
+		len += rc;
+
+		if (VECTOR_NODE_MAGIC_CHECK(node->magic)) {
+			UNEXPECTED("'%s' invalid or corrupt vector_node "
+				   "object => '%.4s'", self->hdr.name,
+				   node->magic);
+			return -1;
+		}
+
+		node->address = (uint32_t) node;
+		tree_node_init(&node->node, node->node.key);
+		splay_insert(&self->tree, &node->node);
+
+		node = NULL;
+	}
+
+	return len;
+}
+
+int vector_send(vector_t * self, mqueue_t * mq)
+{
+	assert(self != NULL);
+	assert(mq != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	mqueue_send(mq, (char *)self, sizeof(*self));
+
+	tree_iter_t it;
+	tree_iter_init(&it, &self->tree, TI_FLAG_FWD);
+
+	vector_node_t *node;
+	tree_for_each(&it, node, node) {
+		assert(!VECTOR_NODE_MAGIC_CHECK(node->magic));
+		mqueue_send(mq, (char *)node, self->hdr.page_size);
+	}
+
+	return 0;
+}
+
+int vector_receive(vector_t * self, mqueue_t * mq)
+{
+	assert(self != NULL);
+	assert(mq != NULL);
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	vector_delete(self);
+
+	mqueue_attr_t attr = mqueue_getattr(mq);
+
+	vector_node_t *node = NULL;
+	size_t rc = posix_memalign((void **)&node, attr.mq_msgsize,
+				   attr.mq_msgsize);
+	if (rc != 0) {
+		ERRNO(errno);
+		return -1;
+	}
+
+	ssize_t len = mqueue_receive(mq, (void *)node, attr.mq_msgsize);
+	assert(0 < len);
+
+	assert(!MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC));
+
+	memcpy(self, (void *)node, sizeof(*self));
+	tree_init(&self->tree, (compare_f) __vector_compare);
+
+	for (size_t i = 0; i < vector_pages(self); i++) {
+		rc = posix_memalign((void **)&node, attr.mq_msgsize,
+				    attr.mq_msgsize);
+		if (rc != 0) {
+			ERRNO(errno);
+			return -1;
+		}
+
+		len = mqueue_receive(mq, (void *)node, attr.mq_msgsize);
+		assert(0 < len);
+
+		assert(!VECTOR_NODE_MAGIC_CHECK(node->magic));
+
+		node->address = (uint32_t) node;
+		tree_node_init(&node->node, node->node.key);
+		splay_insert(&self->tree, &node->node);
+
+		node = NULL;
+	}
+
+	return 0;
+}
+
+void vector_dump(vector_t * self, FILE * out)
+{
+	if (out == NULL)
+		out = stdout;
+
+	if (self != NULL) {
+		assert(!unlikely(MAGIC_CHECK(self->hdr.id, VECTOR_MAGIC)));
+
+		fprintf(out, "%s: page_size: %d elem_size: %d elem_num: %d -- "
+			"size: %d capacity: %d -- page_count: %d\n",
+			self->hdr.name, self->hdr.page_size,
+			self->hdr.elem_size, self->hdr.elem_num,
+			vector_size(self), vector_capacity(self),
+			self->hdr.page_count);
+
+		tree_iter_t it;
+		tree_iter_init(&it, &self->tree, TI_FLAG_FWD);
+
+		vector_node_t *node;
+		tree_for_each(&it, node, node) {
+			fprintf(out, "magic[%.4s] node: %p data: %p -- "
+				"address: %x\n", node->magic, &node->node,
+				node->data, node->address);
+
+			dump_memory(out, (unsigned long)node, node,
+				    self->hdr.page_size);
+		}
+	}
+}
+
+/* ======================================================================= */