|
@@ -1,174 +1,174 @@
|
|
|
-#include "byte_queue.h"
|
|
|
|
|
-
|
|
|
|
|
-static u16 byte_queue_add(const byte_queue_t *queue, u16 value1, u16 value2)
|
|
|
|
|
-{
|
|
|
|
|
- return (value1 + value2) % queue->size;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-static u16 byte_queue_tail_add(const byte_queue_t *queue, u16 value)
|
|
|
|
|
-{
|
|
|
|
|
- return byte_queue_add(queue, queue->tail, value);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-static u16 byte_queue_head_add(const byte_queue_t *queue, u16 value)
|
|
|
|
|
-{
|
|
|
|
|
- return byte_queue_add(queue, queue->head, value);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u8 *byte_queue_head(byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- return queue->buff + queue->head;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-bool byte_queue_empty(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- return (bool) (queue->head == queue->tail);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-bool byte_queue_readable(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- return (bool) (queue->head != queue->tail);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-bool byte_queue_full(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- return (bool) (byte_queue_tail_add(queue, 1) == queue->head);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-bool byte_queue_writeable(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- return (bool) (byte_queue_tail_add(queue, 1) != queue->head);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_get_used(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- if (queue->head <= queue->tail) {
|
|
|
|
|
- return queue->tail - queue->head;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- return queue->size - (queue->head - queue->tail) - 1;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_get_free(const byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- if (queue->tail < queue->head) {
|
|
|
|
|
- return queue->head - queue->tail;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- return queue->size - (queue->tail - queue->head) - 1;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_reset(byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- queue->head = queue->tail = 0;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_init(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
-{
|
|
|
|
|
- queue->buff = buff;
|
|
|
|
|
- queue->size = size;
|
|
|
|
|
- queue->head = queue->tail = 0;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_write(byte_queue_t *queue, const u8 *buff, u16 size)
|
|
|
|
|
-{
|
|
|
|
|
- const u8 *buff_bak = buff;
|
|
|
|
|
- const u8 *buff_end;
|
|
|
|
|
-
|
|
|
|
|
- for (buff_end = buff + size; buff < buff_end; buff++) {
|
|
|
|
|
- u16 tail = byte_queue_tail_add(queue, 1);
|
|
|
|
|
-
|
|
|
|
|
- if (tail == queue->head) {
|
|
|
|
|
- return buff - buff_bak;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- queue->buff[queue->tail] = *buff;
|
|
|
|
|
- queue->tail = tail;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- return size;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_write_byte(byte_queue_t *queue, u8 byte)
|
|
|
|
|
-{
|
|
|
|
|
- return byte_queue_write(queue, &byte, 1);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_read(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
-{
|
|
|
|
|
- u8 *buff_bak = buff;
|
|
|
|
|
- u8 *buff_end;
|
|
|
|
|
-
|
|
|
|
|
- for (buff_end = buff + size; buff < buff_end; buff++) {
|
|
|
|
|
- if (queue->head == queue->tail) {
|
|
|
|
|
- return buff - buff_bak;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- *buff = queue->buff[queue->head];
|
|
|
|
|
- queue->head = byte_queue_head_add(queue, 1);
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- return size;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_fill(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
-{
|
|
|
|
|
- while (size > 0) {
|
|
|
|
|
- u16 length = byte_queue_read(queue, buff, size);
|
|
|
|
|
- size -= length;
|
|
|
|
|
- buff += size;
|
|
|
|
|
- }
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_skip(byte_queue_t *queue, u16 length)
|
|
|
|
|
-{
|
|
|
|
|
- queue->head = byte_queue_head_add(queue, length);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u16 byte_queue_peek(byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- if (queue->tail < queue->head) {
|
|
|
|
|
- return queue->size - queue->head;
|
|
|
|
|
- } else {
|
|
|
|
|
- return queue->tail - queue->head;
|
|
|
|
|
- }
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-// ================================================================================
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_alloc_init(byte_queue_t *queue, u8 *buff, u8 size)
|
|
|
|
|
-{
|
|
|
|
|
- queue->buff = buff;
|
|
|
|
|
- queue->size = size;
|
|
|
|
|
- byte_queue_alloc_reset(queue);
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_alloc_reset(byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- u8 *buff = queue->buff;
|
|
|
|
|
- u8 size = queue->size;
|
|
|
|
|
- u8 index;
|
|
|
|
|
-
|
|
|
|
|
- for (index = 0; index < size; index++) {
|
|
|
|
|
- buff[index] = index;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- queue->tail = size - 1;
|
|
|
|
|
- queue->head = 0;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-u8 byte_queue_alloc(byte_queue_t *queue)
|
|
|
|
|
-{
|
|
|
|
|
- u8 index;
|
|
|
|
|
-
|
|
|
|
|
- if (byte_queue_read(queue, &index, 1) > 0) {
|
|
|
|
|
- return index;
|
|
|
|
|
- }
|
|
|
|
|
-
|
|
|
|
|
- return 0xFF;
|
|
|
|
|
-}
|
|
|
|
|
-
|
|
|
|
|
-void byte_queue_free(byte_queue_t *queue, u8 index)
|
|
|
|
|
-{
|
|
|
|
|
- byte_queue_write(queue, &index, 1);
|
|
|
|
|
-}
|
|
|
|
|
|
|
+#include "byte_queue.h"
|
|
|
|
|
+
|
|
|
|
|
+static u16 byte_queue_add(const byte_queue_t *queue, u16 value1, u16 value2)
|
|
|
|
|
+{
|
|
|
|
|
+ return (value1 + value2) % queue->size;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+static u16 byte_queue_tail_add(const byte_queue_t *queue, u16 value)
|
|
|
|
|
+{
|
|
|
|
|
+ return byte_queue_add(queue, queue->tail, value);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+static u16 byte_queue_head_add(const byte_queue_t *queue, u16 value)
|
|
|
|
|
+{
|
|
|
|
|
+ return byte_queue_add(queue, queue->head, value);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u8 *byte_queue_head(byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ return queue->buff + queue->head;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool byte_queue_empty(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ return (bool) (queue->head == queue->tail);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool byte_queue_readable(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ return (bool) (queue->head != queue->tail);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool byte_queue_full(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ return (bool) (byte_queue_tail_add(queue, 1) == queue->head);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+bool byte_queue_writeable(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ return (bool) (byte_queue_tail_add(queue, 1) != queue->head);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_get_used(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ if (queue->head <= queue->tail) {
|
|
|
|
|
+ return queue->tail - queue->head;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return queue->size - (queue->head - queue->tail) - 1;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_get_free(const byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ if (queue->tail < queue->head) {
|
|
|
|
|
+ return queue->head - queue->tail;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return queue->size - (queue->tail - queue->head) - 1;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_reset(byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ queue->head = queue->tail = 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_init(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
+{
|
|
|
|
|
+ queue->buff = buff;
|
|
|
|
|
+ queue->size = size;
|
|
|
|
|
+ queue->head = queue->tail = 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_write(byte_queue_t *queue, const u8 *buff, u16 size)
|
|
|
|
|
+{
|
|
|
|
|
+ const u8 *buff_bak = buff;
|
|
|
|
|
+ const u8 *buff_end;
|
|
|
|
|
+
|
|
|
|
|
+ for (buff_end = buff + size; buff < buff_end; buff++) {
|
|
|
|
|
+ u16 tail = byte_queue_tail_add(queue, 1);
|
|
|
|
|
+
|
|
|
|
|
+ if (tail == queue->head) {
|
|
|
|
|
+ return buff - buff_bak;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ queue->buff[queue->tail] = *buff;
|
|
|
|
|
+ queue->tail = tail;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return size;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_write_byte(byte_queue_t *queue, u8 byte)
|
|
|
|
|
+{
|
|
|
|
|
+ return byte_queue_write(queue, &byte, 1);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_read(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 *buff_bak = buff;
|
|
|
|
|
+ u8 *buff_end;
|
|
|
|
|
+
|
|
|
|
|
+ for (buff_end = buff + size; buff < buff_end; buff++) {
|
|
|
|
|
+ if (queue->head == queue->tail) {
|
|
|
|
|
+ return buff - buff_bak;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ *buff = queue->buff[queue->head];
|
|
|
|
|
+ queue->head = byte_queue_head_add(queue, 1);
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return size;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_fill(byte_queue_t *queue, u8 *buff, u16 size)
|
|
|
|
|
+{
|
|
|
|
|
+ while (size > 0) {
|
|
|
|
|
+ u16 length = byte_queue_read(queue, buff, size);
|
|
|
|
|
+ size -= length;
|
|
|
|
|
+ buff += size;
|
|
|
|
|
+ }
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_skip(byte_queue_t *queue, u16 length)
|
|
|
|
|
+{
|
|
|
|
|
+ queue->head = byte_queue_head_add(queue, length);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u16 byte_queue_peek(byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ if (queue->tail < queue->head) {
|
|
|
|
|
+ return queue->size - queue->head;
|
|
|
|
|
+ } else {
|
|
|
|
|
+ return queue->tail - queue->head;
|
|
|
|
|
+ }
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+// ================================================================================
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_alloc_init(byte_queue_t *queue, u8 *buff, u8 size)
|
|
|
|
|
+{
|
|
|
|
|
+ queue->buff = buff;
|
|
|
|
|
+ queue->size = size;
|
|
|
|
|
+ byte_queue_alloc_reset(queue);
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_alloc_reset(byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 *buff = queue->buff;
|
|
|
|
|
+ u8 size = queue->size;
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ for (index = 0; index < size; index++) {
|
|
|
|
|
+ buff[index] = index;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ queue->tail = size - 1;
|
|
|
|
|
+ queue->head = 0;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+u8 byte_queue_alloc(byte_queue_t *queue)
|
|
|
|
|
+{
|
|
|
|
|
+ u8 index;
|
|
|
|
|
+
|
|
|
|
|
+ if (byte_queue_read(queue, &index, 1) > 0) {
|
|
|
|
|
+ return index;
|
|
|
|
|
+ }
|
|
|
|
|
+
|
|
|
|
|
+ return 0xFF;
|
|
|
|
|
+}
|
|
|
|
|
+
|
|
|
|
|
+void byte_queue_free(byte_queue_t *queue, u8 index)
|
|
|
|
|
+{
|
|
|
|
|
+ byte_queue_write(queue, &index, 1);
|
|
|
|
|
+}
|