af_queue.cpp

00001 /*
00002  * af_queue.c
00003  *
00004  *  Created on: Apr 27, 2015
00005  *      Author: chrisatkiban
00006  */
00007 
00008 #include <stddef.h>
00009 #include <stdbool.h>
00010 #include "af_queue.h"
00011 
00012 static Stream *_theLog = NULL;
00013 static uint8_t (*m_p_preemption_disable)(void);
00014 static void (*m_p_preemption_enable)(uint8_t is_nested);
00015 
00016 static void __af_queue_put(queue_t *p_q, af_queue_elem_desc_t *p_desc)
00017 {
00018     if (p_q->p_head == NULL) {
00019         p_q->p_tail = p_q->p_head = p_desc;
00020     } else {
00021         p_q->p_tail->p_next_alloc = p_desc;
00022         p_q->p_tail = p_desc;
00023     }
00024 }
00025 
00026 static void _af_queue_put(queue_t *p_q, af_queue_elem_desc_t *p_desc, bool interrupt_context)
00027 {
00028     if (!interrupt_context) {
00029         uint8_t is_nested;
00030 
00031         is_nested = m_p_preemption_disable();
00032         {
00033             __af_queue_put( p_q, p_desc );
00034         }
00035         m_p_preemption_enable(is_nested);
00036     } else {
00037         __af_queue_put( p_q, p_desc );
00038     }
00039 }
00040 
00041 static af_queue_elem_desc_t *__af_queue_elem_alloc(queue_t *p_q)
00042 {
00043     af_queue_elem_desc_t *p_desc = NULL;
00044 
00045     if (p_q->p_free_head != NULL) {
00046         p_desc = p_q->p_free_head;
00047         p_q->p_free_head = p_desc->p_next_free;
00048         p_desc->p_next_alloc = NULL;
00049     }
00050 
00051     return p_desc;
00052 }
00053 
00054 static void *_af_queue_elem_alloc(queue_t *p_q, bool interrupt_context)
00055 {
00056     af_queue_elem_desc_t *p_desc;
00057 
00058     if (!interrupt_context) {
00059         uint8_t is_nested;
00060 
00061         is_nested = m_p_preemption_disable();
00062         {
00063             p_desc = __af_queue_elem_alloc(p_q);
00064         }
00065         m_p_preemption_enable(is_nested);
00066     } else {
00067         p_desc = __af_queue_elem_alloc(p_q);
00068     }
00069 
00070     return p_desc ? p_desc->data : NULL;
00071 }
00072 
00073 static af_queue_elem_desc_t *__af_queue_get(queue_t *p_q)
00074 {
00075     af_queue_elem_desc_t *p_desc = p_q->p_head;
00076 
00077     if (p_desc != NULL) {
00078         p_q->p_head = p_desc->p_next_alloc;
00079         p_desc->p_next_alloc = NULL;
00080     }
00081 
00082     if (p_q->p_head == NULL) {
00083         p_q->p_tail = NULL;
00084     }
00085 
00086     return p_desc;
00087 }
00088 
00089 static void *_af_queue_get(queue_t *p_q, bool interrupt_context)
00090 {
00091     af_queue_elem_desc_t *p_desc;
00092 
00093     if (!interrupt_context) {
00094         uint8_t is_nested;
00095 
00096         is_nested = m_p_preemption_disable();
00097         {
00098             p_desc = __af_queue_get(p_q);
00099         }
00100         m_p_preemption_enable(is_nested);
00101     } else {
00102         p_desc = __af_queue_get(p_q);
00103     }
00104 
00105     return p_desc ? p_desc->data : NULL;
00106 }
00107 
00108 static void __af_queue_elem_free(queue_t *p_q, void *p_data)
00109 {
00110 #pragma GCC diagnostic push
00111 #pragma GCC diagnostic ignored "-Wcast-align"
00112     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));
00113 #pragma GCC diagnostic push
00114     af_queue_elem_desc_t *p_tmp_desc;
00115 
00116     p_tmp_desc = p_q->p_free_head;
00117     p_q->p_free_head = p_desc;
00118     p_desc->p_next_free = p_tmp_desc;
00119 }
00120 
00121 static void _af_queue_elem_free(queue_t *p_q, void *p_data, bool interrupt_context)
00122 {
00123     if (!interrupt_context) {
00124         uint8_t is_nested;
00125 
00126         is_nested = m_p_preemption_disable();
00127         {
00128             __af_queue_elem_free(p_q, p_data);
00129         }
00130         m_p_preemption_enable(is_nested);
00131     } else {
00132         __af_queue_elem_free(p_q, p_data);
00133     }
00134 }
00135 
00136 static void *_af_queue_peek(queue_t *p_q, bool interrupt_context)
00137 {
00138     return p_q->p_head ? p_q->p_head->data : NULL;
00139 }
00140 
00141 static void *_af_queue_peek_tail(queue_t *p_q, bool interrupt_context)
00142 {
00143     return p_q->p_tail ? p_q->p_tail->data : NULL;
00144 }
00145 
00146 void af_queue_put(queue_t *p_q, void *p_data)
00147 {
00148     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));
00149     _af_queue_put(p_q, p_desc, false);
00150 }
00151 
00152 void af_queue_put_from_interrupt(queue_t *p_q, void *p_data)
00153 {
00154     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));
00155     _af_queue_put(p_q, p_desc, true);
00156 }
00157 
00158 void *af_queue_elem_alloc(queue_t *p_q)
00159 {
00160     return _af_queue_elem_alloc(p_q, false);
00161 }
00162 
00163 void *af_queue_elem_alloc_from_interrupt(queue_t *p_q)
00164 {
00165     return _af_queue_elem_alloc(p_q, true);
00166 }
00167 
00168 void *af_queue_get(queue_t *p_q)
00169 {
00170     return _af_queue_get(p_q, false);
00171 }
00172 
00173 void *af_queue_get_from_interrupt(queue_t *p_q)
00174 {
00175     return _af_queue_get(p_q, true);
00176 }
00177 
00178 void *af_queue_peek(queue_t *p_q)
00179 {
00180     return _af_queue_peek(p_q, false);
00181 }
00182 
00183 void *af_queue_peek_from_interrupt(queue_t *p_q)
00184 {
00185     return _af_queue_peek(p_q, true);
00186 }
00187 
00188 void *af_queue_peek_tail(queue_t *p_q)
00189 {
00190     return _af_queue_peek_tail(p_q, false);
00191 }
00192 
00193 void *af_queue_peek_tail_from_interrupt(queue_t *p_q)
00194 {
00195     return _af_queue_peek_tail(p_q, true);
00196 }
00197 
00198 void af_queue_elem_free(queue_t *p_q, void *p_data)
00199 {
00200     _af_queue_elem_free(p_q, p_data, false);
00201 }
00202 
00203 void af_queue_elem_free_from_interrupt(queue_t *p_q, void *p_data)
00204 {
00205     _af_queue_elem_free(p_q, p_data, true);
00206 }
00207 
00208 void af_queue_init(queue_t *p_q, int elem_size, int max_elem, uint8_t *p_mem)
00209 {
00210     af_queue_elem_desc_t *p_desc;
00211     af_queue_elem_desc_t *p_desc_next;
00212     int offset;
00213     int i = 0;
00214 
00215     p_q->p_head = NULL;
00216     p_q->p_tail = NULL;
00217 
00218     // string all elements together and onto the null-terminated free list to start
00219     p_q->p_free_head = (af_queue_elem_desc_t *)p_mem;
00220 
00221     for (i = 0; i < max_elem - 1; ++i) {
00222         offset = i * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
00223         p_desc = (af_queue_elem_desc_t *)(p_mem + offset);
00224 
00225         offset = (i + 1) * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
00226         p_desc_next = (af_queue_elem_desc_t *)(p_mem + offset);
00227         p_desc->p_next_free = p_desc_next;
00228     }
00229 
00230     offset = (max_elem - 1) * (ALIGN_SIZE(sizeof(af_queue_elem_desc_t), 4) + ALIGN_SIZE(elem_size, 4));
00231     p_desc = (af_queue_elem_desc_t *)(p_mem + offset);
00232     p_desc->p_next_free = NULL;
00233 }
00234 
00235 void af_queue_init_system(uint8_t (*p_preemption_disable)(void), void (*p_preemption_enable)(uint8_t is_nested), Stream *theLog)
00236 {
00237     m_p_preemption_disable = p_preemption_disable;
00238     m_p_preemption_enable = p_preemption_enable;
00239     _theLog = theLog;
00240 }
00241 
00242 void af_queue_dump(queue_t *p_q)
00243 {
00244     af_queue_elem_desc_t *p_elem;
00245 
00246     if (_theLog != NULL) {
00247         _theLog->printf("Q %X", (int)p_q);
00248         _theLog->printf(" free_head %X", (int)p_q->p_free_head);
00249         _theLog->printf(" head %X", (int)p_q->p_head);
00250         _theLog->printf(" tail %X", (int)p_q->p_tail);
00251 
00252         _theLog->printf("In Queue: \n"); // Not all allocated are in queue (until af_queue_put)
00253         p_elem = p_q->p_head;
00254         while (p_elem) {
00255             _theLog->printf("%X\n", (int) p_elem);
00256             p_elem = p_elem->p_next_alloc;
00257         }
00258 
00259         _theLog->printf("Free:\n");
00260         p_elem = p_q->p_free_head;
00261         while (p_elem) {
00262             _theLog->printf("%X\n", (int) p_elem);
00263             p_elem = p_elem->p_next_free;
00264         }
00265     }
00266 }