[KEYSTONE2,13/15] linux-ks2: Add shared resources handling

diff --git a/platform/linux-keystone2/Makefile.am b/platform/linux-keystone2/Makefile.am
index 282f406..f3d8471 100644
--- a/platform/linux-keystone2/Makefile.am
+++ b/platform/linux-keystone2/Makefile.am
@@ -67,6 +67,7 @@  odpplatinclude_HEADERS = \
 		  $(srcdir)/include/odp/plat/packet_types.h \
 		  $(srcdir)/include/odp/plat/pool_types.h \
 		  $(srcdir)/include/odp/plat/queue_types.h \
+		  $(srcdir)/include/odp/plat/shared_resource.h \
 		  $(srcdir)/include/odp/plat/state.h \
 		  $(srcdir)/include/odp/plat/ti_mcsdk.h \
 		  $(linux_generic_srcdir)/include/odp/plat/atomic_types.h \
@@ -140,6 +141,7 @@  __LIB__libodp_la_SOURCES = \
 			   mcsdk/mcsdk_navig.c \
 			   mcsdk/mcsdk_rmclient.c \
 			   mcsdk/sockutils.c \
+			   odp_shr.c \
 			   ../linux-generic/odp_barrier.c \
 			   ../linux-generic/odp_cpumask.c \
 			   ../linux-generic/odp_errno.c \
diff --git a/platform/linux-keystone2/include/odp/plat/shared_resource.h b/platform/linux-keystone2/include/odp/plat/shared_resource.h
new file mode 100644
index 0000000..12893be
--- /dev/null
+++ b/platform/linux-keystone2/include/odp/plat/shared_resource.h
@@ -0,0 +1,150 @@ 
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ */
+
+#ifndef ODP_PLAT_SHM_TABLE_H_
+#define ODP_PLAT_SHM_TABLE_H_
+
+#include <odp/spinlock.h>
+#include <odp_debug_internal.h>
+
+typedef struct entry_head {
+	struct entry_head *next;
+	uint16_t id;
+	odp_spinlock_t lock;
+	bool allocated;
+} odp_shr_head_t;
+
+typedef struct odp_shr_table_s {
+	struct entry_head *top_free;
+	odp_spinlock_t lock;
+	struct entry_head *base;
+	uint32_t entry_size;
+	uint32_t entry_num;
+} *odp_shr_table_t;
+
+/* Define typeof for C99 compatibility */
+#define typeof __typeof__
+
+#define ODP_SHR_TABLE_INVALID	((odp_shr_table_t)NULL)
+
+odp_shr_table_t _odp_shr_table_create(const char *name,
+			   uint32_t entry_size,
+			   uint32_t entry_num,
+			   uint32_t entry_align,
+			   uint16_t head_offset);
+void odp_shr_table_destroy(odp_shr_table_t table);
+
+void odp_shr_table_lock(odp_shr_table_t table);
+void odp_shr_table_unlock(odp_shr_table_t table);
+odp_shr_head_t *_odp_shr_alloc_nolock(odp_shr_table_t table);
+odp_shr_head_t *_odp_shr_alloc(odp_shr_table_t table);
+void _odp_shr_free(odp_shr_table_t table, odp_shr_head_t *head);
+odp_shr_head_t *_odp_shr_next(odp_shr_table_t table, odp_shr_head_t *head);
+odp_shr_head_t *_odp_shr_first_allocated(odp_shr_table_t table);
+odp_shr_head_t *_odp_shr_next_allocated(odp_shr_table_t table,
+					odp_shr_head_t *head);
+
+static inline
+uint16_t _odp_shr_id(odp_shr_head_t *entry)
+{
+	ODP_ASSERT(entry, "Bad entry");
+	return entry->id;
+}
+
+static inline
+uint32_t _odp_shr_table_num_entries(odp_shr_table_t table)
+{
+	ODP_ASSERT(table, "Bad table");
+	return table->entry_num;
+}
+
+static inline
+uint32_t _odp_shr_size(odp_shr_table_t table)
+{
+	ODP_ASSERT(table, "Bad table");
+	return table->entry_size;
+}
+
+static inline
+odp_shr_head_t *_odp_shr_from_id(odp_shr_table_t table, uint16_t id)
+{
+	ODP_ASSERT(table, "Bad table");
+	ODP_ASSERT(id < table->entry_num, "Wrong entry ID");
+	return (odp_shr_head_t *)((uintptr_t)table->base +
+				  id * table->entry_size);
+}
+
+static inline
+bool _odp_shr_is_allocated(odp_shr_head_t *entry)
+{
+	ODP_ASSERT(entry, "Bad entry");
+	return entry->allocated;
+}
+
+static inline
+void _odp_shr_lock(odp_shr_head_t *head)
+{
+	odp_spinlock_lock(&head->lock);
+}
+
+static inline
+void _odp_shr_unlock(odp_shr_head_t *head)
+{
+	odp_spinlock_unlock(&head->lock);
+}
+
+#define odp_shr(head, type) \
+		({ typeof(head) __head = (head); \
+		   (__head ? ODP_CONTAINEROF(__head, type, entry_head) : NULL);\
+		})
+
+#define odp_shr_from_id(table, id, type)	     \
+		odp_shr(_odp_shr_from_id(table, id), \
+				    type)
+
+#define odp_shr_next(entry)			     \
+	odp_shr(_odp_shr_next(&(entry)->entry_head), \
+			    typeof(*entry))
+
+#define odp_shr_first_allocated(table, type)	     \
+	odp_shr(_odp_shr_first_allocated(table),     \
+			    type)
+
+#define odp_shr_next_allocated(table, entry)			      \
+	odp_shr(_odp_shr_next_allocated(table, &(entry)->entry_head), \
+					typeof(*entry))
+
+#define odp_shr_table_for_each_entry(table, entry)			     \
+	for (entry = odp_shr_from_id(table, 0, typeof(*entry));		     \
+	     entry;							     \
+	     entry = odp_shr_next(entry))
+
+#define odp_shr_table_for_each_allocated_entry(table, entry)		     \
+	for (entry = odp_shr_first_allocated(table, typeof(*entry));         \
+	     entry;							     \
+	     entry = odp_shr_next_allocated(table, entry))
+
+#define odp_shr_table_create(name, type, num)				     \
+	_odp_shr_table_create(name, sizeof(type), num, ODP_ALIGNOF(type),    \
+			      ODP_OFFSETOF(type, entry_head))
+
+#define odp_shr_alloc(table, type)	odp_shr(_odp_shr_alloc(table), type)
+
+#define odp_shr_alloc_nolock(table, type)			     \
+		odp_shr(_odp_shr_alloc_nolock(table), type)
+
+#define odp_shr_free(table, entry) _odp_shr_free(table, &(entry)->entry_head)
+
+#define odp_shr_is_allocated(entry)			     \
+		_odp_shr_is_allocated(&(entry)->entry_head)
+
+#define odp_shr_lock(entry)	_odp_shr_lock(&(entry)->entry_head)
+#define odp_shr_unlock(entry)	_odp_shr_unlock(&(entry)->entry_head)
+
+#define odp_shr_id(entry)	_odp_shr_id(&(entry)->entry_head)
+#endif
diff --git a/platform/linux-keystone2/odp_shr.c b/platform/linux-keystone2/odp_shr.c
new file mode 100644
index 0000000..ff0f570
--- /dev/null
+++ b/platform/linux-keystone2/odp_shr.c
@@ -0,0 +1,179 @@ 
+/*
+ * Copyright (c) 2015, Linaro Limited
+ * Copyright (c) 2015, Texas Instruments Incorporated
+ * All rights reserved.
+ *
+ * SPDX-License-Identifier:     BSD-3-Clause
+ */
+
+#include <odp/shared_memory.h>
+#include <odp/align.h>
+#include <odp/plat/align.h>
+#include <odp/plat/shared_resource.h>
+
+static void push_entry(odp_shr_head_t **top,
+			      odp_shr_head_t *head)
+{
+	head->next = *top;
+	*top = head;
+}
+
+static odp_shr_head_t *pop_entry(odp_shr_head_t **top)
+{
+	odp_shr_head_t *head;
+
+	head = *top;
+	if (!head)
+		return NULL;
+	*top = head->next;
+	return head;
+}
+
+void odp_shr_table_lock(odp_shr_table_t table)
+{
+	odp_spinlock_lock(&table->lock);
+}
+
+void odp_shr_table_unlock(odp_shr_table_t table)
+{
+	odp_spinlock_unlock(&table->lock);
+}
+
+odp_shr_head_t *_odp_shr_alloc_nolock(odp_shr_table_t table)
+{
+	ODP_ASSERT(table, "Bad table");
+	odp_shr_head_t *head;
+	head = pop_entry(&table->top_free);
+	ODP_ASSERT(!head->allocated, "Got already allocated entry");
+	head->allocated = 1;
+	return head;
+}
+
+odp_shr_head_t *_odp_shr_alloc(odp_shr_table_t table)
+{
+	ODP_ASSERT(table, "Bad table");
+	odp_shr_head_t *head;
+	odp_shr_table_lock(table);
+	head = _odp_shr_alloc_nolock(table);
+	odp_shr_table_unlock(table);
+	ODP_DBG("Allocated entry head: %p\n", head);
+	return head;
+}
+
+void _odp_shr_free(odp_shr_table_t table, odp_shr_head_t *head)
+{
+	ODP_ASSERT(head, "Bad head");
+	ODP_ASSERT(head->allocated, "Freeing already freed entry");
+	ODP_ASSERT(table, "Bad table");
+	odp_shr_table_lock(table);
+	head->allocated = 0;
+	push_entry(&table->top_free, head);
+	odp_shr_table_unlock(table);
+}
+
+odp_shr_table_t _odp_shr_table_create(const char *name,
+			   uint32_t entry_size,
+			   uint32_t entry_num,
+			   uint32_t entry_align,
+			   uint16_t head_offset)
+{
+	odp_shm_t shm;
+	void *addr;
+	uint32_t size;
+	uint32_t table_head_size;
+	odp_shr_table_t table;
+	odp_shr_head_t *head;
+	uint32_t i;
+
+	if (entry_align == 0)
+		entry_align = ODP_CACHE_LINE_SIZE;
+
+	table_head_size = ODP_ALIGN_ROUNDUP(sizeof(odp_shr_table_t),
+					      entry_align);
+	entry_size = ODP_ALIGN_ROUNDUP(entry_size, entry_align);
+	size = table_head_size + entry_size * entry_num;
+
+	shm = odp_shm_reserve(name, size, entry_align, ODP_SHM_SW_ONLY);
+	if (shm == ODP_SHM_INVALID)
+		return ODP_SHR_TABLE_INVALID;
+
+	addr = odp_shm_addr(shm);
+	if (addr == NULL) {
+		odp_shm_free(shm);
+		return ODP_SHR_TABLE_INVALID;
+	}
+
+	memset(addr, 0, size);
+
+	table = addr;
+	odp_spinlock_init(&table->lock);
+	table->top_free = NULL;
+	table->entry_num = entry_num;
+	table->entry_size = entry_size;
+
+	addr = (uint8_t *)addr + table_head_size + head_offset;
+	table->base = addr;
+
+	for (i = 0; i < entry_num; i++) {
+		ODP_ASSERT(ODP_ALIGNED_CHECK((uint8_t *)addr - head_offset,
+					     entry_align),
+			   "Wrong entry alignment");
+		head = addr;
+		head->id = i;
+		odp_spinlock_init(&head->lock);
+		push_entry(&table->top_free, head);
+		addr = (uint8_t *)addr + entry_size;
+	}
+
+	return table;
+}
+
+void odp_shr_table_destroy(odp_shr_table_t table)
+{
+	(void)table;
+	ODP_UNIMPLEMENTED();
+}
+
+odp_shr_head_t *_odp_shr_next(odp_shr_table_t table, odp_shr_head_t *head)
+{
+	ODP_ASSERT(head, "Bad head");
+	ODP_ASSERT(table, "Bad table");
+	uint16_t id = _odp_shr_id(head);
+	if (++id >= _odp_shr_table_num_entries(table))
+		return NULL;
+	return _odp_shr_from_id(table, id);
+}
+
+
+#define _odp_shr_table_for_each(table, head)			\
+	for (head = _odp_shr_from_id(table, 0);			\
+	     head;						\
+	     head = _odp_shr_next(table, head))
+
+#define _odp_shr_table_for_each_continue(table, head)		\
+	for (head = _odp_shr_next(table, head);			\
+	     head;						\
+	     head = _odp_shr_next(table, head))
+
+odp_shr_head_t *_odp_shr_first_allocated(odp_shr_table_t table)
+{
+	odp_shr_head_t *head;
+	_odp_shr_table_for_each(table, head) {
+		if (head->allocated)
+			return head;
+	}
+	return NULL;
+}
+
+odp_shr_head_t *_odp_shr_next_allocated(odp_shr_table_t table,
+					odp_shr_head_t *head)
+{
+	ODP_ASSERT(head, "Bad head");
+	ODP_ASSERT(table, "Bad table");
+
+	_odp_shr_table_for_each_continue(table, head) {
+		if (head->allocated)
+			return head;
+	}
+	return NULL;
+}