diff --git a/src/modules/TraceRouteModule.cpp b/src/modules/TraceRouteModule.cpp index aa0b6a1eb..f390aafcd 100644 --- a/src/modules/TraceRouteModule.cpp +++ b/src/modules/TraceRouteModule.cpp @@ -16,17 +16,37 @@ bool TraceRouteModule::handleReceivedProtobuf(const meshtastic_MeshPacket &mp, m void TraceRouteModule::alterReceivedProtobuf(meshtastic_MeshPacket &p, meshtastic_RouteDiscovery *r) { auto &incoming = p.decoded; - // Only append an ID for the request (one way) and if we are not the destination (the reply will have our NodeNum already) - if (!incoming.request_id && p.to != nodeDB->getNodeNum()) { - appendMyID(r); - printRoute(r, p.from, NODENUM_BROADCAST); + // Only append IDs for the request (one way) + if (!incoming.request_id) { + // Insert unknown hops if necessary + insertUnknownHops(p, r); + // Don't add ourselves if we are the destination (the reply will have our NodeNum already) + if (p.to != nodeDB->getNodeNum()) { + appendMyID(r); + printRoute(r, p.from, NODENUM_BROADCAST); + } // Set updated route to the payload of the to be flooded packet p.decoded.payload.size = pb_encode_to_bytes(p.decoded.payload.bytes, sizeof(p.decoded.payload.bytes), &meshtastic_RouteDiscovery_msg, r); } } +void TraceRouteModule::insertUnknownHops(meshtastic_MeshPacket &p, meshtastic_RouteDiscovery *r) +{ + // Only insert unknown hops if hop_start is valid + if (p.hop_start != 0 && p.hop_limit <= p.hop_start) { + uint8_t hopsTaken = p.hop_start - p.hop_limit; + int8_t diff = hopsTaken - r->route_count; + for (uint8_t i = 0; i < diff; i++) { + if (r->route_count < sizeof(r->route) / sizeof(r->route[0])) { + r->route[r->route_count] = NODENUM_BROADCAST; // This will represent an unknown hop + r->route_count += 1; + } + } + } +} + void TraceRouteModule::appendMyID(meshtastic_RouteDiscovery *updated) { // Length of route array can normally not be exceeded due to the max. hop_limit of 7 diff --git a/src/modules/TraceRouteModule.h b/src/modules/TraceRouteModule.h index 15e01debd..18a5ac0cb 100644 --- a/src/modules/TraceRouteModule.h +++ b/src/modules/TraceRouteModule.h @@ -19,6 +19,9 @@ class TraceRouteModule : public ProtobufModule void alterReceivedProtobuf(meshtastic_MeshPacket &p, meshtastic_RouteDiscovery *r) override; private: + // Call to add unknown hops (e.g. when a node couldn't decrypt it) to the route based on hopStart and current hopLimit + void insertUnknownHops(meshtastic_MeshPacket &p, meshtastic_RouteDiscovery *r); + // Call to add your ID to the route array of a RouteDiscovery message void appendMyID(meshtastic_RouteDiscovery *r);