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 #include "queue.h"
00044 #include "mbed.h"
00045 #include "Peripherals.h"
00046 
00047 int queue_reset(struct queue_t *q)
00048 {
00049     if (!q)
00050         return E_INVALID;
00051 
00052     __disable_irq();
00053     q->wr = q->base;
00054     q->rd = q->base;
00055     q->num_item = 0;
00056     q->ovf_item = 0;
00057     __enable_irq();
00058     return 0;
00059 }
00060 
00061 int queue_len(struct queue_t *q)
00062 {
00063     int num_elements;
00064 
00065     if (!q)
00066         return E_INVALID;
00067 
00068     __disable_irq();
00069     num_elements = q->num_item;
00070     __enable_irq();
00071 
00072     return num_elements;
00073 }
00074 
00075 
00076 int queue_init(struct queue_t *q, void *buf, int item_size, int buffer_size)
00077 {
00078     if (!q || !buf)
00079         return E_INVALID;
00080 
00081     if (buffer_size % item_size != 0)
00082         return E_INVALID; // Padding problem
00083 
00084     __disable_irq();
00085     q->num_item = 0;
00086     q->ovf_item = 0;
00087     q->base = buf;
00088     q->wr = buf;
00089     q->rd = buf;
00090     q->item_size = item_size;
00091     q->buffer_size = buffer_size;
00092     __enable_irq();
00093     return 0;
00094 }
00095 
00096 void queue_destroy(struct queue_t *q)
00097 {
00098 /* TODO: This is placeholder function, double check the implementation */
00099     free((void *)q->base);
00100     free((void *)q);
00101 }
00102 
00103 int enqueue(struct queue_t *q, void *data)
00104 {
00105     int ret = 0;
00106 
00107     if (!q || !data)
00108         return E_INVALID; // Invalid pointer
00109 
00110     __disable_irq();
00111     if (q->wr == q->rd)
00112         ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
00113 
00114     if (((uint32_t)q->wr) >= ((uint32_t)q->base + q->buffer_size))
00115         q->wr = q->base;
00116 
00117     memcpy((void *)q->wr, data, q->item_size);
00118     q->wr = (void *)((uint32_t)q->wr + q->item_size);
00119     q->num_item++;
00120     __enable_irq();
00121     return ret;
00122 }
00123 
00124 int dequeue(struct queue_t *q, void *data)
00125 {
00126     int fifo_size = q->buffer_size / q->item_size;
00127 
00128     if (!q || !data)
00129         return E_INVALID;
00130 
00131     __disable_irq();
00132     if (q->num_item <= 0) {
00133         __enable_irq();
00134         return -2;
00135     }
00136 
00137     if (q->num_item > fifo_size) {
00138         uint32_t curr_rd_off = (((uint32_t)q->rd - (uint32_t)q->base) + q->num_item * q->item_size);
00139         q->ovf_item = q->num_item - fifo_size;
00140         q->rd = (void *)((uint32_t)q->base + (curr_rd_off % q->buffer_size));
00141         q->num_item = fifo_size; // OVF number samples are already gone.
00142         pr_info("%s:%d - %d samples lost, avail:%d \n",
00143                 __func__, __LINE__, q->ovf_item, q->num_item);
00144     } else
00145         q->ovf_item = 0;
00146 
00147     if (((uint32_t)q->rd) >= ((uint32_t)q->base + q->buffer_size))
00148         q->rd = q->base;
00149 
00150     memcpy(data, (void *)q->rd, q->item_size);
00151     q->rd = (void *)((uint32_t)q->rd + q->item_size);
00152     q->num_item--;
00153     __enable_irq();
00154 
00155 #if defined(QUEUE_DEBUG)
00156     do {
00157         static int cnt;
00158 
00159         if (cnt++ % 100 == 0)
00160             pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
00161     } while(0);
00162 #endif
00163 
00164     return 0;
00165 }
00166 
00167 int queue_usage(struct queue_t *q, int *total, int *nm_item)
00168 {
00169     if (!q)
00170         return E_INVALID;
00171 
00172     *total = q->buffer_size / q->item_size;
00173     *nm_item = q->num_item;
00174 
00175     return 0;
00176 }
00177 
00178 int queue_pop(struct queue_t *q)
00179 {
00180     int fifo_size = q->buffer_size / q->item_size;
00181 
00182     if (!q)
00183         return E_INVALID;
00184 
00185     __disable_irq();
00186     if (q->num_item <= 0) {
00187         __enable_irq();
00188         return -2;
00189     }
00190 
00191     if (q->num_item > fifo_size) {
00192         uint32_t curr_rd_off = (((uint32_t)q->rd - (uint32_t)q->base) + q->num_item * q->item_size);
00193         q->ovf_item = q->num_item - fifo_size;
00194         q->rd = (void *)((uint32_t)q->base + (curr_rd_off % q->buffer_size));
00195         q->num_item = fifo_size; // OVF number samples are already gone.
00196         pr_info("%s:%d - %d samples lost, avail:%d \n",
00197                 __func__, __LINE__, q->ovf_item, q->num_item);
00198     } else
00199         q->ovf_item = 0;
00200 
00201     if (((uint32_t)q->rd) >= ((uint32_t)q->base + q->buffer_size))
00202         q->rd = q->base;
00203 
00204     q->rd = (void *)((uint32_t)q->rd + q->item_size);
00205     q->num_item--;
00206     __enable_irq();
00207 
00208 #if defined(QUEUE_DEBUG)
00209     do {
00210         static int cnt;
00211 
00212         if (cnt++ % 100 == 0)
00213             pr_debug("$ Fifo size: %d, usage: %d\n", fifo_size, q->num_item);
00214     } while(0);
00215 #endif
00216 
00217     return 0;
00218 }
00219 
00220 int queue_front(struct queue_t *q, void *data)
00221 {
00222     int fifo_size = q->buffer_size / q->item_size;
00223     void *rd = 0;
00224 
00225     if (!q || !data)
00226         return E_INVALID;
00227 
00228     __disable_irq();
00229     if (q->num_item <= 0) {
00230         __enable_irq();
00231         return -2;
00232     }
00233 
00234     if (q->num_item > fifo_size) {
00235         uint32_t curr_rd_off = (((uint32_t)q->rd - (uint32_t)q->base) + q->num_item * q->item_size);
00236         rd = (void *)((uint32_t)q->base + (curr_rd_off % q->buffer_size));
00237         pr_info("%s:%d - %d samples lost, avail:%d \n",
00238                 __func__, __LINE__, q->ovf_item, q->num_item);
00239     } else {
00240         q->ovf_item = 0;
00241         rd = q->rd;
00242     }
00243 
00244     if (((uint32_t)q->rd) >= ((uint32_t)q->base + q->buffer_size))
00245         rd = q->base;
00246 
00247     memcpy(data, (void *)rd, q->item_size);
00248     __enable_irq();
00249 
00250     return 0;
00251 }
00252 
00253 int enqueue_string(struct queue_t *q, char *data, int sz)
00254 {
00255     int ret = 0;
00256     int buf_index;
00257     char *wr_ptr;
00258 
00259     if (!q || !data || sz <= 0)
00260         return E_UNKNOWN; // Invalid parameters
00261 
00262     __disable_irq();
00263     if (q->wr == q->rd)
00264         ret = (q->num_item != 0) ? -2 : 0; // Is FIFO Full or Empty?
00265 
00266     if (((uint32_t)q->wr) >= ((uint32_t)q->base + q->buffer_size))
00267         q->wr = q->base;
00268 
00269     if ((q->num_item + sz) > q->buffer_size) {
00270         __enable_irq();
00271 #if defined(QUEUE_DEBUG)
00272         {
00273             char buf[128];
00274             int len;
00275             len = sprintf(buf, "\r\n**** %s - Fifo is full. num_item: %d, sz: %d, buffer size: %d\r\n",
00276                     __func__, q->num_item, sz, q->buffer_size);
00277             UART_Write(UART_PORT, (uint8_t*)buf, len);
00278         }
00279 #endif
00280         return E_UNKNOWN;
00281     }
00282 
00283     buf_index = (uint32_t)q->wr - (uint32_t)q->base;
00284     wr_ptr = (char *)q->base;
00285     q->num_item += sz;
00286     while(sz--)
00287         wr_ptr[buf_index++ % q->buffer_size] = *data++;
00288 
00289     q->wr = (void *)((uint32_t)q->base + buf_index % q->buffer_size);
00290     __enable_irq();
00291     return ret;
00292 }
00293 
00294 int dequeue_string(struct queue_t *q, char *buf, char delimiter, int buffer_size)
00295 {
00296     char *rd_ptr;
00297     int buf_index;
00298     int len;
00299 
00300     if (!q || !buf || buffer_size <= 0)
00301         return E_UNKNOWN;
00302 
00303     __disable_irq();
00304     if (q->num_item <= 0) {
00305         __enable_irq();
00306         return E_UNKNOWN;
00307     }
00308 
00309     rd_ptr = (char *)q->base;
00310     buf_index = (uint32_t)q->rd - (uint32_t)q->base;
00311     len = q->num_item;
00312 
00313     while (buffer_size-- && q->num_item--) {
00314         char tmp = rd_ptr[buf_index % q->buffer_size];
00315         rd_ptr[buf_index % q->buffer_size] = 0; // Remove this later on
00316         buf_index++;
00317         *buf++ = tmp;
00318         if (tmp == delimiter)
00319             break;
00320     }
00321 
00322     if (q->num_item < 0) {
00323         /* Data corruption in FIFO */
00324         q->num_item = 0;
00325     } else
00326         len -= q->num_item;
00327 
00328     q->rd = (void *)((uint32_t)q->base + buf_index % q->buffer_size);
00329     __enable_irq();
00330 
00331     return len;
00332 }
00333 
00334 
00335 
00336 #if 0
00337 void queue_test(void)
00338 {
00339     int ret;
00340     ppg_data_t ppg_test = { 0, };
00341     ppg_data_t ppg_test_out = { 0, };
00342     int i, j, ii, jj;
00343     static ppg_data_t ppg_data[10];
00344     static queue_t queue;
00345 
00346     srand((unsigned)time(NULL));
00347     ret = queue_init(&queue, &ppg_data, sizeof(ppg_data_t), sizeof(ppg_data));
00348     while (1) {
00349         ii = rand() % 20;
00350         for (i = 0; i < ii; i++) {
00351             /* Test data */
00352             ppg_test.timestamp++;
00353             ppg_test.ir++;
00354             ppg_test.red++;
00355             ppg_test.green++;
00356             /* Test functions */
00357             ret = enqueue(&queue, &ppg_test);
00358         }
00359         jj = rand() % 20;
00360         for (j = 0; j < jj; j++) {
00361             ret = dequeue(&queue, &ppg_test_out);
00362         }
00363     }
00364 }
00365 #endif