lib: Add a fifo implementation.

Implement a fifo to accomodate outstanding IPIs for a specific hart
at the same time.

Signed-off-by: Atish Patra <atish.patra@wdc.com>

2 files changed, 86 insertions, 0 deletions

diff --git a/lib/objects.mk b/lib/objects.mk
index ebee592..137d7f0 100644
--- a/lib/objects.mk
+++ b/lib/objects.mk
@@ -15,6 +15,7 @@ lib-objs-y += riscv_locks.o
 lib-objs-y += sbi_console.o
 lib-objs-y += sbi_ecall.o
 lib-objs-y += sbi_emulate_csr.o
+lib-objs-y += sbi_fifo.o
 lib-objs-y += sbi_hart.o
 lib-objs-y += sbi_illegal_insn.o
 lib-objs-y += sbi_init.o
@@ -23,3 +24,6 @@ lib-objs-y += sbi_misaligned_ldst.o
 lib-objs-y += sbi_system.o
 lib-objs-y += sbi_timer.o
 lib-objs-y += sbi_trap.o
+
+# External Libraries to include
+PLATFORM_INCLUDE_LIBC=y
diff --git a/lib/sbi_fifo.c b/lib/sbi_fifo.c
new file mode 100644
index 0000000..9d9a5b9
--- /dev/null
+++ b/lib/sbi_fifo.c
@@ -0,0 +1,82 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra<atish.patra@wdc.com>
+ *
+ */
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_fifo.h>
+#include <plat/string.h>
+
+void sbi_fifo_init(struct sbi_fifo *fifo, unsigned long entries,
+		   unsigned long size)
+{
+	fifo->head = -1;
+	fifo->tail = -1;
+	fifo->num_entries = entries;
+	fifo->entrysize = size;
+	SPIN_LOCK_INIT(&fifo->qlock);
+	memset(fifo->queue, 0, entries * size);
+}
+
+bool sbi_fifo_is_full(struct sbi_fifo *fifo)
+{
+	if (((fifo->head == fifo->num_entries-1) && fifo->tail == 0) ||
+	     (fifo->tail == fifo->head + 1)) {
+		return TRUE;
+	}
+	return FALSE;
+}
+
+bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
+{
+	if (fifo->head == -1)
+		return TRUE;
+	return FALSE;
+}
+
+int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
+{
+	if (!fifo || !data)
+		return -1;
+	spin_lock(&fifo->qlock);
+	if (sbi_fifo_is_full(fifo)) {
+		spin_unlock(&fifo->qlock);
+		return -1;
+	}
+	if (fifo->tail == -1)
+		fifo->tail = 0;
+	fifo->head = (fifo->head + 1) % fifo->num_entries;
+	memcpy(fifo->queue + fifo->head * fifo->entrysize, data,
+	       fifo->entrysize);
+	spin_unlock(&fifo->qlock);
+
+	return 0;
+}
+
+int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
+{
+	if (!fifo || !data)
+		return -1;
+
+	spin_lock(&fifo->qlock);
+	if (sbi_fifo_is_empty(fifo)) {
+		spin_unlock(&fifo->qlock);
+		return -1;
+	}
+	memcpy(data, fifo->queue + fifo->tail * fifo->entrysize,
+	       fifo->entrysize);
+
+	if (fifo->tail == fifo->head) {
+		fifo->tail = -1;
+		fifo->head = -1;
+	} else {
+		fifo->tail = (fifo->tail + 1) % fifo->num_entries;
+	}
+	spin_unlock(&fifo->qlock);
+
+	return 0;
+}