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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
|
#pragma once
#include <Dispatcher.h>
template <uint8_t N>
class FixedPacketQueue {
public:
FixedPacketQueue() : _num(0) {
memset(_table, 0, sizeof(_table));
memset(_pri_table, 0, sizeof(_pri_table));
memset(_schedule_table, 0, sizeof(_schedule_table));
}
mesh::Packet* get(uint32_t now) {
uint8_t min_pri = 0xFF;
int best_idx = -1;
for (uint8_t j = 0; j < _num; ++j) {
if ((int32_t)(_schedule_table[j] - now) > 0) {
continue;
}
if (_pri_table[j] < min_pri) {
min_pri = _pri_table[j];
best_idx = j;
}
}
return removeByIdx(best_idx);
}
bool add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
if (!packet || _num >= N) {
return false;
}
_table[_num] = packet;
_pri_table[_num] = priority;
_schedule_table[_num] = scheduled_for;
++_num;
return true;
}
int count() const {
return _num;
}
int countBefore(uint32_t now) const {
if (now == 0xFFFFFFFFUL) {
return _num;
}
int n = 0;
for (uint8_t j = 0; j < _num; ++j) {
if ((int32_t)(_schedule_table[j] - now) <= 0) {
++n;
}
}
return n;
}
mesh::Packet* itemAt(int i) const {
return (i >= 0 && i < _num) ? _table[i] : NULL;
}
mesh::Packet* removeByIdx(int i) {
if (i < 0 || i >= _num) {
return NULL;
}
mesh::Packet* item = _table[i];
--_num;
while (i < _num) {
_table[i] = _table[i + 1];
_pri_table[i] = _pri_table[i + 1];
_schedule_table[i] = _schedule_table[i + 1];
++i;
}
return item;
}
private:
mesh::Packet* _table[N];
uint8_t _pri_table[N];
uint32_t _schedule_table[N];
uint8_t _num;
};
template <uint8_t N>
class FixedPoolPacketManager : public mesh::PacketManager {
public:
FixedPoolPacketManager() {
for (uint8_t i = 0; i < N; ++i) {
_unused.add(&_packets[i], 0, 0);
}
}
mesh::Packet* allocNew() override {
return _unused.removeByIdx(0);
}
void free(mesh::Packet* packet) override {
_unused.add(packet, 0, 0);
}
void queueOutbound(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) override {
if (!_send_queue.add(packet, priority, scheduled_for)) {
MESH_DEBUG_PRINTLN("queueOutbound: send queue full, dropping packet");
free(packet);
}
}
mesh::Packet* getNextOutbound(uint32_t now) override {
return _send_queue.get(now);
}
int getOutboundCount(uint32_t now) const override {
return _send_queue.countBefore(now);
}
int getOutboundTotal() const override {
return _send_queue.count();
}
int getFreeCount() const override {
return _unused.count();
}
mesh::Packet* getOutboundByIdx(int i) override {
return _send_queue.itemAt(i);
}
mesh::Packet* removeOutboundByIdx(int i) override {
return _send_queue.removeByIdx(i);
}
void queueInbound(mesh::Packet* packet, uint32_t scheduled_for) override {
if (!_rx_queue.add(packet, 0, scheduled_for)) {
MESH_DEBUG_PRINTLN("queueInbound: rx queue full, dropping packet");
free(packet);
}
}
mesh::Packet* getNextInbound(uint32_t now) override {
return _rx_queue.get(now);
}
private:
mesh::Packet _packets[N];
FixedPacketQueue<N> _unused;
FixedPacketQueue<N> _send_queue;
FixedPacketQueue<N> _rx_queue;
};
|
