Orefatoi / afLib_1_3

an old afLib which supports both SPI and UART

af_queue.cpp@1:112741fe45d1, 2018-04-23 (annotated)

Committer:
Rhyme
Date:
Mon Apr 23 06:15:05 2018 +0000
Revision:
1:112741fe45d1
Parent:
0:6f371c791202 
afLib1.3 first mbed version with working UART

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhyme 0:6f371c791202 1 /*
Rhyme 0:6f371c791202 2 * af_queue.c
Rhyme 0:6f371c791202 3 *
Rhyme 0:6f371c791202 4 * Created on: Apr 27, 2015
Rhyme 0:6f371c791202 5 * Author: chrisatkiban
Rhyme 0:6f371c791202 6 */
Rhyme 0:6f371c791202 7
Rhyme 0:6f371c791202 8 #include <stddef.h>
Rhyme 0:6f371c791202 9 #include <stdbool.h>
Rhyme 0:6f371c791202 10 #include "af_queue.h"
Rhyme 0:6f371c791202 11
Rhyme 0:6f371c791202 12 static Stream *_theLog = NULL;
Rhyme 0:6f371c791202 13 static uint8_t (*m_p_preemption_disable)(void);
Rhyme 0:6f371c791202 14 static void (*m_p_preemption_enable)(uint8_t is_nested);
Rhyme 0:6f371c791202 15
Rhyme 0:6f371c791202 16 static void __af_queue_put(queue_t *p_q, af_queue_elem_desc_t *p_desc)
Rhyme 0:6f371c791202 17 {
Rhyme 0:6f371c791202 18 if (p_q->p_head == NULL) {
Rhyme 0:6f371c791202 19 p_q->p_tail = p_q->p_head = p_desc;
Rhyme 0:6f371c791202 20 } else {
Rhyme 0:6f371c791202 21 p_q->p_tail->p_next_alloc = p_desc;
Rhyme 0:6f371c791202 22 p_q->p_tail = p_desc;
Rhyme 0:6f371c791202 23 }
Rhyme 0:6f371c791202 24 }
Rhyme 0:6f371c791202 25
Rhyme 0:6f371c791202 26 static void _af_queue_put(queue_t *p_q, af_queue_elem_desc_t *p_desc, bool interrupt_context)
Rhyme 0:6f371c791202 27 {
Rhyme 0:6f371c791202 28 if (!interrupt_context) {
Rhyme 0:6f371c791202 29 uint8_t is_nested;
Rhyme 0:6f371c791202 30
Rhyme 0:6f371c791202 31 is_nested = m_p_preemption_disable();
Rhyme 0:6f371c791202 32 {
Rhyme 0:6f371c791202 33 __af_queue_put( p_q, p_desc );
Rhyme 0:6f371c791202 34 }
Rhyme 0:6f371c791202 35 m_p_preemption_enable(is_nested);
Rhyme 0:6f371c791202 36 } else {
Rhyme 0:6f371c791202 37 __af_queue_put( p_q, p_desc );
Rhyme 0:6f371c791202 38 }
Rhyme 0:6f371c791202 39 }
Rhyme 0:6f371c791202 40
Rhyme 0:6f371c791202 41 static af_queue_elem_desc_t *__af_queue_elem_alloc(queue_t *p_q)
Rhyme 0:6f371c791202 42 {
Rhyme 0:6f371c791202 43 af_queue_elem_desc_t *p_desc = NULL;
Rhyme 0:6f371c791202 44
Rhyme 0:6f371c791202 45 if (p_q->p_free_head != NULL) {
Rhyme 0:6f371c791202 46 p_desc = p_q->p_free_head;
Rhyme 0:6f371c791202 47 p_q->p_free_head = p_desc->p_next_free;
Rhyme 0:6f371c791202 48 p_desc->p_next_alloc = NULL;
Rhyme 0:6f371c791202 49 }
Rhyme 0:6f371c791202 50
Rhyme 0:6f371c791202 51 return p_desc;
Rhyme 0:6f371c791202 52 }
Rhyme 0:6f371c791202 53
Rhyme 0:6f371c791202 54 static void *_af_queue_elem_alloc(queue_t *p_q, bool interrupt_context)
Rhyme 0:6f371c791202 55 {
Rhyme 0:6f371c791202 56 af_queue_elem_desc_t *p_desc;
Rhyme 0:6f371c791202 57
Rhyme 0:6f371c791202 58 if (!interrupt_context) {
Rhyme 0:6f371c791202 59 uint8_t is_nested;
Rhyme 0:6f371c791202 60
Rhyme 0:6f371c791202 61 is_nested = m_p_preemption_disable();
Rhyme 0:6f371c791202 62 {
Rhyme 0:6f371c791202 63 p_desc = __af_queue_elem_alloc(p_q);
Rhyme 0:6f371c791202 64 }
Rhyme 0:6f371c791202 65 m_p_preemption_enable(is_nested);
Rhyme 0:6f371c791202 66 } else {
Rhyme 0:6f371c791202 67 p_desc = __af_queue_elem_alloc(p_q);
Rhyme 0:6f371c791202 68 }
Rhyme 0:6f371c791202 69
Rhyme 0:6f371c791202 70 return p_desc ? p_desc->data : NULL;
Rhyme 0:6f371c791202 71 }
Rhyme 0:6f371c791202 72
Rhyme 0:6f371c791202 73 static af_queue_elem_desc_t *__af_queue_get(queue_t *p_q)
Rhyme 0:6f371c791202 74 {
Rhyme 0:6f371c791202 75 af_queue_elem_desc_t *p_desc = p_q->p_head;
Rhyme 0:6f371c791202 76
Rhyme 0:6f371c791202 77 if (p_desc != NULL) {
Rhyme 0:6f371c791202 78 p_q->p_head = p_desc->p_next_alloc;
Rhyme 0:6f371c791202 79 p_desc->p_next_alloc = NULL;
Rhyme 0:6f371c791202 80 }
Rhyme 0:6f371c791202 81
Rhyme 0:6f371c791202 82 if (p_q->p_head == NULL) {
Rhyme 0:6f371c791202 83 p_q->p_tail = NULL;
Rhyme 0:6f371c791202 84 }
Rhyme 0:6f371c791202 85
Rhyme 0:6f371c791202 86 return p_desc;
Rhyme 0:6f371c791202 87 }
Rhyme 0:6f371c791202 88
Rhyme 0:6f371c791202 89 static void *_af_queue_get(queue_t *p_q, bool interrupt_context)
Rhyme 0:6f371c791202 90 {
Rhyme 0:6f371c791202 91 af_queue_elem_desc_t *p_desc;
Rhyme 0:6f371c791202 92
Rhyme 0:6f371c791202 93 if (!interrupt_context) {
Rhyme 0:6f371c791202 94 uint8_t is_nested;
Rhyme 0:6f371c791202 95
Rhyme 0:6f371c791202 96 is_nested = m_p_preemption_disable();
Rhyme 0:6f371c791202 97 {
Rhyme 0:6f371c791202 98 p_desc = __af_queue_get(p_q);
Rhyme 0:6f371c791202 99 }
Rhyme 0:6f371c791202 100 m_p_preemption_enable(is_nested);
Rhyme 0:6f371c791202 101 } else {
Rhyme 0:6f371c791202 102 p_desc = __af_queue_get(p_q);
Rhyme 0:6f371c791202 103 }
Rhyme 0:6f371c791202 104
Rhyme 0:6f371c791202 105 return p_desc ? p_desc->data : NULL;
Rhyme 0:6f371c791202 106 }
Rhyme 0:6f371c791202 107
Rhyme 0:6f371c791202 108 static void __af_queue_elem_free(queue_t *p_q, void *p_data)
Rhyme 0:6f371c791202 109 {
Rhyme 0:6f371c791202 110 #pragma GCC diagnostic push
Rhyme 0:6f371c791202 111 #pragma GCC diagnostic ignored "-Wcast-align"
Rhyme 0:6f371c791202 112 af_queue_elem_desc_t *p_desc = (af_queue_elem_desc_t *)((uint8_t *)p_data - __builtin_offsetof(struct af_queue_elem_desc_s, data));
Rhyme 0:6f371c791202 113 #pragma GCC diagnostic push
Rhyme 0:6f371c791202 114 af_queue_elem_desc_t *p_tmp_desc;
Rhyme 0:6f371c791202 115
Rhyme 0:6f371c791202 116 p_tmp_desc = p_q->p_free_head;
Rhyme 0:6f371c791202 117 p_q->p_free_head = p_desc;
Rhyme 0:6f371c791202 118 p_desc->p_next_free = p_tmp_desc;
Rhyme 0:6f371c791202 119 }
Rhyme 0:6f371c791202 120
Rhyme 0:6f371c791202 121 static void _af_queue_elem_free(queue_t *p_q, void *p_data, bool interrupt_context)
Rhyme 0:6f371c791202 122 {
Rhyme 0:6f371c791202 123 if (!interrupt_context) {
Rhyme 0:6f371c791202 124 uint8_t is_nested;
Rhyme 0:6f371c791202 125
Rhyme 0:6f371c791202 126 is_nested = m_p_preemption_disable();
Rhyme 0:6f371c791202 127 {
Rhyme 0:6f371c791202 128 __af_queue_elem_free(p_q, p_data);
Rhyme 0:6f371c791202 129 }
Rhyme 0:6f371c791202 130 m_p_preemption_enable(is_nested);
Rhyme 0:6f371c791202 131 } else {
Rhyme 0:6f371c791202 132 __af_queue_elem_free(p_q, p_data);
Rhyme 0:6f371c791202 133 }
Rhyme 0:6f371c791202 134 }
Rhyme 0:6f371c791202 135
Rhyme 0:6f371c791202 136 static void *_af_queue_peek(queue_t *p_q, bool interrupt_context)
Rhyme 0:6f371c791202 137 {
Rhyme 0:6f371c791202 138 return p_q->p_head ? p_q->p_head->data : NULL;
Rhyme 0:6f371c791202 139 }
Rhyme 0:6f371c791202 140
Rhyme 0:6f371c791202 141 static void *_af_queue_peek_tail(queue_t *p_q, bool interrupt_context)
Rhyme 0:6f371c791202 142 {
Rhyme 0:6f371c791202 143 return p_q->p_tail ? p_q->p_tail->data : NULL;
Rhyme 0:6f371c791202 144 }
Rhyme 0:6f371c791202 145
Rhyme 0:6f371c791202 146 void af_queue_put(queue_t *p_q, void *p_data)
Rhyme 0:6f371c791202 147 {
Rhyme 0:6f371c791202 148 af_queue_elem_desc_t *p_desc = (af_queue_elem_desc_t *)((uint8_t *)p_data - __builtin_offsetof(struct af_queue_elem_desc_s, data));
Rhyme 0:6f371c791202 149 _af_queue_put(p_q, p_desc, false);
Rhyme 0:6f371c791202 150 }
Rhyme 0:6f371c791202 151
Rhyme 0:6f371c791202 152 void af_queue_put_from_interrupt(queue_t *p_q, void *p_data)
Rhyme 0:6f371c791202 153 {
Rhyme 0:6f371c791202 154 af_queue_elem_desc_t *p_desc = (af_queue_elem_desc_t *)((uint8_t *)p_data - __builtin_offsetof(struct af_queue_elem_desc_s, data));
Rhyme 0:6f371c791202 155 _af_queue_put(p_q, p_desc, true);
Rhyme 0:6f371c791202 156 }
Rhyme 0:6f371c791202 157
Rhyme 0:6f371c791202 158 void *af_queue_elem_alloc(queue_t *p_q)
Rhyme 0:6f371c791202 159 {
Rhyme 0:6f371c791202 160 return _af_queue_elem_alloc(p_q, false);
Rhyme 0:6f371c791202 161 }
Rhyme 0:6f371c791202 162
Rhyme 0:6f371c791202 163 void *af_queue_elem_alloc_from_interrupt(queue_t *p_q)
Rhyme 0:6f371c791202 164 {
Rhyme 0:6f371c791202 165 return _af_queue_elem_alloc(p_q, true);
Rhyme 0:6f371c791202 166 }
Rhyme 0:6f371c791202 167
Rhyme 0:6f371c791202 168 void *af_queue_get(queue_t *p_q)
Rhyme 0:6f371c791202 169 {
Rhyme 0:6f371c791202 170 return _af_queue_get(p_q, false);
Rhyme 0:6f371c791202 171 }
Rhyme 0:6f371c791202 172
Rhyme 0:6f371c791202 173 void *af_queue_get_from_interrupt(queue_t *p_q)
Rhyme 0:6f371c791202 174 {
Rhyme 0:6f371c791202 175 return _af_queue_get(p_q, true);
Rhyme 0:6f371c791202 176 }
Rhyme 0:6f371c791202 177
Rhyme 0:6f371c791202 178 void *af_queue_peek(queue_t *p_q)
Rhyme 0:6f371c791202 179 {
Rhyme 0:6f371c791202 180 return _af_queue_peek(p_q, false);
Rhyme 0:6f371c791202 181 }
Rhyme 0:6f371c791202 182
Rhyme 0:6f371c791202 183 void *af_queue_peek_from_interrupt(queue_t *p_q)
Rhyme 0:6f371c791202 184 {
Rhyme 0:6f371c791202 185 return _af_queue_peek(p_q, true);
Rhyme 0:6f371c791202 186 }
Rhyme 0:6f371c791202 187
Rhyme 0:6f371c791202 188 void *af_queue_peek_tail(queue_t *p_q)
Rhyme 0:6f371c791202 189 {
Rhyme 0:6f371c791202 190 return _af_queue_peek_tail(p_q, false);
Rhyme 0:6f371c791202 191 }
Rhyme 0:6f371c791202 192
Rhyme 0:6f371c791202 193 void *af_queue_peek_tail_from_interrupt(queue_t *p_q)
Rhyme 0:6f371c791202 194 {
Rhyme 0:6f371c791202 195 return _af_queue_peek_tail(p_q, true);
Rhyme 0:6f371c791202 196 }
Rhyme 0:6f371c791202 197
Rhyme 0:6f371c791202 198 void af_queue_elem_free(queue_t *p_q, void *p_data)
Rhyme 0:6f371c791202 199 {
Rhyme 0:6f371c791202 200 _af_queue_elem_free(p_q, p_data, false);
Rhyme 0:6f371c791202 201 }
Rhyme 0:6f371c791202 202
Rhyme 0:6f371c791202 203 void af_queue_elem_free_from_interrupt(queue_t *p_q, void *p_data)
Rhyme 0:6f371c791202 204 {
Rhyme 0:6f371c791202 205 _af_queue_elem_free(p_q, p_data, true);
Rhyme 0:6f371c791202 206 }
Rhyme 0:6f371c791202 207
Rhyme 0:6f371c791202 208 void af_queue_init(queue_t *p_q, int elem_size, int max_elem, uint8_t *p_mem)
Rhyme 0:6f371c791202 209 {
Rhyme 0:6f371c791202 210 af_queue_elem_desc_t *p_desc;
Rhyme 0:6f371c791202 211 af_queue_elem_desc_t *p_desc_next;
Rhyme 0:6f371c791202 212 int offset;
Rhyme 0:6f371c791202 213 int i = 0;
Rhyme 0:6f371c791202 214
Rhyme 0:6f371c791202 215 p_q->p_head = NULL;
Rhyme 0:6f371c791202 216 p_q->p_tail = NULL;
Rhyme 0:6f371c791202 217
Rhyme 0:6f371c791202 218 // string all elements together and onto the null-terminated free list to start
Rhyme 0:6f371c791202 219 p_q->p_free_head = (af_queue_elem_desc_t *)p_mem;
Rhyme 0:6f371c791202 220
Rhyme 0:6f371c791202 221 for (i = 0; i < max_elem - 1; ++i) {
Rhyme 0:6f371c791202 222 offset = i * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
Rhyme 0:6f371c791202 223 p_desc = (af_queue_elem_desc_t *)(p_mem + offset);
Rhyme 0:6f371c791202 224
Rhyme 0:6f371c791202 225 offset = (i + 1) * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
Rhyme 0:6f371c791202 226 p_desc_next = (af_queue_elem_desc_t *)(p_mem + offset);
Rhyme 0:6f371c791202 227 p_desc->p_next_free = p_desc_next;
Rhyme 0:6f371c791202 228 }
Rhyme 0:6f371c791202 229
Rhyme 0:6f371c791202 230 offset = (max_elem - 1) * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
Rhyme 0:6f371c791202 231 p_desc = (af_queue_elem_desc_t *)(p_mem + offset);
Rhyme 0:6f371c791202 232 p_desc->p_next_free = NULL;
Rhyme 0:6f371c791202 233 }
Rhyme 0:6f371c791202 234
Rhyme 0:6f371c791202 235 void af_queue_init_system(uint8_t (*p_preemption_disable)(void), void (*p_preemption_enable)(uint8_t is_nested), Stream *theLog)
Rhyme 0:6f371c791202 236 {
Rhyme 0:6f371c791202 237 m_p_preemption_disable = p_preemption_disable;
Rhyme 0:6f371c791202 238 m_p_preemption_enable = p_preemption_enable;
Rhyme 0:6f371c791202 239 _theLog = theLog;
Rhyme 0:6f371c791202 240 }
Rhyme 0:6f371c791202 241
Rhyme 0:6f371c791202 242 void af_queue_dump(queue_t *p_q)
Rhyme 0:6f371c791202 243 {
Rhyme 0:6f371c791202 244 af_queue_elem_desc_t *p_elem;
Rhyme 0:6f371c791202 245
Rhyme 0:6f371c791202 246 if (_theLog != NULL) {
Rhyme 0:6f371c791202 247 _theLog->printf("Q %X", (int)p_q);
Rhyme 0:6f371c791202 248 _theLog->printf(" free_head %X", (int)p_q->p_free_head);
Rhyme 0:6f371c791202 249 _theLog->printf(" head %X", (int)p_q->p_head);
Rhyme 0:6f371c791202 250 _theLog->printf(" tail %X", (int)p_q->p_tail);
Rhyme 0:6f371c791202 251
Rhyme 0:6f371c791202 252 _theLog->printf("In Queue: \n"); // Not all allocated are in queue (until af_queue_put)
Rhyme 0:6f371c791202 253 p_elem = p_q->p_head;
Rhyme 0:6f371c791202 254 while (p_elem) {
Rhyme 0:6f371c791202 255 _theLog->printf("%X\n", (int) p_elem);
Rhyme 0:6f371c791202 256 p_elem = p_elem->p_next_alloc;
Rhyme 0:6f371c791202 257 }
Rhyme 0:6f371c791202 258
Rhyme 0:6f371c791202 259 _theLog->printf("Free:\n");
Rhyme 0:6f371c791202 260 p_elem = p_q->p_free_head;
Rhyme 0:6f371c791202 261 while (p_elem) {
Rhyme 0:6f371c791202 262 _theLog->printf("%X\n", (int) p_elem);
Rhyme 0:6f371c791202 263 p_elem = p_elem->p_next_free;
Rhyme 0:6f371c791202 264 }
Rhyme 0:6f371c791202 265 }
Rhyme 0:6f371c791202 266 }