chg-stn-motherboard-ti-mcu/ivec_cmplx_queue.c

#include "ivec_cmplx_queue.h"
#include "stdint.h"
#include "string.h"
//#include "ivec_bsw_common.h"

// #define IVEC_BSW_LOG(LOG_STRING,msg, ...) 
#define LOG_STRING          "ivec-cmplx-queue"
#define FIXED_EXTRA_SIZE 8


/**
 * @brief Search for character in circular pu8Buffer and it does not return if null character is found
 *
 * @param start start address of pu8Buffer
 * @param max size of pu8Buffer
 * @param cur_idx start point to search from
 * @param chr character to search
 * @param len total length to search
 * @return uint8_t* address of found character else NULL
 */
uint8_t* __prvCMPLX_Strnchr(uint8_t* start, int max, int cur_idx, uint8_t* substring, int substring_len, int len)
{
    if (cur_idx<0 || cur_idx>max)
        return NULL;
    uint8_t* string = start;
    int i = 0;
    while (len--)
    {
        if ((cur_idx + i) >= max)
        {
            i = 0;
            cur_idx = 0;
        }
        if (string[cur_idx + i] == substring[0])
        {
            int l_sub_string_found = 0;
            int cur_i_idx = cur_idx + i + 1;
            for (int j = 1;j < substring_len;j++)
            {
                l_sub_string_found = 1;
                if (cur_i_idx >= max)
                {
                    cur_i_idx = 0;
                }
                if (string[cur_i_idx] != substring[j])
                {
                    l_sub_string_found = 0;
                    break;
                }
                cur_i_idx++;

            }
            if (l_sub_string_found)
                return &string[cur_idx + i];
        }
        i++;
    }
    return NULL;
}

/**
 * @brief returns if queue is full or not
 *
 * @param queue queue handle
 * @return uint32_t
 *          if i32QueueType==FIXED_ELEMENT_SIZE_QUEUE
 *              true if size is full else false
 *          else if i32QueueType==NONFIXED_ELEMENT_SIZE_QUEUE
 *              remaining size
 */
int i32CMPLX_FifoQueueFull(CmplxFifoQueueHandle_s* queue)
{
    if (queue->u8Init == 0)
        return 0;
    if (queue->i32QueueType == NONFIXED_ELEMENT_SIZE_QUEUE)
    {
        if (queue->i32Rear == queue->i32Front)
            return queue->i32Filled == 0 ? (queue->i32TotalElements - (2 * queue->u8DelimeterLen + FIXED_EXTRA_SIZE)) : 0;
        else
        {
            return (queue->i32Front > queue->i32Rear) ? ((queue->i32TotalElements - queue->i32Front) + (queue->i32Rear) - (2 * queue->u8DelimeterLen + FIXED_EXTRA_SIZE)) : (queue->i32Rear - queue->i32Front - (2 * queue->u8DelimeterLen + FIXED_EXTRA_SIZE));
        }
    }
    else
        return queue->i32Filled == queue->i32TotalElements ? 1 : 0;
}
/**
 * @brief checks if queue is empty or not
 *
 * @param queue queue handle
 * @return uint8_t 1 if queue is empty else 0
 */
uint8_t u8CMPLX_FifoQueueEmpty(CmplxFifoQueueHandle_s* queue)
{
    if (queue->u8Init == 0)
        return 0;
    return queue->i32Filled == 0 ? 1 : 0;
}
/**
 * @brief returs total elements present
 *
 * @param queue
 * @return int
 */
int i32CMPLX_FifoCounts(CmplxFifoQueueHandle_s* queue)
{
    if (queue->u8Init == 0)
        return 0;
    return queue->i32Filled;
}
/**
 * @brief appends data to queue with circular implementation for fixed and non fixed element size
 *
 * @param queue handle
 * @param data pu8Buffer to store from
 * @param a_len total length to store (if i32QueueType==NONFIXED_ELEMENT_SIZE_QUEUE)
 * @return uint8_t 1 success 0 fail
 */
uint8_t u8CMPLX_FifoEnqueue(CmplxFifoQueueHandle_s* queue, void* data, int a_len)
{
    if (queue->u8Init == 0 || data == NULL || a_len == 0)
        return 0;
    uint8_t* l_src = data;
    uint8_t* l_dest = &queue->pu8Buffer[queue->i32Front * queue->i32ElementSize];
    int l_len = queue->i32ElementSize;
    int l_tail_remaining = 0;
    if (queue->i32QueueType == NONFIXED_ELEMENT_SIZE_QUEUE)
    {
        l_len = a_len;
        if (i32CMPLX_FifoQueueFull(queue) <= l_len)
            return 0;
        for (int i = 0;i < queue->u8DelimeterLen;i++)
        {
            if (queue->i32Front >= queue->i32TotalElements)
            {
                queue->i32Front = 0;
            }
            queue->pu8Buffer[queue->i32Front++] = queue->u8Startdelimeter[i];
        }
        for (int i = 0;i < 4;i++)
        {
            if (queue->i32Front >= queue->i32TotalElements)
            {
                queue->i32Front = 0;
            }
            queue->pu8Buffer[queue->i32Front++] = (a_len >> (i * 8)) & 0xff;
        }
        if (queue->i32Front >= queue->i32TotalElements)
        {
            queue->i32Front = 0;
        }
        else if (a_len > (queue->i32TotalElements - queue->i32Front))
        {
            l_tail_remaining = (queue->i32TotalElements - queue->i32Front);
            memcpy(&queue->pu8Buffer[queue->i32Front], l_src, l_tail_remaining);
            queue->i32Front = 0;
        }
        if (a_len - l_tail_remaining > 0)
        {
            memcpy(&queue->pu8Buffer[queue->i32Front], (uint8_t*)(l_src + l_tail_remaining), a_len - l_tail_remaining);
            queue->i32Front += (a_len - l_tail_remaining);
        }

        for (int i = 0;i < 4;i++)
        {
            if (queue->i32Front >= queue->i32TotalElements)
            {
                queue->i32Front = 0;
            }
            queue->pu8Buffer[queue->i32Front++] = (a_len >> (i * 8)) & 0xff;
        }
        for (int i = 0;i < queue->u8DelimeterLen;i++)
        {
            if (queue->i32Front >= queue->i32TotalElements)
            {
                queue->i32Front = 0;
            }
            queue->pu8Buffer[queue->i32Front++] = queue->u8EndDelimeter[i];
        }
        queue->i32Filled++;
        queue->i32RemainingSize = i32CMPLX_FifoQueueFull(queue);
        // queue->i32RemainingSize %= (queue->i32TotalElements);
        // queue->i32Front %= (queue->i32TotalElements);


    }
    else {
        if(queue->i32Filled>=queue->i32TotalElements)
            return 0;
        memcpy(l_dest, l_src, queue->i32ElementSize);
        queue->i32Front++;
        queue->i32Front %= (queue->i32TotalElements);
        queue->i32Filled++;

    }

    return 1;
}
/**
 * @brief extarcts data from queue with circular implementation for fixed and non fixed element size
 *
 * @param queue handle
 * @param data pu8Buffer to store to
 * @param a_len length extracted (if i32QueueType==NONFIXED_ELEMENT_SIZE_QUEUE)
 * @param dump_element 1: dump element (useful if element not required)
 * @return uint8_t 1:success 0: fail
 */
uint8_t u8CMPLX_FifoDequeue(CmplxFifoQueueHandle_s* queue, void* data, int* a_len, uint8_t dump_element)
{
    if (queue->u8Init == 0 || (data == NULL && dump_element == 0) || a_len == NULL)
        return 0;
    *a_len = 0;
    uint8_t* l_src = &queue->pu8Buffer[queue->i32Rear * queue->i32ElementSize];
    uint8_t* l_dest = data;
    // int l_len = queue->i32ElementSize;
    if (queue->i32QueueType == NONFIXED_ELEMENT_SIZE_QUEUE)
    {
        if (u8CMPLX_FifoQueueEmpty(queue))
            return 0;
        int l_queue_i32Rear = queue->i32Rear;
        if (l_queue_i32Rear >= queue->i32TotalElements)
            l_queue_i32Rear = 0;
        uint8_t* l_start = __prvCMPLX_Strnchr(&queue->pu8Buffer[0], queue->i32TotalElements, l_queue_i32Rear, queue->u8Startdelimeter, queue->u8DelimeterLen, queue->i32TotalElements);
        if (l_start == NULL)
            return 2;
        if (l_start - &queue->pu8Buffer[0] != l_queue_i32Rear)
        {
            //IVEC_BSW_LOG(LOG_STRING,"Anomly found %d-%d\n", l_queue_i32Rear, l_start - &queue->pu8Buffer[0]);
        }
        l_queue_i32Rear = l_start - &queue->pu8Buffer[0];
        for (int i = 0;i < queue->u8DelimeterLen;i++)
        {
            l_queue_i32Rear++;
            if (l_queue_i32Rear >= queue->i32TotalElements)
                l_queue_i32Rear = 0;
        }
        int s_len = 0, e_len = 0;
        for (int i = 0;i < 4;i++)
        {
            s_len |= (queue->pu8Buffer[l_queue_i32Rear] << i * 8);
            l_queue_i32Rear++;
            if (l_queue_i32Rear >= queue->i32TotalElements)
                l_queue_i32Rear = 0;
        }
        if (s_len == 0)
            return 3;

        uint8_t* l_end = NULL;
        int l_while_exit = 0;
        while (1)
        {
            e_len = 0;
            l_end = __prvCMPLX_Strnchr(&queue->pu8Buffer[0], queue->i32TotalElements, (l_end == NULL ? l_queue_i32Rear : (l_end - &queue->pu8Buffer[0] + 1)), queue->u8EndDelimeter, queue->u8DelimeterLen, queue->i32TotalElements);
            if (l_end == NULL)
            {
                return 4;
            }
            int l_queue_i32Rear_temp = ((l_end - &queue->pu8Buffer[0] < 4) ? queue->i32TotalElements : 0) + (l_end - &queue->pu8Buffer[0]) - 4;
            for (int i = 0;i < 4;i++)
            {
                e_len |= (queue->pu8Buffer[l_queue_i32Rear_temp] << i * 8);
                l_queue_i32Rear_temp++;
                if (l_queue_i32Rear_temp >= queue->i32TotalElements)
                    l_queue_i32Rear_temp = 0;
            }
            l_while_exit++;
            if (l_while_exit > 100)
            {
                //IVEC_BSW_LOG(LOG_STRING,"return from while %d-%d-%d", s_len, e_len, l_queue_i32Rear_temp);
                return 5;
            }
            if (s_len != e_len)
            {
                //IVEC_BSW_LOG(LOG_STRING,"Invalid Length %d-%d-%d", s_len, e_len, l_queue_i32Rear_temp);
                continue;
            }

            break;
        }
        int len1 = ((queue->i32TotalElements - l_queue_i32Rear) < s_len) ? (queue->i32TotalElements - l_queue_i32Rear) : s_len;
        int len2 = s_len - len1;

        if (len1)
        {
            if (!dump_element)
                memcpy(data, &queue->pu8Buffer[l_queue_i32Rear], len1);
            l_queue_i32Rear += (len1 - 1);
        }
        if (len2)
        {
            if (!dump_element)
                memcpy((uint8_t*)data + len1, &queue->pu8Buffer[0], len2);
            l_queue_i32Rear = (len2 - 1);
        }


        for (int i = 0;i < queue->u8DelimeterLen + 4 + 1;i++)
        {
            l_queue_i32Rear++;
            if (l_queue_i32Rear >= queue->i32TotalElements)
                l_queue_i32Rear = 0;
        }
        *a_len = s_len;
         //l_len = s_len;
        //IVEC_BSW_LOG(LOG_STRING,"deque:%d-%d-%d\n", l_start - queue->pu8Buffer, l_end - queue->pu8Buffer, s_len);
        queue->i32Rear = l_queue_i32Rear;

        queue->i32Filled--;
        queue->i32RemainingSize = i32CMPLX_FifoQueueFull(queue);
        //  queue->i32RemainingSize %= queue->i32TotalElements;
        //  queue->i32Rear %= (queue->i32TotalElements);
    }
    else {
        if(queue->i32Filled<=0)
            return 0;
        if (!dump_element)
            memcpy(l_dest, l_src, queue->i32ElementSize);
        queue->i32Rear++;
        queue->i32Rear %= (queue->i32TotalElements);
        queue->i32Filled--;
    }


    return 1;
}
/**
 * @brief initilises queue
 *
 * @param queue
 * @return uint8_t 1:success 0:fail
 */
uint8_t u8CMPLX_FifoQueueInit(CmplxFifoQueueHandle_s* queue)
{
    if (queue == NULL || queue->pu8Buffer == NULL)
    {
        queue->u8Init = 0;
        return 0;
    }
    if (queue->i32QueueType == NONFIXED_ELEMENT_SIZE_QUEUE)
        queue->i32ElementSize = 1;
    queue->i32Rear = 0;
    queue->i32Front = 0;
    queue->i32RemainingSize = queue->i32TotalElements;
    queue->i32Filled = 0;
    queue->u8Init = 1;
    
   // IVEC_BSW_LOG(LOG_STRING,"Initlising queue with size %lu-%lu-%d-%c-%c\n", queue->i32ElementSize, queue->i32TotalElements, queue->i32QueueType, queue->u8Startdelimeter, queue->u8EndDelimeter);

    return 1;
}
void vCMPLX_FifoQueueFlush(CmplxFifoQueueHandle_s* queue)
{
    //IVEC_BSW_LOG(LOG_STRING,"Flushing queue\n");
    if(queue!=NULL && queue->pu8Buffer!=NULL)
        memset(queue->pu8Buffer, 0, queue->i32TotalElements*queue->i32ElementSize);
    queue->i32Rear = 0;
    queue->i32Front = 0;
    queue->i32RemainingSize = queue->i32TotalElements;
    queue->i32Filled = 0;
}