[hal] Initial CAN implementation for SystemCore

hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java

@@ -23,10 +23,10 @@ public class CANJNI extends JNIWrapper {
   public static final int CAN_SEND_PERIOD_STOP_REPEATING = -1;
 
   /** Mask for "is frame remote" in message ID. */
-  public static final int CAN_IS_FRAME_REMOTE = 0x80000000;
+  public static final int CAN_IS_FRAME_REMOTE = 0x40000000;
 
   /** Mask for "is frame 11 bits" in message ID. */
-  public static final int CAN_IS_FRAME_11BIT = 0x40000000;
+  public static final int CAN_IS_FRAME_11BIT = 0x80000000;
 
   /** Default constructor. */
   public CANJNI() {}

hal/src/main/native/include/hal/CAN.h

@@ -26,12 +26,12 @@
 /**
  * Mask for "is frame remote" in message ID.
  */
-#define HAL_CAN_IS_FRAME_REMOTE 0x80000000
+#define HAL_CAN_IS_FRAME_REMOTE 0x40000000
 
 /**
  * Mask for "is frame 11 bits" in message ID.
  */
-#define HAL_CAN_IS_FRAME_11BIT 0x40000000
+#define HAL_CAN_IS_FRAME_11BIT 0x80000000
 
 #define HAL_ERR_CANSessionMux_InvalidBuffer -44086
 #define HAL_ERR_CANSessionMux_MessageNotFound -44087

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

@@ -4,23 +4,273 @@
 
 #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 <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 loopRunner;
+  wpi::mutex writeMutex[NUM_CAN_BUSES];
+  int socketHandle[NUM_CAN_BUSES];
+  wpi::mutex timerMutex;
+  // 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::vector<std::pair<uint8_t, canfd_frame>>>
+      timedFrames;
+  // packet to time
+  wpi::DenseMap<uint32_t, uint16_t> 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();
+};
+
+}  // namespace
+
+static SocketCanState* canState;
 
 namespace hal::init {
-void InitializeCAN() {}
+void InitializeCAN() {
+  canState = new SocketCanState{};
+}
 }  // namespace hal::init
 
+bool SocketCanState::InitializeBuses() {
+  bool success = true;
+  loopRunner.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;
+}
+
+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) {
-  *status = HAL_HANDLE_ERROR;
-  return;
+  // 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;
+  }
+
+  if (periodMs == HAL_CAN_SEND_PERIOD_STOP_REPEATING) {
+    // TODO(thadhouse) actually stop
+    *status = 0;
+    return;
+  }
+
+  bool isFd = false;
+  messageID = MapMessageIdToSocketCan(messageID);
+
+  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) {
+    // TODO(thadhouse) set repeating
+  }
 }
 void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
                             uint8_t* data, uint8_t* dataSize,
                             uint32_t* timeStamp, int32_t* status) {
-  *status = HAL_HANDLE_ERROR;
+  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,

hal/src/main/native/systemcore/CANAPI.cpp

@@ -9,6 +9,7 @@
 
 #include <wpi/DenseMap.h>
 #include <wpi/mutex.h>
+#include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
 #include "hal/CAN.h"
@@ -58,12 +59,7 @@ static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
 extern "C" {
 
 uint32_t HAL_GetCANPacketBaseTime(void) {
-  timespec t;
-  clock_gettime(CLOCK_MONOTONIC, &t);
-
-  // Convert t to milliseconds
-  uint64_t ms = t.tv_sec * 1000ull + t.tv_nsec / 1000000ull;
-  return ms & 0xFFFFFFFF;
+  return wpi::Now() / 1000;
 }
 
 HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,

hal/src/main/native/systemcore/CANInternal.h

@@ -0,0 +1,9 @@
+// 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.
+
+#pragma once
+
+namespace hal {
+bool InitializeCanBuses();
+}  // namespace hal

hal/src/main/native/systemcore/HAL.cpp

@@ -25,6 +25,7 @@
 #include <wpi/print.h>
 #include <wpi/timestamp.h>
 
+#include "CANInternal.h"
 #include "HALInitializer.h"
 #include "HALInternal.h"
 #include "SystemServer.h"
@@ -337,6 +338,11 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
 
   prctl(PR_SET_PDEATHSIG, SIGTERM);
 
+  if (!hal::InitializeCanBuses()) {
+    std::printf("Failed to initialize can buses\n");
+    return false;
+  }
+
   hal::InitializeSystemServer();
 
   // // Return false if program failed to kill an existing program