scom_frame_queue.c

Go to the documentation of this file.

#define HAL_LOG_CHANNEL SCOM
#define HAL_LOG_SUBCHANNEL FRAME

#include "hal.h"
#include "scom_frame_queue.h"
//#include "scom_log.h"
#include "scom_crc.h"

#if !defined SCOM_FRAME_QUEUE_LOCK_TIMEOUT
#define SCOM_FRAME_QUEUE_LOCK_TIMEOUT                   5000
#endif

// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Init
// --------------------------------------------------------------------------------------
bool SCOMFrameQueue_Init(SCOMFrameQueue *frameQueue)
{
        size_t i;

        if (frameQueue) {
                frameQueue->lock = OSMUTEX_Create();
                if (frameQueue->lock) {
                        if (frameQueue->pool) {
                                // we use pool
                                for (i = 0; i < frameQueue->size; i++) {
          frameQueue->pool[i] = (SCOMFrameQueueItem ) {
            .next = NULL,
            .state = SCOM_FRAME_EMPTY,
            .frame = {
              .sync = 0xFF,
              .sof = SCOM_SOF_CHARACTER
            }
          };
        }
      }
                        frameQueue->head = NULL;
                        return true;
                }
        }

        return false;

} /* SCOMFrameQueue_Init */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Deinit
// --------------------------------------------------------------------------------------
void SCOMFrameQueue_Deinit(SCOMFrameQueue *frameQueue)
{
        if ((frameQueue) && (frameQueue->lock)) {
                OSMUTEX_Destroy(frameQueue->lock);
        }

} /* SCOMFrameQueue_Deinit */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Insert
// --------------------------------------------------------------------------------------
bool SCOMFrameQueue_Insert(SCOMFrameQueue *frameQueue, SCOMFrameQueueItem *frame)
{
        SCOMFrameQueueItem *currentPosition;
        SCOMFrameQueueItem *prevPosition;
        size_t i;
        uint8_t framePriority;

        if ((frameQueue) && (frame) && (frameQueue->lock)) {

                // acquire exclusive access to the queue
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {

                        if (frameQueue->head == NULL) {
                                // no frames in queue, this will be the first item
                                frameQueue->head = frame;
                                frame->next = NULL;
                                OSMUTEX_Give(frameQueue->lock);
                                return true;
                        }

                        i = frameQueue->size;
                        framePriority = SCOMFrame_GetPriority(&frame->frame);

                        currentPosition = frameQueue->head;
                        prevPosition = NULL;
                        do {
                                if (SCOMFrame_GetPriority(&currentPosition->frame) < framePriority || currentPosition->state == SCOM_FRAME_EMPTY) {
                                        // current element has lower priority than our new item
                                        if (prevPosition) {
                                                // there is a previous item
                                                // so we make the previous item lead to our new one ...
                                                prevPosition->next = frame;
                                                // and the new one lead to the current one
                                                frame->next = currentPosition;
                                        } else {
                                                // there is no previous position,
                                                // so we're inserting the item at the beginning of the queue...
                                                frameQueue->head = frame;
                                                // ..and make it lead to the old head
                                                frame->next = currentPosition;
                                        }
                                        // release exclusive access to the queue
                                        OSMUTEX_Give(frameQueue->lock);
                                        return true;
                                }
                                prevPosition = currentPosition;
                                currentPosition = currentPosition->next;
                                i--;
                        } while ((currentPosition) && (i));

                        // place for the new item has not been found
                        if (i) {
                                // attach item at the end of queue
                                prevPosition->next = frame;
                                frame->next = NULL;
                                // release exclusive access to the queue
                                OSMUTEX_Give(frameQueue->lock);
                                return true;
                        }
                        // release exclusive access to the queue
                        OSMUTEX_Give(frameQueue->lock);
                }
        }

        return false;

} /* SCOMFrameQueue_Insert */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Delete
// --------------------------------------------------------------------------------------
bool SCOMFrameQueue_Delete(SCOMFrameQueue *frameQueue, SCOMFrameQueueItem *frame)
{
        SCOMFrameQueueItem *currentPosition;
        size_t i;

        if ((frameQueue) && (frame) && (frameQueue->lock) && (frameQueue->head)) {

                // acquire exclusive access to the queue
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {

                        if (frameQueue->head == frame){
                                // we're deleting the first item in the queue
                                frameQueue->head = frame->next;
                                frame->next = NULL;
                                // release exclusive access to the queue
                                OSMUTEX_Give(frameQueue->lock);
                                return true;
                        }

                        currentPosition = frameQueue->head;
                        i = frameQueue->size;

                        while(i--) {
                                if(currentPosition->next == frame) {
                                        currentPosition->next = frame->next;
                                        frame->next = NULL;
                                        // release exclusive access to the queue
                                        OSMUTEX_Give(frameQueue->lock);
                                        return true;
                                }
                                currentPosition = currentPosition->next;
                        }
                        // release exclusive access to the queue
                        OSMUTEX_Give(frameQueue->lock);
                }
        }

        return false;

} /* SCOMFrameQueue_Delete */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Append
// --------------------------------------------------------------------------------------
bool SCOMFrameQueue_Append(SCOMFrameQueue *frameQueue, SCOMFrameQueueItem *frame)
{
        if ((frameQueue) && (frame) && (frameQueue->lock)) {
                // acquire exclusive access to the queue
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {

                        frame->next = NULL;

                        if(frameQueue->head == NULL){
                                frameQueue->head = frame;
                                OSMUTEX_Give(frameQueue->lock);
                                return true;
                        } else {
                                SCOMFrameQueueItem * currentPosition = frameQueue->head;
                                while (currentPosition->next != NULL) {
                                        currentPosition = currentPosition->next;
                                }

                                currentPosition->next = frame;
                                // release exclusive access to the queue
                                OSMUTEX_Give(frameQueue->lock);
                                return true;

                        }
                        // release exclusive access to the queue
                        OSMUTEX_Give(frameQueue->lock);
                }
        }
        return false;

} /* SCOMFrameQueue_Append */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_GetFirstInQueue
// --------------------------------------------------------------------------------------
SCOMFrameQueueItem *SCOMFrameQueue_GetFirstInQueue(SCOMFrameQueue *frameQueue)
{
        SCOMFrameQueueItem *result = NULL;
        if (frameQueue) {
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {
                        result = frameQueue->head;
                        OSMUTEX_Give(frameQueue->lock);
                }
        }
        return result;
} /* SCOMFrameQueue_GetFirstInQueue */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_PullOutFirstInQueue
// --------------------------------------------------------------------------------------
SCOMFrameQueueItem *SCOMFrameQueue_PullOutFirstInQueue(SCOMFrameQueue *frameQueue)
{
        SCOMFrameQueueItem* result = NULL;

        if ((frameQueue) && (frameQueue->lock)) {
                // acquire exclusive access to the queue
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {

                        if (frameQueue->head) {
                                result = frameQueue->head;
                                frameQueue->head = frameQueue->head->next;
                        }

                        // release exclusive access to the queue
                        OSMUTEX_Give(frameQueue->lock);
                }
        }

        return result;

} /* SCOMFrameQueue_PullOutFirstInQueue */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Lock
// --------------------------------------------------------------------------------------
SCOMFrameQueueItem *SCOMFrameQueue_Lock(SCOMFrameQueue *frameQueue)
{
        size_t i;
        SCOMFrameQueueItem *result = NULL;

        if ((frameQueue) && (frameQueue->lock)) {

                // acquire exclusive access to the queue
                HAL_ASSERT_AND_EXECUTE((0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT))) {

                        if (frameQueue->pool) {
                                // we have a pool to use
                                for(i = 0; i < frameQueue->size; i++) {
                                        if(frameQueue->pool[i].state == SCOM_FRAME_EMPTY){
                                                frameQueue->pool[i].state = SCOM_FRAME_ALLOCATED;
                                                result = &frameQueue->pool[i];
                                                result->frame.size = 0;
                                                result->frame.ctrl = 0;
                                                break;
                                        }
                                }
                        } else {
#if !defined SCOM_DO_NOT_USE_HEAP
                                // no pool, we use heap memory
                                result = (SCOMFrameQueueItem*)HEAP_Alloc(sizeof(SCOMFrameQueueItem));
                                if (result) {
                                        result->frame.sync = 0xFF;
                                        result->frame.sof = SCOM_SOF_CHARACTER;
                                        result->frame.size = 0;
                                        result->frame.ctrl = 0;
                                        result->state = SCOM_FRAME_ALLOCATED;
                                        result->next = NULL;

                                        // in case of heap-based frame queue we make the queue grow dynamically
                                        frameQueue->size++;
                                }
#endif // SCOM_DO_NOT_USE_HEAP
                        }
                        // release exclusive access to the queue
                        OSMUTEX_Give(frameQueue->lock);
                }

        }

        return result;

} /* SCOMFrameQueue_Lock */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_Unlock
// --------------------------------------------------------------------------------------
void SCOMFrameQueue_Unlock(SCOMFrameQueue *frameQueue, SCOMFrameQueueItem* frameQueueItem)
{
        // if needed - delete the item from the queue
        SCOMFrameQueue_Delete(frameQueue, frameQueueItem);

        if (frameQueue) {
                if (frameQueue->pool) {
                        // we have a pool to use
                        // mark the item as free to use
                        frameQueueItem->state = SCOM_FRAME_EMPTY;
                } else {
#if !defined SCOM_DO_NOT_USE_HEAP
                        // no pool, heap memory was used
                        HEAP_Free(frameQueueItem);
                        // in case of heap-based frame queue we make the queue shring dynamically
                        if (frameQueue->size) {
                                frameQueue->size--;
                        }
#endif
                }
        }

} /* SCOMFrameQueue_Unlock */


// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_GetItemByFrame
// --------------------------------------------------------------------------------------
SCOMFrameQueueItem * SCOMFrameQueue_GetItemByFrame(const SCOMFrameQueue *frameQueue, const SCOMFrame* frame)
{
        size_t i;

        // Note: exclusive access to the queue is not needed here, as we do not modify anything and the
        //       data layout is constant

        if (frameQueue->pool) {
                // we use pool
                for (i = 0; i < frameQueue->size; i++) {
                        if (&(frameQueue->pool[i].frame) == frame) {
                                //match!
                                return &frameQueue->pool[i];
                        }
                }
        } else {
                // we use heap
                return (SCOMFrameQueueItem*)frame;
        }

        return NULL;

} /* SCOMFrameQueue_GetItemByDataPtr */

// --------------------------------------------------------------------------------------
//  SCOMFrameQueue_GetFreeSpace
// --------------------------------------------------------------------------------------
size_t SCOMFrameQueue_GetFreeSpace(const SCOMFrameQueue *frameQueue)
{
        size_t i;
        size_t emptyFrameCounter = 0;

        if ((frameQueue) && (frameQueue->lock)) {

                if (frameQueue->pool) {
                        // we use pool
                        // acquire exclusive access to the queue
                        if (0 == OSMUTEX_Take(frameQueue->lock, SCOM_FRAME_QUEUE_LOCK_TIMEOUT)) {

                                for(i = 0; i < frameQueue->size; i++) {
                                        if(frameQueue->pool[i].state == SCOM_FRAME_EMPTY){
                                                emptyFrameCounter++;
                                        }
                                }

                                // release exclusive access to the queue
                                OSMUTEX_Give(frameQueue->lock);
                        }
                } else {
                        // we use heap
                        return 1;
                }
        }

        return emptyFrameCounter;

} /* SCOMFrameQueue_GetFreeSpace */