1 /*
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2019 Western Digital Corporation or its affiliates.
5 *
6 * Authors:
7 * Atish Patra<atish.patra@wdc.com>
8 *
9 */
10 #include <sbi/riscv_locks.h>
11 #include <sbi/sbi_error.h>
12 #include <sbi/sbi_fifo.h>
13 #include <sbi/sbi_string.h>
14
sbi_fifo_init(struct sbi_fifo * fifo,void * queue_mem,u16 entries,u16 entry_size)15 void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem, u16 entries,
16 u16 entry_size)
17 {
18 fifo->queue = queue_mem;
19 fifo->num_entries = entries;
20 fifo->entry_size = entry_size;
21 SPIN_LOCK_INIT(&fifo->qlock);
22 fifo->avail = fifo->tail = 0;
23 sbi_memset(fifo->queue, 0, (size_t)entries * entry_size);
24 }
25
26 /* Note: must be called with fifo->qlock held */
__sbi_fifo_is_full(struct sbi_fifo * fifo)27 static inline bool __sbi_fifo_is_full(struct sbi_fifo *fifo)
28 {
29 return (fifo->avail == fifo->num_entries) ? TRUE : FALSE;
30 }
31
sbi_fifo_avail(struct sbi_fifo * fifo)32 u16 sbi_fifo_avail(struct sbi_fifo *fifo)
33 {
34 u16 ret;
35
36 if (!fifo)
37 return 0;
38
39 spin_lock(&fifo->qlock);
40 ret = fifo->avail;
41 spin_unlock(&fifo->qlock);
42
43 return ret;
44 }
45
sbi_fifo_is_full(struct sbi_fifo * fifo)46 bool sbi_fifo_is_full(struct sbi_fifo *fifo)
47 {
48 bool ret;
49
50 spin_lock(&fifo->qlock);
51 ret = __sbi_fifo_is_full(fifo);
52 spin_unlock(&fifo->qlock);
53
54 return ret;
55 }
56
57 /* Note: must be called with fifo->qlock held */
__sbi_fifo_enqueue(struct sbi_fifo * fifo,void * data)58 static inline void __sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
59 {
60 u32 head;
61
62 head = (u32)fifo->tail + fifo->avail;
63 if (head >= fifo->num_entries)
64 head = head - fifo->num_entries;
65
66 sbi_memcpy(fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
67
68 fifo->avail++;
69 }
70
71
72 /* Note: must be called with fifo->qlock held */
__sbi_fifo_is_empty(struct sbi_fifo * fifo)73 static inline bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
74 {
75 return (fifo->avail == 0) ? TRUE : FALSE;
76 }
77
sbi_fifo_is_empty(struct sbi_fifo * fifo)78 bool sbi_fifo_is_empty(struct sbi_fifo *fifo)
79 {
80 bool ret;
81
82 spin_lock(&fifo->qlock);
83 ret = __sbi_fifo_is_empty(fifo);
84 spin_unlock(&fifo->qlock);
85
86 return ret;
87 }
88
89 /* Note: must be called with fifo->qlock held */
__sbi_fifo_reset(struct sbi_fifo * fifo)90 static inline void __sbi_fifo_reset(struct sbi_fifo *fifo)
91 {
92 size_t size = (size_t)fifo->num_entries * fifo->entry_size;
93
94 fifo->avail = 0;
95 fifo->tail = 0;
96 sbi_memset(fifo->queue, 0, size);
97 }
98
sbi_fifo_reset(struct sbi_fifo * fifo)99 bool sbi_fifo_reset(struct sbi_fifo *fifo)
100 {
101 if (!fifo)
102 return FALSE;
103
104 spin_lock(&fifo->qlock);
105 __sbi_fifo_reset(fifo);
106 spin_unlock(&fifo->qlock);
107
108 return TRUE;
109 }
110
111 /**
112 * Provide a helper function to do inplace update to the fifo.
113 * Note: The callback function is called with lock being held.
114 *
115 * **Do not** invoke any other fifo function from callback. Otherwise, it will
116 * lead to deadlock.
117 */
sbi_fifo_inplace_update(struct sbi_fifo * fifo,void * in,int (* fptr)(void * in,void * data))118 int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
119 int (*fptr)(void *in, void *data))
120 {
121 int i, index = 0;
122 int ret = SBI_FIFO_UNCHANGED;
123 void *entry;
124
125 if (!fifo || !in)
126 return ret;
127
128 spin_lock(&fifo->qlock);
129
130 if (__sbi_fifo_is_empty(fifo)) {
131 spin_unlock(&fifo->qlock);
132 return ret;
133 }
134
135 for (i = 0; i < fifo->avail; i++) {
136 index = fifo->tail + i;
137 if (index >= fifo->num_entries)
138 index = index - fifo->num_entries;
139 entry = (void *)fifo->queue + (u32)index * fifo->entry_size;
140 ret = fptr(in, entry);
141
142 if (ret == SBI_FIFO_SKIP || ret == SBI_FIFO_UPDATED) {
143 break;
144 }
145 }
146 spin_unlock(&fifo->qlock);
147
148 return ret;
149 }
150
sbi_fifo_enqueue(struct sbi_fifo * fifo,void * data)151 int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
152 {
153 if (!fifo || !data)
154 return SBI_EINVAL;
155
156 spin_lock(&fifo->qlock);
157
158 if (__sbi_fifo_is_full(fifo)) {
159 spin_unlock(&fifo->qlock);
160 return SBI_ENOSPC;
161 }
162 __sbi_fifo_enqueue(fifo, data);
163
164 spin_unlock(&fifo->qlock);
165
166 return 0;
167 }
168
sbi_fifo_dequeue(struct sbi_fifo * fifo,void * data)169 int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
170 {
171 if (!fifo || !data)
172 return SBI_EINVAL;
173
174 spin_lock(&fifo->qlock);
175
176 if (__sbi_fifo_is_empty(fifo)) {
177 spin_unlock(&fifo->qlock);
178 return SBI_ENOENT;
179 }
180
181 sbi_memcpy(data, fifo->queue + (u32)fifo->tail * fifo->entry_size,
182 fifo->entry_size);
183
184 fifo->avail--;
185 fifo->tail++;
186 if (fifo->tail >= fifo->num_entries)
187 fifo->tail = 0;
188
189 spin_unlock(&fifo->qlock);
190
191 return 0;
192 }
193