daq.cpp

Go to the documentation of this file.

// Copyright (c) 2024 anabrid GmbH
// Contact: https://www.anabrid.com/licensing/
// SPDX-License-Identifier: MIT OR GPL-2.0-or-later
 
#include <Arduino.h>
#include <DMAChannel.h>
#include <algorithm>
#include <bitset>
 
#include <carrier/carrier.h>
#include <daq/teensy/daq.h>
#include <utils/logging.h>
#include <utils/running_avg.h>
 
namespace daq {
 
namespace stream {
 
namespace details {
 
// We need a memory segment to implement a ring buffer.
// Its size should be a power-of-2-multiple of NUM_CHANNELS.
// The DMA implements ring buffer by restricting the N lower bits of the destination address to change.
// That means the memory segment must start at a memory address with enough lower bits being zero,
// see manual "data queues [with] power-of-2 size bytes, [...] should start at a 0-modulo-size address".
// One can put this into DMAMEM for increased performance, but that did not work for me right away.
// *MUST* be at least 2*NUM_CHANNELS, otherwise some math later does not work.
__attribute__((aligned(BUFFER_SIZE * 4))) std::array<volatile uint32_t, BUFFER_SIZE> buffer = {0};
 
DMAChannel channel(false);
const run::Run *run = nullptr;
run::RunDataHandler *run_data_handler = nullptr;
volatile bool first_data = false;
volatile bool last_data = false;
volatile bool overflow_data = false;
 
// NOT FLASHMEM
void interrupt() {
  // Serial.println(__PRETTY_FUNCTION__);
  auto is_half = (channel.TCD->CITER != channel.TCD->BITER);
  if (is_half) {
    overflow_data |= first_data;
    first_data = true;
  } else {
    overflow_data |= last_data;
    last_data = true;
  }
 
  // Clear interrupt
  channel.clearInterrupt();
  // Memory barrier
#ifdef ARDUINO
  asm("DSB");
#endif
}
 
} // namespace details
 
} // namespace stream
 
} // namespace daq
 
FLASHMEM void daq::enable() { details::flexio::get_flexio()->port().CTRL |= FLEXIO_CTRL_FLEXEN; }
 
FLASHMEM void daq::disable() { details::flexio::get_flexio()->port().CTRL &= ~FLEXIO_CTRL_FLEXEN; }
 
FLASHMEM void daq::reset() {
  disable();
  details::flexio::get_flexio()->port().CTRL |= FLEXIO_CTRL_SWRST;
  delayNanoseconds(100);
  details::flexio::get_flexio()->port().CTRL &= ~FLEXIO_CTRL_SWRST;
  delayNanoseconds(100);
}
 
FLASHMEM status daq::details::flexio::_init_once() {
  // Configure and enable clock
  get_flexio()->setClockSettings(3, 0, 0);
  get_flexio()->hardware().clock_gate_register |= get_flexio()->hardware().clock_gate_mask;
 
  // Put MOSI pins into flexio mode
  uint8_t _flexio_pin_cnvst = get_flexio()->mapIOPinToFlexPin(PIN_CNVST);
  uint8_t _flexio_pin_clk = get_flexio()->mapIOPinToFlexPin(PIN_CLK);
  uint8_t _flexio_pin_gate = get_flexio()->mapIOPinToFlexPin(PIN_GATE);
  // Check if any of the pins could not be mapped to the same FlexIO module
  if (_flexio_pin_cnvst == 0xff or _flexio_pin_clk == 0xff or _flexio_pin_gate == 0xff)
    return status("Could not map pins to FlexIO module.");
  get_flexio()->setIOPinToFlexMode(PIN_CNVST);
  get_flexio()->setIOPinToFlexMode(PIN_CLK);
  get_flexio()->setIOPinToFlexMode(PIN_GATE);
 
  // Put MISO pins into flexio mode
  for (auto pin_miso : PINS_MISO) {
    if (get_flexio()->mapIOPinToFlexPin(pin_miso) == 0xff)
      return status("Could not map pins to FlexIO module.");
    get_flexio()->setIOPinToFlexMode(pin_miso);
  }
 
  return status::success();
}
 
FLASHMEM void daq::details::flexio::_init_timers() {
  get_flexio()->port().TIMCTL[_gated_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_gated_timer_idx] = 0;
  get_flexio()->port().TIMCMP[_gated_timer_idx] = 0;
 
  get_flexio()->port().TIMCTL[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCMP[_sample_timer_idx] = 0;
 
  get_flexio()->port().TIMCTL[_cnvst_timer_idx] =
      FLEXIO_TIMCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PIN_CNVST)) | FLEXIO_TIMCTL_PINCFG(0b11) |
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _sample_timer_idx + 3) | FLEXIO_TIMCTL_TIMOD(0b10);
  get_flexio()->port().TIMCFG[_cnvst_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b111);
  get_flexio()->port().TIMCMP[_cnvst_timer_idx] = 0x0000'10'FF;
 
  // Add a delay timer to control delay between CNVST and first CLK.
  // Basically a second CNVST timer which is slightly longer and used to trigger first CLK.
  get_flexio()->port().TIMCTL[_delay_timer_idx] =
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _sample_timer_idx + 3) | FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_delay_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b111);
  get_flexio()->port().TIMCMP[_delay_timer_idx] = 300;
 
  get_flexio()->port().TIMCTL[_clk_timer_idx] =
      FLEXIO_TIMCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PIN_CLK)) | FLEXIO_TIMCTL_PINCFG(0b11) |
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _delay_timer_idx + 3) | FLEXIO_TIMCTL_TRGPOL |
      FLEXIO_TIMCTL_TIMOD(0b01);
  get_flexio()->port().TIMCFG[_clk_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b110);
  // langsam
  // flexio->port().TIMCMP[_clk_timer_idx] = 0x0000'1F'60;
  // vielfaches von bit-bang algorithm:
  get_flexio()->port().TIMCMP[_clk_timer_idx] = 0x0000'1B'07;
  // maximal schnell
  // flexio->port().TIMCMP[_clk_timer_idx] = 0x0000'1F'05;
}
 
FLASHMEM void daq::details::flexio::_init_shifters() {
  for (auto _pin_miso_idx = 0u; _pin_miso_idx < PINS_MISO.size(); _pin_miso_idx++) {
    // Trigger all shifters from CLK
    get_flexio()->port().SHIFTCTL[_pin_miso_idx] =
        FLEXIO_SHIFTCTL_TIMSEL(_clk_timer_idx) | FLEXIO_SHIFTCTL_TIMPOL |
        FLEXIO_SHIFTCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PINS_MISO[_pin_miso_idx])) |
        FLEXIO_SHIFTCTL_SMOD(1);
    get_flexio()->port().SHIFTCFG[_pin_miso_idx] = 0;
  }
}
 
FLASHMEM status daq::init() {
  RETURN_IF_FAILED(details::flexio::_init_once());
  details::flexio::_init_timers();
  details::flexio::_init_shifters();
  enable();
  return status::success();
}
 
FLASHMEM status daq::calibrate(carrier::Carrier &carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
  LOG(ANABRID_DEBUG_CALIBRATION, "Resetting current ADC connections...");
  // Disconnect all signal connections in ADC switching matrix to force them to a zero
  auto old_adc_channels = carrier.get_adc_channels();
  carrier.reset_adc_channels();
  // Ensure the ADC bus is actually the one connected to the ADCs (and not the SH gain measurement)
  auto old_adc_bus = carrier.ctrl_block->get_adc_bus();
  carrier.ctrl_block->reset_adc_bus();
  // Write to hardware
  if (!carrier.write_to_hardware())
    return status("Error writing disconnect of ADCs to hardware.");
 
  // Read zero offsets and save them
  LOG(ANABRID_DEBUG_CALIBRATION, "Reading ADC zero offsets:");
  auto zeros_raw = daq::average(daq::sample_raw);
  std::transform(zeros_raw.begin(), zeros_raw.end(), details::raw_zero_offsets.begin(),
                 [](auto zero_raw) { return details::RAW_ZERO_IDEAL - zero_raw; });
  for (auto raw_zero_offset : details::raw_zero_offsets)
    LOG(ANABRID_DEBUG_CALIBRATION, raw_zero_offset);
 
  // Reset previous changes
  LOG(ANABRID_DEBUG_CALIBRATION, "Restoring previous ADC connections...");
  static_cast<void>(carrier.set_adc_channels(old_adc_channels));
  carrier.ctrl_block->set_adc_bus(old_adc_bus);
  // Write to hardware
  if (!carrier.write_to_hardware())
    return status("Error writing previous connections of ADCs to hardware.");
 
  // Sanity check, otherwise return successful
  if (std::any_of(details::raw_zero_offsets.begin(), details::raw_zero_offsets.end(),
                  [](auto value) { return abs(value) > 100; }))
    return status("Zero offsets of ADCs are suspiciously large.");
  return status::success();
}
 
FLASHMEM std::array<uint16_t, daq::NUM_CHANNELS> daq::sample_raw() {
  using namespace details::flexio;
 
  // Clear SHIFTSTAT (by writing 0xFF (sic!)), which might be partially set by a previous acquisition
  get_flexio()->port().SHIFTSTAT = 0xFF;
 
  // Disable and reset _sample_timer_idx
  get_flexio()->port().TIMCTL[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_sample_timer_idx] = 0;
  delayNanoseconds(42);
  // Generate just one sample pulse
  get_flexio()->port().TIMCMP[_sample_timer_idx] = 1;
  get_flexio()->port().TIMCTL[_sample_timer_idx] = FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_sample_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010);
 
  // Wait until shift buffer is filled or timeout in case of hardware errors
  for (auto _ = 0u; _ < 50; _++) {
    if (get_flexio()->port().SHIFTSTAT & 0xFF)
      return {static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[0]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[1]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[2]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[3]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[4]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[5]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[6]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[7])};
    delayNanoseconds(100);
  }
  // Returning zero in error case may hide the error
  // Of course, returning RAW_SUSPICIOUS_VALUE is only slightly better :)
  return {details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE,
          details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE,
          details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE};
}
 
FLASHMEM std::array<float, daq::NUM_CHANNELS> daq::sample() { return raw_to_float(sample_raw()); }
 
FLASHMEM std::array<float, daq::NUM_CHANNELS> daq::average(size_t samples, unsigned int delay_us) {
  return average(daq::sample, samples, delay_us);
}
 
FLASHMEM daq::stream::Scope daq::stream::get(const run::Run &run, run::RunDataHandler *const data_handler,
                                             bool start) {
  return {run, data_handler, start};
}
 
FLASHMEM unsigned int daq::stream::details::get_number_of_data_vectors_in_buffer() {
  // Note that this does not consider whether these have been streamed out yet
  return channel.TCD->BITER - channel.TCD->CITER;
}
 
FLASHMEM std::array<volatile uint32_t, daq::stream::details::BUFFER_SIZE> daq::stream::details::get_buffer() {
  return buffer;
}
 
// NOT FLASHMEM
status daq::stream::process(const run::Run &run, run::RunDataHandler *const data_handler, bool partial) {
  // Pointer to the part of the buffer which (may) contain partial data at end of acquisition time
  static auto partial_buffer_part = details::buffer.data();
 
  if (!run.daq_config)
    return status("Invalid DAQ configuration.");
  if (details::overflow_data)
    return status("DMA overflow.");
 
  // Default values for streaming
  volatile uint32_t *active_buffer_part;
  size_t outer_count = details::BUFFER_SIZE / run.daq_config.get_num_channels_min_power_of_two() / 2;
 
  // Change streaming parameters depending on whether the first or second half of buffer is streamed
  if (details::first_data) {
    active_buffer_part = details::buffer.data();
    partial_buffer_part = details::buffer.data() + details::BUFFER_SIZE / 2;
    details::first_data = false;
  } else if (details::last_data) {
    active_buffer_part = details::buffer.data() + details::BUFFER_SIZE / 2;
    partial_buffer_part = details::buffer.data();
    details::last_data = false;
  } else if (partial) {
    // Stream the remaining partially filled part of the buffer.
    // This should be done exactly once, after the data acquisition stopped.
    active_buffer_part = partial_buffer_part;
    if (partial_buffer_part == details::buffer.data())
      // If we have streamed out the second part the last time,
      // the partial data is in the first part of the buffer
      // and get_number_of_data_vectors_in_buffer returns only
      // the number of partial data vectors to stream.
      outer_count = details::get_number_of_data_vectors_in_buffer();
    else
      // If we have streamed out the first part the last time,
      // the partial data is in the second half,
      // and get_number_of_data_vectors_in_buffer does not consider this
      // and returns number of vectors in first_half plus number of partials
      // in second half, and we have to subtract the number in the first half.
      outer_count = details::get_number_of_data_vectors_in_buffer() - outer_count;
    // In case of multi-cycle OP computations (e.g. quantum circuits), we want to
    // do a partial stream after each OP cycle. When acquisition continues in the next cycle,
    // it is easiest to fill the buffer from the beginning again and reset all relevant counters.
    details::channel.TCD->CITER = details::channel.TCD->BITER;
    details::channel.TCD->DADDR = details::buffer.data();
    details::first_data = details::last_data = details::overflow_data = false;
    if (!outer_count) {
      return status::success();
    }
  } else
    return status::success();
 
  data_handler->handle(active_buffer_part, outer_count, run.daq_config.get_num_channels_min_power_of_two(), run);
  return status::success();
}
 
FLASHMEM status daq::stream::stop(const run::Run &run) {
  details::channel.disable();
 
  // TODO: In case of streams not synchronized with OP state,
  //       this function should probably actually stop the timers.
 
  auto flexio = FlexIOHandler::flexIOHandler_list[1];
  // Disable DMA trigger generation
  flexio->port().SHIFTSDEN = 0;
  // Clear global data for ::daq::dma::interrupt
  details::run_data_handler = nullptr;
  details::run = nullptr;
  details::first_data = details::last_data = details::overflow_data = false;
 
  // Check shifter errors
  // Unused channels (when get_num_channels() < 8) always go into SHIFTERR,
  // since they are written to due to always being active, but not read from by the DMA.
  uint32_t _shifterr_mask =
      (run.daq_config.get_num_channels_min_power_of_two() > 1) ? ((1u << run.daq_config.get_num_channels_min_power_of_two()) - 1) : 1u;
  if (flexio->port().SHIFTERR & _shifterr_mask)
    return status("SHIFTERR was asserted during stream.");
  // Clear all SHIFTERR
  flexio->port().SHIFTERR = 0xFF;
 
  return status::success();
}
 
FLASHMEM status daq::stream::start(const run::Run &run, run::RunDataHandler *const data_handler) {
  using namespace daq::details::flexio;
  // Care: FlexIO clock is enabled in _init_once() and this function will get stuck without it
 
  disable();
  // Reset is necessary before each continuous acquisition,
  // otherwise internal counter values are not reset and result in unwanted behaviour
  reset();
  _init_timers();
  _init_shifters();
 
  auto _flexio_pin_gate = get_flexio()->mapIOPinToFlexPin(daq::details::PIN_GATE);
  get_flexio()->port().TIMCTL[_gated_timer_idx] = FLEXIO_TIMCTL_TRGSEL(2 * _flexio_pin_gate) |
                                                  FLEXIO_TIMCTL_TRGPOL | FLEXIO_TIMCTL_TRGSRC |
                                                  FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_gated_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b011) | FLEXIO_TIMCFG_TIMENA(0b110);
  get_flexio()->port().TIMCMP[_gated_timer_idx] = 239;
 
  get_flexio()->port().TIMCTL[_sample_timer_idx] =
      FLEXIO_TIMCTL_TRGSEL(4 * _gated_timer_idx + 3) | FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_sample_timer_idx] = FLEXIO_TIMCFG_TIMDEC(0b01) | FLEXIO_TIMCFG_TIMENA(0b110);
  get_flexio()->port().TIMCMP[_sample_timer_idx] = (1'000'000 / run.daq_config.get_sample_rate()) * 2 - 1;
 
  enable();
 
  // Update global pointers for ::daq::dma::interrupt
  details::run_data_handler = data_handler;
  details::run = &run;
 
  /*
   *  Configure DMA
   */
 
  // Select shifter zero to generate DMA events.
  // Which shifter is selected should not matter, as long as it is used.
  uint8_t shifter_dma_idx = 0;
  // Set shifter to generate DMA events.
  get_flexio()->port().SHIFTSDEN = 1 << shifter_dma_idx;
  // Configure DMA channel
  details::channel.begin();
 
  // Configure minor and major loop of DMA process.
  // One DMA request (SHIFTBUF store) triggers one major loop.
  // A major loop consists of several minor loops.
  // For each loop, source address and destination address is adjusted and data is copied.
  // The number of minor loops is implicitly defined by how much data they should copy.
  // The approach here is to use the minor loops to copy all shift registers when one triggers the DMA.
  // That means each major loop copies over NUM_CHANNELS data points.
  // Triggering multiple major loops fills up the ring buffer.
  // Once all major loops are done, the ring buffer is full and an interrupt is triggered to handle the data.
  // For such configurations DMAChannel provides sourceCircular() and destinationCircular() functions,
  // which are not quite able to handle our case (BITER/CITER and ATTR_DST must be different).
  // That's why we do it by hand :)
 
  // BITER "beginning iteration count" is the number of major loops.
  // Each major loop fills part (see TCD->NBYTES) of the ring buffer.
  details::channel.TCD->BITER = details::buffer.size() / run.daq_config.get_num_channels_min_power_of_two();
  // CITER "current major loop iteration count" is the current number of major loops left to perform.
  // It can be used to check progress of the process. It's reset to BITER whe we filled the buffer once.
  details::channel.TCD->CITER = details::buffer.size() / run.daq_config.get_num_channels_min_power_of_two();
 
  // Configure source address and its adjustments.
  // We want to circularly copy from SHIFTBUFBIS.
  // In principle, we are only interested in the last 16 bits of each SHIFTBUFBIS.
  // Unfortunately, configuring it in such a way was not successful -- maybe in the future.
  // Instead, we copy the full 32 bit of data.
  // SADDR "source address start" is where the DMA process starts.
  // Set it to the beginning of the SHIFTBUFBIS array.
  details::channel.TCD->SADDR = get_flexio()->port().SHIFTBUFBIS;
  // NBYTES "minor byte transfer count" is the number of bytes transferred in one minor loop.
  // Set it such that the whole SHIFTBUFBIS array is copied, which is 4 bytes * NUM_CHANNELS
  details::channel.TCD->NBYTES = 4 * run.daq_config.get_num_channels_min_power_of_two();
  // SOFF "source address offset" is the offset added onto SADDR for each minor loop.
  // Set it to 4 bytes, equaling the 32 bits each shift register has.
  details::channel.TCD->SOFF = 4;
  // ATTR_SRC "source address attribute" is an attribute setting the circularity and the transfer size.
  // The format is [5bit MOD][3bit SIZE].
  // The 5bit MOD is the number of lower address bites allowed to change, effectively circling back to SADDR.
  // The 3bit SIZE defines the source data transfer size.
  // Set MOD to 5, allowing the address bits to change until the end of the SHIFTBUFBIS array and cycling.
  // MOD = 5 because 2^5 = 32, meaning address cycles after 32 bytes, which are 8*32 bits.
  // Set SIZE to 0b010 for 32 bit transfer size.
  uint8_t MOD_SRC = __builtin_ctz(run.daq_config.get_num_channels_min_power_of_two() * 4);
  details::channel.TCD->ATTR_SRC = ((MOD_SRC & 0b11111) << 3) | 0b010;
  // SLAST "last source address adjustment" is an adjustment applied to SADDR after all major iterations.
  // We don't want any adjustments, since we already use ATTR_SRC to implement a circular buffer.
  details::channel.TCD->SLAST = 0;
 
  // Configure destination address and its adjustments.
  // We want to circularly copy into a memory ring buffer.
  // Since we always copy 32 bit from SADDR, we will have the memory buffer in a 32 bit layout as well,
  // even though we are again only interested in the lower 16 bits.
  // DADDR "destination address start" is the start of the destination ring buffer.
  // Set to address of ring buffer.
  details::channel.TCD->DADDR = details::buffer.data();
  // DOOFF "destination address offset" is the offset added to DADDR for each minor loop.
  // Set to 4 bytes, equaling the 32 bits each shift register has.
  details::channel.TCD->DOFF = 4;
  // ATTR_SRC "destination address attribute" is analogous to ATTR_SRC
  // Set first 5 bit MOD according to size of ring buffer.
  // Since only power-of-two number of channels N<=8 is allowed, N*NUM_CHANNELS will always fit for each N.
  // Set last 3 bits to 0b010 for 32 bit transfer size.
  uint8_t MOD = __builtin_ctz(details::BUFFER_SIZE * 4);
  // Check if memory buffer address is aligned such that MOD lower bits are zero (maybe this is too pedantic?)
  if (reinterpret_cast<uintptr_t>(details::buffer.data()) & ~(~static_cast<uintptr_t>(0) << MOD)) {
    return ("DMA buffer memory range is not sufficiently aligned.");
  }
  details::channel.TCD->ATTR_DST = ((MOD & 0b11111) << 3) | 0b010;
  // DLASTSGA "last destination address adjustment or next TCD" is similar to SLAST.
  // We don't want any adjustments, since we already use ATTR_DST to implement a circular buffer.
  details::channel.TCD->SLAST = 0;
 
  // Call an interrupt when done
  details::channel.attachInterrupt(details::interrupt);
  details::channel.interruptAtCompletion();
  details::channel.interruptAtHalf();
  // Trigger from "shifter full" DMA event
  details::channel.triggerAtHardwareEvent(get_flexio()->shiftersDMAChannel(shifter_dma_idx));
  // Enable dma channel
  details::channel.enable();
  if (details::channel.error()) {
    return ("dma::channel.error");
  }
 
  return status::success();
}