Unify frame metadata setting (addrs and owning_device)

src/avclan/peripheral.hpp

@@ -9,12 +9,19 @@
 #include "device.hpp"
 #include "frame.hpp"
 
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <tuple>
+#include <utility>
 
 namespace avclan {
+
+enum class Party : uint8_t { Sender, Recipient };
+
 template <DeviceInterface... Devs> class Peripheral {
+  using enum Party;
+
 public:
   using Error = detail::Error;
 
@@ -41,7 +48,8 @@ public:
   };
   Error::Send send(Frame *out, Frame::Print print) {
     // To "forge" a controller_addr, instantiate a new/different Peripheral
-    postmark(out);
+    stamp<Sender>(out);
+    out->control = 0xF;
     return bus.send(out, print);
   };
 
@@ -60,7 +68,7 @@ public:
     static const uint8_t lancheck_resp[] = {0x00, to_underlying(COMM_CTRL),
                                             to_underlying(LAN), 0xFF, 0xFF};
 
-    out->peripheral_addr = controller_;
+    stamp<Recipient>(out);
 
     const uint8_t *data = in->data;
     const uint8_t b0 = *data++;
@@ -103,15 +111,14 @@ public:
         case PACK3(COMMUNICATION_V1, COMM_CTRL,
                    to_underlying(Advertise_Function)):
         case PACK3(COMMUNICATION_V2, COMM_CTRL,
-                   to_underlying(Advertise_Function)):
+                   to_underlying(Advertise_Function)): {
+          auto enable_d = [](auto &d, auto &out) { d.enable(out); };
           ((Devs::id == static_cast<Device>(b3)
-            ? [&] {
-                out->owning_device = Devs::id;
-                std::get<Devs>(devices_).enable(out);
-              }()
-            : void()),
+                ? originate(std::get<Devs>(devices_), out, enable_d)
+                : void()),
            ...);
           break;
+        }
         case PACK3(COMMUNICATION_V1, COMM_CTRL, to_underlying(Ping_Req)):
         case PACK3(COMMUNICATION_V2, COMM_CTRL, to_underlying(Ping_Req)): {
           out->is_unicast = true;
@@ -128,7 +135,7 @@ public:
         case PACK3(COMMUNICATION_V2, COMM_CTRL,
                    to_underlying(List_Functions_Req)): {
           controller_ = in->controller_addr;
-          out->peripheral_addr = controller_;
+          stamp<Recipient>(out); // re-stamp now that controller_ is known
           out->is_unicast = true;
           const uint8_t list_functions_resp[] = {
               0x00, to_underlying(COMM_CTRL), from,
@@ -142,9 +149,10 @@ public:
         default: break;
       }
     } else if (in->peripheral_addr == address_ && b0 == 0x00) {
+      auto handle_d = [&](auto &d, auto &out) { d.handle(in, out); };
       ((Devs::id == static_cast<Device>(b2)
-        ? device_preroute(std::get<Devs>(devices_), in, out),
-        0 : 0),
+            ? originate(std::get<Devs>(devices_), out, handle_d)
+            : void()),
        ...);
     }
   }
@@ -154,31 +162,64 @@ public:
   void react(Frame *out, Error::Send err) {
     if (((Devs::id == out->owning_device) || ...))
       ((Devs::id == out->owning_device
-        ? std::get<Devs>(devices_).react(out, err),
-        0 : 0),
+            ? void(std::get<Devs>(devices_).react(out, err))
+            : void()),
        ...);
     else
       out->reaction = 0;
   }
 
-  template <class F> void poll_devices(F &&fun) {
-    (poller(std::get<Devs>(devices_), fun), ...);
+  bool pending() const { return (std::get<Devs>(devices_).pending() || ...); }
+
+  // Service ready devices in round-robin order
+  bool emit(Frame *out) {
+    auto does_emit = [&](DeviceInterface auto &dev) -> bool {
+      if (!dev.pending())
+        return false;
+      originate(dev, out, [](auto &d, auto &out) { d.emit(out); });
+      dev.resolvepending();
+      return true;
+    };
+
+    // Runtime tuple index helper
+    auto does_index_emit = [&](std::size_t t) -> bool {
+      return [&]<std::size_t... Is>(std::index_sequence<Is...>) {
+        return (((Is == t) && does_emit(std::get<Is>(devices_))) || ...);
+      }(std::index_sequence_for<Devs...>{});
+    };
+
+    constexpr std::size_t N = sizeof...(Devs);
+    if constexpr (N == 1) { // round-robin not needed
+      return does_emit(std::get<0>(devices_));
+    } else {
+      static uint8_t rr_ = 0; // round-robin cursor
+      const std::size_t start = rr_;
+      for (std::size_t t = start; t < N; ++t) // [start, N)
+        if (does_index_emit(t)) {
+          rr_ = (t + 1 == N) ? 0 : t + 1;
+          return true;
+        }
+      for (std::size_t t = 0; t < start; ++t) // [0, start); t+1 <= start < N
+        if (does_index_emit(t)) {
+          rr_ = t + 1;
+          return true;
+        }
+      return false;
+    }
   }
 
 private:
-  void postmark(Frame *out) const {
-    out->controller_addr = address_;
-    out->control = 0xF;
+  template <Party P> void stamp(Frame *out) const {
+    if constexpr (P == Sender)
+      out->controller_addr = address_;
+    else
+      out->peripheral_addr = controller_;
   }
 
-  template <DeviceInterface Dev, class F> void poller(Dev &dev, F &&fun) {
-    if (dev.pending() && fun(dev))
-      dev.resolvepending();
-  }
-  template <DeviceInterface Dev>
-  void device_preroute(Dev &dev, const Frame *in, Frame *out) {
-    out->owning_device = Dev::id;
-    dev.handle(in, out);
+  void originate(DeviceInterface auto &dev, Frame *out, auto &&fill) {
+    out->owning_device = std::remove_reference_t<decltype(dev)>::id;
+    stamp<Recipient>(out); // default set FIRST; fill() may override
+    fill(dev, out);
   }
 
   Bus &bus;