xqueue.cpp

00001 #include "xqueue.h"
00002 
00003 #ifndef MIN
00004     #define MIN(x,y) ( (x) < (y) ? (x) : (y) )
00005 #endif
00006 
00007 void xqueue_init(xqueue_t * const queue, uint8_t * const dataBuffer, const size_t size){
00008     queue->capacity = size;
00009     queue->capacityMask = size-1;
00010     queue->data = dataBuffer;
00011     queue->rCount = 0;
00012     queue->wCount = 0;
00013     queue->head = 0;
00014     queue->tail = 0;
00015 }
00016 
00017 size_t xqueue_count(const xqueue_t * const queue){
00018     return queue->wCount - queue->rCount;
00019 }
00020 
00021 bool xqueue_empty(const xqueue_t * const queue){
00022     return queue->wCount == queue->rCount;
00023 }
00024 
00025 bool xqueue_full(const xqueue_t * const queue){
00026     return xqueue_count(queue) >= queue->capacity;
00027 }
00028 
00029 size_t xqueue_space(const xqueue_t * const queue){
00030     return queue->capacity - xqueue_count(queue);
00031 }
00032 
00033 uint8_t xqueue_front(const xqueue_t * const queue){
00034     return queue->data[queue->tail];
00035 }
00036 
00037 uint8_t xqueue_back(const xqueue_t * const queue){
00038     return queue->data[(queue->head - 1) & queue->capacityMask];
00039 }
00040 
00041 uint8_t xqueue_at(const xqueue_t * const queue, const size_t index){
00042     return queue->data[(queue->tail + index) & queue->capacityMask];
00043 }
00044 
00045 void xqueue_clear(xqueue_t * const queue){
00046     queue->rCount = 0;
00047     queue->wCount = 0;
00048     queue->head = 0;
00049     queue->tail = 0;
00050 }
00051 
00052 uint8_t xqueue_push(xqueue_t * const queue, const uint8_t value){
00053     if(xqueue_full(queue))
00054         xqueue_drop(queue);
00055 
00056     *(queue->data + queue->head) = value;
00057     queue->head = (queue->head + 1) & queue->capacityMask;
00058     queue->wCount++;
00059     return value;
00060 }
00061 
00062 void xqueue_push16(xqueue_t * const queue, const uint16_t value){
00063     xqueue_push8(queue, HO(value));
00064     xqueue_push8(queue, LO(value));
00065 }
00066 
00067 void xqueue_push32(xqueue_t * const queue, const uint32_t value){
00068     xqueue_push8(queue, HHO(value));
00069     xqueue_push8(queue, HLO(value));
00070     xqueue_push8(queue, LHO(value));
00071     xqueue_push8(queue, LLO(value));
00072 }
00073 
00074 void xqueue_push_buffer(xqueue_t * const queue, const uint8_t * values, const size_t nvalues){
00075     size_t ii;
00076     if(nvalues >= queue->capacity)
00077         return;
00078     else if(nvalues > xqueue_space(queue))
00079         xqueue_drop_many(queue, nvalues - xqueue_space(queue));
00080     
00081     for(ii = 0; ii < nvalues; ii++){
00082         *(queue->data + queue->head) = *values++;
00083         queue->head = (queue->head + 1) & queue->capacityMask;
00084     }
00085     queue->wCount += ii;
00086 }
00087 
00088 void xqueue_push_string(xqueue_t * const queue, const unsigned char * str){
00089     size_t ii;
00090     for(ii = 0; *str; ii++){
00091         if(xqueue_full(queue))
00092             xqueue_drop(queue);
00093         *(queue->data + queue->head) = *str++;
00094         queue->head = (queue->head + 1) & queue->capacityMask;
00095         queue->wCount++;
00096     }
00097 }
00098 
00099 uint8_t xqueue_pop(xqueue_t * const queue){
00100     uint8_t value = *(queue->data + queue->tail);
00101     queue->tail = (queue->tail + 1) & queue->capacityMask;
00102     queue->rCount++;
00103     return value;
00104 }
00105 
00106 uint16_t xqueue_pop16(xqueue_t * const queue){
00107     return MHO(xqueue_pop8(queue)) | MLO(xqueue_pop8(queue));
00108 }
00109 
00110 uint32_t xqueue_pop32(xqueue_t * const queue){
00111     return MHHO(xqueue_pop8(queue)) | MHLO(xqueue_pop8(queue)) | MLHO(xqueue_pop8(queue)) | MLLO(xqueue_pop8(queue));
00112 }
00113 
00114 void xqueue_pop_buffer(xqueue_t * const queue,  uint8_t * values, const size_t nvalues){
00115     size_t ii;
00116     for(ii = 0; ii < nvalues && !xqueue_empty(queue); ii++){
00117         *values++ = *(queue->data + queue->tail);
00118         queue->tail = (queue->tail + 1) & queue->capacityMask;
00119         queue->rCount++;
00120     }
00121 }
00122 
00123 void xqueue_drop(xqueue_t * const queue){
00124     queue->tail = (queue->tail + 1) & queue->capacityMask;
00125     queue->rCount++;
00126 }
00127 
00128 void xqueue_drop_many(xqueue_t * const queue, size_t n){
00129     n = MIN(n, xqueue_count(queue));
00130 
00131     queue->tail = (queue->tail + n) & queue->capacityMask;
00132     queue->rCount += n;
00133 }
00134 
00135 uint8_t xqueue_checksum(const xqueue_t * const queue, size_t nvalues, const size_t nAdvance){
00136     uint8_t cxsum = 0;
00137     size_t ii;
00138     nvalues = MIN(nvalues, xqueue_count(queue) - nAdvance);
00139     for(ii = 0; ii < nvalues; ii++){
00140         cxsum += xqueue_at(queue, ii + nAdvance);
00141     }
00142     return cxsum;
00143 }