Queue and Message

#ifndef __QUEUE_H__
#define __QUEUE_H__

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

/*
 * Queues can have more than one producer but only one consumer.
 * This means that more than one task or interrupt handler is allowed to store
 * new data in the queue but only one task is allowed toget data from the queue.
 *
 * Queues accept messages of various size. When putting a message into a queue,
 * the message size is passed as a parameter.
 *
 * Retrieving a message from the queue does not copy the message, but returns
 * a pointer to the message and its size. Thisenhances performance because the
 * data is copied only once, when the message is written into the queue.
 *
 * The retrieving function has to delete every message after processing it.
 * A new message can only be retrieved from the queue when the previous message
 * was deleted from the queue.
 *
 * |---------------------- size -------------------------|
 * |        |------------ msgCnt ---------------|        |
 * [ .... ] [ size : message ] [ size : message ] [ .... ]
 * |        |                                     |
 * |pData   |offsetFirst                          |offsetLast
 *
 */
typedef struct TAG_QUEUE
{
  uint8_t * Memory;
  uint32_t Capacity;
  uint32_t MessageCount;
  uint32_t ReadIndex;
  uint32_t WriteIndex;
  uint32_t IsUsing;
  uint32_t InProgressCount;
} QUEUE;

// Creates and initializes a message queue.
QUEUE * Q_Create( uint32_t Capacity );

// Deletes a specific queue.
void Q_Delete( QUEUE * Queue );

// Initializes a message queue.
void Q_Init( QUEUE * Queue, uint8_t * Memory, uint32_t Capacity );

// Deletes the last retrieved message in a queue.
void Q_Purge( QUEUE * Queue );

// Deletes all message in a queue.
void Q_Clear( QUEUE * Queue );

// Returns the number of messages currently in a queue
uint32_t Q_GetCount( QUEUE * Queue );

// Returns the first message size
uint32_t Q_GetSize( QUEUE * Queue );

// Delivers information whether the queue is actually in use.
// A queue must not be cleared or deleted when it is in use.
uint32_t Q_IsUsing( QUEUE * Queue );

// Stores a new message of given size in a queue.
uint32_t Q_Wirte( QUEUE * Queue, void * Message, uint32_t Size );

// Retrieves a message from a queue
uint32_t Q_Read( QUEUE * Queue, void ** Message );

#endif /* __QUEUE_H__ */

#include "queue.h"

#include "cmsis_os.h"
#include "macro_misc.h"

// Creates and initializes a message Queue.
QUEUE * Q_Create( uint32_t Capacity )
{
  uint32_t Size = ALIGN_UP( sizeof(QUEUE),  ) + ALIGN_UP( Capacity,  );
  QUEUE *Queue = (QUEUE *) osMalloc( Size, osWaitForever );
  if ( Queue == NULL )
    return NULL;

  uint8_t * Memory = //
    (uint8_t *) ( ( (uint32_t) ( Queue ) ) + ALIGN_UP( sizeof(QUEUE),  ) );

  Q_Init( Queue, Memory, ALIGN_UP( Capacity,  ) );

  return Queue;
}

// Deletes a specific Queue.
// A Queue must not be cleared or deleted when it is in use.
void Q_Delete( QUEUE * Queue )
{
  if ( Queue->IsUsing ==  )
    osFree( Queue );
}

// Deletes all messages in a Queue.
// A Queue must not be cleared or deleted when it is in use.
void Q_Clear( QUEUE * Queue )
{
  if ( Queue->IsUsing ==  )
    Queue->MessageCount = ;
}

// Initializes a message Queue.
void Q_Init( QUEUE * Queue, uint8_t * Memory, uint32_t Capacity )
{
  int32_t Delta = (uint32_t) Memory & ;
  if ( Delta )
  {
    Delta -= ;
    Capacity += Delta;
    Memory -= Delta;
  }
  memset( Queue, , sizeof(QUEUE) );
  Queue->Capacity = Capacity;
  Queue->Memory = Memory;
}

// Returns the number of messages currently in a Queue
uint32_t Q_GetCount( QUEUE * Queue )
{
  return Queue->MessageCount - Queue->InProgressCount;
}

// Returns the first message size
uint32_t Q_GetSize( QUEUE * Queue )
{
  uint32_t MessageSize = ;
  if ( Queue->MessageCount )
    MessageSize = *(uint32_t *) ( &Queue->Memory[ Queue->ReadIndex ] );
  return MessageSize;
}

// Delivers information whether the Queue is actually in use.
// A Queue must not be cleared or deleted when it is in use.
uint32_t Q_IsUsing( QUEUE * Queue )
{
  return Queue->IsUsing;
}

// Stores a new message of given size in a Queue.
// 0 : Queue could not be stored (Queue is full).
// 1 : Success; message stored.
uint32_t Q_Write( QUEUE * Queue, void * Message, uint32_t Size )
{
  uint32_t ReadIndexVal;
  uint32_t WriteIndexPending;
  uint32_t WriteIndexVal;
  uint32_t MessageSize =  + ALIGN_UP( Size,  );
  int32_t * Memory = (int32_t *) Queue->Memory;

  uint32_t Value = osDisableInterrupt( );

  if ( Queue->MessageCount ==  )
  {
    // read next message from head of memory
    Queue->ReadIndex = ;

    // Queue could not be stored (memory is full).
    WriteIndexVal = -;

    if ( Queue->Capacity >= MessageSize )
      WriteIndexVal = ;
  }
  else
  {
    Memory = (int32_t *) Queue->Memory;
    WriteIndexPending = Queue->WriteIndex;
    int32_t SizePending = Memory[ WriteIndexPending ];

    if ( SizePending <  )
    {
      // other task is writting ... but it is preemptived by our task
      // WriteIndexPending has been updated
      // [ Last Queue ] [ --- Other Queue --- ] [ Our Mesage ]
      //                  | WriteIndexPending
      SizePending = -SizePending;
    }
    else
    {
      // [ Last Queue ] [ Our Mesage ]
      //                  | WriteIndexPending
    }

    // where our task will write ...
    WriteIndexVal = WriteIndexPending +  + ALIGN_UP( SizePending,  );
    ReadIndexVal = Queue->ReadIndex;
    if ( ReadIndexVal >= WriteIndexVal )
    {
      // [ Our Mesage ] [ Last Queue ]
      // |<------------>|ReadIndexVal
      // |WriteIndexVal
      if ( ReadIndexVal - WriteIndexVal < MessageSize )
        WriteIndexVal = -;
    }
    else
    {
      // [ Our Mesage ] [ Available Space ]
      // |WriteIndexVal                   |Capacity
      // |<------------------------------>|
      uint32_t sizeAvailableTail = Queue->Capacity - WriteIndexVal;
      if ( sizeAvailableTail < MessageSize )
      {
        // try to write to head of memory
        // [ Our Mesage ] [ Last Queue ]
        // |<------------>|ReadIndexVal
        // |0
        if ( ReadIndexVal < MessageSize )
          WriteIndexVal = -;
        else if ( sizeAvailableTail >  )
        {
          // can not read message from tail of memory
          // Marker for Q_Purge()
          Memory[ WriteIndexVal ] = ;
          // write to head of memory
          WriteIndexVal = ;
        }
      }
    }
  }

  // store message to memory
  if ( WriteIndexVal != - )
  {
    // WriteIndexPending for other task if our task be preemptived
    Queue->WriteIndex = WriteIndexVal;
    Queue->MessageCount++;
    Memory[ WriteIndexVal ] = -Size; // SizePending for other task
    Queue->InProgressCount++;

    osRestoreInterrupt( Value );
    //
    memcpy( &Memory[ WriteIndexVal +  ], Message, Size );
    //
    Value = osDisableInterrupt( );
    Memory[ WriteIndexVal ] = Size; // Size for this message
    Queue->InProgressCount--;
  }

  osRestoreInterrupt( Value );
  return ( WriteIndexVal != - );
}

// Retrieves a message from a Queue
// not allowed while the queue is in use.
uint32_t Q_Read( QUEUE * Queue, void ** Message )
{
  uint32_t MessageSize = ;
  uint32_t * Memory = (uint32_t *) Queue->Memory;

  uint32_t Value = osDisableInterrupt( );
  if ( ( Queue->IsUsing ==  ) && ( Queue->MessageCount ) )
  {
    MessageSize = Memory[ Queue->ReadIndex ];
    *Message = (void *) ( (uint32_t) ( &Memory[ Queue->ReadIndex ] ) +  );
    Queue->IsUsing = ;
  }
  osRestoreInterrupt( Value );
  return MessageSize;
}

// Deletes the last retrieved message in a Queue.
void Q_Purge( QUEUE * Queue )
{
  uint32_t Value = osDisableInterrupt( );
  if ( Queue->IsUsing )
  {
    uint32_t * Memory = (uint32_t *) Queue->Memory;
    uint32_t MessageSize =  + ALIGN_UP( Memory[ Queue->ReadIndex ],  );
    Queue->MessageCount--;
    uint32_t NextReadIndexVal = Queue->ReadIndex + MessageSize;
    Queue->ReadIndex = NextReadIndexVal;
    if ( Queue->Capacity - NextReadIndexVal <  )
      Queue->ReadIndex = ;
    else if ( Queue->MessageCount )
    {
      // Marked by Q_Write(), Next readable message at head of memory
      if ( Memory[ NextReadIndexVal ] ==  )
        Queue->ReadIndex = ;
    }
    Queue->IsUsing = ;
  }
  osRestoreInterrupt( Value );
}