queue.cpp

00001 /*******************************************************************************
00002 * Copyright (C) Maxim Integrated Products, Inc., All rights Reserved.
00003 * 
00004 * This software is protected by copyright laws of the United States and
00005 * of foreign countries. This material may also be protected by patent laws
00006 * and technology transfer regulations of the United States and of foreign
00007 * countries. This software is furnished under a license agreement and/or a
00008 * nondisclosure agreement and may only be used or reproduced in accordance
00009 * with the terms of those agreements. Dissemination of this information to
00010 * any party or parties not specified in the license agreement and/or
00011 * nondisclosure agreement is expressly prohibited.
00012 *
00013 * The above copyright notice and this permission notice shall be included
00014 * in all copies or substantial portions of the Software.
00015 *
00016 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00017 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00018 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00019 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00020 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00021 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00022 * OTHER DEALINGS IN THE SOFTWARE.
00023 *
00024 * Except as contained in this notice, the name of Maxim Integrated
00025 * Products, Inc. shall not be used except as stated in the Maxim Integrated
00026 * Products, Inc. Branding Policy.
00027 *
00028 * The mere transfer of this software does not imply any licenses
00029 * of trade secrets, proprietary technology, copyrights, patents,
00030 * trademarks, maskwork rights, or any other form of intellectual
00031 * property whatsoever. Maxim Integrated Products, Inc. retains all
00032 * ownership rights.
00033 *******************************************************************************
00034 */
00035 
00036 /*
00037  * TODO:
00038  * Add a function to enqueue data block instead of one by one.
00039  * Write function definitions in the header file as doxygen format
00040  * Init function will also allocate memory for queue buffer, providing the buffer will not necessary
00041  *
00042  * */
00043 
00044 #include "queue.h"
00045 #include "mbed.h"
00046 #include "Peripherals.h"
00047 
00048 int queue_reset(struct queue_t *q)
00049 {
00050     if (!q)
00051         return -EINVAL;
00052 
00053     q->wr = q->base;
00054     q->rd = q->base;
00055     q->num_item = 0;
00056     q->ovf_item = 0;
00057 #ifdef QUEUE_USAGE_STATS
00058     q->pop_cnt = 0;
00059     q->push_cnt = 0;
00060     q->stats_period_cnt = 100; // Default
00061 #endif
00062     return 0;
00063 }
00064 
00065 int queue_update_items_size(struct queue_t *q, int item_size)
00066 {
00067     if (!q)
00068         return -EINVAL;
00069 
00070     queue_reset(q);
00071     q->item_size = item_size;
00072     q->buffer_size = q->max_buffer_size - (q->max_buffer_size % item_size);
00073     return 0;
00074 }
00075 
00076 int queue_len(struct queue_t *q)
00077 {
00078     int num_elements;
00079 
00080     if (!q)
00081         return -EINVAL;
00082 
00083     num_elements = q->num_item;
00084     return num_elements;
00085 }
00086 
00087 
00088 int queue_init(struct queue_t *q, void *buf, int item_size, int buffer_size)
00089 {
00090     if (!q || !buf)
00091         return -EINVAL;
00092 
00093     if (buffer_size % item_size != 0)
00094         return -EINVAL; // Padding problem
00095 
00096     q->num_item = 0;
00097     q->ovf_item = 0;
00098     q->base = (char *)buf;
00099     q->wr =  (char *)buf;
00100     q->rd =  (char *)buf;
00101     q->item_size = item_size;
00102     q->buffer_size = buffer_size;
00103     q->max_buffer_size = buffer_size;
00104     q->name = NULL;
00105 
00106 #ifdef QUEUE_USAGE_STATS
00107     q->pop_cnt = 0;
00108     q->push_cnt = 0;
00109     q->stats_period_cnt = 100; // Default
00110 #endif
00111 
00112     return 0;
00113 }
00114 
00115 int queue_init_by_name(struct queue_t *q, void *buf, int item_size, int buffer_size, const char *name)
00116 {
00117     int ret = queue_init(q, buf, item_size, buffer_size);
00118 
00119     if (ret < 0)
00120         return ret;
00121 
00122     q->name = (char *)name;
00123     return 0;
00124 }
00125 
00126 
00127 void queue_destroy(struct queue_t *q)
00128 {
00129 /* TODO: This is placeholder function, double check the implementation */
00130     free((void *)q->base);
00131     free((void *)q);
00132 }
00133 
00134 int enqueue(struct queue_t *q, void *data)
00135 {
00136     int ret = 0;
00137 
00138     if (!q || !data)
00139         return -EINVAL; // Invalid pointer
00140 
00141     if (q->wr == q->rd)
00142         ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
00143 
00144     if (q->wr >= (q->base + q->buffer_size))
00145         q->wr = q->base;
00146 
00147     memcpy((void *)q->wr, data, q->item_size);
00148     q->wr = q->wr + q->item_size;
00149     q->num_item++;
00150     int fifo_size = q->buffer_size / q->item_size;
00151     if (q->num_item > fifo_size)
00152         q->ovf_item = q->num_item - fifo_size;
00153     else
00154         q->ovf_item = 0;
00155     return ret;
00156 }
00157 
00158 int enqueue_test(struct queue_t *q, void *data)
00159 {
00160     if (!q || !data)
00161         return -EINVAL; // Invalid pointer
00162 
00163     q->num_item++;
00164     return 0;
00165 }
00166 
00167 
00168 int dequeue_test(struct queue_t *q, void *data)
00169 {
00170     if (!q || !data)
00171         return -EINVAL;
00172 
00173     if (q->num_item > 0)
00174         q->num_item--;
00175     else
00176         return -2;
00177     return 0;
00178 }
00179 
00180 int dequeue(struct queue_t *q, void *data)
00181 {
00182     int fifo_size = q->buffer_size / q->item_size;
00183 
00184     if (!q || !data)
00185         return -EINVAL;
00186 
00187     if (q->num_item <= 0) {
00188         return -2;
00189     }
00190 
00191     if (q->num_item > fifo_size) {
00192         uint32_t curr_rd_off = ((uint32_t)(q->rd - q->base) + q->num_item * q->item_size);
00193         q->rd = (q->base + (curr_rd_off % q->buffer_size));
00194         q->num_item = fifo_size; // OVF number samples are already gone.
00195 
00196 #ifdef QUEUE_USAGE_STATS
00197         q->pop_cnt++;
00198 
00199         if ((q->pop_cnt % q->stats_period_cnt) == 0) {
00200             if (q->name) {
00201                 pr_info("%s:%d (%s) - %d samples lost, avail:%d \n",__func__, __LINE__, q->name, q->ovf_item, q->num_item);
00202             } else {
00203                 pr_info("%s:%d - %d samples lost, avail:%d \n", __func__, __LINE__, q->ovf_item, q->num_item);
00204             }
00205         }
00206 #endif
00207     }
00208 
00209     if (q->rd >= (q->base + q->buffer_size))
00210         q->rd = q->base;
00211 
00212     memcpy(data, (void *)q->rd, q->item_size);
00213     q->rd = q->rd + q->item_size;
00214     q->num_item--;
00215     if (q->num_item <= fifo_size)
00216         q->ovf_item = 0;
00217 
00218 #if defined(QUEUE_DEBUG)
00219     do {
00220         static int cnt;
00221 
00222         if (cnt++ % 100 == 0) {
00223             if (q->name) {
00224                 pr_debug("%s - $ Fifo size: %d, usage: %d\n", q->name, fifo_size, q->num_item);
00225             } else {
00226                 pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
00227             }
00228         }
00229     } while(0);
00230 #endif
00231 
00232     return 0;
00233 }
00234 
00235 bool queue_is_full(struct queue_t *q)
00236 {
00237     if (!q)
00238         return -EINVAL;
00239 
00240     int num_items = q->buffer_size / q->item_size;
00241 
00242 
00243     if (num_items > q->num_item)
00244         return false;
00245     return true;
00246 }
00247 
00248 int queue_usage(struct queue_t *q, int *total, int *nm_item)
00249 {
00250     if (!q)
00251         return -EINVAL;
00252 
00253     *total = q->buffer_size / q->item_size;
00254     *nm_item = q->num_item;
00255 
00256     return 0;
00257 }
00258 
00259 int queue_pop(struct queue_t *q)
00260 {
00261     int fifo_size = q->buffer_size / q->item_size;
00262 
00263     if (!q)
00264         return -EINVAL;
00265 
00266     if (q->num_item <= 0) {
00267         return -2;
00268     }
00269 
00270     if (q->num_item > fifo_size) {
00271         uint32_t curr_rd_off = ((uint32_t)(q->rd - q->base) + q->num_item * q->item_size);
00272         q->ovf_item = q->num_item - fifo_size;
00273         q->rd = q->base + (curr_rd_off % q->buffer_size);
00274         q->num_item = fifo_size; // OVF number samples are already gone.
00275 
00276 #ifdef QUEUE_USAGE_STATS
00277         q->push_cnt++;
00278 
00279         if ((q->push_cnt % q->stats_period_cnt) == 0) {
00280             if (q->name) {
00281                 pr_info("%s:%d (%s) - %d samples lost, avail:%d \n",__func__, __LINE__, q->name, q->ovf_item, q->num_item);
00282             } else {
00283                 pr_info("%s:%d - %d samples lost, avail:%d \n", __func__, __LINE__, q->ovf_item, q->num_item);
00284             }
00285         }
00286 #endif
00287     } else
00288         q->ovf_item = 0;
00289 
00290     if (q->rd >= (q->base + q->buffer_size))
00291         q->rd = q->base;
00292 
00293     q->rd = q->rd + q->item_size;
00294     q->num_item--;
00295 
00296 #if defined(QUEUE_DEBUG)
00297     do {
00298         static int cnt;
00299 
00300         if (cnt++ % 100 == 0) {
00301             if (q->name)
00302                 pr_debug("%s - $ Fifo size: %d, usage: %d\n", q->name, fifo_size, q->num_item);
00303             else
00304                 pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
00305         }
00306     } while(0);
00307 #endif
00308 
00309     return 0;
00310 }
00311 
00312 int queue_pop_n(struct queue_t *q, int n)
00313 {
00314     int fifo_size = q->buffer_size / q->item_size;
00315 
00316     if (!q || n < 1)
00317         return -EINVAL;
00318 
00319     if (q->num_item < n) {
00320         return -2;
00321     }
00322 
00323     if (q->num_item > fifo_size) {
00324         if (q->name) {
00325             pr_info("%s:%d ( %s ) - %d samples lost, avail:%d \n",
00326                 __func__, __LINE__, q->name, q->num_item - fifo_size, fifo_size);
00327         } else {
00328             pr_info("%s:%d - %d samples lost, avail:%d \n",
00329                 __func__, __LINE__, q->num_item - fifo_size, fifo_size);
00330         }
00331         q->num_item = fifo_size; // OVF number samples are already gone.
00332         n = fifo_size;
00333         uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
00334         q->ovf_item = q->num_item - fifo_size;
00335         q->rd = q->base + curr_rd_off % q->buffer_size;
00336     } else {
00337         q->ovf_item = 0;
00338     }
00339 
00340     if (q->rd >= (q->base + q->buffer_size))
00341         q->rd = q->base;
00342 
00343 
00344     uint32_t rd_sz = q->item_size * n;
00345     uint32_t to_end = (uint32_t)(q->base + q->buffer_size - q->rd);
00346     if (to_end < rd_sz) {
00347         rd_sz -= to_end;
00348         q->rd = q->base;
00349     }
00350 
00351     q->rd = q->rd + rd_sz;
00352     q->num_item -= n;
00353 
00354     return 0;
00355 }
00356 
00357 int queue_front(struct queue_t *q, void *data)
00358 {
00359 #if 0
00360     return queue_front_n(q, data, 1);
00361 #endif
00362 
00363     int fifo_size = q->buffer_size / q->item_size;
00364     void *rd = 0;
00365 
00366     if (!q || !data)
00367         return -EINVAL;
00368 
00369     if (q->num_item <= 0) {
00370         return -2;
00371     }
00372 
00373     if (q->num_item > fifo_size) {
00374         uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
00375         rd = q->base + (curr_rd_off % q->buffer_size);
00376         if (q->name) {
00377             pr_info("%s:%d ( %s )- %d samples lost, avail:%d cap:%d \n",
00378                 __func__, __LINE__, q->name, q->ovf_item, q->num_item, fifo_size);
00379         } else {
00380             pr_info("%s:%d - %d samples lost, avail:%d cap:%d \n",
00381                 __func__, __LINE__, q->ovf_item, q->num_item, fifo_size);
00382         }
00383     } else {
00384         q->ovf_item = 0;
00385         rd = q->rd;
00386     }
00387 
00388     if (q->rd >= (q->base + q->buffer_size))
00389         rd = q->base;
00390 
00391     memcpy(data, (void *)rd, q->item_size);
00392     return 0;
00393 }
00394 
00395 int queue_front_n(struct queue_t *q, void *data, int n, int buf_sz)
00396 {
00397     int fifo_size = q->buffer_size / q->item_size;
00398     char *rd = 0;
00399     char *wr = (char *)data;
00400 
00401     if (!q || !data || n < 1)
00402         return -EINVAL;
00403 
00404     if (q->num_item < n) {
00405         return -2;
00406     }
00407 
00408     if (q->num_item > fifo_size) {
00409         if (q->name) {
00410             pr_info("\n%s:%d ( %s ) - %d samples lost, avail:%d \n",
00411                     __func__, __LINE__, q->name, q->num_item - fifo_size, fifo_size);
00412         } else {
00413             pr_info("\n%s:%d - %d samples lost, avail:%d \n",
00414                 __func__, __LINE__, q->num_item - fifo_size, fifo_size);
00415         }
00416         q->num_item = fifo_size;
00417         n = fifo_size;
00418         uint32_t curr_rd_off = (uint32_t)(q->rd - q->base) + q->num_item * q->item_size;
00419         rd = q->base + (curr_rd_off % q->buffer_size);
00420     } else {
00421         q->ovf_item = 0;
00422         rd = q->rd;
00423     }
00424 
00425     if (q->rd >= (q->base + q->buffer_size))
00426         rd = q->base;
00427 
00428     uint32_t rd_sz = q->item_size * n;
00429 
00430     if (buf_sz < rd_sz) {
00431         if (q->name) {
00432             pr_info("\n%s:%d ( %s ) - Insufficient buffer size: %d\n",
00433                     __func__, __LINE__, q->name, buf_sz);
00434         } else {
00435             pr_info("\n%s:%d - Insufficient buffer size: %d\n",
00436                 __func__, __LINE__, buf_sz);
00437         }
00438         return -EINVAL;
00439     }
00440 
00441     uint32_t to_end = (uint32_t)(q->base + q->buffer_size - q->rd);
00442     if (to_end < rd_sz) {
00443         memcpy(wr, rd, to_end);
00444         rd_sz -= to_end;
00445         rd = q->base;
00446         wr += to_end;
00447         memcpy(wr, rd, rd_sz);
00448 
00449     } else {
00450         memcpy(wr, rd, rd_sz);
00451     }
00452 
00453     return 0;
00454 }
00455 
00456 int enqueue_string(struct queue_t *q, char *data, int sz)
00457 {
00458     int ret = 0;
00459     int buf_index;
00460     char *wr_ptr;
00461 
00462     if (!q || !data || sz <= 0)
00463         return -EFAULT; // Invalid parameters
00464 
00465     if (q->wr == q->rd)
00466         ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
00467 
00468     if (q->wr >= (q->base + q->buffer_size))
00469         q->wr = q->base;
00470 
00471     if ((q->num_item + sz) > q->buffer_size) {
00472 #if defined(QUEUE_DEBUG)
00473         {
00474             char buf[128];
00475             int len;
00476 
00477             if (q->name)
00478                 len = sprintf(buf, "\r\n**** %s - ( %s ) - Fifo is full. num_item: %d, sz: %d, buffer size: %d\r\n",
00479                         __func__, q->name, q->num_item, sz, q->buffer_size);
00480             else
00481                 len = sprintf(buf, "\r\n**** %s - Fifo is full. num_item: %d, sz: %d, buffer size: %d\r\n",
00482                         __func__, q->num_item, sz, q->buffer_size);
00483 
00484             UART_Write(UART_PORT, (uint8_t*)buf, len);
00485         }
00486 #endif
00487         return -ENOMEM;
00488     }
00489 
00490     buf_index = (uint32_t)(q->wr - q->base);
00491     wr_ptr = q->base;
00492     q->num_item += sz;
00493     while(sz--)
00494         wr_ptr[buf_index++ % q->buffer_size] = *data++;
00495 
00496     q->wr = q->base + buf_index % q->buffer_size;
00497     return ret;
00498 }
00499 
00500 int dequeue_string(struct queue_t *q, char *buf, int buffer_size)
00501 {
00502     char *rd_ptr;
00503     int buf_index;
00504     int len;
00505 
00506     if (!q || !buf || buffer_size <= 0)
00507         return -EFAULT;
00508 
00509     if (q->num_item <= 0) {
00510         return -EPERM;
00511     }
00512 
00513     rd_ptr = (char *)q->base;
00514     buf_index = (uint32_t)(q->rd - q->base);
00515     len = q->num_item;
00516 
00517     while (buffer_size-- && q->num_item--) {
00518         char tmp = rd_ptr[buf_index % q->buffer_size];
00519         rd_ptr[buf_index % q->buffer_size] = 0; // Remove this later on
00520         buf_index++;
00521         *buf++ = tmp;
00522         if (tmp == '\0')
00523             break;
00524     }
00525 
00526     if (q->num_item < 0) {
00527         /* Data corruption in FIFO */
00528         q->num_item = 0;
00529     } else
00530         len -= q->num_item;
00531 
00532     q->rd = q->base + buf_index % q->buffer_size;
00533 
00534     return len;
00535 }
00536 
00537 int queue_str_len(struct queue_t *q)
00538 {
00539     char *rd_ptr;
00540     int buf_index;
00541     int len, i;
00542 
00543     if (!q)
00544         return -EFAULT;
00545 
00546     if (q->num_item <= 0) {
00547         return 0;
00548     }
00549 
00550     rd_ptr = q->base;
00551     buf_index = (uint32_t)(q->rd - q->base);
00552     i = q->num_item;
00553     len = 0;
00554 
00555     while (i--) {
00556         char tmp = rd_ptr[buf_index % q->buffer_size];
00557         buf_index++;
00558         if (tmp == '\0')
00559             break;
00560         len++;
00561     }
00562 
00563     return len;
00564 }
00565 
00566 #if 0
00567 void queue_test(void)
00568 {
00569     int ret;
00570     ppg_data_t ppg_test = { 0, };
00571     ppg_data_t ppg_test_out = { 0, };
00572     int i, j, ii, jj;
00573     static ppg_data_t ppg_data[10];
00574     static queue_t queue;
00575 
00576     srand((unsigned)time(NULL));
00577     ret = queue_init(&queue, &ppg_data, sizeof(ppg_data_t), sizeof(ppg_data));
00578     while (1) {
00579         ii = rand() % 20;
00580         for (i = 0; i < ii; i++) {
00581             /* Test data */
00582             ppg_test.timestamp++;
00583             ppg_test.ir++;
00584             ppg_test.red++;
00585             ppg_test.green++;
00586             /* Test functions */
00587             ret = enqueue(&queue, &ppg_test);
00588         }
00589         jj = rand() % 20;
00590         for (j = 0; j < jj; j++) {
00591             ret = dequeue(&queue, &ppg_test_out);
00592         }
00593     }
00594 }
00595 #endif
00596 void queue_n_test(void)
00597 {
00598     struct queue_t q;
00599     uint8_t buf[5];
00600     uint8_t peek_buf[5];
00601     int error;
00602     int i;
00603     error = queue_init(&q, &buf[0], 1, sizeof(buf));
00604     if (error)
00605         printf("queue_init error :(\r\n");
00606 
00607     uint8_t val = 0;
00608     enqueue(&q, &val);
00609     val = 1;
00610     enqueue(&q, &val);
00611     val = 2;
00612     enqueue(&q, &val);
00613     val = 3;
00614     enqueue(&q, &val);
00615     val = 4;
00616     enqueue(&q, &val);
00617 
00618     printf("enqueued 0,1,2,3,4\r\n");
00619 
00620     error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
00621     if (error) {
00622         printf("queue_front_n n=5 error :(\r\n");
00623     } else {
00624         printf("queue_front_n n=5: ");
00625         for (i = 0; i < 5; i++) {
00626             printf("%d ", peek_buf[i]);
00627         }
00628         printf("\r\n");
00629     }
00630 
00631     error = queue_front_n(&q, &peek_buf, 6, sizeof(peek_buf));
00632     if (error)
00633         printf("queue_front_n n=6 error :)\r\n");
00634     else
00635         printf("queue_front_n n=6 succeeded :(\r\n");
00636 
00637     error = queue_pop_n(&q, 2);
00638     if (error)
00639         printf("queue_pop_n n=2 error :(\r\n");
00640     else
00641         printf("queue_pop_n n=2 succeeded :)\r\n");
00642 
00643     error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
00644     if (error)
00645         printf("queue_front_n n=5 error :)\r\n");
00646 
00647     error = queue_front_n(&q, &peek_buf, 3, sizeof(peek_buf));
00648     if (error) {
00649         printf("queue_front_n n=3 error :(\r\n");
00650     } else {
00651         printf("queue_front_n n=3: ");
00652         for (i = 0; i < 3; i++) {
00653             printf("%d ", peek_buf[i]);
00654         }
00655         printf("\r\n");
00656     }
00657 
00658     val = 0;
00659     enqueue(&q, &val);
00660     val = 1;
00661     enqueue(&q, &val);
00662 
00663     printf("enqueued 0,1\r\n");
00664 
00665     error = queue_front_n(&q, &peek_buf, 5, sizeof(peek_buf));
00666     if (error) {
00667         printf("queue_front_n n=5 error :(\r\n");
00668     } else {
00669         printf("queue_front_n n=5: ");
00670         for (i = 0; i < 5; i++) {
00671             printf("%d ", peek_buf[i]);
00672         }
00673         printf("\r\n");
00674     }
00675 
00676     error = queue_pop_n(&q, 4);
00677     if (error)
00678         printf("queue_pop_n n=4 error :(\r\n");
00679     else
00680         printf("queue_pop_n n=4 succeeded :)\r\n");
00681 
00682     error = queue_front_n(&q, &peek_buf, 1, sizeof(peek_buf));
00683     if (error) {
00684         printf("queue_front_n n=1 error :(\r\n");
00685     } else {
00686         printf("queue_front_n n=1: ");
00687         for (i = 0; i < 1; i++) {
00688             printf("%d ", peek_buf[i]);
00689         }
00690         printf("\r\n");
00691     }
00692 }