allwpilib/hal/src/main/native/systemcore/CAN.cpp

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "hal/CAN.h"

#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/types.h>

#include <cstdio>
#include <memory>
#include <optional>
#include <utility>
#include <vector>

#include <wpi/DenseMap.h>
#include <wpi/mutex.h>
#include <wpi/timestamp.h>

#include "hal/Errors.h"
#include "hal/Threads.h"
#include "wpinet/EventLoopRunner.h"
#include "wpinet/uv/Poll.h"
#include "wpinet/uv/Timer.h"

#define NUM_CAN_BUSES 1

namespace {

static constexpr uint32_t MatchingBitMask = CAN_EFF_MASK | CAN_RTR_FLAG;

static_assert(CAN_RTR_FLAG == HAL_CAN_IS_FRAME_REMOTE);
static_assert(CAN_EFF_FLAG == HAL_CAN_IS_FRAME_11BIT);

uint32_t MapMessageIdToSocketCan(uint32_t id) {
  // Message and RTR map directly
  uint32_t toRet = id & MatchingBitMask;

  // Reverse the 11 bit flag
  if ((id & HAL_CAN_IS_FRAME_11BIT) == 0) {
    toRet |= CAN_EFF_FLAG;
  }

  return toRet;
}

uint32_t MapSocketCanToMessageId(uint32_t id) {
  // Message and RTR map directly
  uint32_t toRet = id & MatchingBitMask;

  // Reverse the 11 bit flag
  if ((id & CAN_EFF_FLAG) == 0) {
    toRet |= HAL_CAN_IS_FRAME_11BIT;
  }

  return toRet;
}

struct FrameStore {
  canfd_frame frame;
  uint64_t timestamp{0};
};

struct SocketCanState {
  wpi::EventLoopRunner readLoopRunner;
  wpi::EventLoopRunner writeLoopRunner;
  wpi::mutex writeMutex[NUM_CAN_BUSES];
  int socketHandle[NUM_CAN_BUSES];
  // ms to count/timer map
  wpi::DenseMap<uint16_t, std::pair<size_t, std::weak_ptr<wpi::uv::Timer>>>
      timers;
  // ms to bus mask/packet
  wpi::DenseMap<uint16_t, std::array<std::optional<canfd_frame>, NUM_CAN_BUSES>>
      timedFrames;
  // packet to time
  wpi::DenseMap<uint32_t, std::array<uint16_t, NUM_CAN_BUSES>> packetToTime;

  wpi::mutex readMutex[NUM_CAN_BUSES];
  // TODO(thadhouse) we need a MUCH better way of doing this masking
  wpi::DenseMap<uint32_t, FrameStore> readFrames[NUM_CAN_BUSES];

  bool InitializeBuses();

  void TimerCallback(uint16_t time);

  void RemovePeriodic(uint8_t busMask, uint32_t messageId);
  void AddPeriodic(wpi::uv::Loop& loop, uint8_t busMask, uint16_t time,
                   const canfd_frame& frame);
};

}  // namespace

static SocketCanState* canState;

namespace hal::init {
void InitializeCAN() {
  canState = new SocketCanState{};
}
}  // namespace hal::init

bool SocketCanState::InitializeBuses() {
  bool success = true;
  readLoopRunner.ExecSync([this, &success](wpi::uv::Loop& loop) {
    int32_t status = 0;
    HAL_SetCurrentThreadPriority(true, 50, &status);
    if (status != 0) {
      std::printf("Failed to set CAN thread priority\n");
    }

    for (int i = 0; i < NUM_CAN_BUSES; i++) {
      std::scoped_lock lock{writeMutex[i]};
      socketHandle[i] = socket(PF_CAN, SOCK_RAW, CAN_RAW);
      if (socketHandle[i] == -1) {
        success = false;
        return;
      }

      ifreq ifr;
      std::snprintf(ifr.ifr_name, sizeof(ifr.ifr_name), "can%d", i);

      if (ioctl(socketHandle[i], SIOCGIFINDEX, &ifr) == -1) {
        success = false;
        return;
      }

      sockaddr_can addr;
      std::memset(&addr, 0, sizeof(addr));
      addr.can_family = AF_CAN;
      addr.can_ifindex = ifr.ifr_ifindex;

      if (bind(socketHandle[i], reinterpret_cast<const sockaddr*>(&addr),
               sizeof(addr)) == -1) {
        success = false;
        return;
      }

      std::printf("Successfully bound to can interface %d\n", i);

      auto poll = wpi::uv::Poll::Create(loop, socketHandle[i]);
      if (!poll) {
        success = false;
        return;
      }

      poll->pollEvent.connect(
          [this, fd = socketHandle[i], canIndex = i](int mask) {
            if (mask & UV_READABLE) {
              canfd_frame frame;
              int rVal = read(fd, &frame, sizeof(frame));
              if (rVal <= 0) {
                // TODO(thadhouse) error handling
                return;
              }
              if (frame.can_id & CAN_ERR_FLAG) {
                // Do nothing if this is an error frame
                return;
              }

              uint32_t messageId = MapSocketCanToMessageId(frame.can_id);
              uint64_t timestamp = wpi::Now();
              // Ensure FDF flag is set for the read later.
              if (rVal == CANFD_MTU) {
                frame.flags = CANFD_FDF;
              }

              std::scoped_lock lock{readMutex[canIndex]};
              auto& msg = readFrames[canIndex][messageId];
              msg.frame = frame;
              msg.timestamp = timestamp;
            }
          });

      poll->Start(UV_READABLE);
    }
  });
  return success;
}

void SocketCanState::TimerCallback(uint16_t time) {
  auto& busFrames = timedFrames[time];
  for (size_t i = 0; i < busFrames.size(); i++) {
    const auto& frame = busFrames[i];
    if (!frame.has_value()) {
      continue;
    }
    std::scoped_lock lock{writeMutex[i]};
    int mtu = (frame->flags & CANFD_FDF) ? CANFD_MTU : CAN_MTU;
    send(canState->socketHandle[i], &*frame, mtu, 0);
  }
}

void SocketCanState::RemovePeriodic(uint8_t busId, uint32_t messageId) {
  // Find time, and remove from map
  auto& time = packetToTime[messageId][busId];
  auto storedTime = time;
  time = 0;

  // Its already been removed
  if (storedTime == 0) {
    return;
  }

  // Reset frame
  timedFrames[storedTime][busId].reset();

  auto& timer = timers[storedTime];
  // Stop the timer
  timer.first--;
  if (timer.first == 0) {
    if (auto l = timer.second.lock()) {
      l->Stop();
    }
  }
}

void SocketCanState::AddPeriodic(wpi::uv::Loop& loop, uint8_t busId,
                                 uint16_t time, const canfd_frame& frame) {
  packetToTime[frame.can_id][busId] = time;
  timedFrames[time][busId] = frame;
  auto& timer = timers[time];
  timer.first++;
  if (timer.first == 1) {
    auto newTimer = wpi::uv::Timer::Create(loop);
    newTimer->timeout.connect([this, time] { TimerCallback(time); });
    newTimer->Start(wpi::uv::Timer::Time{time}, wpi::uv::Timer::Time{time});
  }
}

namespace hal {
bool InitializeCanBuses() {
  return canState->InitializeBuses();
}
}  // namespace hal

namespace {}  // namespace

extern "C" {

void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
                         uint8_t dataSize, int32_t periodMs, int32_t* status) {
  // TODO(thadhouse) this will become a parameter
  // isFd will also be a part of this parameter
  uint8_t busId = 0;

  if (busId >= NUM_CAN_BUSES) {
    *status = PARAMETER_OUT_OF_RANGE;
    return;
  }

  bool isFd = false;
  messageID = MapMessageIdToSocketCan(messageID);

  if (periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING) {
    canState->writeLoopRunner.ExecSync([messageID, busId](wpi::uv::Loop&) {
      canState->RemovePeriodic(busId, messageID);
    });

    *status = 0;
    return;
  }

  canfd_frame frame;
  std::memset(&frame, 0, sizeof(frame));
  frame.can_id = messageID;
  frame.flags = isFd ? CANFD_FDF | CANFD_BRS : 0;
  if (dataSize) {
    auto size = (std::min)(dataSize, static_cast<uint8_t>(sizeof(frame.data)));
    std::memcpy(frame.data, data, size);
    frame.len = size;
  }

  int mtu = isFd ? CANFD_MTU : CAN_MTU;
  {
    std::scoped_lock lock{canState->writeMutex[busId]};
    int result = send(canState->socketHandle[busId], &frame, mtu, 0);
    if (result != mtu) {
      // TODO(thadhouse) better error
      *status = HAL_ERR_CANSessionMux_InvalidBuffer;
      return;
    }
  }

  if (periodMs > 0) {
    canState->writeLoopRunner.ExecAsync(
        [busId, periodMs, frame](wpi::uv::Loop& loop) {
          canState->AddPeriodic(loop, busId, periodMs, frame);
        });
  }
}
void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
                            uint8_t* data, uint8_t* dataSize,
                            uint32_t* timeStamp, int32_t* status) {
  uint8_t busId = 0;

  if (busId >= NUM_CAN_BUSES) {
    *status = PARAMETER_OUT_OF_RANGE;
    return;
  }

  std::scoped_lock lock{canState->readMutex[busId]};

  // TODO(thadhouse) this is going to be wrong, but we're going to assume that
  // any lookup without the 11 bit mask set wants to look for a 29 bit frame.
  // Also, only do fast lookups for 29 bit frames

  // Fast case is the following.
  // Mask doesn't include 11 bit flag
  // Mask doesn't include RTR flag
  // Mask is full
  if (messageIDMask == CAN_EFF_MASK) {
    //  We're doing a fast lookup
    auto& msg = canState->readFrames[busId][*messageID];
    if (msg.timestamp == 0) {
      *status = HAL_ERR_CANSessionMux_MessageNotFound;
      return;
    }
    if ((msg.frame.flags & CANFD_FDF) || msg.frame.len > 8) {
      std::printf("FD frames not supported for read right now\n");
      *status = HAL_ERR_CANSessionMux_InvalidBuffer;
      return;
    }
    // TODO(thadhouse) this time needs to be fixed up.
    *timeStamp = msg.timestamp / 1000;
    std::memcpy(data, msg.frame.data, msg.frame.len);
    *dataSize = msg.frame.len;
    *status = 0;
    msg.timestamp = 0;
    return;
  }

  std::printf("Slow lookup not supported yet\n");

  // Add support for slow lookup later
  *status = HAL_ERR_CANSessionMux_NotAllowed;
  return;
}
void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
                               uint32_t messageIDMask, uint32_t maxMessages,
                               int32_t* status) {
  *sessionHandle = 0;
  *status = HAL_HANDLE_ERROR;
  return;
}
void HAL_CAN_CloseStreamSession(uint32_t sessionHandle) {}
void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
                               struct HAL_CANStreamMessage* messages,
                               uint32_t messagesToRead, uint32_t* messagesRead,
                               int32_t* status) {
  *status = HAL_HANDLE_ERROR;
  return;
}
void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
                          uint32_t* txFullCount, uint32_t* receiveErrorCount,
                          uint32_t* transmitErrorCount, int32_t* status) {
  *status = HAL_HANDLE_ERROR;
  return;
}
}  // extern "C"