SERCOM async/DMA for Wire, SPI, and UART

.gitignore

@@ -4,3 +4,6 @@
 bootloaders/*/build/
 *~
 /libraries/**/build/
+.ci-home
+.ci_home
+*.log

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;
+};

cores/arduino/RingBuffer.h

@@ -22,6 +22,7 @@
 #define _RING_BUFFER_
 
 #include <stdint.h>
+#include <type_traits>
 
 // Define constants and variables for buffering incoming serial data.  We're
 // using a ring buffer (I think), in which head is the index of the location
@@ -32,16 +33,19 @@
 #define SERIAL_BUFFER_SIZE 350
 #endif
 
-template <int N>
+template <int N, typename T = uint8_t>
 class RingBufferN
 {
   public:
-    uint8_t _aucBuffer[N] ;
+    T _aucBuffer[N] ;
     volatile int _iHead ;
     volatile int _iTail ;
 
   public:
     RingBufferN( void ) ;
+    bool store( const T& c ) ;
+    bool read( T& out ) ;
+    bool peek( T& out ) const ;
     void store_char( uint8_t c ) ;
     void clear();
     int read_char();
@@ -57,15 +61,16 @@ class RingBufferN
 typedef RingBufferN<SERIAL_BUFFER_SIZE> RingBuffer;
 
 
-template <int N>
-RingBufferN<N>::RingBufferN( void )
+template <int N, typename T>
+RingBufferN<N, T>::RingBufferN( void )
 {
-    memset( _aucBuffer, 0, N ) ;
+    for (int i = 0; i < N; ++i)
+      _aucBuffer[i] = T{};
     clear();
 }
 
-template <int N>
-void RingBufferN<N>::store_char( uint8_t c )
+template <int N, typename T>
+bool RingBufferN<N, T>::store( const T& c )
 {
   int i = nextIndex(_iHead);
 
@@ -77,30 +82,58 @@ void RingBufferN<N>::store_char( uint8_t c )
   {
     _aucBuffer[_iHead] = c ;
     _iHead = i ;
+    return true;
   }
+  return false;
 }
 
-template <int N>
-void RingBufferN<N>::clear()
+template <int N, typename T>
+bool RingBufferN<N, T>::read( T& out )
 {
-  _iHead = 0;
-  _iTail = 0;
+  if(_iTail == _iHead)
+    return false;
+
+  out = _aucBuffer[_iTail];
+  _iTail = nextIndex(_iTail);
+  return true;
 }
 
-template <int N>
-int RingBufferN<N>::read_char()
+template <int N, typename T>
+bool RingBufferN<N, T>::peek( T& out ) const
 {
   if(_iTail == _iHead)
-    return -1;
+    return false;
 
-  uint8_t value = _aucBuffer[_iTail];
-  _iTail = nextIndex(_iTail);
+  out = _aucBuffer[_iTail];
+  return true;
+}
+
+template <int N, typename T>
+void RingBufferN<N, T>::store_char( uint8_t c )
+{
+  static_assert(std::is_same<T, uint8_t>::value, "store_char only valid for uint8_t buffers");
+  (void)store(static_cast<T>(c));
+}
+
+template <int N, typename T>
+void RingBufferN<N, T>::clear()
+{
+  _iHead = 0;
+  _iTail = 0;
+}
 
+template <int N, typename T>
+int RingBufferN<N, T>::read_char()
+{
+  static_assert(std::is_same<T, uint8_t>::value, "read_char only valid for uint8_t buffers");
+  uint8_t value;
+  if (!read(value))
+    return -1;
   return value;
 }
 
-template <int N>
-int RingBufferN<N>::available()
+template <int N, typename T>
+int RingBufferN<N, T>::available()
 {
   int delta = _iHead - _iTail;
 
@@ -110,32 +143,34 @@ int RingBufferN<N>::available()
     return delta;
 }
 
-template <int N>
-int RingBufferN<N>::availableForStore()
+template <int N, typename T>
+int RingBufferN<N, T>::availableForStore()
 {
   if (_iHead >= _iTail)
     return N - 1 - _iHead + _iTail;
   else
     return _iTail - _iHead - 1;
 }
 
-template <int N>
-int RingBufferN<N>::peek()
+template <int N, typename T>
+int RingBufferN<N, T>::peek()
 {
-  if(_iTail == _iHead)
+  static_assert(std::is_same<T, uint8_t>::value, "peek() only valid for uint8_t buffers");
+  uint8_t value;
+  if (!peek(value))
     return -1;
 
-  return _aucBuffer[_iTail];
+  return value;
 }
 
-template <int N>
-int RingBufferN<N>::nextIndex(int index)
+template <int N, typename T>
+int RingBufferN<N, T>::nextIndex(int index)
 {
   return (uint32_t)(index + 1) % N;
 }
 
-template <int N>
-bool RingBufferN<N>::isFull()
+template <int N, typename T>
+bool RingBufferN<N, T>::isFull()
 {
   return (nextIndex(_iHead) == _iTail);
 }