diff --git a/src/mesh/PacketCache.cpp b/src/mesh/PacketCache.cpp new file mode 100644 index 000000000..0edf81741 --- /dev/null +++ b/src/mesh/PacketCache.cpp @@ -0,0 +1,253 @@ +#include "PacketCache.h" +#include "Router.h" + +PacketCache packetCache{}; + +/** + * Allocate a new cache entry and copy the packet header and payload into it + */ +PacketCacheEntry *PacketCache::cache(const meshtastic_MeshPacket *p, bool preserveMetadata) +{ + size_t payload_size = + (p->which_payload_variant == meshtastic_MeshPacket_encrypted_tag) ? p->encrypted.size : p->decoded.payload.size; + PacketCacheEntry *e = (PacketCacheEntry *)malloc(sizeof(PacketCacheEntry) + payload_size + + (preserveMetadata ? sizeof(PacketCacheMetadata) : 0)); + if (!e) { + LOG_ERROR("Unable to allocate memory for packet cache entry"); + return NULL; + } + + *e = {}; + e->header.from = p->from; + e->header.to = p->to; + e->header.id = p->id; + e->header.channel = p->channel; + e->header.next_hop = p->next_hop; + e->header.relay_node = p->relay_node; + e->header.flags = (p->hop_limit & PACKET_FLAGS_HOP_LIMIT_MASK) | (p->want_ack ? PACKET_FLAGS_WANT_ACK_MASK : 0) | + (p->via_mqtt ? PACKET_FLAGS_VIA_MQTT_MASK : 0) | + ((p->hop_start << PACKET_FLAGS_HOP_START_SHIFT) & PACKET_FLAGS_HOP_START_MASK); + + PacketCacheMetadata m{}; + if (preserveMetadata) { + e->has_metadata = true; + m.rx_rssi = (uint8_t)(p->rx_rssi + 200); + m.rx_snr = (uint8_t)((p->rx_snr + 30.0f) / 0.25f); + m.rx_time = p->rx_time; + m.transport_mechanism = p->transport_mechanism; + m.priority = p->priority; + } + if (p->which_payload_variant == meshtastic_MeshPacket_encrypted_tag) { + e->encrypted = true; + e->payload_len = p->encrypted.size; + memcpy(((unsigned char *)e) + sizeof(PacketCacheEntry), p->encrypted.bytes, p->encrypted.size); + } else if (p->which_payload_variant == meshtastic_MeshPacket_decoded_tag) { + e->encrypted = false; + if (preserveMetadata) { + m.portnum = p->decoded.portnum; + m.want_response = p->decoded.want_response; + m.emoji = p->decoded.emoji; + m.bitfield = p->decoded.bitfield; + if (p->decoded.reply_id) + m.reply_id = p->decoded.reply_id; + else if (p->decoded.request_id) + m.request_id = p->decoded.request_id; + } + e->payload_len = p->decoded.payload.size; + memcpy(((unsigned char *)e) + sizeof(PacketCacheEntry), p->decoded.payload.bytes, p->decoded.payload.size); + } else { + LOG_ERROR("Unable to cache packet with unknown payload type %d", p->which_payload_variant); + free(e); + return NULL; + } + if (preserveMetadata) + memcpy(((unsigned char *)e) + sizeof(PacketCacheEntry) + e->payload_len, &m, sizeof(m)); + + size += sizeof(PacketCacheEntry) + e->payload_len + (e->has_metadata ? sizeof(PacketCacheMetadata) : 0); + insert(e); + return e; +}; + +/** + * Dump a list of packets into the provided buffer + */ +void PacketCache::dump(void *dest, const PacketCacheEntry **entries, size_t num_entries) +{ + unsigned char *pos = (unsigned char *)dest; + for (size_t i = 0; i < num_entries; i++) { + size_t entry_len = + sizeof(PacketCacheEntry) + entries[i]->payload_len + (entries[i]->has_metadata ? sizeof(PacketCacheMetadata) : 0); + memcpy(pos, entries[i], entry_len); + pos += entry_len; + } +} + +/** + * Calculate the length of buffer needed to dump the specified entries + */ +size_t PacketCache::dumpSize(const PacketCacheEntry **entries, size_t num_entries) +{ + size_t total_size = 0; + for (size_t i = 0; i < num_entries; i++) { + total_size += sizeof(PacketCacheEntry) + entries[i]->payload_len; + if (entries[i]->has_metadata) + total_size += sizeof(PacketCacheMetadata); + } + return total_size; +} + +/** + * Find a packet in the cache + */ +PacketCacheEntry *PacketCache::find(NodeNum from, PacketId id) +{ + uint16_t h = PACKET_HASH(from, id); + PacketCacheEntry *e = buckets[PACKET_CACHE_BUCKET(h)]; + while (e) { + if (e->header.from == from && e->header.id == id) + return e; + e = e->next; + } + return NULL; +} + +/** + * Find a packet in the cache by its hash + */ +PacketCacheEntry *PacketCache::find(PacketHash h) +{ + PacketCacheEntry *e = buckets[PACKET_CACHE_BUCKET(h)]; + while (e) { + if (PACKET_HASH(e->header.from, e->header.id) == h) + return e; + e = e->next; + } + return NULL; +} + +/** + * Load a list of packets from the provided buffer + */ +bool PacketCache::load(void *src, PacketCacheEntry **entries, size_t num_entries) +{ + memset(entries, 0, sizeof(PacketCacheEntry *) * num_entries); + unsigned char *pos = (unsigned char *)src; + for (size_t i = 0; i < num_entries; i++) { + PacketCacheEntry e{}; + memcpy(&e, pos, sizeof(PacketCacheEntry)); + size_t entry_len = sizeof(PacketCacheEntry) + e.payload_len + (e.has_metadata ? sizeof(PacketCacheMetadata) : 0); + entries[i] = (PacketCacheEntry *)malloc(entry_len); + size += entry_len; + if (!entries[i]) { + LOG_ERROR("Unable to allocate memory for packet cache entry"); + for (size_t j = 0; j < i; j++) { + size -= sizeof(PacketCacheEntry) + entries[j]->payload_len + + (entries[j]->has_metadata ? sizeof(PacketCacheMetadata) : 0); + free(entries[j]); + entries[j] = NULL; + } + return false; + } + memcpy(entries[i], pos, entry_len); + pos += entry_len; + } + for (size_t i = 0; i < num_entries; i++) + insert(entries[i]); + return true; +} + +/** + * Copy the cached packet into the provided MeshPacket structure + */ +void PacketCache::rehydrate(const PacketCacheEntry *e, meshtastic_MeshPacket *p) +{ + if (!e || !p) + return; + + *p = {}; + p->from = e->header.from; + p->to = e->header.to; + p->id = e->header.id; + p->channel = e->header.channel; + p->next_hop = e->header.next_hop; + p->relay_node = e->header.relay_node; + p->hop_limit = e->header.flags & PACKET_FLAGS_HOP_LIMIT_MASK; + p->want_ack = !!(e->header.flags & PACKET_FLAGS_WANT_ACK_MASK); + p->via_mqtt = !!(e->header.flags & PACKET_FLAGS_VIA_MQTT_MASK); + p->hop_start = (e->header.flags & PACKET_FLAGS_HOP_START_MASK) >> PACKET_FLAGS_HOP_START_SHIFT; + p->which_payload_variant = e->encrypted ? meshtastic_MeshPacket_encrypted_tag : meshtastic_MeshPacket_decoded_tag; + + unsigned char *payload = ((unsigned char *)e) + sizeof(PacketCacheEntry); + PacketCacheMetadata m{}; + if (e->has_metadata) { + memcpy(&m, (payload + e->payload_len), sizeof(m)); + p->rx_rssi = ((int)m.rx_rssi) - 200; + p->rx_snr = ((float)m.rx_snr * 0.25f) - 30.0f; + p->rx_time = m.rx_time; + p->transport_mechanism = (meshtastic_MeshPacket_TransportMechanism)m.transport_mechanism; + p->priority = (meshtastic_MeshPacket_Priority)m.priority; + } + if (e->encrypted) { + memcpy(p->encrypted.bytes, payload, e->payload_len); + p->encrypted.size = e->payload_len; + } else { + memcpy(p->decoded.payload.bytes, payload, e->payload_len); + p->decoded.payload.size = e->payload_len; + if (e->has_metadata) { + // Decrypted-only metadata + p->decoded.portnum = (meshtastic_PortNum)m.portnum; + p->decoded.want_response = m.want_response; + p->decoded.emoji = m.emoji; + p->decoded.bitfield = m.bitfield; + if (m.reply_id) + p->decoded.reply_id = m.reply_id; + else if (m.request_id) + p->decoded.request_id = m.request_id; + } + } +} + +/** + * Release a cache entry + */ +void PacketCache::release(PacketCacheEntry *e) +{ + if (!e) + return; + remove(e); + size -= sizeof(PacketCacheEntry) + e->payload_len + (e->has_metadata ? sizeof(PacketCacheMetadata) : 0); + free(e); +} + +/** + * Insert a new entry into the hash table + */ +void PacketCache::insert(PacketCacheEntry *e) +{ + assert(e); + PacketHash h = PACKET_HASH(e->header.from, e->header.id); + PacketCacheEntry **target = &buckets[PACKET_CACHE_BUCKET(h)]; + e->next = *target; + *target = e; + num_entries++; +} + +/** + * Remove an entry from the hash table + */ +void PacketCache::remove(PacketCacheEntry *e) +{ + assert(e); + PacketHash h = PACKET_HASH(e->header.from, e->header.id); + PacketCacheEntry **target = &buckets[PACKET_CACHE_BUCKET(h)]; + while (*target) { + if (*target == e) { + *target = e->next; + e->next = NULL; + num_entries--; + break; + } else { + target = &(*target)->next; + } + } +} \ No newline at end of file diff --git a/src/mesh/PacketCache.h b/src/mesh/PacketCache.h new file mode 100644 index 000000000..81ad455da --- /dev/null +++ b/src/mesh/PacketCache.h @@ -0,0 +1,75 @@ +#pragma once +#include "RadioInterface.h" + +#define PACKET_HASH(a, b) ((((a ^ b) >> 16) ^ (a ^ b)) & 0xFFFF) // 16 bit fold of packet (from, id) tuple +typedef uint16_t PacketHash; + +#define PACKET_CACHE_BUCKETS 64 // Number of hash table buckets +#define PACKET_CACHE_BUCKET(h) (((h >> 12) ^ (h >> 6) ^ h) & 0x3F) // Fold hash down to 6-bit bucket index + +typedef struct PacketCacheEntry { + PacketCacheEntry *next; + PacketHeader header; + uint16_t payload_len = 0; + union { + uint16_t bitfield; + struct { + uint8_t encrypted : 1; // Payload is encrypted + uint8_t has_metadata : 1; // Payload includes PacketCacheMetadata + uint8_t : 6; // Reserved for future use + uint16_t : 8; // Reserved for future use + }; + }; +} PacketCacheEntry; + +typedef struct PacketCacheMetadata { + PacketCacheMetadata() : _bitfield(0), reply_id(0), _bitfield2(0) {} + union { + uint32_t _bitfield; + struct { + uint16_t portnum : 9; // meshtastic_MeshPacket::decoded::portnum + uint16_t want_response : 1; // meshtastic_MeshPacket::decoded::want_response + uint16_t emoji : 1; // meshtastic_MeshPacket::decoded::emoji + uint16_t bitfield : 5; // meshtastic_MeshPacket::decoded::bitfield (truncated) + uint8_t rx_rssi : 8; // meshtastic_MeshPacket::rx_rssi (map via actual RSSI + 200) + uint8_t rx_snr : 8; // meshtastic_MeshPacket::rx_snr (map via (p->rx_snr + 30.0f) / 0.25f) + }; + }; + union { + uint32_t reply_id; // meshtastic_MeshPacket::decoded.reply_id + uint32_t request_id; // meshtastic_MeshPacket::decoded.request_id + }; + uint32_t rx_time = 0; // meshtastic_MeshPacket::rx_time + uint8_t transport_mechanism = 0; // meshtastic_MeshPacket::transport_mechanism + struct { + uint8_t _bitfield2; + union { + uint8_t priority : 7; // meshtastic_MeshPacket::priority + uint8_t reserved : 1; // Reserved for future use + }; + }; +} PacketCacheMetadata; + +class PacketCache +{ + public: + PacketCacheEntry *cache(const meshtastic_MeshPacket *p, bool preserveMetadata); + static void dump(void *dest, const PacketCacheEntry **entries, size_t num_entries); + size_t dumpSize(const PacketCacheEntry **entries, size_t num_entries); + PacketCacheEntry *find(NodeNum from, PacketId id); + PacketCacheEntry *find(PacketHash h); + bool load(void *src, PacketCacheEntry **entries, size_t num_entries); + size_t getNumEntries() { return num_entries; } + size_t getSize() { return size; } + void rehydrate(const PacketCacheEntry *e, meshtastic_MeshPacket *p); + void release(PacketCacheEntry *e); + + private: + PacketCacheEntry *buckets[PACKET_CACHE_BUCKETS]{}; + size_t num_entries = 0; + size_t size = 0; + void insert(PacketCacheEntry *e); + void remove(PacketCacheEntry *e); +}; + +extern PacketCache packetCache; \ No newline at end of file