I spent most of monday discussing how to overcome a lot of my historic project management issues with my proctor, Mr. Christy, primarily re-affirming the importance of the minimum viable products and rapid prototyping.

Wednesday we discussed a project overview. After school, and throughout Tuesday, I finished up the I2C and the SPI utils implementation.

Thursday I spent a ridiculous amount of time trying to compile a real-time kernel for the pi 5, first on my own computer, and then later on the pi itself, which took even longer to compile. I eventually ended the day by testing and confirming my I2C bus implementation. I now am looking to implement my IMU node, using a provided library.

Both use a singleton pattern to give out unique instances of the class for each bus (in the case of I2C) or each bus and Cs (In the case of SPI). Each Cs instance on has to hold a shared pointer to the bus mutex to allow it to lock the bus while writing/reading.

I also used std::expected to allow returning errors as values because I really just want to code in Rust.

static std::expected<std::shared_ptr<I2cBus>, std::error_code>
get_instance(BusNum bus_num) noexcept
{
  static std::mutex registry_mutex;
  static std::array<std::weak_ptr<I2cBus>, kBusPaths.size()> bus_registry;

  std::lock_guard lock(registry_mutex);

  auto idx = static_cast<uint8_t>(bus_num);

  // returns weak_ptr<I2cBus>(), if key does not yet exist
  auto &weak = bus_registry[idx];

  // check if already opened
  if (std::shared_ptr<I2cBus> shared = weak.lock()) {
    return shared;
  }

  // returns -1 if err
  int fd = open(kBusPaths[idx], O_RDWR);
  if (fd < 0) {
    return std::unexpected(std::error_code(errno, std::system_category()));
  }

  auto shared = std::shared_ptr<I2cBus>(new I2cBus(fd)); // creates new bus
  weak = shared;                                         // Add to registry map
  return shared;
}

We use an array of weak pointers to each possible I2cBus instance. Weak pointers are simply used to observe if the I2cBus object has been initialized, but do not prevent it from being destructed. When we want to create a new I2cBus object, or return another shared pointer to an existing one, we cannot allow other threads to access the registry, else we would risk a race condition, so we also include a registry mutex here.

The if statement is likely the most confusing part here. weak.lock() returns an empty shared_ptr when the original entry in the bus registry is null (not yet initialized). Note that the equal sign is an assignment operator not a equality comparison. If the pointer is not empty, the statement returns true, and we simply return the shared pointer. If not we open the path, initialize the I2cBus, and set the registry entry.

The SPI implementation is similar albeit a bit more complicated:

[[nodiscard]] static std::expected<std::shared_ptr<SpiBus>, std::error_code>
get_instance(Bus bus, uint8_t cs) noexcept
{
  static std::mutex registry_mutex;
  static std::map<std::pair<Bus, uint8_t>, std::weak_ptr<SpiBus>> cs_registry;
  static std::map<Bus, std::shared_ptr<std::mutex>> bus_mutexes;

  std::lock_guard lock(registry_mutex);

  auto path = get_path(bus, cs);
  if (!path) {
    return std::unexpected(path.error());
  }

  auto key = std::make_pair(bus, cs);
  auto &weak = cs_registry[key];
  if (weak.lock()) {
    return std::unexpected(make_error_code(SpiErrorCode::cs_in_use));
  }

  // get mutex pointer
  auto [it, inserted] = bus_mutexes.emplace(bus, nullptr);
  if (inserted) {
    it->second = std::make_shared<std::mutex>();
  };

  // returns -1 if err
  int fd = open(path.value(), O_RDWR);
  if (fd < 0) {
    return std::unexpected(std::error_code(errno, std::system_category()));
  }

  auto shared = std::shared_ptr<SpiBus>(new SpiBus(fd, it->second));
  weak = shared;
  return shared;
};

Here we hold a seperate map of bus mutexes, because different instances need to hold shared pointer to these centrally held mutexes. Our registry is also more complicated, because we need to key by both Bus and Cs. Maps are used, rather than arrays, because the number of Cs lines changes for the different buses.

Our if checks are a bit different here. We actually guard against the lock case, because that means the Cs is already in use. We then lazy initialize the mutex for the given bus. Paths are unique by each cs, so we fetch that, and then return the singleton object.

Both Spi and I2c variants use a simple transaction function, and then have a couple convience read, write and transfer functions that use the lower-level transaction API.

// i2c_bus.hpp
[[nodiscard]] std::expected<void, std::error_code> transaction(std::span<i2c_msg> msgs) noexcept
{
  i2c_rdwr_ioctl_data data{.msgs = msgs.data(), .nmsgs = static_cast<uint32_t>(msgs.size())};

  std::lock_guard lock(bus_mutex_);
  if (ioctl(fd_, I2C_RDWR, &data) < 0) {
    return std::unexpected(std::error_code(errno, std::system_category()));
  }
  return {};
}

[[nodiscard]] std::expected<void, std::error_code>
transfer(uint8_t addr, std::span<const uint8_t> write_buf, std::span<uint8_t> read_buf) noexcept
{
  i2c_msg msgs[2]{
      {.addr = addr,
        .flags = 0,
        .len = static_cast<uint16_t>(write_buf.size()),
        .buf = const_cast<uint8_t *>(write_buf.data())},
      {.addr = addr,
        .flags = I2C_M_RD,
        .len = static_cast<uint16_t>(read_buf.size()),
        .buf = const_cast<uint8_t *>(read_buf.data())},
  };

  return transaction(msgs);
}


// spi_bus.hpp
[[nodiscard]] std::expected<void, std::error_code>
transaction(std::span<spi_ioc_transfer> msgs) noexcept
{
  // 0 is ripped from SPI_IOC_MESSAGE implemetnation
  uint32_t request = _IOC(_IOC_WRITE, SPI_IOC_MAGIC, 0, msgs.size() * sizeof(spi_ioc_transfer));

  std::lock_guard lock(*bus_mutex_);
  if (ioctl(fd_, request, msgs.data()) < 0) {
    return std::unexpected(std::error_code(errno, std::system_category()));
  }
  return {};
};

[[nodiscard]] std::expected<void, std::error_code>
transfer(std::span<uint8_t> read_buf, std::span<const uint8_t> write_buf) noexcept
{
  if (read_buf.size() != write_buf.size()) {
    return std::unexpected(
        std::error_code(make_error_code(SpiErrorCode::missmatched_transfer_bus_size)));
  }

  // zero all fields; 0 is used as use default as set by configure()
  spi_ioc_transfer transfer{};
  transfer.tx_buf = static_cast<uint64_t>(reinterpret_cast<uintptr_t>(write_buf.data()));
  transfer.rx_buf = static_cast<uint64_t>(reinterpret_cast<uintptr_t>(read_buf.data()));
  transfer.len = static_cast<uint32_t>(write_buf.size());

  return transaction({&transfer, 1});
}

Dropping both instances was a bit annoying because closing the fd handle is a fallible operation, but deconstructors cannot return values. I settled on a close function which can be called, otherwise we fall through to the normal deconstructor. This required some careful checking to avoid closing the fd handle twice. I wanted to use ROS logging, but I didn’t want to pull that as a dependency for simple testing, so I made a quick definition as well. Code is similar for both Buses, so I’m only going to show the Spi Bus.

#ifdef SPI_UTILS_ROS_LOGGING
#  include <rclcpp/logging.hpp>
#  define SPI_BUS_LOG_ERROR(fmt, ...) RCLCPP_ERROR(rclcpp::get_logger("SpiBus"), fmt, ##__VA_ARGS__)
#else
#  include <cstdio>
#  define SPI_BUS_LOG_ERROR(fmt, ...)                                                              \
    std::fprintf(stderr, "[SpiBus] ERROR: " fmt "\n", ##__VA_ARGS__)
#endif

// [most of the class]

[[nodiscard]] std::expected<void, std::error_code> close() noexcept
{
  if (fd_ < 0)
    return {};

  if (::close(fd_) < 0) {
    return std::unexpected(std::error_code(errno, std::system_category()));
  }
  fd_ = -1;
  return {};
}

~SpiBus()
{
  if (fd_ >= 0) {
    if (::close(fd_) < 0) {
      SPI_BUS_LOG_ERROR("Failed to close SPI bus fd %d: %s", fd_, std::strerror(errno));
    }
    fd_ = -1;
  }
}

-1 is used as the empty handle sentinel value. (I HATE SENTINAL VALUES GET ME BACK TO RUST I WANT RUST ENUMS)

This required me to learn a bit more CMAKE in order to make SPI_UTILS_ROS_LOGGING a parameter passed when building.

option(I2C_UTILS_ROS_LOGGING "Use rclcpp logging" OFF)

if(I2C_UTILS_ROS_LOGGING)
  find_package(rclcpp REQUIRED)
  target_link_libraries(i2c_utils INTERFACE rclcpp::rclcpp)
  target_compile_definitions(i2c_utils INTERFACE I2C_UTILS_ROS_LOGGING)
  ament_export_dependencies(rclcpp)
endif()

A similar section is included in the SPI utils package.

Lastly I also set up testing infrastructure so the i2c and spi utils packages would build their own test executable that could be later run to verify it was working, at the end of Thursday.

if(BUILD_TESTING)
  # [other code]
  add_executable(test_i2c_bus test/test_i2c_bus.cpp)
  target_link_libraries(test_i2c_bus PRIVATE i2c_utils)
endif()

Here the build_testing environment variable creates an executable that isn’t built when other packages pull this library.