mirror of
https://github.com/liquidraver/ZephCore.git
synced 2026-10-18 07:20:18 +00:00
core: restore upstream shape for the shared mesh core
- Mesh, Dispatcher, Packet, packet pool and the core headers rebuilt from Arduino MeshCore's text; ZephCore changes re-applied, each fenced `// ZEPHCORE:` (diff vs upstream: 4540 -> 1147 lines) - upstream header layout; MainBoard gains handleCommand/loop and the other upstream members; RNG::nextInt back in Utils.cpp - packet logging moved to the roles' log hooks (helpers/PacketLog.h) - zero-hop direct ACKs deduplicated and length-checked as upstream - RadioIncludes.h -> src/, LoRaConfig.h -> helpers/ - .editorconfig: shared files keep upstream formatting Verified: 10 role builds, per-function disassembly review, bench DM/ACK regression (T1000-E, Wio L1, XIAO S3 on 1.17.4) incl. via live repeater.
This commit is contained in:
liquidraver
@@ -1,215 +1,191 @@
|
||||
/*
|
||||
* SPDX-License-Identifier: MIT
|
||||
* Static pool PacketManager - no dynamic allocation
|
||||
*/
|
||||
|
||||
#include <mesh/StaticPoolPacketManager.h>
|
||||
#include <mesh/Packet.h>
|
||||
#include <string.h>
|
||||
|
||||
// ZEPHCORE: log module for the pool's warnings.
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(zephcore_pktpool, CONFIG_ZEPHCORE_LORA_LOG_LEVEL);
|
||||
|
||||
namespace mesh {
|
||||
// ZEPHCORE: static storage — no heap in the packet path. Upstream sizes the pool
|
||||
// at runtime (new[]); here pool and queues are compile-time and file-local.
|
||||
#define POOL_SIZE 32
|
||||
#define QUEUE_SIZE 32
|
||||
|
||||
#define POOL_SIZE 32
|
||||
#define QUEUE_SIZE 32
|
||||
namespace {
|
||||
|
||||
struct PacketQueue {
|
||||
Packet *_table[QUEUE_SIZE];
|
||||
uint8_t _pri_table[QUEUE_SIZE];
|
||||
uint32_t _schedule_table[QUEUE_SIZE];
|
||||
int _num;
|
||||
mesh::Packet* _table[QUEUE_SIZE];
|
||||
uint8_t _pri_table[QUEUE_SIZE];
|
||||
uint32_t _schedule_table[QUEUE_SIZE];
|
||||
int _num;
|
||||
|
||||
PacketQueue() : _num(0) {
|
||||
memset(_table, 0, sizeof(_table));
|
||||
}
|
||||
PacketQueue() : _num(0) {
|
||||
memset(_table, 0, sizeof(_table));
|
||||
}
|
||||
|
||||
int countBefore(uint32_t now) const {
|
||||
int n = 0;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if ((int32_t)(_schedule_table[j] - now) > 0) continue;
|
||||
n++;
|
||||
}
|
||||
return n;
|
||||
}
|
||||
int count() const { return _num; }
|
||||
mesh::Packet* itemAt(int i) const {
|
||||
if (i < 0 || i >= _num) return NULL;
|
||||
return _table[i];
|
||||
}
|
||||
|
||||
Packet *get(uint32_t now) {
|
||||
uint8_t min_pri = 0xFF;
|
||||
int best_idx = -1;
|
||||
for (int 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;
|
||||
}
|
||||
}
|
||||
if (best_idx < 0) return nullptr;
|
||||
int countBefore(uint32_t now) const {
|
||||
if (now == 0xFFFFFFFF) return _num; // sentinel: count all entries regardless of schedule
|
||||
|
||||
return removeByIdx(best_idx);
|
||||
}
|
||||
int n = 0;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if ((int32_t)(_schedule_table[j] - now) > 0) continue; // scheduled for future... ignore for now
|
||||
n++;
|
||||
}
|
||||
return n;
|
||||
}
|
||||
|
||||
Packet *removeByIdx(int i) {
|
||||
if (i >= _num) return nullptr;
|
||||
Packet *item = _table[i];
|
||||
for (int j = i; j < _num - 1; j++) {
|
||||
_table[j] = _table[j + 1];
|
||||
_pri_table[j] = _pri_table[j + 1];
|
||||
_schedule_table[j] = _schedule_table[j + 1];
|
||||
}
|
||||
_num--;
|
||||
return item;
|
||||
}
|
||||
mesh::Packet* get(uint32_t now) {
|
||||
uint8_t min_pri = 0xFF;
|
||||
int best_idx = -1;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if ((int32_t)(_schedule_table[j] - now) > 0) continue; // scheduled for future... ignore for now
|
||||
if (_pri_table[j] < min_pri) { // select most important priority amongst non-future entries
|
||||
min_pri = _pri_table[j];
|
||||
best_idx = j;
|
||||
}
|
||||
}
|
||||
if (best_idx < 0) return NULL; // empty, or all items are still in the future
|
||||
|
||||
bool add(Packet *packet, uint8_t priority, uint32_t scheduled_for) {
|
||||
if (_num >= QUEUE_SIZE) return false;
|
||||
_table[_num] = packet;
|
||||
_pri_table[_num] = priority;
|
||||
_schedule_table[_num] = scheduled_for;
|
||||
_num++;
|
||||
return true;
|
||||
}
|
||||
return removeByIdx(best_idx);
|
||||
}
|
||||
|
||||
/* Find index of the entry with the highest priority number (least
|
||||
* important). Returns -1 when the queue is empty. */
|
||||
int findLowestPriority() const {
|
||||
if (_num == 0) return -1;
|
||||
uint8_t worst = 0;
|
||||
int idx = 0;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if (_pri_table[j] >= worst) {
|
||||
worst = _pri_table[j];
|
||||
idx = j;
|
||||
}
|
||||
}
|
||||
return idx;
|
||||
}
|
||||
mesh::Packet* removeByIdx(int i) {
|
||||
if (i < 0 || i >= _num) return NULL; // invalid index
|
||||
|
||||
uint32_t scheduleAt(int i) const {
|
||||
return (i < _num) ? _schedule_table[i] : 0;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
/* Priority of the next packet that get(now) would return, without
|
||||
* removing it. Returns 0xFF if no due packet exists. */
|
||||
uint8_t peekPriority(uint32_t now) const {
|
||||
uint8_t best = 0xFF;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if ((int32_t)(_schedule_table[j] - now) > 0) continue;
|
||||
if (_pri_table[j] < best) best = _pri_table[j];
|
||||
}
|
||||
return best;
|
||||
}
|
||||
bool add(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
|
||||
if (_num == QUEUE_SIZE) {
|
||||
return false;
|
||||
}
|
||||
_table[_num] = packet;
|
||||
_pri_table[_num] = priority;
|
||||
_schedule_table[_num] = scheduled_for;
|
||||
_num++;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool reschedule(int i, uint32_t new_scheduled_for) {
|
||||
if (i >= _num) return false;
|
||||
_schedule_table[i] = new_scheduled_for;
|
||||
return true;
|
||||
}
|
||||
|
||||
int count() const { return _num; }
|
||||
Packet *itemAt(int i) const { return (i < _num) ? _table[i] : nullptr; }
|
||||
// ZEPHCORE: eviction and schedule access (reactive backoff, re-queue priority).
|
||||
int findLowestPriority() const { // least important entry, -1 when empty
|
||||
if (_num == 0) return -1;
|
||||
uint8_t worst = 0;
|
||||
int idx = 0;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if (_pri_table[j] >= worst) {
|
||||
worst = _pri_table[j];
|
||||
idx = j;
|
||||
}
|
||||
}
|
||||
return idx;
|
||||
}
|
||||
uint32_t scheduleAt(int i) const { return (i < _num) ? _schedule_table[i] : 0; }
|
||||
uint8_t peekPriority(uint32_t now) const { // what get(now) would return; 0xFF if none due
|
||||
uint8_t best = 0xFF;
|
||||
for (int j = 0; j < _num; j++) {
|
||||
if ((int32_t)(_schedule_table[j] - now) > 0) continue;
|
||||
if (_pri_table[j] < best) best = _pri_table[j];
|
||||
}
|
||||
return best;
|
||||
}
|
||||
bool reschedule(int i, uint32_t new_scheduled_for) {
|
||||
if (i >= _num) return false;
|
||||
_schedule_table[i] = new_scheduled_for;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
static Packet _packet_pool[POOL_SIZE];
|
||||
static PacketQueue _unused;
|
||||
static PacketQueue _send_queue;
|
||||
static bool _initialized = false;
|
||||
mesh::Packet packet_pool[POOL_SIZE];
|
||||
PacketQueue unused, send_queue;
|
||||
bool initialized = false;
|
||||
|
||||
static void init_pool() {
|
||||
if (_initialized) return;
|
||||
_initialized = true;
|
||||
for (int i = 0; i < POOL_SIZE; i++) {
|
||||
_unused.add(&_packet_pool[i], 0, 0);
|
||||
}
|
||||
// load up our unusued Packet pool (on first use: no static-init ordering hazard)
|
||||
void init_pool() {
|
||||
if (initialized) return;
|
||||
initialized = true;
|
||||
for (int i = 0; i < POOL_SIZE; i++) {
|
||||
unused.add(&packet_pool[i], 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Packet *StaticPoolPacketManager::allocNew()
|
||||
{
|
||||
init_pool();
|
||||
return _unused.removeByIdx(0);
|
||||
} // namespace
|
||||
|
||||
mesh::Packet* StaticPoolPacketManager::allocNew() {
|
||||
init_pool(); // ZEPHCORE
|
||||
return unused.removeByIdx(0); // just get first one (returns NULL if empty)
|
||||
}
|
||||
|
||||
void StaticPoolPacketManager::free(Packet *packet)
|
||||
{
|
||||
if (packet == nullptr) return;
|
||||
|
||||
/* _unused is sized to hold the whole pool, so the only way this add can
|
||||
* fail is a slot being returned twice. That is silent corruption — the
|
||||
* second free drops the packet on the floor and the pool shrinks by one
|
||||
* for the rest of the run — so say so rather than ignoring the return.
|
||||
* Upstream 1d5283de. */
|
||||
if (!_unused.add(packet, 0, 0)) {
|
||||
LOG_WRN("free: unused queue full, possible double-free");
|
||||
}
|
||||
void StaticPoolPacketManager::free(mesh::Packet* packet) {
|
||||
if (packet == NULL) return;
|
||||
if (!unused.add(packet, 0, 0)) {
|
||||
LOG_WRN("free: unused queue full, possible double-free"); // ZEPHCORE: WRN, not debug-only
|
||||
}
|
||||
}
|
||||
|
||||
void StaticPoolPacketManager::queueOutbound(Packet *packet, uint8_t priority, uint32_t scheduled_for)
|
||||
{
|
||||
if (packet == nullptr) return;
|
||||
void StaticPoolPacketManager::queueOutbound(mesh::Packet* packet, uint8_t priority, uint32_t scheduled_for) {
|
||||
if (packet == NULL) return;
|
||||
if (send_queue.add(packet, priority, scheduled_for)) return;
|
||||
|
||||
if (_send_queue.add(packet, priority, scheduled_for)) return;
|
||||
|
||||
/* Queue full — evict the least-important entry to make room.
|
||||
* Only evict if the new packet is higher priority (lower number). */
|
||||
int worst = _send_queue.findLowestPriority();
|
||||
if (worst >= 0 && _send_queue._pri_table[worst] > priority) {
|
||||
uint8_t evicted_pri = _send_queue._pri_table[worst];
|
||||
Packet *evicted = _send_queue.removeByIdx(worst);
|
||||
LOG_WRN("queueOutbound: FULL — evicted type=%d pri=%d for type=%d pri=%d",
|
||||
evicted->getPayloadType(), evicted_pri,
|
||||
packet->getPayloadType(), priority);
|
||||
free(evicted);
|
||||
_send_queue.add(packet, priority, scheduled_for);
|
||||
} else {
|
||||
LOG_WRN("queueOutbound: FULL (%d entries) — dropping type=%d pri=%d",
|
||||
_send_queue.count(), packet->getPayloadType(), priority);
|
||||
free(packet);
|
||||
}
|
||||
// ZEPHCORE: queue full — evict the least important entry if the new packet is
|
||||
// more important (lower number); otherwise drop the new one (as upstream).
|
||||
int worst = send_queue.findLowestPriority();
|
||||
if (worst >= 0 && send_queue._pri_table[worst] > priority) {
|
||||
uint8_t evicted_pri = send_queue._pri_table[worst];
|
||||
mesh::Packet* evicted = send_queue.removeByIdx(worst);
|
||||
LOG_WRN("queueOutbound: FULL — evicted type=%d pri=%d for type=%d pri=%d",
|
||||
evicted->getPayloadType(), evicted_pri, packet->getPayloadType(), priority);
|
||||
free(evicted);
|
||||
send_queue.add(packet, priority, scheduled_for);
|
||||
} else {
|
||||
LOG_WRN("queueOutbound: FULL (%d entries) — dropping type=%d pri=%d",
|
||||
send_queue.count(), packet->getPayloadType(), priority);
|
||||
free(packet);
|
||||
}
|
||||
}
|
||||
|
||||
Packet *StaticPoolPacketManager::getNextOutbound(uint32_t now)
|
||||
{
|
||||
return _send_queue.get(now);
|
||||
mesh::Packet* StaticPoolPacketManager::getNextOutbound(uint32_t now) {
|
||||
return send_queue.get(now);
|
||||
}
|
||||
|
||||
int StaticPoolPacketManager::getOutboundCount(uint32_t now) const
|
||||
{
|
||||
return _send_queue.countBefore(now);
|
||||
int StaticPoolPacketManager::getOutboundCount(uint32_t now) const {
|
||||
return send_queue.countBefore(now);
|
||||
}
|
||||
|
||||
int StaticPoolPacketManager::getOutboundTotal() const
|
||||
{
|
||||
return _send_queue.count();
|
||||
int StaticPoolPacketManager::getOutboundTotal() const {
|
||||
return send_queue.count();
|
||||
}
|
||||
|
||||
int StaticPoolPacketManager::getFreeCount() const
|
||||
{
|
||||
return _unused.count();
|
||||
int StaticPoolPacketManager::getFreeCount() const {
|
||||
return unused.count();
|
||||
}
|
||||
|
||||
Packet *StaticPoolPacketManager::getOutboundByIdx(int i)
|
||||
{
|
||||
return _send_queue.itemAt(i);
|
||||
mesh::Packet* StaticPoolPacketManager::getOutboundByIdx(int i) {
|
||||
return send_queue.itemAt(i);
|
||||
}
|
||||
mesh::Packet* StaticPoolPacketManager::removeOutboundByIdx(int i) {
|
||||
return send_queue.removeByIdx(i);
|
||||
}
|
||||
|
||||
Packet *StaticPoolPacketManager::removeOutboundByIdx(int i)
|
||||
{
|
||||
return _send_queue.removeByIdx(i);
|
||||
// ZEPHCORE: schedule access instead of the inbound queue.
|
||||
uint32_t StaticPoolPacketManager::getOutboundSchedule(int i) const {
|
||||
return send_queue.scheduleAt(i);
|
||||
}
|
||||
|
||||
uint32_t StaticPoolPacketManager::getOutboundSchedule(int i) const
|
||||
{
|
||||
return _send_queue.scheduleAt(i);
|
||||
bool StaticPoolPacketManager::rescheduleOutbound(int i, uint32_t new_scheduled_for) {
|
||||
return send_queue.reschedule(i, new_scheduled_for);
|
||||
}
|
||||
|
||||
bool StaticPoolPacketManager::rescheduleOutbound(int i, uint32_t new_scheduled_for)
|
||||
{
|
||||
return _send_queue.reschedule(i, new_scheduled_for);
|
||||
uint8_t StaticPoolPacketManager::peekNextOutboundPriority(uint32_t now) const {
|
||||
return send_queue.peekPriority(now);
|
||||
}
|
||||
|
||||
uint8_t StaticPoolPacketManager::peekNextOutboundPriority(uint32_t now) const
|
||||
{
|
||||
return _send_queue.peekPriority(now);
|
||||
}
|
||||
|
||||
} /* namespace mesh */
|
||||
|
||||
Reference in New Issue
Block a user