123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184 |
- /*
- * 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_error.h>
- #include <sbi/sbi_fifo.h>
- #include <plat/string.h>
- void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem, u16 entries,
- u16 entry_size)
- {
- fifo->queue = queue_mem;
- fifo->num_entries = entries;
- fifo->entry_size = entry_size;
- SPIN_LOCK_INIT(&fifo->qlock);
- fifo->avail = 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;
- }
- u16 sbi_fifo_avail(struct sbi_fifo *fifo)
- {
- u16 ret;
- if (!fifo)
- return 0;
- spin_lock(&fifo->qlock);
- ret = fifo->avail;
- spin_unlock(&fifo->qlock);
- return ret;
- }
- bool sbi_fifo_is_full(struct sbi_fifo *fifo)
- {
- 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)
- {
- bool ret;
- spin_lock(&fifo->qlock);
- ret = __sbi_fifo_is_empty(fifo);
- spin_unlock(&fifo->qlock);
- return ret;
- }
- /* Note: must be called with fifo->qlock held */
- static inline void __sbi_fifo_reset(struct sbi_fifo *fifo)
- {
- fifo->avail = 0;
- fifo->tail = 0;
- memset(fifo->queue, 0, fifo->num_entries * fifo->entry_size);
- }
- bool sbi_fifo_reset(struct sbi_fifo *fifo)
- {
- if (!fifo)
- return FALSE;
- spin_lock(&fifo->qlock);
- __sbi_fifo_reset(fifo);
- spin_unlock(&fifo->qlock);
- return TRUE;
- }
- /**
- * Provide a helper function to do inplace update to the fifo.
- * Note: The callback function is called with lock being held.
- *
- * **Do not** invoke any other fifo function from callback. Otherwise, it will
- * lead to deadlock.
- */
- int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
- int (*fptr)(void *in, void *data))
- {
- int i, index = 0;
- int ret = SBI_FIFO_UNCHANGED;
- void *entry;
- if (!fifo || !in)
- return ret;
- spin_lock(&fifo->qlock);
- if (__sbi_fifo_is_empty(fifo)) {
- spin_unlock(&fifo->qlock);
- return ret;
- }
- for (i = 0; i < fifo->avail; i++) {
- index = fifo->tail + i;
- if (index >= fifo->num_entries)
- index = index - fifo->num_entries;
- entry = (void *)fifo->queue + (u32)index * fifo->entry_size;
- ret = fptr(in, entry);
- if (ret == SBI_FIFO_SKIP || ret == SBI_FIFO_UPDATED) {
- break;
- } else if (ret == SBI_FIFO_RESET) {
- __sbi_fifo_reset(fifo);
- break;
- }
- }
- spin_unlock(&fifo->qlock);
- return ret;
- }
- int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
- {
- u32 head;
- if (!fifo || !data)
- return SBI_EINVAL;
- spin_lock(&fifo->qlock);
- if (__sbi_fifo_is_full(fifo)) {
- spin_unlock(&fifo->qlock);
- return SBI_ENOSPC;
- }
- head = (u32)fifo->tail + fifo->avail;
- if (head >= fifo->num_entries)
- head = head - fifo->num_entries;
- memcpy(fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
- fifo->avail++;
- spin_unlock(&fifo->qlock);
- return 0;
- }
- int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
- {
- if (!fifo || !data)
- return SBI_EINVAL;
- spin_lock(&fifo->qlock);
- if (__sbi_fifo_is_empty(fifo)) {
- spin_unlock(&fifo->qlock);
- return SBI_ENOENT;
- }
- memcpy(data, fifo->queue + (u32)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;
- }
|