#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), 4 ) + ALIGN_UP( Capacity, 4 ); QUEUE *Queue = (QUEUE *) osMalloc( Size, osWaitForever ); if ( Queue == NULL ) return NULL; uint8_t * Memory = // (uint8_t *) ( ( (uint32_t) ( Queue ) ) + ALIGN_UP( sizeof(QUEUE), 4 ) ); Q_Init( Queue, Memory, ALIGN_UP( Capacity, 4 ) ); 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 == 0 ) 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 == 0 ) Queue->MessageCount = 0; } // Initializes a message Queue. void Q_Init( QUEUE * Queue, uint8_t * Memory, uint32_t Capacity ) { int32_t Delta = (uint32_t) Memory & 3; if ( Delta ) { Delta -= 4; Capacity += Delta; Memory -= Delta; } memset( Queue, 0, 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 = 0; 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 = 4 + ALIGN_UP( Size, 4 ); int32_t * Memory = (int32_t *) Queue->Memory; uint32_t Value = osDisableInterrupt( ); if ( Queue->MessageCount == 0 ) { // read next message from head of memory Queue->ReadIndex = 0; // Queue could not be stored (memory is full). WriteIndexVal = -1; if ( Queue->Capacity >= MessageSize ) WriteIndexVal = 0; } else { Memory = (int32_t *) Queue->Memory; WriteIndexPending = Queue->WriteIndex; int32_t SizePending = Memory[ WriteIndexPending ]; if ( SizePending < 0 ) { // 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 + 4 + ALIGN_UP( SizePending, 4 ); ReadIndexVal = Queue->ReadIndex; if ( ReadIndexVal >= WriteIndexVal ) { // [ Our Mesage ] [ Last Queue ] // |<------------>|ReadIndexVal // |WriteIndexVal if ( ReadIndexVal - WriteIndexVal < MessageSize ) WriteIndexVal = -1; } 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 = -1; else if ( sizeAvailableTail > 4 ) { // can not read message from tail of memory // Marker for Q_Purge() Memory[ WriteIndexVal ] = 0; // write to head of memory WriteIndexVal = 0; } } } } // store message to memory if ( WriteIndexVal != -1 ) { // 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 + 4 ], Message, Size ); // Value = osDisableInterrupt( ); Memory[ WriteIndexVal ] = Size; // Size for this message Queue->InProgressCount--; } osRestoreInterrupt( Value ); return ( WriteIndexVal != -1 ); } // 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 = 0; uint32_t * Memory = (uint32_t *) Queue->Memory; uint32_t Value = osDisableInterrupt( ); if ( ( Queue->IsUsing == 0 ) && ( Queue->MessageCount ) ) { MessageSize = Memory[ Queue->ReadIndex ]; *Message = (void *) ( (uint32_t) ( &Memory[ Queue->ReadIndex ] ) + 4 ); Queue->IsUsing = 1; } 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 = 4 + ALIGN_UP( Memory[ Queue->ReadIndex ], 4 ); Queue->MessageCount--; uint32_t NextReadIndexVal = Queue->ReadIndex + MessageSize; Queue->ReadIndex = NextReadIndexVal; if ( Queue->Capacity - NextReadIndexVal < 5 ) Queue->ReadIndex = 0; else if ( Queue->MessageCount ) { // Marked by Q_Write(), Next readable message at head of memory if ( Memory[ NextReadIndexVal ] == 0 ) Queue->ReadIndex = 0; } Queue->IsUsing = 0; } osRestoreInterrupt( Value ); }