1 /*
2 * Copyright (c) 2016-2017 Nordic Semiconductor ASA
3 * Copyright (c) 2016 Vinayak Kariappa Chettimada
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 /**
9 * FIFO-style "memory queue" permitting enqueue at tail and dequeue from head.
10 * Element's payload is a pointer to arbitrary memory.
11 *
12 * Implemented as a singly-linked list, with always one-more element.
13 * The linked list must always contain at least one link-element, as emptiness
14 * is given by head == tail.
15 * For a queue to be valid, it must be initialized with an initial link-element.
16 *
17 * Invariant: The tail element's mem pointer is DontCare.
18 *
19 * Note that at enqueue, memory is not coupled with its accompanying
20 * link-element, but the link-element before it!
21 *
22 * Call | State after call
23 * ------------------------+-------------------------------
24 * memq_init(I,H,T) | H -> I[] <- T
25 * memq_enqueue(A,a,T); | H -> I[a] -> A[] <- T
26 * memq_enqueue(B,b,T); | H -> I[a] -> A[b] -> B[] <- T
27 * memq_dequeue(T,H,dest); | H -> A[b] -> B[] <- T # I and a as return and dest
28 *
29 * where H is the pointer to Head link-element (oldest element).
30 * where T is the pointer to Tail link-element (newest element).
31 * where I[] means the initial link-element, whose mem pointer is DontCare.
32 * where A[b] means the A'th link-element, whose mem pointer is b.
33 */
34
35 #include <stddef.h>
36
37 #include <soc.h>
38
39 #include "hal/cpu.h"
40
41 #include "memq.h"
42
43 /**
44 * @brief Initialize a memory queue to be empty and valid.
45 *
46 * @param link[in] Initial link-element. Not associated with any mem
47 * @param head[out] Head of queue. Will be updated
48 * @param tail[out] Tail of queue. Will be updated
49 * @return Initial link-element
50 */
memq_init(memq_link_t * link,memq_link_t ** head,memq_link_t ** tail)51 memq_link_t *memq_init(memq_link_t *link, memq_link_t **head, memq_link_t **tail)
52 {
53 /* Head and tail pointer to the initial link - forms an empty queue */
54 *head = *tail = link;
55
56 return link;
57 }
58
59 /**
60 * @brief De-initialize a memory queue to be empty and invalid.
61 *
62 * @param head[in,out] Head of queue. Will be updated
63 * @param tail[in,out] Tail of queue. Will be updated
64 * @return Head of queue before invalidation; NULL if queue was empty
65 */
memq_deinit(memq_link_t ** head,memq_link_t ** tail)66 memq_link_t *memq_deinit(memq_link_t **head, memq_link_t **tail)
67 {
68 memq_link_t *old_head;
69
70 /* If head and tail are not equal, then queue is not empty */
71 if (*head != *tail) {
72 return NULL;
73 }
74
75 old_head = *head;
76 *head = *tail = NULL;
77
78 return old_head;
79 }
80
81 /**
82 * @brief Enqueue at the tail of the queue
83 * @details Enqueue is destructive so tail will change to new tail
84 * NOTE: The memory will not be associated with the link-element, but
85 * rather the second-to-last link-element.
86 *
87 * @param link[in] Element to be appended. Becomes new tail
88 * @param mem[in] The memory payload to be enqueued. Pointed to by old tail
89 * @param tail[in,out] Tail of queue. Will be updated to point to link
90 * @return New tail. Note: Does not point to the new mem
91 */
memq_enqueue(memq_link_t * link,void * mem,memq_link_t ** tail)92 memq_link_t *memq_enqueue(memq_link_t *link, void *mem, memq_link_t **tail)
93 {
94 /* Let the old tail element point to the new tail element */
95 (*tail)->next = link;
96
97 /* Let the old tail element point the the new memory */
98 (*tail)->mem = mem;
99
100 /* Update the tail-pointer to point to the new tail element.
101 * The new tail-element is not expected to point to anything sensible
102 */
103 cpu_dmb(); /* Ensure data accesses are synchronized */
104 *tail = link; /* Commit: enqueue of memq node */
105
106 return link;
107 }
108
109 /**
110 * @brief Non-destructive peek of head of queue.
111 *
112 * @param head[in] Pointer to head link-element of queue
113 * @param tail[in] Pointer to tail link-element of queue
114 * @param mem[out] The memory pointed to by head-element
115 * @return head or NULL if queue is empty
116 */
memq_peek(memq_link_t * head,memq_link_t * tail,void ** mem)117 memq_link_t *memq_peek(memq_link_t *head, memq_link_t *tail, void **mem)
118 {
119 /* If head and tail are equal, then queue empty */
120 if (head == tail) {
121 return NULL;
122 }
123
124 /* Extract the head link-element's memory */
125 if (mem) {
126 *mem = head->mem;
127 }
128
129 return head; /* queue was not empty */
130 }
131
132 /**
133 * @brief Non-destructive peek of nth (zero indexed) element of queue.
134 *
135 * @param head[in] Pointer to head link-element of queue
136 * @param tail[in] Pointer to tail link-element of queue
137 * @param n[in] Nth element of queue to peek into
138 * @param mem[out] The memory pointed to by head-element
139 * @return head or NULL if queue is empty
140 */
memq_peek_n(memq_link_t * head,memq_link_t * tail,uint8_t n,void ** mem)141 memq_link_t *memq_peek_n(memq_link_t *head, memq_link_t *tail, uint8_t n,
142 void **mem)
143 {
144 /* Traverse to Nth element, zero indexed */
145 do {
146 /* Use memq peek to get the current head and its mem */
147 head = memq_peek(head, tail, mem);
148 if (head == NULL) {
149 return NULL; /* Nth element is empty */
150 }
151
152 /* Progress to next element */
153 head = head->next;
154 } while (n--);
155
156 return head; /* queue was not empty */
157 }
158
159 /**
160 * @brief Remove and returns the head of queue.
161 * @details Dequeue is destructive so head will change to new head
162 *
163 * @param tail[in] Pointer to tail link-element of queue
164 * @param head[in,out] Pointer to head link-element of queue. Will be updated
165 * @param mem[out] The memory pointed to by head-element
166 * @return head or NULL if queue is empty
167 */
memq_dequeue(memq_link_t * tail,memq_link_t ** head,void ** mem)168 memq_link_t *memq_dequeue(memq_link_t *tail, memq_link_t **head, void **mem)
169 {
170 memq_link_t *old_head;
171
172 /* Use memq peek to get the old head and its mem */
173 old_head = memq_peek(*head, tail, mem);
174 if (old_head == NULL) {
175 return NULL; /* queue is empty */
176 }
177
178 /* Update the head-pointer to point to the new head element */
179 *head = old_head->next;
180
181 return old_head;
182 }
183
