KM3NeT CLB  2.0
KM3NeT CLB v2 Embedded Software
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
varqueue.c
1 /*
2  * KM3NeT CLB v2 Firmware
3  *
4  * Copyright 2013 KM3NeT Collaboration
5  *
6  * All Rights Reserved.
7  *
8  * varqueue.c
9  *
10  * Created on: 24 mei 2013
11  * Author: Vincent van Beveren
12  */
13 
14 #include "coll/varqueue.h"
15 #include <assert.h>
16 #include <string.h>
17 
18 #define _VQ_HEADER_SIZE 2
19 
20 
21 #define _VQ_B(VQ_PTR, POS, OFFSET) \
22  (VQ_PTR->mem[( POS + OFFSET ) % VQ_PTR->size])
23 
24 bool vqQueue(VarQueue * const vq, uint16_t length, uint8_t * buffer)
25 {
26  assert(buffer != 0);
27  assert(length > 0);
28 
29  if (vqFree(vq) < length) return false;
30 
31  _VQ_B(vq, vq->write, 0) = 0xFF & ( length >> 8 );
32  _VQ_B(vq, vq->write, 1) = 0xFF & length;
33  vq->write = ( vq->write + 2 ) % ( vq->size );
34 
35  if (vq->write + length < vq->size) {
36  // fits in queue
37  memcpy(vq->mem + vq->write, buffer, length);
38  } else {
39  // does not fit in queue
40  int l1 = vq->size - vq->write - 1;
41  memcpy(vq->mem + vq->write, buffer, l1);
42  memcpy(vq->mem, buffer + l1, length - l1);
43  }
44  vq->write = ( vq->write + length ) % vq->size;
45  vq->used += length + 2;
46  return true;
47 }
48 
49 
50 bool vqDeQueue(VarQueue * const vq, uint16_t * length, uint8_t * buffer)
51 {
52  assert(vq != 0);
53  assert(length != 0);
54  assert(buffer != 0);
55 
56  if (vq->used == 0) return false;
57 
58  int lenQ = vqDeQueueLength(vq);
59 
60  if (lenQ > *length) return false;
61 
62  vq->read = ( vq->read + 2 ) % ( vq->size );
63 
64 
65  if (vq->read + lenQ < vq->size)
66  {
67  // fits in queue
68  memcpy(buffer, vq->mem + vq->read, lenQ);
69  } else
70  {
71  // does not fit in queue
72  int l1 = vq->size - vq->read - 1;
73  memcpy(buffer, vq->mem + vq->read, l1);
74  memcpy(buffer + l1, vq->mem, lenQ - l1);
75  }
76  vq->read = ( vq->read + lenQ ) % vq->size;
77  *length = lenQ;
78  vq->used -= lenQ + 2;
79 
80  return true;
81 }
82 
83 
84 bool vqPeekQueue(VarQueue * const vq, uint16_t * length, uint8_t * buffer)
85 {
86  uint16_t rp = vq->read;
87  uint16_t us = vq->used;
88  if (vqDeQueue(vq, length, buffer))
89  {
90  vq->read = rp;
91  vq->used = us;
92 
93  return true;
94  }
95  return false;
96 }
97 
98 bool vqRemove(VarQueue * const vq)
99 {
100  uint16_t l = vqDeQueueLength(vq);
101  if (l == 0) return false;
102  vq->used -= l + 2;
103  vq->read = (vq->read + l + 2) % vq->size;
104  return true;
105 }
106 
107 
108 uint16_t vqDeQueueLength(VarQueue * const vq)
109 {
110  if (vq->used == 0) return 0;
111  return _VQ_B(vq, vq->read, 0) << 8 | _VQ_B(vq, vq->read, 1);
112 }
113 
114 uint16_t vqFree(VarQueue * const vq)
115 {
116  int x = vq->size - vq->used;
117  if (x < _VQ_HEADER_SIZE) return 0;
118  return x - _VQ_HEADER_SIZE;
119 }
A variable length queue implementation.
bool vqDeQueue(VarQueue *const vq, uint16_t *length, uint8_t *buffer)
DeQueues an element from the queue.
Definition: varqueue.c:50
bool vqQueue(VarQueue *const vq, uint16_t length, uint8_t *buffer)
Queues an element on the queue.
Definition: varqueue.c:24
uint8_t *const mem
Pointer to memory.
Definition: varqueue.h:52
Variable length queue structure.
Definition: varqueue.h:50
uint16_t vqDeQueueLength(VarQueue *const vq)
Returns the lenght of the next element to dequeue.
Definition: varqueue.c:108
const uint16_t size
Size of the memory.
Definition: varqueue.h:53
uint16_t read
read index
Definition: varqueue.h:55
uint16_t used
bytes used
Definition: varqueue.h:54
uint16_t vqFree(VarQueue *const vq)
Returns the number of bytes free for the next element.
Definition: varqueue.c:114
uint16_t write
write index
Definition: varqueue.h:56
bool vqRemove(VarQueue *const vq)
Removes the first element from the queue (not the last).
Definition: varqueue.c:98