Arun Raj / Mbed OS MAXREFDES101_SOURCE

TEST

Dependencies:   max32630fthr Adafruit_FeatherOLED USBDevice

Utilities/queue.cpp@4:291477e8690d, 2020-04-19 (annotated)

Committer:
wwwarunraj
Date:
Sun Apr 19 11:19:57 2020 +0000
Revision:
4:291477e8690d
Parent:
3:2fe2ff1ca0dc 
19/04

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gmehmet 1:f60eafbf009a 1 /*******************************************************************************
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 2 * Copyright (C) Maxim Integrated Products, Inc., All rights Reserved.
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 3 *
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 4 * This software is protected by copyright laws of the United States and
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 5 * of foreign countries. This material may also be protected by patent laws
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 6 * and technology transfer regulations of the United States and of foreign
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 7 * countries. This software is furnished under a license agreement and/or a
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 8 * nondisclosure agreement and may only be used or reproduced in accordance
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 9 * with the terms of those agreements. Dissemination of this information to
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 10 * any party or parties not specified in the license agreement and/or
Emre.Eken@IST-LT-36262.maxim-ic.internal 3:2fe2ff1ca0dc 11 * nondisclosure agreement is expressly prohibited.
gmehmet 1:f60eafbf009a 12 *
gmehmet 1:f60eafbf009a 13 * The above copyright notice and this permission notice shall be included
gmehmet 1:f60eafbf009a 14 * in all copies or substantial portions of the Software.
gmehmet 1:f60eafbf009a 15 *
gmehmet 1:f60eafbf009a 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
gmehmet 1:f60eafbf009a 17 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
gmehmet 1:f60eafbf009a 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
gmehmet 1:f60eafbf009a 19 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
gmehmet 1:f60eafbf009a 20 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
gmehmet 1:f60eafbf009a 21 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
gmehmet 1:f60eafbf009a 22 * OTHER DEALINGS IN THE SOFTWARE.
gmehmet 1:f60eafbf009a 23 *
gmehmet 1:f60eafbf009a 24 * Except as contained in this notice, the name of Maxim Integrated
gmehmet 1:f60eafbf009a 25 * Products, Inc. shall not be used except as stated in the Maxim Integrated
gmehmet 1:f60eafbf009a 26 * Products, Inc. Branding Policy.
gmehmet 1:f60eafbf009a 27 *
gmehmet 1:f60eafbf009a 28 * The mere transfer of this software does not imply any licenses
gmehmet 1:f60eafbf009a 29 * of trade secrets, proprietary technology, copyrights, patents,
gmehmet 1:f60eafbf009a 30 * trademarks, maskwork rights, or any other form of intellectual
gmehmet 1:f60eafbf009a 31 * property whatsoever. Maxim Integrated Products, Inc. retains all
gmehmet 1:f60eafbf009a 32 * ownership rights.
gmehmet 1:f60eafbf009a 33 *******************************************************************************
gmehmet 1:f60eafbf009a 34 */
gmehmet 1:f60eafbf009a 35
gmehmet 1:f60eafbf009a 36 /*
gmehmet 1:f60eafbf009a 37 * TODO:
gmehmet 1:f60eafbf009a 38 * Add a function to enqueue data block instead of one by one.
gmehmet 1:f60eafbf009a 39 * Write function definitions in the header file as doxygen format
gmehmet 1:f60eafbf009a 40 * Init function will also allocate memory for queue buffer, providing the buffer will not necessary
gmehmet 1:f60eafbf009a 41 *
gmehmet 1:f60eafbf009a 42 * */
gmehmet 1:f60eafbf009a 43
gmehmet 1:f60eafbf009a 44 #include "queue.h"
gmehmet 1:f60eafbf009a 45 #include "mbed.h"
gmehmet 1:f60eafbf009a 46 #include "Peripherals.h"
gmehmet 1:f60eafbf009a 47
gmehmet 1:f60eafbf009a 48 int queue_reset(struct queue_t *q)
gmehmet 1:f60eafbf009a 49 {
gmehmet 1:f60eafbf009a 50 if (!q)
gmehmet 1:f60eafbf009a 51 return -EINVAL;
gmehmet 1:f60eafbf009a 52
gmehmet 1:f60eafbf009a 53 q->wr = q->base;
gmehmet 1:f60eafbf009a 54 q->rd = q->base;
gmehmet 1:f60eafbf009a 55 q->num_item = 0;
gmehmet 1:f60eafbf009a 56 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 57 #ifdef QUEUE_USAGE_STATS
gmehmet 1:f60eafbf009a 58 q->pop_cnt = 0;
gmehmet 1:f60eafbf009a 59 q->push_cnt = 0;
gmehmet 1:f60eafbf009a 60 q->stats_period_cnt = 100; // Default
gmehmet 1:f60eafbf009a 61 #endif
gmehmet 1:f60eafbf009a 62 return 0;
gmehmet 1:f60eafbf009a 63 }
gmehmet 1:f60eafbf009a 64
gmehmet 1:f60eafbf009a 65 int queue_update_items_size(struct queue_t *q, int item_size)
gmehmet 1:f60eafbf009a 66 {
gmehmet 1:f60eafbf009a 67 if (!q)
gmehmet 1:f60eafbf009a 68 return -EINVAL;
gmehmet 1:f60eafbf009a 69
gmehmet 1:f60eafbf009a 70 queue_reset(q);
gmehmet 1:f60eafbf009a 71 q->item_size = item_size;
gmehmet 1:f60eafbf009a 72 q->buffer_size = q->max_buffer_size - (q->max_buffer_size % item_size);
gmehmet 1:f60eafbf009a 73 return 0;
gmehmet 1:f60eafbf009a 74 }
gmehmet 1:f60eafbf009a 75
gmehmet 1:f60eafbf009a 76 int queue_len(struct queue_t *q)
gmehmet 1:f60eafbf009a 77 {
gmehmet 1:f60eafbf009a 78 int num_elements;
gmehmet 1:f60eafbf009a 79
gmehmet 1:f60eafbf009a 80 if (!q)
gmehmet 1:f60eafbf009a 81 return -EINVAL;
gmehmet 1:f60eafbf009a 82
gmehmet 1:f60eafbf009a 83 num_elements = q->num_item;
gmehmet 1:f60eafbf009a 84 return num_elements;
gmehmet 1:f60eafbf009a 85 }
gmehmet 1:f60eafbf009a 86
gmehmet 1:f60eafbf009a 87
gmehmet 1:f60eafbf009a 88 int queue_init(struct queue_t *q, void *buf, int item_size, int buffer_size)
gmehmet 1:f60eafbf009a 89 {
gmehmet 1:f60eafbf009a 90 if (!q || !buf)
gmehmet 1:f60eafbf009a 91 return -EINVAL;
gmehmet 1:f60eafbf009a 92
gmehmet 1:f60eafbf009a 93 if (buffer_size % item_size != 0)
gmehmet 1:f60eafbf009a 94 return -EINVAL; // Padding problem
gmehmet 1:f60eafbf009a 95
gmehmet 1:f60eafbf009a 96 q->num_item = 0;
gmehmet 1:f60eafbf009a 97 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 98 q->base = (char *)buf;
gmehmet 1:f60eafbf009a 99 q->wr = (char *)buf;
gmehmet 1:f60eafbf009a 100 q->rd = (char *)buf;
gmehmet 1:f60eafbf009a 101 q->item_size = item_size;
gmehmet 1:f60eafbf009a 102 q->buffer_size = buffer_size;
gmehmet 1:f60eafbf009a 103 q->max_buffer_size = buffer_size;
gmehmet 1:f60eafbf009a 104 q->name = NULL;
gmehmet 1:f60eafbf009a 105
gmehmet 1:f60eafbf009a 106 #ifdef QUEUE_USAGE_STATS
gmehmet 1:f60eafbf009a 107 q->pop_cnt = 0;
gmehmet 1:f60eafbf009a 108 q->push_cnt = 0;
gmehmet 1:f60eafbf009a 109 q->stats_period_cnt = 100; // Default
gmehmet 1:f60eafbf009a 110 #endif
gmehmet 1:f60eafbf009a 111
gmehmet 1:f60eafbf009a 112 return 0;
gmehmet 1:f60eafbf009a 113 }
gmehmet 1:f60eafbf009a 114
gmehmet 1:f60eafbf009a 115 int queue_init_by_name(struct queue_t *q, void *buf, int item_size, int buffer_size, const char *name)
gmehmet 1:f60eafbf009a 116 {
gmehmet 1:f60eafbf009a 117 int ret = queue_init(q, buf, item_size, buffer_size);
gmehmet 1:f60eafbf009a 118
gmehmet 1:f60eafbf009a 119 if (ret < 0)
gmehmet 1:f60eafbf009a 120 return ret;
gmehmet 1:f60eafbf009a 121
gmehmet 1:f60eafbf009a 122 q->name = (char *)name;
gmehmet 1:f60eafbf009a 123 return 0;
gmehmet 1:f60eafbf009a 124 }
gmehmet 1:f60eafbf009a 125
gmehmet 1:f60eafbf009a 126
gmehmet 1:f60eafbf009a 127 void queue_destroy(struct queue_t *q)
gmehmet 1:f60eafbf009a 128 {
gmehmet 1:f60eafbf009a 129 /* TODO: This is placeholder function, double check the implementation */
gmehmet 1:f60eafbf009a 130 free((void *)q->base);
gmehmet 1:f60eafbf009a 131 free((void *)q);
gmehmet 1:f60eafbf009a 132 }
gmehmet 1:f60eafbf009a 133
gmehmet 1:f60eafbf009a 134 int enqueue(struct queue_t *q, void *data)
gmehmet 1:f60eafbf009a 135 {
gmehmet 1:f60eafbf009a 136 int ret = 0;
gmehmet 1:f60eafbf009a 137
gmehmet 1:f60eafbf009a 138 if (!q || !data)
gmehmet 1:f60eafbf009a 139 return -EINVAL; // Invalid pointer
gmehmet 1:f60eafbf009a 140
gmehmet 1:f60eafbf009a 141 if (q->wr == q->rd)
gmehmet 1:f60eafbf009a 142 ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
gmehmet 1:f60eafbf009a 143
gmehmet 1:f60eafbf009a 144 if (q->wr >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 145 q->wr = q->base;
gmehmet 1:f60eafbf009a 146
gmehmet 1:f60eafbf009a 147 memcpy((void *)q->wr, data, q->item_size);
gmehmet 1:f60eafbf009a 148 q->wr = q->wr + q->item_size;
gmehmet 1:f60eafbf009a 149 q->num_item++;
gmehmet 1:f60eafbf009a 150 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 151 if (q->num_item > fifo_size)
gmehmet 1:f60eafbf009a 152 q->ovf_item = q->num_item - fifo_size;
gmehmet 1:f60eafbf009a 153 else
gmehmet 1:f60eafbf009a 154 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 155 return ret;
gmehmet 1:f60eafbf009a 156 }
gmehmet 1:f60eafbf009a 157
gmehmet 1:f60eafbf009a 158 int enqueue_test(struct queue_t *q, void *data)
gmehmet 1:f60eafbf009a 159 {
gmehmet 1:f60eafbf009a 160 if (!q || !data)
gmehmet 1:f60eafbf009a 161 return -EINVAL; // Invalid pointer
gmehmet 1:f60eafbf009a 162
gmehmet 1:f60eafbf009a 163 q->num_item++;
gmehmet 1:f60eafbf009a 164 return 0;
gmehmet 1:f60eafbf009a 165 }
gmehmet 1:f60eafbf009a 166
gmehmet 1:f60eafbf009a 167
gmehmet 1:f60eafbf009a 168 int dequeue_test(struct queue_t *q, void *data)
gmehmet 1:f60eafbf009a 169 {
gmehmet 1:f60eafbf009a 170 if (!q || !data)
gmehmet 1:f60eafbf009a 171 return -EINVAL;
gmehmet 1:f60eafbf009a 172
gmehmet 1:f60eafbf009a 173 if (q->num_item > 0)
gmehmet 1:f60eafbf009a 174 q->num_item--;
gmehmet 1:f60eafbf009a 175 else
gmehmet 1:f60eafbf009a 176 return -2;
gmehmet 1:f60eafbf009a 177 return 0;
gmehmet 1:f60eafbf009a 178 }
gmehmet 1:f60eafbf009a 179
gmehmet 1:f60eafbf009a 180 int dequeue(struct queue_t *q, void *data)
gmehmet 1:f60eafbf009a 181 {
gmehmet 1:f60eafbf009a 182 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 183
gmehmet 1:f60eafbf009a 184 if (!q || !data)
gmehmet 1:f60eafbf009a 185 return -EINVAL;
gmehmet 1:f60eafbf009a 186
gmehmet 1:f60eafbf009a 187 if (q->num_item <= 0) {
gmehmet 1:f60eafbf009a 188 return -2;
gmehmet 1:f60eafbf009a 189 }
gmehmet 1:f60eafbf009a 190
gmehmet 1:f60eafbf009a 191 if (q->num_item > fifo_size) {
gmehmet 1:f60eafbf009a 192 uint32_t curr_rd_off = ((uint32_t)(q->rd - q->base) + q->num_item * q->item_size);
gmehmet 1:f60eafbf009a 193 q->rd = (q->base + (curr_rd_off % q->buffer_size));
gmehmet 1:f60eafbf009a 194 q->num_item = fifo_size; // OVF number samples are already gone.
gmehmet 1:f60eafbf009a 195
gmehmet 1:f60eafbf009a 196 #ifdef QUEUE_USAGE_STATS
gmehmet 1:f60eafbf009a 197 q->pop_cnt++;
gmehmet 1:f60eafbf009a 198
gmehmet 1:f60eafbf009a 199 if ((q->pop_cnt % q->stats_period_cnt) == 0) {
gmehmet 1:f60eafbf009a 200 if (q->name) {
gmehmet 1:f60eafbf009a 201 pr_info("%s:%d (%s) - %d samples lost, avail:%d \n",__func__, __LINE__, q->name, q->ovf_item, q->num_item);
gmehmet 1:f60eafbf009a 202 } else {
gmehmet 1:f60eafbf009a 203 pr_info("%s:%d - %d samples lost, avail:%d \n", __func__, __LINE__, q->ovf_item, q->num_item);
gmehmet 1:f60eafbf009a 204 }
gmehmet 1:f60eafbf009a 205 }
gmehmet 1:f60eafbf009a 206 #endif
gmehmet 1:f60eafbf009a 207 }
gmehmet 1:f60eafbf009a 208
gmehmet 1:f60eafbf009a 209 if (q->rd >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 210 q->rd = q->base;
gmehmet 1:f60eafbf009a 211
gmehmet 1:f60eafbf009a 212 memcpy(data, (void *)q->rd, q->item_size);
gmehmet 1:f60eafbf009a 213 q->rd = q->rd + q->item_size;
gmehmet 1:f60eafbf009a 214 q->num_item--;
gmehmet 1:f60eafbf009a 215 if (q->num_item <= fifo_size)
gmehmet 1:f60eafbf009a 216 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 217
gmehmet 1:f60eafbf009a 218 #if defined(QUEUE_DEBUG)
gmehmet 1:f60eafbf009a 219 do {
gmehmet 1:f60eafbf009a 220 static int cnt;
gmehmet 1:f60eafbf009a 221
gmehmet 1:f60eafbf009a 222 if (cnt++ % 100 == 0) {
gmehmet 1:f60eafbf009a 223 if (q->name) {
gmehmet 1:f60eafbf009a 224 pr_debug("%s - $ Fifo size: %d, usage: %d\n", q->name, fifo_size, q->num_item);
gmehmet 1:f60eafbf009a 225 } else {
gmehmet 1:f60eafbf009a 226 pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
gmehmet 1:f60eafbf009a 227 }
gmehmet 1:f60eafbf009a 228 }
gmehmet 1:f60eafbf009a 229 } while(0);
gmehmet 1:f60eafbf009a 230 #endif
gmehmet 1:f60eafbf009a 231
gmehmet 1:f60eafbf009a 232 return 0;
gmehmet 1:f60eafbf009a 233 }
gmehmet 1:f60eafbf009a 234
gmehmet 1:f60eafbf009a 235 bool queue_is_full(struct queue_t *q)
gmehmet 1:f60eafbf009a 236 {
gmehmet 1:f60eafbf009a 237 if (!q)
gmehmet 1:f60eafbf009a 238 return -EINVAL;
gmehmet 1:f60eafbf009a 239
gmehmet 1:f60eafbf009a 240 int num_items = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 241
gmehmet 1:f60eafbf009a 242
gmehmet 1:f60eafbf009a 243 if (num_items > q->num_item)
gmehmet 1:f60eafbf009a 244 return false;
gmehmet 1:f60eafbf009a 245 return true;
gmehmet 1:f60eafbf009a 246 }
gmehmet 1:f60eafbf009a 247
gmehmet 1:f60eafbf009a 248 int queue_usage(struct queue_t *q, int *total, int *nm_item)
gmehmet 1:f60eafbf009a 249 {
gmehmet 1:f60eafbf009a 250 if (!q)
gmehmet 1:f60eafbf009a 251 return -EINVAL;
gmehmet 1:f60eafbf009a 252
gmehmet 1:f60eafbf009a 253 *total = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 254 *nm_item = q->num_item;
gmehmet 1:f60eafbf009a 255
gmehmet 1:f60eafbf009a 256 return 0;
gmehmet 1:f60eafbf009a 257 }
gmehmet 1:f60eafbf009a 258
gmehmet 1:f60eafbf009a 259 int queue_pop(struct queue_t *q)
gmehmet 1:f60eafbf009a 260 {
gmehmet 1:f60eafbf009a 261 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 262
gmehmet 1:f60eafbf009a 263 if (!q)
gmehmet 1:f60eafbf009a 264 return -EINVAL;
gmehmet 1:f60eafbf009a 265
gmehmet 1:f60eafbf009a 266 if (q->num_item <= 0) {
gmehmet 1:f60eafbf009a 267 return -2;
gmehmet 1:f60eafbf009a 268 }
gmehmet 1:f60eafbf009a 269
gmehmet 1:f60eafbf009a 270 if (q->num_item > fifo_size) {
gmehmet 1:f60eafbf009a 271 uint32_t curr_rd_off = ((uint32_t)(q->rd - q->base) + q->num_item * q->item_size);
gmehmet 1:f60eafbf009a 272 q->ovf_item = q->num_item - fifo_size;
gmehmet 1:f60eafbf009a 273 q->rd = q->base + (curr_rd_off % q->buffer_size);
gmehmet 1:f60eafbf009a 274 q->num_item = fifo_size; // OVF number samples are already gone.
gmehmet 1:f60eafbf009a 275
gmehmet 1:f60eafbf009a 276 #ifdef QUEUE_USAGE_STATS
gmehmet 1:f60eafbf009a 277 q->push_cnt++;
gmehmet 1:f60eafbf009a 278
gmehmet 1:f60eafbf009a 279 if ((q->push_cnt % q->stats_period_cnt) == 0) {
gmehmet 1:f60eafbf009a 280 if (q->name) {
gmehmet 1:f60eafbf009a 281 pr_info("%s:%d (%s) - %d samples lost, avail:%d \n",__func__, __LINE__, q->name, q->ovf_item, q->num_item);
gmehmet 1:f60eafbf009a 282 } else {
gmehmet 1:f60eafbf009a 283 pr_info("%s:%d - %d samples lost, avail:%d \n", __func__, __LINE__, q->ovf_item, q->num_item);
gmehmet 1:f60eafbf009a 284 }
gmehmet 1:f60eafbf009a 285 }
gmehmet 1:f60eafbf009a 286 #endif
gmehmet 1:f60eafbf009a 287 } else
gmehmet 1:f60eafbf009a 288 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 289
gmehmet 1:f60eafbf009a 290 if (q->rd >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 291 q->rd = q->base;
gmehmet 1:f60eafbf009a 292
gmehmet 1:f60eafbf009a 293 q->rd = q->rd + q->item_size;
gmehmet 1:f60eafbf009a 294 q->num_item--;
gmehmet 1:f60eafbf009a 295
gmehmet 1:f60eafbf009a 296 #if defined(QUEUE_DEBUG)
gmehmet 1:f60eafbf009a 297 do {
gmehmet 1:f60eafbf009a 298 static int cnt;
gmehmet 1:f60eafbf009a 299
gmehmet 1:f60eafbf009a 300 if (cnt++ % 100 == 0) {
gmehmet 1:f60eafbf009a 301 if (q->name)
gmehmet 1:f60eafbf009a 302 pr_debug("%s - $ Fifo size: %d, usage: %d\n", q->name, fifo_size, q->num_item);
gmehmet 1:f60eafbf009a 303 else
gmehmet 1:f60eafbf009a 304 pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
gmehmet 1:f60eafbf009a 305 }
gmehmet 1:f60eafbf009a 306 } while(0);
gmehmet 1:f60eafbf009a 307 #endif
gmehmet 1:f60eafbf009a 308
gmehmet 1:f60eafbf009a 309 return 0;
gmehmet 1:f60eafbf009a 310 }
gmehmet 1:f60eafbf009a 311
gmehmet 1:f60eafbf009a 312 int queue_pop_n(struct queue_t *q, int n)
gmehmet 1:f60eafbf009a 313 {
gmehmet 1:f60eafbf009a 314 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 315
gmehmet 1:f60eafbf009a 316 if (!q || n < 1)
gmehmet 1:f60eafbf009a 317 return -EINVAL;
gmehmet 1:f60eafbf009a 318
gmehmet 1:f60eafbf009a 319 if (q->num_item < n) {
gmehmet 1:f60eafbf009a 320 return -2;
gmehmet 1:f60eafbf009a 321 }
gmehmet 1:f60eafbf009a 322
gmehmet 1:f60eafbf009a 323 if (q->num_item > fifo_size) {
gmehmet 1:f60eafbf009a 324 if (q->name) {
gmehmet 1:f60eafbf009a 325 pr_info("%s:%d ( %s ) - %d samples lost, avail:%d \n",
gmehmet 1:f60eafbf009a 326 __func__, __LINE__, q->name, q->num_item - fifo_size, fifo_size);
gmehmet 1:f60eafbf009a 327 } else {
gmehmet 1:f60eafbf009a 328 pr_info("%s:%d - %d samples lost, avail:%d \n",
gmehmet 1:f60eafbf009a 329 __func__, __LINE__, q->num_item - fifo_size, fifo_size);
gmehmet 1:f60eafbf009a 330 }
gmehmet 1:f60eafbf009a 331 q->num_item = fifo_size; // OVF number samples are already gone.
gmehmet 1:f60eafbf009a 332 n = fifo_size;
gmehmet 1:f60eafbf009a 333 uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
gmehmet 1:f60eafbf009a 334 q->ovf_item = q->num_item - fifo_size;
gmehmet 1:f60eafbf009a 335 q->rd = q->base + curr_rd_off % q->buffer_size;
gmehmet 1:f60eafbf009a 336 } else {
gmehmet 1:f60eafbf009a 337 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 338 }
gmehmet 1:f60eafbf009a 339
gmehmet 1:f60eafbf009a 340 if (q->rd >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 341 q->rd = q->base;
gmehmet 1:f60eafbf009a 342
gmehmet 1:f60eafbf009a 343
gmehmet 1:f60eafbf009a 344 uint32_t rd_sz = q->item_size * n;
gmehmet 1:f60eafbf009a 345 uint32_t to_end = (uint32_t)(q->base + q->buffer_size - q->rd);
gmehmet 1:f60eafbf009a 346 if (to_end < rd_sz) {
gmehmet 1:f60eafbf009a 347 rd_sz -= to_end;
gmehmet 1:f60eafbf009a 348 q->rd = q->base;
gmehmet 1:f60eafbf009a 349 }
gmehmet 1:f60eafbf009a 350
gmehmet 1:f60eafbf009a 351 q->rd = q->rd + rd_sz;
gmehmet 1:f60eafbf009a 352 q->num_item -= n;
gmehmet 1:f60eafbf009a 353
gmehmet 1:f60eafbf009a 354 return 0;
gmehmet 1:f60eafbf009a 355 }
gmehmet 1:f60eafbf009a 356
gmehmet 1:f60eafbf009a 357 int queue_front(struct queue_t *q, void *data)
gmehmet 1:f60eafbf009a 358 {
gmehmet 1:f60eafbf009a 359 #if 0
gmehmet 1:f60eafbf009a 360 return queue_front_n(q, data, 1);
gmehmet 1:f60eafbf009a 361 #endif
gmehmet 1:f60eafbf009a 362
gmehmet 1:f60eafbf009a 363 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 364 void *rd = 0;
gmehmet 1:f60eafbf009a 365
gmehmet 1:f60eafbf009a 366 if (!q || !data)
gmehmet 1:f60eafbf009a 367 return -EINVAL;
gmehmet 1:f60eafbf009a 368
gmehmet 1:f60eafbf009a 369 if (q->num_item <= 0) {
gmehmet 1:f60eafbf009a 370 return -2;
gmehmet 1:f60eafbf009a 371 }
gmehmet 1:f60eafbf009a 372
gmehmet 1:f60eafbf009a 373 if (q->num_item > fifo_size) {
gmehmet 1:f60eafbf009a 374 uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
gmehmet 1:f60eafbf009a 375 rd = q->base + (curr_rd_off % q->buffer_size);
gmehmet 1:f60eafbf009a 376 if (q->name) {
gmehmet 1:f60eafbf009a 377 pr_info("%s:%d ( %s )- %d samples lost, avail:%d cap:%d \n",
gmehmet 1:f60eafbf009a 378 __func__, __LINE__, q->name, q->ovf_item, q->num_item, fifo_size);
gmehmet 1:f60eafbf009a 379 } else {
gmehmet 1:f60eafbf009a 380 pr_info("%s:%d - %d samples lost, avail:%d cap:%d \n",
gmehmet 1:f60eafbf009a 381 __func__, __LINE__, q->ovf_item, q->num_item, fifo_size);
gmehmet 1:f60eafbf009a 382 }
gmehmet 1:f60eafbf009a 383 } else {
gmehmet 1:f60eafbf009a 384 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 385 rd = q->rd;
gmehmet 1:f60eafbf009a 386 }
gmehmet 1:f60eafbf009a 387
gmehmet 1:f60eafbf009a 388 if (q->rd >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 389 rd = q->base;
gmehmet 1:f60eafbf009a 390
gmehmet 1:f60eafbf009a 391 memcpy(data, (void *)rd, q->item_size);
gmehmet 1:f60eafbf009a 392 return 0;
gmehmet 1:f60eafbf009a 393 }
gmehmet 1:f60eafbf009a 394
gmehmet 1:f60eafbf009a 395 int queue_front_n(struct queue_t *q, void *data, int n, int buf_sz)
gmehmet 1:f60eafbf009a 396 {
gmehmet 1:f60eafbf009a 397 int fifo_size = q->buffer_size / q->item_size;
gmehmet 1:f60eafbf009a 398 char *rd = 0;
gmehmet 1:f60eafbf009a 399 char *wr = (char *)data;
gmehmet 1:f60eafbf009a 400
gmehmet 1:f60eafbf009a 401 if (!q || !data || n < 1)
gmehmet 1:f60eafbf009a 402 return -EINVAL;
gmehmet 1:f60eafbf009a 403
gmehmet 1:f60eafbf009a 404 if (q->num_item < n) {
gmehmet 1:f60eafbf009a 405 return -2;
gmehmet 1:f60eafbf009a 406 }
gmehmet 1:f60eafbf009a 407
gmehmet 1:f60eafbf009a 408 if (q->num_item > fifo_size) {
gmehmet 1:f60eafbf009a 409 if (q->name) {
gmehmet 1:f60eafbf009a 410 pr_info("\n%s:%d ( %s ) - %d samples lost, avail:%d \n",
gmehmet 1:f60eafbf009a 411 __func__, __LINE__, q->name, q->num_item - fifo_size, fifo_size);
gmehmet 1:f60eafbf009a 412 } else {
gmehmet 1:f60eafbf009a 413 pr_info("\n%s:%d - %d samples lost, avail:%d \n",
gmehmet 1:f60eafbf009a 414 __func__, __LINE__, q->num_item - fifo_size, fifo_size);
gmehmet 1:f60eafbf009a 415 }
gmehmet 1:f60eafbf009a 416 q->num_item = fifo_size;
gmehmet 1:f60eafbf009a 417 n = fifo_size;
gmehmet 1:f60eafbf009a 418 uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
gmehmet 1:f60eafbf009a 419 rd = q->base + (curr_rd_off % q->buffer_size);
gmehmet 1:f60eafbf009a 420 } else {
gmehmet 1:f60eafbf009a 421 q->ovf_item = 0;
gmehmet 1:f60eafbf009a 422 rd = q->rd;
gmehmet 1:f60eafbf009a 423 }
gmehmet 1:f60eafbf009a 424
gmehmet 1:f60eafbf009a 425 if (q->rd >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 426 rd = q->base;
gmehmet 1:f60eafbf009a 427
gmehmet 1:f60eafbf009a 428 uint32_t rd_sz = q->item_size * n;
gmehmet 1:f60eafbf009a 429
gmehmet 1:f60eafbf009a 430 if (buf_sz < rd_sz) {
gmehmet 1:f60eafbf009a 431 if (q->name) {
gmehmet 1:f60eafbf009a 432 pr_info("\n%s:%d ( %s ) - Insufficient buffer size: %d\n",
gmehmet 1:f60eafbf009a 433 __func__, __LINE__, q->name, buf_sz);
gmehmet 1:f60eafbf009a 434 } else {
gmehmet 1:f60eafbf009a 435 pr_info("\n%s:%d - Insufficient buffer size: %d\n",
gmehmet 1:f60eafbf009a 436 __func__, __LINE__, buf_sz);
gmehmet 1:f60eafbf009a 437 }
gmehmet 1:f60eafbf009a 438 return -EINVAL;
gmehmet 1:f60eafbf009a 439 }
gmehmet 1:f60eafbf009a 440
gmehmet 1:f60eafbf009a 441 uint32_t to_end = (uint32_t)(q->base + q->buffer_size - q->rd);
gmehmet 1:f60eafbf009a 442 if (to_end < rd_sz) {
gmehmet 1:f60eafbf009a 443 memcpy(wr, rd, to_end);
gmehmet 1:f60eafbf009a 444 rd_sz -= to_end;
gmehmet 1:f60eafbf009a 445 rd = q->base;
gmehmet 1:f60eafbf009a 446 wr += to_end;
gmehmet 1:f60eafbf009a 447 memcpy(wr, rd, rd_sz);
gmehmet 1:f60eafbf009a 448
gmehmet 1:f60eafbf009a 449 } else {
gmehmet 1:f60eafbf009a 450 memcpy(wr, rd, rd_sz);
gmehmet 1:f60eafbf009a 451 }
gmehmet 1:f60eafbf009a 452
gmehmet 1:f60eafbf009a 453 return 0;
gmehmet 1:f60eafbf009a 454 }
gmehmet 1:f60eafbf009a 455
gmehmet 1:f60eafbf009a 456 int enqueue_string(struct queue_t *q, char *data, int sz)
gmehmet 1:f60eafbf009a 457 {
gmehmet 1:f60eafbf009a 458 int ret = 0;
gmehmet 1:f60eafbf009a 459 int buf_index;
gmehmet 1:f60eafbf009a 460 char *wr_ptr;
gmehmet 1:f60eafbf009a 461
gmehmet 1:f60eafbf009a 462 if (!q || !data || sz <= 0)
gmehmet 1:f60eafbf009a 463 return -EFAULT; // Invalid parameters
gmehmet 1:f60eafbf009a 464
gmehmet 1:f60eafbf009a 465 if (q->wr == q->rd)
gmehmet 1:f60eafbf009a 466 ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
gmehmet 1:f60eafbf009a 467
gmehmet 1:f60eafbf009a 468 if (q->wr >= (q->base + q->buffer_size))
gmehmet 1:f60eafbf009a 469 q->wr = q->base;
gmehmet 1:f60eafbf009a 470
gmehmet 1:f60eafbf009a 471 if ((q->num_item + sz) > q->buffer_size) {
gmehmet 1:f60eafbf009a 472 #if defined(QUEUE_DEBUG)
gmehmet 1:f60eafbf009a 473 {
gmehmet 1:f60eafbf009a 474 char buf[128];
gmehmet 1:f60eafbf009a 475 int len;
gmehmet 1:f60eafbf009a 476
gmehmet 1:f60eafbf009a 477 if (q->name)
gmehmet 1:f60eafbf009a 478 len = sprintf(buf, "\r\n**** %s - ( %s ) - Fifo is full. num_item: %d, sz: %d, buffer size: %d\r\n",
gmehmet 1:f60eafbf009a 479 __func__, q->name, q->num_item, sz, q->buffer_size);
gmehmet 1:f60eafbf009a 480 else
gmehmet 1:f60eafbf009a 481 len = sprintf(buf, "\r\n**** %s - Fifo is full. num_item: %d, sz: %d, buffer size: %d\r\n",
gmehmet 1:f60eafbf009a 482 __func__, q->num_item, sz, q->buffer_size);
gmehmet 1:f60eafbf009a 483
gmehmet 1:f60eafbf009a 484 UART_Write(UART_PORT, (uint8_t*)buf, len);
gmehmet 1:f60eafbf009a 485 }
gmehmet 1:f60eafbf009a 486 #endif
gmehmet 1:f60eafbf009a 487 return -ENOMEM;
gmehmet 1:f60eafbf009a 488 }
gmehmet 1:f60eafbf009a 489
gmehmet 1:f60eafbf009a 490 buf_index = (uint32_t)(q->wr - q->base);
gmehmet 1:f60eafbf009a 491 wr_ptr = q->base;
gmehmet 1:f60eafbf009a 492 q->num_item += sz;
gmehmet 1:f60eafbf009a 493 while(sz--)
gmehmet 1:f60eafbf009a 494 wr_ptr[buf_index++ % q->buffer_size] = *data++;
gmehmet 1:f60eafbf009a 495
gmehmet 1:f60eafbf009a 496 q->wr = q->base + buf_index % q->buffer_size;
gmehmet 1:f60eafbf009a 497 return ret;
gmehmet 1:f60eafbf009a 498 }
gmehmet 1:f60eafbf009a 499
gmehmet 1:f60eafbf009a 500 int dequeue_string(struct queue_t *q, char *buf, int buffer_size)
gmehmet 1:f60eafbf009a 501 {
gmehmet 1:f60eafbf009a 502 char *rd_ptr;
gmehmet 1:f60eafbf009a 503 int buf_index;
gmehmet 1:f60eafbf009a 504 int len;
gmehmet 1:f60eafbf009a 505
gmehmet 1:f60eafbf009a 506 if (!q || !buf || buffer_size <= 0)
gmehmet 1:f60eafbf009a 507 return -EFAULT;
gmehmet 1:f60eafbf009a 508
gmehmet 1:f60eafbf009a 509 if (q->num_item <= 0) {
gmehmet 1:f60eafbf009a 510 return -EPERM;
gmehmet 1:f60eafbf009a 511 }
gmehmet 1:f60eafbf009a 512
gmehmet 1:f60eafbf009a 513 rd_ptr = (char *)q->base;
gmehmet 1:f60eafbf009a 514 buf_index = (uint32_t)(q->rd - q->base);
gmehmet 1:f60eafbf009a 515 len = q->num_item;
gmehmet 1:f60eafbf009a 516
gmehmet 1:f60eafbf009a 517 while (buffer_size-- && q->num_item--) {
gmehmet 1:f60eafbf009a 518 char tmp = rd_ptr[buf_index % q->buffer_size];
gmehmet 1:f60eafbf009a 519 rd_ptr[buf_index % q->buffer_size] = 0; // Remove this later on
gmehmet 1:f60eafbf009a 520 buf_index++;
gmehmet 1:f60eafbf009a 521 *buf++ = tmp;
gmehmet 1:f60eafbf009a 522 if (tmp == '\0')
gmehmet 1:f60eafbf009a 523 break;
gmehmet 1:f60eafbf009a 524 }
gmehmet 1:f60eafbf009a 525
gmehmet 1:f60eafbf009a 526 if (q->num_item < 0) {
gmehmet 1:f60eafbf009a 527 /* Data corruption in FIFO */
gmehmet 1:f60eafbf009a 528 q->num_item = 0;
gmehmet 1:f60eafbf009a 529 } else
gmehmet 1:f60eafbf009a 530 len -= q->num_item;
gmehmet 1:f60eafbf009a 531
gmehmet 1:f60eafbf009a 532 q->rd = q->base + buf_index % q->buffer_size;
gmehmet 1:f60eafbf009a 533
gmehmet 1:f60eafbf009a 534 return len;
gmehmet 1:f60eafbf009a 535 }
gmehmet 1:f60eafbf009a 536
gmehmet 1:f60eafbf009a 537 int queue_str_len(struct queue_t *q)
gmehmet 1:f60eafbf009a 538 {
gmehmet 1:f60eafbf009a 539 char *rd_ptr;
gmehmet 1:f60eafbf009a 540 int buf_index;
gmehmet 1:f60eafbf009a 541 int len, i;
gmehmet 1:f60eafbf009a 542
gmehmet 1:f60eafbf009a 543 if (!q)
gmehmet 1:f60eafbf009a 544 return -EFAULT;
gmehmet 1:f60eafbf009a 545
gmehmet 1:f60eafbf009a 546 if (q->num_item <= 0) {
gmehmet 1:f60eafbf009a 547 return 0;
gmehmet 1:f60eafbf009a 548 }
gmehmet 1:f60eafbf009a 549
gmehmet 1:f60eafbf009a 550 rd_ptr = q->base;
gmehmet 1:f60eafbf009a 551 buf_index = (uint32_t)(q->rd - q->base);
gmehmet 1:f60eafbf009a 552 i = q->num_item;
gmehmet 1:f60eafbf009a 553 len = 0;
gmehmet 1:f60eafbf009a 554
gmehmet 1:f60eafbf009a 555 while (i--) {
gmehmet 1:f60eafbf009a 556 char tmp = rd_ptr[buf_index % q->buffer_size];
gmehmet 1:f60eafbf009a 557 buf_index++;
gmehmet 1:f60eafbf009a 558 if (tmp == '\0')
gmehmet 1:f60eafbf009a 559 break;
gmehmet 1:f60eafbf009a 560 len++;
gmehmet 1:f60eafbf009a 561 }
gmehmet 1:f60eafbf009a 562
gmehmet 1:f60eafbf009a 563 return len;
gmehmet 1:f60eafbf009a 564 }
gmehmet 1:f60eafbf009a 565
gmehmet 1:f60eafbf009a 566 #if 0
gmehmet 1:f60eafbf009a 567 void queue_test(void)
gmehmet 1:f60eafbf009a 568 {
gmehmet 1:f60eafbf009a 569 int ret;
gmehmet 1:f60eafbf009a 570 ppg_data_t ppg_test = { 0, };
gmehmet 1:f60eafbf009a 571 ppg_data_t ppg_test_out = { 0, };
gmehmet 1:f60eafbf009a 572 int i, j, ii, jj;
gmehmet 1:f60eafbf009a 573 static ppg_data_t ppg_data[10];
gmehmet 1:f60eafbf009a 574 static queue_t queue;
gmehmet 1:f60eafbf009a 575
gmehmet 1:f60eafbf009a 576 srand((unsigned)time(NULL));
gmehmet 1:f60eafbf009a 577 ret = queue_init(&queue, &ppg_data, sizeof(ppg_data_t), sizeof(ppg_data));
gmehmet 1:f60eafbf009a 578 while (1) {
gmehmet 1:f60eafbf009a 579 ii = rand() % 20;
gmehmet 1:f60eafbf009a 580 for (i = 0; i < ii; i++) {
gmehmet 1:f60eafbf009a 581 /* Test data */
gmehmet 1:f60eafbf009a 582 ppg_test.timestamp++;
gmehmet 1:f60eafbf009a 583 ppg_test.ir++;
gmehmet 1:f60eafbf009a 584 ppg_test.red++;
gmehmet 1:f60eafbf009a 585 ppg_test.green++;
gmehmet 1:f60eafbf009a 586 /* Test functions */
gmehmet 1:f60eafbf009a 587 ret = enqueue(&queue, &ppg_test);
gmehmet 1:f60eafbf009a 588 }
gmehmet 1:f60eafbf009a 589 jj = rand() % 20;
gmehmet 1:f60eafbf009a 590 for (j = 0; j < jj; j++) {
gmehmet 1:f60eafbf009a 591 ret = dequeue(&queue, &ppg_test_out);
gmehmet 1:f60eafbf009a 592 }
gmehmet 1:f60eafbf009a 593 }
gmehmet 1:f60eafbf009a 594 }
gmehmet 1:f60eafbf009a 595 #endif
gmehmet 1:f60eafbf009a 596 void queue_n_test(void)
gmehmet 1:f60eafbf009a 597 {
gmehmet 1:f60eafbf009a 598 struct queue_t q;
gmehmet 1:f60eafbf009a 599 uint8_t buf[5];
gmehmet 1:f60eafbf009a 600 uint8_t peek_buf[5];
gmehmet 1:f60eafbf009a 601 int error;
gmehmet 1:f60eafbf009a 602 int i;
gmehmet 1:f60eafbf009a 603 error = queue_init(&q, &buf[0], 1, sizeof(buf));
gmehmet 1:f60eafbf009a 604 if (error)
gmehmet 1:f60eafbf009a 605 printf("queue_init error :(\r\n");
gmehmet 1:f60eafbf009a 606
gmehmet 1:f60eafbf009a 607 uint8_t val = 0;
gmehmet 1:f60eafbf009a 608 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 609 val = 1;
gmehmet 1:f60eafbf009a 610 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 611 val = 2;
gmehmet 1:f60eafbf009a 612 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 613 val = 3;
gmehmet 1:f60eafbf009a 614 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 615 val = 4;
gmehmet 1:f60eafbf009a 616 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 617
gmehmet 1:f60eafbf009a 618 printf("enqueued 0,1,2,3,4\r\n");
gmehmet 1:f60eafbf009a 619
gmehmet 1:f60eafbf009a 620 error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 621 if (error) {
gmehmet 1:f60eafbf009a 622 printf("queue_front_n n=5 error :(\r\n");
gmehmet 1:f60eafbf009a 623 } else {
gmehmet 1:f60eafbf009a 624 printf("queue_front_n n=5: ");
gmehmet 1:f60eafbf009a 625 for (i = 0; i < 5; i++) {
gmehmet 1:f60eafbf009a 626 printf("%d ", peek_buf[i]);
gmehmet 1:f60eafbf009a 627 }
gmehmet 1:f60eafbf009a 628 printf("\r\n");
gmehmet 1:f60eafbf009a 629 }
gmehmet 1:f60eafbf009a 630
gmehmet 1:f60eafbf009a 631 error = queue_front_n(&q, &peek_buf, 6, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 632 if (error)
gmehmet 1:f60eafbf009a 633 printf("queue_front_n n=6 error :)\r\n");
gmehmet 1:f60eafbf009a 634 else
gmehmet 1:f60eafbf009a 635 printf("queue_front_n n=6 succeeded :(\r\n");
gmehmet 1:f60eafbf009a 636
gmehmet 1:f60eafbf009a 637 error = queue_pop_n(&q, 2);
gmehmet 1:f60eafbf009a 638 if (error)
gmehmet 1:f60eafbf009a 639 printf("queue_pop_n n=2 error :(\r\n");
gmehmet 1:f60eafbf009a 640 else
gmehmet 1:f60eafbf009a 641 printf("queue_pop_n n=2 succeeded :)\r\n");
gmehmet 1:f60eafbf009a 642
gmehmet 1:f60eafbf009a 643 error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 644 if (error)
gmehmet 1:f60eafbf009a 645 printf("queue_front_n n=5 error :)\r\n");
gmehmet 1:f60eafbf009a 646
gmehmet 1:f60eafbf009a 647 error = queue_front_n(&q, &peek_buf, 3, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 648 if (error) {
gmehmet 1:f60eafbf009a 649 printf("queue_front_n n=3 error :(\r\n");
gmehmet 1:f60eafbf009a 650 } else {
gmehmet 1:f60eafbf009a 651 printf("queue_front_n n=3: ");
gmehmet 1:f60eafbf009a 652 for (i = 0; i < 3; i++) {
gmehmet 1:f60eafbf009a 653 printf("%d ", peek_buf[i]);
gmehmet 1:f60eafbf009a 654 }
gmehmet 1:f60eafbf009a 655 printf("\r\n");
gmehmet 1:f60eafbf009a 656 }
gmehmet 1:f60eafbf009a 657
gmehmet 1:f60eafbf009a 658 val = 0;
gmehmet 1:f60eafbf009a 659 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 660 val = 1;
gmehmet 1:f60eafbf009a 661 enqueue(&q, &val);
gmehmet 1:f60eafbf009a 662
gmehmet 1:f60eafbf009a 663 printf("enqueued 0,1\r\n");
gmehmet 1:f60eafbf009a 664
gmehmet 1:f60eafbf009a 665 error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 666 if (error) {
gmehmet 1:f60eafbf009a 667 printf("queue_front_n n=5 error :(\r\n");
gmehmet 1:f60eafbf009a 668 } else {
gmehmet 1:f60eafbf009a 669 printf("queue_front_n n=5: ");
gmehmet 1:f60eafbf009a 670 for (i = 0; i < 5; i++) {
gmehmet 1:f60eafbf009a 671 printf("%d ", peek_buf[i]);
gmehmet 1:f60eafbf009a 672 }
gmehmet 1:f60eafbf009a 673 printf("\r\n");
gmehmet 1:f60eafbf009a 674 }
gmehmet 1:f60eafbf009a 675
gmehmet 1:f60eafbf009a 676 error = queue_pop_n(&q, 4);
gmehmet 1:f60eafbf009a 677 if (error)
gmehmet 1:f60eafbf009a 678 printf("queue_pop_n n=4 error :(\r\n");
gmehmet 1:f60eafbf009a 679 else
gmehmet 1:f60eafbf009a 680 printf("queue_pop_n n=4 succeeded :)\r\n");
gmehmet 1:f60eafbf009a 681
gmehmet 1:f60eafbf009a 682 error = queue_front_n(&q, &peek_buf, 1, sizeof(peek_buf));
gmehmet 1:f60eafbf009a 683 if (error) {
gmehmet 1:f60eafbf009a 684 printf("queue_front_n n=1 error :(\r\n");
gmehmet 1:f60eafbf009a 685 } else {
gmehmet 1:f60eafbf009a 686 printf("queue_front_n n=1: ");
gmehmet 1:f60eafbf009a 687 for (i = 0; i < 1; i++) {
gmehmet 1:f60eafbf009a 688 printf("%d ", peek_buf[i]);
gmehmet 1:f60eafbf009a 689 }
gmehmet 1:f60eafbf009a 690 printf("\r\n");
gmehmet 1:f60eafbf009a 691 }
gmehmet 1:f60eafbf009a 692 }