[09/10] Keystone2: Add initial HW queues support

diff --git a/platform/linux-keystone2/include/odp_buffer_internal.h b/platform/linux-keystone2/include/odp_buffer_internal.h
index 044da4e..030b107 100644
--- a/platform/linux-keystone2/include/odp_buffer_internal.h
+++ b/platform/linux-keystone2/include/odp_buffer_internal.h
@@ -58,7 +58,7 @@  typedef union odp_buffer_bits_t {
 typedef struct odp_buffer_hdr_t {
 	Cppi_HostDesc   desc;
 	void		*buf_vaddr;
-	odp_queue_t	free_queue;
+	uint32_t	free_queue;
 	int type;
 	struct odp_buffer_hdr_t *next;       /* next buf in a list */
 	odp_buffer_bits_t        handle;     /* handle */
diff --git a/platform/linux-keystone2/include/odp_buffer_pool_internal.h b/platform/linux-keystone2/include/odp_buffer_pool_internal.h
index 6ee3eb0..a77331c 100644
--- a/platform/linux-keystone2/include/odp_buffer_pool_internal.h
+++ b/platform/linux-keystone2/include/odp_buffer_pool_internal.h
@@ -57,7 +57,7 @@  struct pool_entry_s {
 	size_t                  payload_size;
 	size_t                  payload_align;
 	int                     buf_type;
-	odp_queue_t             free_queue;
+	uint32_t		free_queue;
 
 	uintptr_t               buf_base;
 	size_t                  buf_size;
diff --git a/platform/linux-keystone2/include/odp_queue_internal.h b/platform/linux-keystone2/include/odp_queue_internal.h
index 8b6c517..c7c84d6 100644
--- a/platform/linux-keystone2/include/odp_queue_internal.h
+++ b/platform/linux-keystone2/include/odp_queue_internal.h
@@ -22,6 +22,7 @@  extern "C" {
 #include <odp_buffer_internal.h>
 #include <odp_packet_io.h>
 #include <odp_align.h>
+#include <configs/odp_config_platform.h>
 
 
 #define USE_TICKETLOCK
@@ -72,6 +73,7 @@  struct queue_entry_s {
 	odp_queue_param_t param;
 	odp_pktio_t       pktin;
 	odp_pktio_t       pktout;
+	uint32_t          hw_queue;
 	char              name[ODP_QUEUE_NAME_LEN];
 };
 
@@ -113,6 +115,24 @@  static inline queue_entry_t *queue_to_qentry(odp_queue_t handle)
 	return get_qentry(queue_id);
 }
 
+static inline void _ti_hw_queue_push_desc(uint32_t hw_queue,
+						odp_buffer_hdr_t *buf_hdr)
+{
+	ti_em_osal_hw_queue_push_size(hw_queue,
+				      (void *)&buf_hdr->desc,
+				      sizeof(Cppi_HostDesc),
+				      TI_EM_MEM_PUBLIC_DESC);
+}
+
+static inline odp_buffer_hdr_t *_ti_hw_queue_pop_desc(uint32_t hw_queue)
+{
+	return ti_em_osal_hw_queue_pop(hw_queue,
+					TI_EM_MEM_PUBLIC_DESC);
+}
+
+odp_queue_t _odp_queue_create(const char *name, odp_queue_type_t type,
+			     odp_queue_param_t *param, uint32_t hw_queue);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/platform/linux-keystone2/source/odp_buffer_pool.c b/platform/linux-keystone2/source/odp_buffer_pool.c
index fe39fad..5170433 100644
--- a/platform/linux-keystone2/source/odp_buffer_pool.c
+++ b/platform/linux-keystone2/source/odp_buffer_pool.c
@@ -18,6 +18,7 @@ 
 #include <odp_hints.h>
 #include <odp_debug.h>
 #include <odp_sync.h>
+#include <odp_queue_internal.h>
 
 #include <string.h>
 #include <stdlib.h>
@@ -218,10 +219,7 @@  static int link_bufs(pool_entry_t *pool)
 				   &tag);
 
 		odp_sync_stores();
-		ti_em_osal_hw_queue_push_size(pool->s.free_queue,
-					 (void*)hdr,
-					 sizeof(Cppi_HostDesc),
-					 TI_EM_MEM_PUBLIC_DESC);
+		_ti_hw_queue_push_desc(pool->s.free_queue, hdr);
 		buf_addr.v += buf_size;
 		buf_addr.p += buf_size;
 		desc_index++;
@@ -303,10 +301,7 @@  odp_buffer_t odp_buffer_alloc(odp_buffer_pool_t pool_id)
 void odp_buffer_free(odp_buffer_t buf)
 {
 	odp_buffer_hdr_t *hdr = odp_buf_to_hdr(buf);
-	ti_em_osal_hw_queue_push_size(hdr->free_queue,
-				 (void*)hdr,
-				 sizeof(Cppi_HostDesc),
-				 TI_EM_MEM_PUBLIC_DESC);
+	_ti_hw_queue_push_desc(hdr->free_queue, hdr);
 }
 
 void odp_buffer_pool_print(odp_buffer_pool_t pool_id)
diff --git a/platform/linux-keystone2/source/odp_queue.c b/platform/linux-keystone2/source/odp_queue.c
index 7c1fffe..a8768c2 100644
--- a/platform/linux-keystone2/source/odp_queue.c
+++ b/platform/linux-keystone2/source/odp_queue.c
@@ -15,6 +15,7 @@ 
 #include <odp_shared_memory.h>
 #include <odp_schedule_internal.h>
 #include <odp_config.h>
+#include <configs/odp_config_platform.h>
 #include <odp_packet_io_internal.h>
 #include <odp_packet_io_queue.h>
 #include <odp_debug.h>
@@ -110,6 +111,12 @@  int odp_queue_init_global(void)
 		queue_entry_t *queue = get_qentry(i);
 		LOCK_INIT(&queue->s.lock);
 		queue->s.handle = queue_from_id(i);
+		queue->s.status = QUEUE_STATUS_FREE;
+		/*
+		 * TODO: HW queue is mapped dirrectly to queue_entry_t
+		 * instance. It may worth to allocate HW queue on open.
+		 */
+		queue->s.hw_queue = TI_ODP_PUBLIC_QUEUE_BASE_IDX + i;
 	}
 
 	ODP_DBG("done\n");
@@ -132,8 +139,8 @@  odp_queue_type_t odp_queue_type(odp_queue_t handle)
 	return queue->s.type;
 }
 
-odp_queue_t odp_queue_create(const char *name, odp_queue_type_t type,
-			     odp_queue_param_t *param)
+odp_queue_t _odp_queue_create(const char *name, odp_queue_type_t type,
+			      odp_queue_param_t *param, uint32_t hw_queue)
 {
 	uint32_t i;
 	queue_entry_t *queue;
@@ -147,6 +154,18 @@  odp_queue_t odp_queue_create(const char *name, odp_queue_type_t type,
 
 		LOCK(&queue->s.lock);
 		if (queue->s.status == QUEUE_STATUS_FREE) {
+			if (hw_queue)
+				queue->s.hw_queue = hw_queue;
+			/*
+			 * Don't open hw queue if its number is specified
+			 * as it is most probably opened by Linux kernel
+			 */
+			else if (ti_em_osal_hw_queue_open(queue->s.hw_queue)
+				 != EM_OK) {
+				UNLOCK(&queue->s.lock);
+				continue;
+			}
+
 			queue_init(queue, name, type, param);
 
 			if (type == ODP_QUEUE_TYPE_SCHED ||
@@ -179,6 +198,12 @@  odp_queue_t odp_queue_create(const char *name, odp_queue_type_t type,
 	return handle;
 }
 
+odp_queue_t odp_queue_create(const char *name, odp_queue_type_t type,
+			     odp_queue_param_t *param)
+{
+	return _odp_queue_create(name, type, param, 0);
+}
+
 
 odp_buffer_t queue_sched_buf(odp_queue_t handle)
 {
@@ -223,65 +248,51 @@  odp_queue_t odp_queue_lookup(const char *name)
 
 int queue_enq(queue_entry_t *queue, odp_buffer_hdr_t *buf_hdr)
 {
-	int sched = 0;
+	_ti_hw_queue_push_desc(queue->s.hw_queue, buf_hdr);
 
-	LOCK(&queue->s.lock);
-	if (queue->s.head == NULL) {
-		/* Empty queue */
-		queue->s.head = buf_hdr;
-		queue->s.tail = buf_hdr;
-		buf_hdr->next = NULL;
-	} else {
-		queue->s.tail->next = buf_hdr;
-		queue->s.tail = buf_hdr;
-		buf_hdr->next = NULL;
-	}
-
-	if (queue->s.status == QUEUE_STATUS_NOTSCHED) {
-		queue->s.status = QUEUE_STATUS_SCHED;
-		sched = 1; /* retval: schedule queue */
+	if (queue->s.type == ODP_QUEUE_TYPE_SCHED) {
+		int sched = 0;
+		LOCK(&queue->s.lock);
+		if (queue->s.status == QUEUE_STATUS_NOTSCHED) {
+			queue->s.status = QUEUE_STATUS_SCHED;
+			sched = 1;
+		}
+		UNLOCK(&queue->s.lock);
+		/* Add queue to scheduling */
+		if (sched)
+			odp_schedule_queue(queue->s.handle,
+					   queue->s.param.sched.prio);
 	}
-	UNLOCK(&queue->s.lock);
-
-	/* Add queue to scheduling */
-	if (sched == 1)
-		odp_schedule_queue(queue->s.handle, queue->s.param.sched.prio);
-
 	return 0;
 }
 
 
 int queue_enq_multi(queue_entry_t *queue, odp_buffer_hdr_t *buf_hdr[], int num)
 {
-	int sched = 0;
 	int i;
-	odp_buffer_hdr_t *tail;
 
-	for (i = 0; i < num - 1; i++)
-		buf_hdr[i]->next = buf_hdr[i+1];
-
-	tail = buf_hdr[num-1];
-	buf_hdr[num-1]->next = NULL;
-
-	LOCK(&queue->s.lock);
-	/* Empty queue */
-	if (queue->s.head == NULL)
-		queue->s.head = buf_hdr[0];
-	else
-		queue->s.tail->next = buf_hdr[0];
-
-	queue->s.tail = tail;
-
-	if (queue->s.status == QUEUE_STATUS_NOTSCHED) {
-		queue->s.status = QUEUE_STATUS_SCHED;
-		sched = 1; /* retval: schedule queue */
+	/*
+	 * TODO: Should this series of buffers be enqueued atomically?
+	 * Can another buffer be pushed in this queue in the middle?
+	 */
+	for (i = 0; i < num; i++) {
+		/* TODO: Implement multi dequeue a lower level */
+		_ti_hw_queue_push_desc(queue->s.hw_queue, buf_hdr[i]);
 	}
-	UNLOCK(&queue->s.lock);
-
-	/* Add queue to scheduling */
-	if (sched == 1)
-		odp_schedule_queue(queue->s.handle, queue->s.param.sched.prio);
 
+	if (queue->s.type == ODP_QUEUE_TYPE_SCHED) {
+		int sched = 0;
+		LOCK(&queue->s.lock);
+		if (queue->s.status == QUEUE_STATUS_NOTSCHED) {
+			queue->s.status = QUEUE_STATUS_SCHED;
+			sched = 1;
+		}
+		UNLOCK(&queue->s.lock);
+		/* Add queue to scheduling */
+		if (sched)
+			odp_schedule_queue(queue->s.handle,
+					   queue->s.param.sched.prio);
+	}
 	return 0;
 }
 
@@ -318,63 +329,41 @@  int odp_queue_enq(odp_queue_t handle, odp_buffer_t buf)
 
 odp_buffer_hdr_t *queue_deq(queue_entry_t *queue)
 {
-	odp_buffer_hdr_t *buf_hdr = NULL;
+	odp_buffer_hdr_t *buf_hdr;
 
-	LOCK(&queue->s.lock);
+	buf_hdr = (odp_buffer_hdr_t *)ti_em_osal_hw_queue_pop(queue->s.hw_queue,
+			TI_EM_MEM_PUBLIC_DESC);
 
-	if (queue->s.head == NULL) {
-		/* Already empty queue */
-		if (queue->s.status == QUEUE_STATUS_SCHED &&
-		    queue->s.type != ODP_QUEUE_TYPE_PKTIN)
+	if (!buf_hdr && queue->s.type == ODP_QUEUE_TYPE_SCHED) {
+		LOCK(&queue->s.lock);
+		if (!buf_hdr && queue->s.status == QUEUE_STATUS_SCHED)
 			queue->s.status = QUEUE_STATUS_NOTSCHED;
-	} else {
-		buf_hdr       = queue->s.head;
-		queue->s.head = buf_hdr->next;
-		buf_hdr->next = NULL;
-
-		if (queue->s.head == NULL) {
-			/* Queue is now empty */
-			queue->s.tail = NULL;
-		}
+		UNLOCK(&queue->s.lock);
 	}
 
-	UNLOCK(&queue->s.lock);
-
 	return buf_hdr;
 }
 
 
 int queue_deq_multi(queue_entry_t *queue, odp_buffer_hdr_t *buf_hdr[], int num)
 {
-	int i = 0;
-
-	LOCK(&queue->s.lock);
-
-	if (queue->s.head == NULL) {
-		/* Already empty queue */
-		if (queue->s.status == QUEUE_STATUS_SCHED &&
-		    queue->s.type != ODP_QUEUE_TYPE_PKTIN)
-			queue->s.status = QUEUE_STATUS_NOTSCHED;
-	} else {
-		odp_buffer_hdr_t *hdr = queue->s.head;
-
-		for (; i < num && hdr; i++) {
-			buf_hdr[i]       = hdr;
-			/* odp_prefetch(hdr->addr); */
-			hdr              = hdr->next;
-			buf_hdr[i]->next = NULL;
-		}
-
-		queue->s.head = hdr;
-
-		if (hdr == NULL) {
-			/* Queue is now empty */
-			queue->s.tail = NULL;
+	int i;
+	for (i = 0; i < num; i++) {
+		/* TODO: Implement multi dequeue a lower level */
+		buf_hdr[i] = (odp_buffer_hdr_t *)ti_em_osal_hw_queue_pop(
+				     queue->s.hw_queue,
+				     TI_EM_MEM_PUBLIC_DESC);
+		if (!buf_hdr[i]) {
+			if (queue->s.type != ODP_QUEUE_TYPE_SCHED)
+				break;
+			LOCK(&queue->s.lock);
+			if (queue->s.status == QUEUE_STATUS_SCHED)
+				queue->s.status = QUEUE_STATUS_NOTSCHED;
+			UNLOCK(&queue->s.lock);
+			break;
 		}
 	}
 
-	UNLOCK(&queue->s.lock);
-
 	return i;
 }