1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
|
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include "eos.h"
#include "msgq.h"
#define IDX_MASK(IDX, SIZE) ((IDX) & ((SIZE) - 1))
#define IDX_HALF ((uint8_t)1 << (sizeof(uint8_t) * 8 - 1))
#define IDX_LT(a,b) ((uint8_t)((uint8_t)(a) - (uint8_t)(b)) > IDX_HALF)
#define IDX_LTE(a,b) ((uint8_t)((uint8_t)(b) - (uint8_t)(a)) < IDX_HALF)
void eos_msgq_init(EOSMsgQ *msgq, EOSMsgItem *array, uint8_t size) {
msgq->idx_r = 0;
msgq->idx_w = 0;
msgq->size = size;
msgq->array = array;
}
uint8_t eos_msgq_len(EOSMsgQ *msgq) {
return (uint8_t)(msgq->idx_w - msgq->idx_r);
}
int eos_msgq_push(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint16_t len) {
return eos_msgq_push_widx(msgq, type, buffer, len, NULL);
}
int eos_msgq_push_widx(EOSMsgQ *msgq, unsigned char type, unsigned char *buffer, uint16_t len, uint8_t *_idx) {
if ((uint8_t)(msgq->idx_w - msgq->idx_r) == msgq->size) return EOS_ERR_FULL;
uint8_t idx = IDX_MASK(msgq->idx_w, msgq->size);
msgq->array[idx].type = type;
msgq->array[idx].buffer = buffer;
msgq->array[idx].len = len;
msgq->idx_w++;
if (_idx) *_idx = idx;
return EOS_OK;
}
void eos_msgq_pop(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, uint16_t *len) {
eos_msgq_pop_widx(msgq, type, buffer, len, NULL);
}
void eos_msgq_pop_widx(EOSMsgQ *msgq, unsigned char *type, unsigned char **buffer, uint16_t *len, uint8_t *_idx) {
if (msgq->idx_r == msgq->idx_w) {
*type = 0;
*buffer = NULL;
*len = 0;
} else {
uint8_t idx = IDX_MASK(msgq->idx_r, msgq->size);
*type = msgq->array[idx].type;
*buffer = msgq->array[idx].buffer;
*len = msgq->array[idx].len;
msgq->idx_r++;
if (_idx) *_idx = idx;
}
}
int eos_msgq_find(EOSMsgQ *msgq, unsigned char type, unsigned char *selector, uint16_t sel_len, unsigned char **buffer, uint16_t *len) {
uint8_t i, j, idx;
unsigned char *_buffer;
uint16_t _len;
if (msgq->idx_r == msgq->idx_w) {
if (buffer && len) {
*buffer = NULL;
*len = 0;
}
return EOS_ERR_NOTFOUND;
}
idx = IDX_MASK(msgq->idx_r, msgq->size);
if (type == msgq->array[idx].type) {
_buffer = msgq->array[idx].buffer;
_len = msgq->array[idx].len;
if ((selector == NULL) || (sel_len == 0) || ((sel_len <= _len) && (memcmp(selector, _buffer, sel_len) == 0))) {
msgq->idx_r++;
if (buffer && len) {
*buffer = _buffer;
*len = _len;
}
return EOS_OK;
}
}
for (i = msgq->idx_r + 1; IDX_LT(i, msgq->idx_w); i++) {
idx = IDX_MASK(i, msgq->size);
if (type== msgq->array[idx].type) {
_buffer = msgq->array[idx].buffer;
_len = msgq->array[idx].len;
if ((selector == NULL) || (sel_len == 0) || ((sel_len <= _len) && (memcmp(selector, _buffer, sel_len) == 0))) {
for (j = i + 1; IDX_LT(j, msgq->idx_w); j++) {
msgq->array[IDX_MASK(j - 1, msgq->size)] = msgq->array[IDX_MASK(j, msgq->size)];
}
msgq->idx_w--;
if (buffer && len) {
*buffer = _buffer;
*len = _len;
}
return EOS_OK;
}
}
}
if (buffer && len) {
*buffer = NULL;
*len = 0;
}
return EOS_ERR_NOTFOUND;
}
void eos_bufq_init(EOSBufQ *bufq, unsigned char **array, uint8_t size) {
bufq->idx_r = 0;
bufq->idx_w = 0;
bufq->size = size;
bufq->array = array;
}
uint8_t eos_bufq_len(EOSBufQ *bufq) {
return (uint8_t)(bufq->idx_w - bufq->idx_r);
}
int eos_bufq_push(EOSBufQ *bufq, unsigned char *buffer) {
if ((uint8_t)(bufq->idx_w - bufq->idx_r) == bufq->size) return EOS_ERR_FULL;
bufq->array[IDX_MASK(bufq->idx_w++, bufq->size)] = buffer;
return EOS_OK;
}
unsigned char *eos_bufq_pop(EOSBufQ *bufq) {
if (bufq->idx_r == bufq->idx_w) return NULL;
return bufq->array[IDX_MASK(bufq->idx_r++, bufq->size)];
}
|
