TraceRouteModule

src/mesh/FloodingRouter.cpp

@@ -41,6 +41,11 @@ void FloodingRouter::sniffReceived(const MeshPacket *p, const Routing *c)
 
                 tosend->hop_limit--; // bump down the hop count
 
+                // If it is a traceRoute request, update the route that it went via me
+                if (p->which_payload_variant == MeshPacket_decoded_tag && traceRouteModule->wantPacket(p)) {
+                    traceRouteModule->updateRoute(tosend);
+                }
+
                 printPacket("Rebroadcasting received floodmsg to neighbors", p);
                 // Note: we are careful to resend using the original senders node id
                 // We are careful not to call our hooked version of send() - because we don't want to check this again

src/mesh/FloodingRouter.h

@@ -2,6 +2,7 @@
 
 #include "PacketHistory.h"
 #include "Router.h"
+#include "modules/TraceRouteModule.h"
 
 /**
  * This is a mixin that extends Router with the ability to do Naive Flooding (in the standard mesh protocol sense)

src/modules/Modules.cpp

@@ -11,6 +11,7 @@
 #include "modules/ReplyModule.h"
 #include "modules/RoutingModule.h"
 #include "modules/TextMessageModule.h"
+#include "modules/TraceRouteModule.h"
 #include "modules/WaypointModule.h"
 #if HAS_TELEMETRY
 #include "modules/Telemetry/DeviceTelemetry.h"
@@ -40,6 +41,7 @@ void setupModules()
     positionModule = new PositionModule();
     waypointModule = new WaypointModule();
     textMessageModule = new TextMessageModule();
+    traceRouteModule = new TraceRouteModule();
     
     // Note: if the rest of meshtastic doesn't need to explicitly use your module, you do not need to assign the instance
     // to a global variable.

src/modules/TraceRouteModule.cpp

@@ -0,0 +1,86 @@
+#include "TraceRouteModule.h"
+#include "MeshService.h"
+#include "FloodingRouter.h"
+
+TraceRouteModule *traceRouteModule; 
+
+
+bool TraceRouteModule::handleReceivedProtobuf(const MeshPacket &mp, RouteDiscovery *r)
+{ 
+    // Only handle a response
+    if (mp.decoded.request_id) {
+        printRoute(r, mp.to, mp.from);
+    }
+    
+    return false; // let it be handled by RoutingModule
+}
+
+
+void TraceRouteModule::updateRoute(MeshPacket* p)
+{ 
+    auto &incoming = p->decoded;
+    // Only append an ID for the request (one way)
+    if (!incoming.request_id) { 
+        RouteDiscovery scratch;
+        RouteDiscovery *updated = NULL;
+        memset(&scratch, 0, sizeof(scratch));
+        pb_decode_from_bytes(incoming.payload.bytes, incoming.payload.size, RouteDiscovery_fields, &scratch);
+        updated = &scratch;
+
+        appendMyID(updated);
+        printRoute(updated, 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), RouteDiscovery_fields, updated);
+    }
+}
+
+
+void TraceRouteModule::appendMyID(RouteDiscovery* updated) 
+{
+    // Length of route array can normally not be exceeded due to the max. hop_limit of 7
+    if (updated->route_count < sizeof(updated->route)/sizeof(updated->route[0])) { 
+        updated->route[updated->route_count] = myNodeInfo.my_node_num;
+        updated->route_count += 1;        
+    } else {
+        DEBUG_MSG("WARNING: Route exceeded maximum hop limit, are you bridging networks?\n");
+    }
+}
+
+
+void TraceRouteModule::printRoute(RouteDiscovery* r, uint32_t origin, uint32_t dest) 
+{
+    DEBUG_MSG("Route traced:\n");
+    DEBUG_MSG("0x%x --> ", origin);
+    for (uint8_t i=0; i<r->route_count; i++) {
+        DEBUG_MSG("0x%x --> ", r->route[i]); 
+    }
+    if (dest != NODENUM_BROADCAST) DEBUG_MSG("0x%x\n", dest); else DEBUG_MSG("...\n");
+}
+
+
+MeshPacket* TraceRouteModule::allocReply() 
+{
+    assert(currentRequest);
+
+    // Copy the payload of the current request
+    auto req = *currentRequest;
+    auto &p = req.decoded;
+    RouteDiscovery scratch;
+    RouteDiscovery *updated = NULL;
+    memset(&scratch, 0, sizeof(scratch));
+    pb_decode_from_bytes(p.payload.bytes, p.payload.size, RouteDiscovery_fields, &scratch);
+    updated = &scratch;
+
+    printRoute(updated, req.from, req.to);
+
+    // Create a MeshPacket with this payload and set it as the reply
+    MeshPacket* reply = allocDataProtobuf(*updated); 
+
+    return reply;
+}
+
+
+TraceRouteModule::TraceRouteModule() : ProtobufModule("traceroute", PortNum_TRACEROUTE_APP, RouteDiscovery_fields) {
+    ourPortNum = PortNum_TRACEROUTE_APP; 
+}

src/modules/TraceRouteModule.h

@@ -0,0 +1,36 @@
+#pragma once
+#include "ProtobufModule.h"
+
+
+/**
+ * A module that traces the route to a certain destination node
+ */
+class TraceRouteModule : public ProtobufModule<RouteDiscovery>
+{
+    public:
+        TraceRouteModule();
+
+        // Let FloodingRouter call updateRoute upon rebroadcasting a TraceRoute request
+        friend class FloodingRouter;
+
+    protected:
+        bool handleReceivedProtobuf(const MeshPacket &mp, RouteDiscovery *r) override;
+
+        virtual MeshPacket *allocReply() override;
+
+        /* Call before rebroadcasting a RouteDiscovery payload in order to update 
+           the route array containing the IDs of nodes this packet went through */
+        void updateRoute(MeshPacket* p);
+    
+    private:
+        // Call to add your ID to the route array of a RouteDiscovery message
+        void appendMyID(RouteDiscovery *r);
+
+        /* Call to print the route array of a RouteDiscovery message.
+           Set origin to where the request came from.
+           Set dest to the ID of its destination, or NODENUM_BROADCAST if it has not yet arrived there. */
+        void printRoute(RouteDiscovery* r, uint32_t origin, uint32_t dest); 
+
+};
+
+extern TraceRouteModule *traceRouteModule;