Add shared PendSV deferred service dispatcher

cores/arduino/PendSV.cpp

@@ -0,0 +1,116 @@
+#include "PendSV.h"
+
+#include <Arduino.h>
+#include <limits.h>
+
+namespace {
+PendSV pendSv;
+}
+
+PendSV &PendSV::instance() {
+  return pendSv;
+}
+
+uint32_t PendSV::enterCritical() {
+  const uint32_t primask = __get_PRIMASK();
+  __disable_irq();
+  return primask;
+}
+
+void PendSV::exitCritical(uint32_t primask) {
+  __set_PRIMASK(primask);
+}
+
+bool PendSV::registerService(uint8_t serviceId, ServiceFn fn, void *context) {
+  if (serviceId >= kMaxServices || fn == nullptr)
+    return false;
+
+  const uint32_t primask = enterCritical();
+  pendingCount_[serviceId] = 0;
+  pendingMask_ &= ~(1u << serviceId);
+  services_[serviceId].fn = fn;
+  services_[serviceId].context = context;
+  exitCritical(primask);
+
+  return true;
+}
+
+void PendSV::clearService(uint8_t serviceId) {
+  if (serviceId >= kMaxServices)
+    return;
+
+  const uint32_t primask = enterCritical();
+  services_[serviceId].fn = nullptr;
+  services_[serviceId].context = nullptr;
+  pendingCount_[serviceId] = 0;
+  pendingMask_ &= ~(1u << serviceId);
+  exitCritical(primask);
+}
+
+void PendSV::setPending(uint8_t serviceId) {
+  if (serviceId >= kMaxServices)
+    return;
+
+  const uint32_t primask = enterCritical();
+  uint16_t &pendingCount = pendingCount_[serviceId];
+  if (pendingCount < UINT16_MAX)
+    ++pendingCount;
+  pendingMask_ |= (1u << serviceId);
+  exitCritical(primask);
+
+  __DMB();
+  SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+}
+
+void PendSV::dispatchPending() {
+  // Bound one PendSV entry so high-rate producers do not monopolize return to
+  // thread mode. Remaining work re-pends PendSV below.
+  uint8_t dispatched = 0;
+
+  while (dispatched < kDispatchBudget) {
+    uint32_t primask = enterCritical();
+    const uint32_t pending = pendingMask_;
+    if (pending == 0) {
+      exitCritical(primask);
+      return;
+    }
+
+    const uint8_t serviceId = static_cast<uint8_t>(__builtin_ctz(pending));
+    uint16_t &pendingCount = pendingCount_[serviceId];
+
+    if (pendingCount == 0) {
+      // Defensive scrub in case mask and count drift out of sync.
+      pendingMask_ &= ~(1u << serviceId);
+      exitCritical(primask);
+      continue;
+    }
+
+    --pendingCount;
+    if (pendingCount == 0)
+      pendingMask_ &= ~(1u << serviceId);
+
+    ServiceEntry entry = services_[serviceId];
+    exitCritical(primask);
+
+    // Pending work without a registered service is intentionally dropped.
+    // Producers are expected to register before calling setPending(), and
+    // clearService() cancels queued work for that service.
+    if (entry.fn != nullptr)
+      entry.fn(serviceId, entry.context);
+
+    ++dispatched;
+  }
+
+  const uint32_t primask = enterCritical();
+  const bool hasRemaining = (pendingMask_ != 0);
+  exitCritical(primask);
+
+  if (hasRemaining) {
+    __DMB();
+    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
+  }
+}
+
+extern "C" void PendSV_Handler(void) {
+  PendSV::instance().dispatchPending();
+}

cores/arduino/PendSV.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <array>
+#include <stdint.h>
+
+class PendSV {
+  public:
+    using ServiceFn = void (*)(uint8_t serviceId, void *context);
+    static constexpr uint8_t kMaxServices = 32;
+    static constexpr uint8_t kDispatchBudget = 16;
+    static_assert(kMaxServices <= 32, "PendSV pending mask supports at most 32 services");
+
+    static PendSV &instance();
+
+    bool registerService(uint8_t serviceId, ServiceFn fn, void *context = nullptr);
+    // Cancels queued work that has not started dispatching. If a callback was
+    // already copied for dispatch, its context must remain valid until it returns.
+    void clearService(uint8_t serviceId);
+    void dispatchPending();
+    void setPending(uint8_t serviceId);
+
+  private:
+    static uint32_t enterCritical();
+    static void exitCritical(uint32_t primask);
+
+    struct ServiceEntry {
+      ServiceFn fn = nullptr;
+      void *context = nullptr;
+    };
+
+    std::array<ServiceEntry, kMaxServices> services_{};
+    std::array<uint16_t, kMaxServices> pendingCount_{};
+    volatile uint32_t pendingMask_ = 0;
+};