pw_router: Extract packet priority and forward metadata to egresses

This adds a priority extractor to the router's PacketParser, and defines
a metadata struct which is forwarded to egresses with optional data
extracted from a packet.

Change-Id: I69f856b900e73de22d1ecb86e912aa2f7a96c436
Reviewed-on: https://pigweed-review.googlesource.com/c/pigweed/pigweed/+/48680
Reviewed-by: Wyatt Hepler <hepler@google.com>
diff --git a/pw_router/BUILD.gn b/pw_router/BUILD.gn
index bcf934a..f6fc056 100644
--- a/pw_router/BUILD.gn
+++ b/pw_router/BUILD.gn
@@ -53,7 +53,10 @@
 pw_source_set("egress_function") {
   public_configs = [ ":public_include_path" ]
   public = [ "public/pw_router/egress_function.h" ]
-  public_deps = [ ":egress" ]
+  public_deps = [
+    ":egress",
+    dir_pw_function,
+  ]
 }
 
 pw_doc_group("docs") {
diff --git a/pw_router/docs.rst b/pw_router/docs.rst
index 76fec30..8788826 100644
--- a/pw_router/docs.rst
+++ b/pw_router/docs.rst
@@ -22,6 +22,11 @@
 link. Egress implementations provide a single ``SendPacket`` function, which
 takes the raw packet data and transmits it.
 
+Egresses are provided with optional metadata extracted from the packet, if it
+exists, to aid with transmitting decisions. For example, if packets in a project
+include a priority, egresses may use it to provide quality-of-service by
+dropping certain packets under heavy load.
+
 Some common egress implementations are provided upstream in Pigweed.
 
 StaticRouter
diff --git a/pw_router/public/pw_router/egress.h b/pw_router/public/pw_router/egress.h
index 09097e0..4c4972b 100644
--- a/pw_router/public/pw_router/egress.h
+++ b/pw_router/public/pw_router/egress.h
@@ -13,6 +13,7 @@
 // the License.
 #pragma once
 
+#include <optional>
 #include <span>
 
 #include "pw_bytes/span.h"
@@ -20,6 +21,12 @@
 
 namespace pw::router {
 
+// Data extracted from a packet which is forwarded to the egress.
+struct PacketMetadata {
+  // Project-defined priority of the packet.
+  std::optional<uint32_t> priority;
+};
+
 // Data egress for a router to send packets over some transport system.
 class Egress {
  public:
@@ -29,7 +36,8 @@
   // an error status on failure.
   //
   // TODO(frolv): Document possible return values.
-  virtual Status SendPacket(ConstByteSpan packet) = 0;
+  virtual Status SendPacket(ConstByteSpan packet,
+                            const PacketMetadata& metadata) = 0;
 };
 
 }  // namespace pw::router
diff --git a/pw_router/public/pw_router/egress_function.h b/pw_router/public/pw_router/egress_function.h
index e766766..2bbc69f 100644
--- a/pw_router/public/pw_router/egress_function.h
+++ b/pw_router/public/pw_router/egress_function.h
@@ -15,6 +15,7 @@
 
 #include <span>
 
+#include "pw_function/function.h"
 #include "pw_router/egress.h"
 
 namespace pw::router {
@@ -22,12 +23,17 @@
 // Router egress that dispatches to a free function.
 class EgressFunction final : public Egress {
  public:
-  constexpr EgressFunction(Status (*func)(ConstByteSpan)) : func_(*func) {}
+  EgressFunction(
+      Function<Status(ConstByteSpan, const PacketMetadata&)> function)
+      : func_(std::move(function)) {}
 
-  Status SendPacket(ConstByteSpan packet) final { return func_(packet); }
+  Status SendPacket(ConstByteSpan packet,
+                    const PacketMetadata& metadata) final {
+    return func_(packet, metadata);
+  }
 
  private:
-  Status (&func_)(ConstByteSpan);
+  Function<Status(ConstByteSpan, const PacketMetadata&)> func_;
 };
 
 }  // namespace pw::router
diff --git a/pw_router/public/pw_router/packet_parser.h b/pw_router/public/pw_router/packet_parser.h
index c4ecc8a..4679454 100644
--- a/pw_router/public/pw_router/packet_parser.h
+++ b/pw_router/public/pw_router/packet_parser.h
@@ -41,6 +41,13 @@
   // Guaranteed to only be called if Parse() succeeded and while the data passed
   // to Parse() is valid.
   virtual std::optional<uint32_t> GetDestinationAddress() const = 0;
+
+  // Extracts the project-specific priority of the last parsed packet, if one
+  // exists.
+  //
+  // Guaranteed to only be called if Parse() succeeded and while the data passed
+  // to Parse() is valid.
+  virtual std::optional<uint32_t> GetPriority() const { return std::nullopt; }
 };
 
 }  // namespace pw::router
diff --git a/pw_router/static_router.cc b/pw_router/static_router.cc
index a32e74a..6f1ad54 100644
--- a/pw_router/static_router.cc
+++ b/pw_router/static_router.cc
@@ -21,6 +21,7 @@
 
 Status StaticRouter::RoutePacket(ConstByteSpan packet) {
   uint32_t address;
+  PacketMetadata metadata = {};
 
   {
     // Only packet parsing is synchronized within the router; egresses must be
@@ -39,6 +40,9 @@
     }
 
     address = result.value();
+
+    // Populate the metadata with fields extracted from the packet.
+    metadata.priority = parser_.GetPriority();
   }
 
   auto route = std::find_if(routes_.begin(), routes_.end(), [&](auto r) {
@@ -49,7 +53,8 @@
     return Status::NotFound();
   }
 
-  if (Status status = route->egress.SendPacket(packet); !status.ok()) {
+  if (Status status = route->egress.SendPacket(packet, metadata);
+      !status.ok()) {
     egress_errors_.Increment();
     return Status::Unavailable();
   }
diff --git a/pw_router/static_router_test.cc b/pw_router/static_router_test.cc
index 172cc01..50f3437 100644
--- a/pw_router/static_router_test.cc
+++ b/pw_router/static_router_test.cc
@@ -25,12 +25,16 @@
   static constexpr uint32_t kMagic = 0x8badf00d;
 
   constexpr BasicPacket(uint32_t addr, uint64_t data)
-      : magic(kMagic), address(addr), payload(data) {}
+      : magic(kMagic), address(addr), priority(0), payload(data) {}
+
+  constexpr BasicPacket(uint32_t addr, uint32_t prio, uint64_t data)
+      : magic(kMagic), address(addr), priority(prio), payload(data) {}
 
   ConstByteSpan data() const { return std::as_bytes(std::span(this, 1)); }
 
   uint32_t magic;
   uint32_t address;
+  uint32_t priority;
   uint64_t payload;
 };
 
@@ -48,12 +52,19 @@
     return packet_->address;
   }
 
+  std::optional<uint32_t> GetPriority() const final {
+    PW_DCHECK_NOTNULL(packet_);
+    return packet_->priority;
+  };
+
  private:
   const BasicPacket* packet_;
 };
 
-EgressFunction GoodEgress(+[](ConstByteSpan) { return OkStatus(); });
-EgressFunction BadEgress(+[](ConstByteSpan) {
+EgressFunction GoodEgress(+[](ConstByteSpan, const PacketMetadata&) {
+  return OkStatus();
+});
+EgressFunction BadEgress(+[](ConstByteSpan, const PacketMetadata&) {
   return Status::ResourceExhausted();
 });
 
@@ -67,6 +78,23 @@
             Status::Unavailable());
 }
 
+TEST(StaticRouter, RoutePacket_ForwardsPacketMetadata) {
+  PacketMetadata metadata = {};
+  EgressFunction metadata_egress(
+      [&metadata](ConstByteSpan, const PacketMetadata& md) {
+        metadata = md;
+        return OkStatus();
+      });
+
+  BasicPacketParser parser;
+  StaticRouter::Route routes[] = {{1, metadata_egress}};
+  StaticRouter router(parser, std::span(routes));
+
+  EXPECT_EQ(router.RoutePacket(BasicPacket(1, 71, 0xdddd).data()), OkStatus());
+  ASSERT_TRUE(metadata.priority.has_value());
+  EXPECT_EQ(metadata.priority.value(), 71u);
+}
+
 TEST(StaticRouter, RoutePacket_ReturnsParserError) {
   BasicPacketParser parser;
   constexpr StaticRouter::Route routes[] = {{1, GoodEgress}, {2, BadEgress}};