diff --git a/src/mesh/FloodingRouter.cpp b/src/mesh/FloodingRouter.cpp index 818bacf45..925735903 100644 --- a/src/mesh/FloodingRouter.cpp +++ b/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 diff --git a/src/mesh/FloodingRouter.h b/src/mesh/FloodingRouter.h index 387b4576b..7e6271fc0 100644 --- a/src/mesh/FloodingRouter.h +++ b/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) diff --git a/src/modules/Modules.cpp b/src/modules/Modules.cpp index b0b923863..ece160ced 100644 --- a/src/modules/Modules.cpp +++ b/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. diff --git a/src/modules/TraceRouteModule.cpp b/src/modules/TraceRouteModule.cpp new file mode 100644 index 000000000..1c5fd97d3 --- /dev/null +++ b/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; iroute_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; +} diff --git a/src/modules/TraceRouteModule.h b/src/modules/TraceRouteModule.h new file mode 100644 index 000000000..15b7a564d --- /dev/null +++ b/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 +{ + 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; \ No newline at end of file