#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 );
}