queue.cpp

00001 /*******************************************************************************
00002 * Author: Ismail Kose, Ismail.Kose@maximintegrated.com
00003 * Copyright (C) 2016 Maxim Integrated Products, Inc., All Rights Reserved.
00004 *
00005 * Permission is hereby granted, free of charge, to any person obtaining a
00006 * copy of this software and associated documentation files (the "Software"),
00007 * to deal in the Software without restriction, including without limitation
00008 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
00009 * and/or sell copies of the Software, and to permit persons to whom the
00010 * Software is furnished to do so, subject to the following conditions:
00011 *
00012 * The above copyright notice and this permission notice shall be included
00013 * in all copies or substantial portions of the Software.
00014 *
00015 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00016 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00017 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00018 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
00019 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
00020 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
00021 * OTHER DEALINGS IN THE SOFTWARE.
00022 *
00023 * Except as contained in this notice, the name of Maxim Integrated
00024 * Products, Inc. shall not be used except as stated in the Maxim Integrated
00025 * Products, Inc. Branding Policy.
00026 *
00027 * The mere transfer of this software does not imply any licenses
00028 * of trade secrets, proprietary technology, copyrights, patents,
00029 * trademarks, maskwork rights, or any other form of intellectual
00030 * property whatsoever. Maxim Integrated Products, Inc. retains all
00031 * ownership rights.
00032 *******************************************************************************
00033 */
00034 
00035 /*
00036  * TODO:
00037  * Add a function to enqueue data block instead of one by one.
00038  * Write function definitions in the header file as doxygen format
00039  * Init function will also allocate memory for queue buffer, providing the buffer will not necessary
00040  *
00041  * */
00042 
00043 #define enter_critical_section()
00044 #define exit_critical_section()
00045 
00046 #include "queue.h"
00047 int queue_reset(struct queue_t *q)
00048 {
00049     if (!q)
00050         return -1;
00051 
00052     q->wr = q->base;
00053     q->rd = q->base;
00054     q->num_item = 0;
00055     q->ovf_item = 0;
00056 
00057     return 0;
00058 }
00059 
00060 int queue_init(struct queue_t *q, void *buf, int item_size, int buffer_size)
00061 {
00062     if (!q || !buf)
00063         return -1;
00064 
00065     if (buffer_size % item_size != 0)
00066         return -1; // Padding problem
00067 
00068     q->num_item = 0;
00069     q->base = buf;
00070     q->wr = buf;
00071     q->rd = buf;
00072     q->item_size = item_size;
00073     q->buffer_size = buffer_size;
00074 
00075     return 0;
00076 }
00077 
00078 void queue_destroy(struct queue_t *q)
00079 {
00080 /* TODO: This is placeholder function, double check the implementation */
00081     free((void *)q->base);
00082     free((void *)q);
00083 }
00084 
00085 int enqueue(struct queue_t *q, void *data)
00086 {
00087     int ret = 0;
00088 
00089     if (!q || !data)
00090         return -1; // Invalid pointer
00091 
00092     enter_critical_section();
00093     if (q->wr == q->rd)
00094         ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
00095 
00096     if (((uint32_t)q->wr) >= ((uint32_t)q->base + q->buffer_size))
00097         q->wr = q->base;
00098 
00099     memcpy((void *)q->wr, data, q->item_size);
00100     q->wr = (void *)((uint32_t)q->wr + q->item_size);
00101     q->num_item++;
00102     exit_critical_section();
00103     return ret;
00104 }
00105 
00106 int dequeue(struct queue_t *q, void *data)
00107 {
00108     int fifo_size;
00109 
00110     if (!q || !data)
00111         return -1;
00112 
00113     fifo_size = q->buffer_size / q->item_size;
00114 
00115     enter_critical_section();
00116     if (q->num_item <= 0) {
00117         exit_critical_section();
00118         return -2;
00119     }
00120 
00121     if (q->num_item > fifo_size) {
00122         uint32_t curr_rd_off = (((uint32_t)q->rd - (uint32_t)q->base) + q->num_item * q->item_size);
00123         q->ovf_item = q->num_item - fifo_size;
00124         q->rd = (void *)((uint32_t)q->base + (curr_rd_off % q->buffer_size));
00125         q->num_item = fifo_size; // OVF number samples are already gone.
00126         printf("%s:%d - %d samples lost\n", __func__, __LINE__, q->ovf_item);
00127     } else
00128         q->ovf_item = 0;
00129 
00130     if (((uint32_t)q->rd) >= ((uint32_t)q->base + q->buffer_size))
00131         q->rd = q->base;
00132 
00133     memcpy(data, (void *)q->rd, q->item_size);
00134     q->rd = (void *)((uint32_t)q->rd + q->item_size);
00135     q->num_item--;
00136     exit_critical_section();
00137 
00138 #if defined(QUEUE_DEBUG)
00139     do {
00140         static int cnt;
00141 
00142         if (cnt++ % 100 == 0)
00143             printf("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
00144     } while(0);
00145 #endif
00146 
00147     return 0;
00148 }
00149