Message queue to print incoming SysEx

sysex-queue.ino

#include <MIDI.h>
#include <string.h>

MIDI_CREATE_DEFAULT_INSTANCE();

template<unsigned Size, typename DataType>
struct MessageQueue {
  static constexpr unsigned sMask = Size - 1;

  inline MessageQueue<Size, DataType>()
    : r(0)
    , w(0)
  {
    static_assert((Size & (Size - 1)) == 0, "Size must be a power of 2");
    memset(data, 0, sizeof(DataType) * Size);
  }

  inline void write(DataType value)
  {
    data[w] = value;
    w = (w + 1) & sMask;
  }

  inline DataType read()
  {
    const DataType value = data[r];
    r = (r + 1) & sMask;
    return value;
  }

  inline unsigned size() const
  {
    return (Size + w - r) % Size;
  }

  inline void clear()
  {
    w = 0;
    r = 0;
    memset(data, 0, sizeof(DataType) * Size);
  }

  DataType data[Size];
  unsigned r;
  unsigned w;
};

MessageQueue<32, byte> messageQueue;

void onSysExReceived(byte* data, unsigned size)
{
  for (unsigned i = 0; i < size; ++i)
  {
    messageQueue.write(data[i]);
  }
}

void setup() {
  Serial1.begin(112500);
  MIDI.begin();
  MIDI.setHandleSystemExclusive(onSysExReceived);
  messageQueue.clear();
}

void loop() {
  MIDI.read();
  if (messageQueue.size())
  {
    Serial1.println(messageQueue.read());
  }
}