4 files changed, 73 insertions, 43 deletions

diff --git a/include/sbi/sbi_error.h b/include/sbi/sbi_error.h
index 68076e7..300ee74 100644
--- a/include/sbi/sbi_error.h
+++ b/include/sbi/sbi_error.h
@@ -21,5 +21,6 @@
 #define SBI_ETIMEDOUT	-8
 #define SBI_EIO		-9
 #define SBI_EILL	-10
+#define SBI_ENOSPC	-11
 
 #endif
diff --git a/include/sbi/sbi_fifo.h b/include/sbi/sbi_fifo.h
index ba9769b..1a59518 100644
--- a/include/sbi/sbi_fifo.h
+++ b/include/sbi/sbi_fifo.h
@@ -15,18 +15,22 @@
 #include <sbi/sbi_types.h>
 
 struct sbi_fifo {
-	int head;
-	int tail;
-	spinlock_t qlock;
-	unsigned long entrysize;
-	unsigned long num_entries;
+	/* Static members of struct */
 	void *queue;
+	unsigned long entry_size;
+	unsigned long num_entries;
+	/* Dynamic members of struct protected by lock */
+	spinlock_t qlock;
+	unsigned long avail;
+	unsigned long head;
+	unsigned long tail;
 };
 
 int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data);
 int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data);
-void sbi_fifo_init(struct sbi_fifo *fifo, unsigned long entries,
-		   unsigned long entrysize);
+void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem,
+		   unsigned long entries, unsigned long entry_size);
 bool sbi_fifo_is_empty(struct sbi_fifo *fifo);
 bool sbi_fifo_is_full(struct sbi_fifo *fifo);
+
 #endif
diff --git a/lib/sbi_fifo.c b/lib/sbi_fifo.c
index 1b283e8..c0010e4 100644
--- a/lib/sbi_fifo.c
+++ b/lib/sbi_fifo.c
@@ -8,51 +8,75 @@
  *
  */
 #include <sbi/riscv_locks.h>
+#include <sbi/sbi_error.h>
 #include <sbi/sbi_fifo.h>
 #include <plat/string.h>
 
-void sbi_fifo_init(struct sbi_fifo *fifo, unsigned long entries,
-		   unsigned long size)
+void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem,
+		   unsigned long entries, unsigned long entry_size)
 {
-	fifo->head = -1;
-	fifo->tail = -1;
+	fifo->queue = queue_mem;
 	fifo->num_entries = entries;
-	fifo->entrysize = size;
+	fifo->entry_size = entry_size;
 	SPIN_LOCK_INIT(&fifo->qlock);
-	memset(fifo->queue, 0, entries * size);
+	fifo->avail = fifo->head = fifo->tail = 0;
+	memset(fifo->queue, 0, entries * entry_size);
+}
+
+/* Note: must be called with fifo->qlock held */
+static inline bool __sbi_fifo_is_full(struct sbi_fifo *fifo)
+{
+	return (fifo->avail == fifo->num_entries) ? TRUE : FALSE;
 }
 
 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 ret;
+
+	spin_lock(&fifo->qlock);
+	ret = __sbi_fifo_is_full(fifo);
+	spin_unlock(&fifo->qlock);
+
+	return ret;
+}
+
+/* Note: must be called with fifo->qlock held */
+static inline bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
+{
+	return (fifo->avail == 0) ? TRUE : FALSE;
 }
 
 bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
 {
-	if (fifo->head == -1)
-		return TRUE;
-	return FALSE;
+	bool ret;
+
+	spin_lock(&fifo->qlock);
+	ret = __sbi_fifo_is_empty(fifo);
+	spin_unlock(&fifo->qlock);
+
+	return ret;
 }
 
 int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
 {
 	if (!fifo || !data)
-		return -1;
+		return SBI_EINVAL;
 
 	spin_lock(&fifo->qlock);
-	if (sbi_fifo_is_full(fifo)) {
+
+	if (__sbi_fifo_is_full(fifo)) {
 		spin_unlock(&fifo->qlock);
-		return -1;
+		return SBI_ENOSPC;
 	}
-	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);
+
+	memcpy(fifo->queue + fifo->head * fifo->entry_size, data,
+	       fifo->entry_size);
+
+	fifo->avail++;
+	fifo->head++;
+	if (fifo->head >= fifo->num_entries)
+		fifo->head = 0;
+
 	spin_unlock(&fifo->qlock);
 
 	return 0;
@@ -61,22 +85,23 @@ int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
 int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
 {
 	if (!fifo || !data)
-		return -1;
+		return SBI_EINVAL;
 
 	spin_lock(&fifo->qlock);
-	if (sbi_fifo_is_empty(fifo)) {
+
+	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;
+		return SBI_ENOENT;
 	}
+
+	memcpy(data, fifo->queue + fifo->tail * fifo->entry_size,
+	       fifo->entry_size);
+
+	fifo->avail--;
+	fifo->tail++;
+	if (fifo->tail >= fifo->num_entries)
+		fifo->tail = 0;
+
 	spin_unlock(&fifo->qlock);
 
 	return 0;
diff --git a/lib/sbi_ipi.c b/lib/sbi_ipi.c
index 4253fb1..cc61dbd 100644
--- a/lib/sbi_ipi.c
+++ b/lib/sbi_ipi.c
@@ -184,8 +184,8 @@ int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot)
 	struct sbi_fifo *tlb_info_q = sbi_tlb_fifo_head_ptr(scratch);
 
 	sbi_ipi_data_ptr(scratch)->ipi_type = 0x00;
-	tlb_info_q->queue = sbi_tlb_fifo_mem_ptr(scratch);
-	sbi_fifo_init(tlb_info_q, SBI_TLB_FIFO_NUM_ENTRIES, SBI_TLB_INFO_SIZE);
+	sbi_fifo_init(tlb_info_q, sbi_tlb_fifo_mem_ptr(scratch),
+		      SBI_TLB_FIFO_NUM_ENTRIES, SBI_TLB_INFO_SIZE);
 
 	/* Enable software interrupts */
 	csr_set(CSR_MIE, MIP_MSIP);