Queue.cpp

00001 #include "stdint.h"
00002 #include "Queue.h"
00003 #include <string.h>
00004 
00005 static char StringBuffer[256];
00006 
00007 void InitByteQueue(ByteQueue *BQ,uint16_t Size,uint8_t * Storage) {
00008     uint16_t i;
00009 
00010     BQ->QueueSize = Size;
00011     BQ->ReadPtr=0;
00012     BQ->WritePtr=0;
00013     BQ->QueueStorage = Storage;
00014 
00015     for (i=0;i<BQ->QueueSize;i++) {
00016         BQ->QueueStorage[i] = 0;
00017     }
00018 }
00019 
00020 uint16_t BytesInQueue(ByteQueue *BQ) {
00021     if (BQ->ReadPtr > BQ->WritePtr) {
00022         return (BQ->QueueSize - BQ->ReadPtr + BQ->WritePtr);
00023     } else if (BQ->WritePtr > BQ->ReadPtr) {
00024         return     (BQ->WritePtr - BQ->ReadPtr);
00025     } else {
00026         return 0;
00027     }
00028 }
00029 
00030 int16_t ByteEnqueue(ByteQueue *BQ,uint8_t Val) {
00031     if (BytesInQueue(BQ) == BQ->QueueSize - 1) {
00032         return QUEUE_FULL;
00033     } else {
00034         BQ->QueueStorage[BQ->WritePtr] = Val;
00035         BQ->WritePtr++;
00036 
00037         if (BQ->WritePtr >= BQ->QueueSize) {
00038             BQ->WritePtr = 0;
00039         }
00040         return QUEUE_OK;
00041     }
00042 }
00043 
00044 int16_t ByteArrayEnqueue(ByteQueue *BQ,uint8_t *Buf,uint16_t Len) {
00045     uint16_t i;
00046     for (i=0;i<Len;i++) {
00047         ByteEnqueue(BQ,Buf[i]);
00048     }
00049     return QUEUE_OK;
00050 }
00051 
00052 
00053 int16_t Qprintf(ByteQueue *BQ, const char *FormatString,...)
00054 {
00055  
00056      va_list argptr; 
00057      va_start(argptr,FormatString); 
00058      vsprintf((char *)StringBuffer,FormatString,argptr);
00059      va_end(argptr);   
00060            
00061     return ByteArrayEnqueue(BQ,(uint8_t *)StringBuffer,strlen(StringBuffer));
00062 }
00063 
00064 
00065 int16_t ByteDequeue(ByteQueue *BQ,uint8_t *Val) {
00066 
00067     if (BytesInQueue(BQ) == 0) {
00068         return QUEUE_EMPTY;
00069     } else {
00070         *Val  = BQ->QueueStorage[BQ->ReadPtr];
00071 
00072         BQ->ReadPtr++;
00073 
00074         if (BQ->ReadPtr >= BQ->QueueSize) {
00075             BQ->ReadPtr = 0;
00076         }
00077         return QUEUE_OK;
00078     }
00079 }
00080 
00081 uint8_t ForcedByteDequeue(ByteQueue *BQ)
00082 {
00083     uint8_t RetVal;
00084 
00085     if (BytesInQueue(BQ) == 0) {
00086         return 0;
00087     } else {
00088         RetVal  = BQ->QueueStorage[BQ->ReadPtr];
00089 
00090         BQ->ReadPtr++;
00091 
00092         if (BQ->ReadPtr >= BQ->QueueSize) {
00093             BQ->ReadPtr = 0;
00094         }
00095         return RetVal;
00096     }
00097 }
00098