Add new CAN API (#1036)

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

@@ -0,0 +1,347 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/CANAPI.h"
+
+#include <atomic>
+#include <ctime>
+
+#include <wpi/DenseMap.h>
+
+#include "HAL/CAN.h"
+#include "HAL/Errors.h"
+#include "HAL/HAL.h"
+#include "HAL/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+struct Receives {
+  uint64_t lastTimeStamp;
+  uint8_t data[8];
+  uint8_t length;
+};
+
+struct CANStorage {
+  HAL_CANManufacturer manufacturer;
+  HAL_CANDeviceType deviceType;
+  uint8_t deviceId;
+  wpi::mutex mapMutex;
+  wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
+  wpi::SmallDenseMap<int32_t, Receives> receives;
+};
+}  // namespace
+
+static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>*
+    canHandles;
+
+static std::atomic_bool HasFixedTime{false};
+static uint64_t timeSpanDiff;
+
+static void CheckDeltaTime() {
+  if (HasFixedTime) return;
+  HasFixedTime = true;
+
+  // TODO: Fix locking
+  timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+
+  int32_t status = 0;
+  uint64_t fpgaTime = HAL_GetFPGATime(&status);
+
+  // Convert t to microseconds
+  uint64_t us = t.tv_sec * 1000000 + t.tv_nsec / 1000;
+
+  timeSpanDiff =
+      us - fpgaTime;  // This assumes CLOCK_MONOTONIC is greater then FPGA Time.
+}
+
+static inline uint64_t ConvertToFPGATime(uint32_t canMs) {
+  uint64_t canMsToUs = canMs * 1000;
+  return canMsToUs - timeSpanDiff;
+}
+
+namespace hal {
+namespace init {
+void InitializeCANAPI() {
+  static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
+      cH;
+  canHandles = &cH;
+}
+}  // namespace init
+}  // namespace hal
+
+static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
+  int32_t createdId = 0;
+  createdId |= (static_cast<int32_t>(storage->deviceType) & 0x1F) << 24;
+  createdId |= (static_cast<int32_t>(storage->manufacturer) & 0xFF) << 16;
+  createdId |= (apiId & 0x3FF) << 6;
+  createdId |= (storage->deviceId & 0x3F);
+  return createdId;
+}
+
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+                                int32_t deviceId, HAL_CANDeviceType deviceType,
+                                int32_t* status) {
+  CheckDeltaTime();
+  auto can = std::make_shared<CANStorage>();
+
+  auto handle = canHandles->Allocate(can);
+
+  if (handle == HAL_kInvalidHandle) {
+    *status = NO_AVAILABLE_RESOURCES;
+    return HAL_kInvalidHandle;
+  }
+
+  can->deviceId = deviceId;
+  can->deviceType = deviceType;
+  can->manufacturer = manufacturer;
+
+  return handle;
+}
+
+void HAL_CleanCAN(HAL_CANHandle handle) {
+  auto data = canHandles->Free(handle);
+
+  std::lock_guard<wpi::mutex> lock(data->mapMutex);
+
+  for (auto&& i : data->periodicSends) {
+    int32_t s = 0;
+    HAL_CAN_SendMessage(i.first, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+                        &s);
+    i.second = -1;
+  }
+}
+
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+                        int32_t length, int32_t apiId, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = -1;
+}
+
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+                                 int32_t length, int32_t apiId,
+                                 int32_t repeatMs, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, data, length, repeatMs, status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = repeatMs;
+}
+
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+                                int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+                      status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = -1;
+}
+
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                          int32_t* length, uint64_t* receivedTimestamp,
+                          int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  if (*status == 0) {
+    std::lock_guard<wpi::mutex> lock(can->mapMutex);
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  }
+  *length = dataSize;
+  *receivedTimestamp = timestamp;
+}
+
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                             int32_t* length, uint64_t* receivedTimestamp,
+                             int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}
+
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+                              uint8_t* data, int32_t* length,
+                              uint64_t* receivedTimestamp, int32_t timeoutMs,
+                              int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      // Found, check if new enough
+      uint64_t now = HAL_GetFPGATime(status);
+      if (now - i->second.lastTimeStamp >
+          static_cast<uint64_t>(timeoutMs) * 1000) {
+        // Timeout, return bad status
+        *status = HAL_CAN_TIMEOUT;
+        return;
+      }
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}
+
+void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId,
+                               uint8_t* data, int32_t* length,
+                               uint64_t* receivedTimestamp, int32_t timeoutMs,
+                               int32_t periodMs, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  {
+    std::lock_guard<wpi::mutex> lock(can->mapMutex);
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      uint64_t now = HAL_GetFPGATime(status);
+      // Found, check if new enough
+      if (now - i->second.lastTimeStamp <
+          static_cast<uint64_t>(periodMs) * 1000) {
+        *status = 0;
+        std::memcpy(i->second.data, data, i->second.length);
+        *length = i->second.length;
+        *receivedTimestamp = i->second.lastTimeStamp;
+      }
+    }
+  }
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      // Found, check if new enough
+      uint64_t now = HAL_GetFPGATime(status);
+      if (now - i->second.lastTimeStamp >
+          static_cast<uint64_t>(timeoutMs) * 1000) {
+        // Timeout, return bad status
+        *status = HAL_CAN_TIMEOUT;
+        return;
+      }
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}

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

@@ -49,6 +49,7 @@ void InitializeHAL() {
   InitializeAnalogOutput();
   InitializeAnalogTrigger();
   InitializeCAN();
+  InitializeCANAPI();
   InitializeCompressor();
   InitializeConstants();
   InitializeCounter();
@@ -207,6 +208,8 @@ const char* HAL_GetErrorMessage(int32_t code) {
       return HAL_SERIAL_PORT_OPEN_ERROR_MESSAGE;
     case HAL_SERIAL_PORT_ERROR:
       return HAL_SERIAL_PORT_ERROR_MESSAGE;
+    case HAL_CAN_TIMEOUT:
+      return HAL_CAN_TIMEOUT_MESSAGE;
     default:
       return "Unknown error status";
   }

hal/src/main/native/athena/HALInitializer.h

@@ -26,6 +26,7 @@ extern void InitializeAnalogInternal();
 extern void InitializeAnalogOutput();
 extern void InitializeAnalogTrigger();
 extern void InitializeCAN();
+extern void InitializeCANAPI();
 extern void InitializeCompressor();
 extern void InitializeConstants();
 extern void InitializeCounter();

hal/src/main/native/cpp/jni/CANAPIJNI.cpp

@@ -0,0 +1,241 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <jni.h>
+
+#include <cassert>
+
+#include <wpi/SmallString.h>
+#include <wpi/jni_util.h>
+#include <wpi/raw_ostream.h>
+
+#include "HAL/CAN.h"
+#include "HAL/CANAPI.h"
+#include "HAL/Errors.h"
+#include "HAL/cpp/Log.h"
+#include "HALUtil.h"
+#include "edu_wpi_first_wpilibj_hal_CANAPIJNI.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    initializeCAN
+ * Signature: (III)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_initializeCAN
+  (JNIEnv* env, jclass, jint manufacturer, jint deviceId, jint deviceType)
+{
+  int32_t status = 0;
+  auto handle =
+      HAL_InitializeCAN(static_cast<HAL_CANManufacturer>(manufacturer),
+                        static_cast<int32_t>(deviceId),
+                        static_cast<HAL_CANDeviceType>(deviceType), &status);
+
+  CheckStatusForceThrow(env, status);
+  return handle;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    cleanCAN
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_cleanCAN
+  (JNIEnv* env, jclass, jint handle)
+{
+  HAL_CleanCAN(static_cast<HAL_CANHandle>(handle));
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    writeCANPacket
+ * Signature: (I[BI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_writeCANPacket
+  (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  JByteArrayRef arr{env, data};
+  auto arrRef = arr.array();
+  int32_t status = 0;
+  HAL_WriteCANPacket(halHandle, reinterpret_cast<const uint8_t*>(arrRef.data()),
+                     arrRef.size(), apiId, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    writeCANPacketRepeating
+ * Signature: (I[BII)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_writeCANPacketRepeating
+  (JNIEnv* env, jclass, jint handle, jbyteArray data, jint apiId,
+   jint timeoutMs)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  JByteArrayRef arr{env, data};
+  auto arrRef = arr.array();
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(halHandle,
+                              reinterpret_cast<const uint8_t*>(arrRef.data()),
+                              arrRef.size(), apiId, timeoutMs, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    stopCANPacketRepeating
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_stopCANPacketRepeating
+  (JNIEnv* env, jclass, jint handle, jint apiId)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  int32_t status = 0;
+  HAL_StopCANPacketRepeating(halHandle, apiId, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    readCANPacketNew
+ * Signature: (IILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketNew
+  (JNIEnv* env, jclass, jint handle, jint apiId, jobject data)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  uint8_t dataTemp[8];
+  int32_t dataLength = 0;
+  uint64_t timestamp = 0;
+  int32_t status = 0;
+  HAL_ReadCANPacketNew(halHandle, apiId, dataTemp, &dataLength, &timestamp,
+                       &status);
+  if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (!CheckStatus(env, status)) {
+    return false;
+  }
+  if (dataLength > 8) dataLength = 8;
+
+  jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+  auto javaLen = env->GetArrayLength(toSetArray);
+  if (javaLen < dataLength) dataLength = javaLen;
+  env->SetByteArrayRegion(toSetArray, 0, dataLength,
+                          reinterpret_cast<jbyte*>(dataTemp));
+  return true;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    readCANPacketLatest
+ * Signature: (IILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketLatest
+  (JNIEnv* env, jclass, jint handle, jint apiId, jobject data)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  uint8_t dataTemp[8];
+  int32_t dataLength = 0;
+  uint64_t timestamp = 0;
+  int32_t status = 0;
+  HAL_ReadCANPacketLatest(halHandle, apiId, dataTemp, &dataLength, &timestamp,
+                          &status);
+  if (status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (!CheckStatus(env, status)) {
+    return false;
+  }
+  if (dataLength > 8) dataLength = 8;
+
+  jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+  auto javaLen = env->GetArrayLength(toSetArray);
+  if (javaLen < dataLength) dataLength = javaLen;
+  env->SetByteArrayRegion(toSetArray, 0, dataLength,
+                          reinterpret_cast<jbyte*>(dataTemp));
+  return true;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    readCANPacketTimeout
+ * Signature: (IIILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPacketTimeout
+  (JNIEnv* env, jclass, jint handle, jint apiId, jint timeoutMs, jobject data)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  uint8_t dataTemp[8];
+  int32_t dataLength = 0;
+  uint64_t timestamp = 0;
+  int32_t status = 0;
+  HAL_ReadCANPacketTimeout(halHandle, apiId, dataTemp, &dataLength, &timestamp,
+                           timeoutMs, &status);
+  if (status == HAL_CAN_TIMEOUT ||
+      status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (!CheckStatus(env, status)) {
+    return false;
+  }
+  if (dataLength > 8) dataLength = 8;
+
+  jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+  auto javaLen = env->GetArrayLength(toSetArray);
+  if (javaLen < dataLength) dataLength = javaLen;
+  env->SetByteArrayRegion(toSetArray, 0, dataLength,
+                          reinterpret_cast<jbyte*>(dataTemp));
+  return true;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_CANAPIJNI
+ * Method:    readCANPeriodicPacket
+ * Signature: (IIIILjava/lang/Object;)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_wpilibj_hal_CANAPIJNI_readCANPeriodicPacket
+  (JNIEnv* env, jclass, jint handle, jint apiId, jint timeoutMs, jint periodMs,
+   jobject data)
+{
+  auto halHandle = static_cast<HAL_CANHandle>(handle);
+  uint8_t dataTemp[8];
+  int32_t dataLength = 0;
+  uint64_t timestamp = 0;
+  int32_t status = 0;
+  HAL_ReadCANPeriodicPacket(halHandle, apiId, dataTemp, &dataLength, &timestamp,
+                            timeoutMs, periodMs, &status);
+  if (status == HAL_CAN_TIMEOUT ||
+      status == HAL_ERR_CANSessionMux_MessageNotFound) {
+    return false;
+  }
+  if (!CheckStatus(env, status)) {
+    return false;
+  }
+  if (dataLength > 8) dataLength = 8;
+
+  jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
+  auto javaLen = env->GetArrayLength(toSetArray);
+  if (javaLen < dataLength) dataLength = javaLen;
+  env->SetByteArrayRegion(toSetArray, 0, dataLength,
+                          reinterpret_cast<jbyte*>(dataTemp));
+  return true;
+}
+}  // extern "C"

hal/src/main/native/cpp/jni/HALUtil.cpp

@@ -59,6 +59,7 @@ static JClass pwmConfigDataResultCls;
 static JClass canStatusCls;
 static JClass matchInfoDataCls;
 static JClass accumulatorResultCls;
+static JClass canDataCls;
 
 namespace frc {
 
@@ -237,6 +238,15 @@ void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult,
   env->CallVoidMethod(accumulatorResult, func, (jlong)value, (jlong)count);
 }
 
+jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length,
+                            uint64_t timestamp) {
+  static jmethodID func = env->GetMethodID(canDataCls, "setData", "(IJ)[B");
+
+  jbyteArray retVal = static_cast<jbyteArray>(
+      env->CallObjectMethod(canData, func, (jint)length, (jlong)timestamp));
+  return retVal;
+}
+
 JavaVM* GetJVM() { return jvm; }
 
 }  // namespace frc
@@ -314,6 +324,9 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
   accumulatorResultCls = JClass(env, "edu/wpi/first/wpilibj/AccumulatorResult");
   if (!accumulatorResultCls) return JNI_ERR;
 
+  canDataCls = JClass(env, "edu/wpi/first/wpilibj/CANData");
+  if (!canDataCls) return JNI_ERR;
+
   return sim::SimOnLoad(vm, reserved);
 }
 
@@ -338,6 +351,7 @@ JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
   canStatusCls.free(env);
   matchInfoDataCls.free(env);
   accumulatorResultCls.free(env);
+  canDataCls.free(env);
   jvm = nullptr;
 }
 

hal/src/main/native/cpp/jni/HALUtil.h

@@ -62,6 +62,9 @@ void SetMatchInfoObject(JNIEnv* env, jobject matchStatus,
 void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult,
                                 int64_t value, int64_t count);
 
+jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length,
+                            uint64_t timestamp);
+
 JavaVM* GetJVM();
 
 }  // namespace frc

hal/src/main/native/include/HAL/CANAPI.h

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <stdint.h>
+
+#include "HAL/Types.h"
+
+enum HAL_CANDeviceType : int32_t {
+  HAL_CAN_Dev_kBroadcast = 0,
+  HAL_CAN_Dev_kRobotController = 1,
+  HAL_CAN_Dev_kMotorController = 2,
+  HAL_CAN_Dev_kRelayController = 3,
+  HAL_CAN_Dev_kGyroSensor = 4,
+  HAL_CAN_Dev_kAccelerometer = 5,
+  HAL_CAN_Dev_kUltrasonicSensor = 6,
+  HAL_CAN_Dev_kGearToothSensor = 7,
+  HAL_CAN_Dev_kPowerDistribution = 8,
+  HAL_CAN_Dev_kPneumatics = 9,
+  HAL_CAN_Dev_kMiscellaneous = 10,
+  HAL_CAN_Dev_kFirmwareUpdate = 31
+};
+
+enum HAL_CANManufacturer : int32_t {
+  HAL_CAN_Man_kBroadcast = 0,
+  HAL_CAN_Man_kNI = 1,
+  HAL_CAN_Man_kLM = 2,
+  HAL_CAN_Man_kDEKA = 3,
+  HAL_CAN_Man_kCTRE = 4,
+  HAL_CAN_Man_kMS = 7,
+  HAL_CAN_Man_kTeamUse = 8,
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+                                int32_t deviceId, HAL_CANDeviceType deviceType,
+                                int32_t* status);
+
+void HAL_CleanCAN(HAL_CANHandle handle);
+
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+                        int32_t length, int32_t apiId, int32_t* status);
+
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+                                 int32_t length, int32_t apiId,
+                                 int32_t repeatMs, int32_t* status);
+
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+                                int32_t* status);
+
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                          int32_t* length, uint64_t* receivedTimestamp,
+                          int32_t* status);
+
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                             int32_t* length, uint64_t* receivedTimestamp,
+                             int32_t* status);
+
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+                              uint8_t* data, int32_t* length,
+                              uint64_t* receivedTimestamp, int32_t timeoutMs,
+                              int32_t* status);
+void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId,
+                               uint8_t* data, int32_t* length,
+                               uint64_t* receivedTimestamp, int32_t timeoutMs,
+                               int32_t periodMs, int32_t* status);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif

hal/src/main/native/include/HAL/Errors.h

@@ -103,6 +103,9 @@
 #define HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE \
   "HAL: The priority requested to be set is invalid"
 
+#define HAL_CAN_TIMEOUT -1154
+#define HAL_CAN_TIMEOUT_MESSAGE "HAL: CAN Receive has Timed Out"
+
 #define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
 #define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
 #define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"

hal/src/main/native/include/HAL/Types.h

@@ -43,4 +43,6 @@ typedef HAL_Handle HAL_RelayHandle;
 
 typedef HAL_Handle HAL_SolenoidHandle;
 
+typedef HAL_Handle HAL_CANHandle;
+
 typedef int32_t HAL_Bool;

hal/src/main/native/include/HAL/handles/HandlesInternal.h

@@ -57,7 +57,8 @@ enum class HAL_HandleEnum {
   Solenoid = 15,
   AnalogGyro = 16,
   Vendor = 17,
-  SimulationJni = 18
+  SimulationJni = 18,
+  CAN = 19,
 };
 
 static inline int16_t getHandleIndex(HAL_Handle handle) {

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

@@ -0,0 +1,330 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/CANAPI.h"
+
+#include <atomic>
+#include <ctime>
+
+#include <wpi/DenseMap.h>
+
+#include "HAL/CAN.h"
+#include "HAL/Errors.h"
+#include "HAL/HAL.h"
+#include "HAL/handles/UnlimitedHandleResource.h"
+
+using namespace hal;
+
+namespace {
+struct Receives {
+  uint64_t lastTimeStamp;
+  uint8_t data[8];
+  uint8_t length;
+};
+
+struct CANStorage {
+  HAL_CANManufacturer manufacturer;
+  HAL_CANDeviceType deviceType;
+  uint8_t deviceId;
+  wpi::mutex mapMutex;
+  wpi::SmallDenseMap<int32_t, int32_t> periodicSends;
+  wpi::SmallDenseMap<int32_t, Receives> receives;
+};
+}  // namespace
+
+static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>*
+    canHandles;
+
+static void CheckDeltaTime() {
+  // Noop on sim
+}
+
+static inline uint64_t ConvertToFPGATime(uint32_t canMs) {
+  uint64_t canMsToUs = canMs * 1000;
+  return canMsToUs;
+}
+
+namespace hal {
+namespace init {
+void InitializeCANAPI() {
+  static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
+      cH;
+  canHandles = &cH;
+}
+}  // namespace init
+}  // namespace hal
+
+static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
+  int32_t createdId = 0;
+  createdId |= (static_cast<int32_t>(storage->deviceType) & 0x1F) << 24;
+  createdId |= (static_cast<int32_t>(storage->manufacturer) & 0xFF) << 16;
+  createdId |= (apiId & 0x3FF) << 6;
+  createdId |= (storage->deviceId & 0x3F);
+  return createdId;
+}
+
+HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
+                                int32_t deviceId, HAL_CANDeviceType deviceType,
+                                int32_t* status) {
+  CheckDeltaTime();
+  auto can = std::make_shared<CANStorage>();
+
+  auto handle = canHandles->Allocate(can);
+
+  if (handle == HAL_kInvalidHandle) {
+    *status = NO_AVAILABLE_RESOURCES;
+    return HAL_kInvalidHandle;
+  }
+
+  can->deviceId = deviceId;
+  can->deviceType = deviceType;
+  can->manufacturer = manufacturer;
+
+  return handle;
+}
+
+void HAL_CleanCAN(HAL_CANHandle handle) {
+  auto data = canHandles->Free(handle);
+
+  std::lock_guard<wpi::mutex> lock(data->mapMutex);
+
+  for (auto&& i : data->periodicSends) {
+    int32_t s = 0;
+    HAL_CAN_SendMessage(i.first, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+                        &s);
+    i.second = -1;
+  }
+}
+
+void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
+                        int32_t length, int32_t apiId, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, data, length, HAL_CAN_SEND_PERIOD_NO_REPEAT, status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = -1;
+}
+
+void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
+                                 int32_t length, int32_t apiId,
+                                 int32_t repeatMs, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, data, length, repeatMs, status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = repeatMs;
+}
+
+void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
+                                int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  HAL_CAN_SendMessage(id, nullptr, 0, HAL_CAN_SEND_PERIOD_STOP_REPEATING,
+                      status);
+
+  if (*status != 0) {
+    return;
+  }
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  can->periodicSends[apiId] = -1;
+}
+
+void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                          int32_t* length, uint64_t* receivedTimestamp,
+                          int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  if (*status == 0) {
+    std::lock_guard<wpi::mutex> lock(can->mapMutex);
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  }
+  *length = dataSize;
+  *receivedTimestamp = timestamp;
+}
+
+void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
+                             int32_t* length, uint64_t* receivedTimestamp,
+                             int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}
+
+void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
+                              uint8_t* data, int32_t* length,
+                              uint64_t* receivedTimestamp, int32_t timeoutMs,
+                              int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      // Found, check if new enough
+      uint64_t now = HAL_GetFPGATime(status);
+      if (now - i->second.lastTimeStamp >
+          static_cast<uint64_t>(timeoutMs) * 1000) {
+        // Timeout, return bad status
+        *status = HAL_CAN_TIMEOUT;
+        return;
+      }
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}
+
+void HAL_ReadCANPeriodicPacket(HAL_CANHandle handle, int32_t apiId,
+                               uint8_t* data, int32_t* length,
+                               uint64_t* receivedTimestamp, int32_t timeoutMs,
+                               int32_t periodMs, int32_t* status) {
+  auto can = canHandles->Get(handle);
+  if (!can) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  auto id = CreateCANId(can.get(), apiId);
+
+  {
+    std::lock_guard<wpi::mutex> lock(can->mapMutex);
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      uint64_t now = HAL_GetFPGATime(status);
+      // Found, check if new enough
+      if (now - i->second.lastTimeStamp <
+          static_cast<uint64_t>(periodMs) * 1000) {
+        *status = 0;
+        std::memcpy(i->second.data, data, i->second.length);
+        *length = i->second.length;
+        *receivedTimestamp = i->second.lastTimeStamp;
+      }
+    }
+  }
+
+  uint32_t messageId = 0;
+  uint8_t dataSize = 0;
+  uint32_t ts = 0;
+  HAL_CAN_ReceiveMessage(&messageId, id, data, &dataSize, &ts, status);
+
+  uint64_t timestamp = ConvertToFPGATime(ts);
+
+  std::lock_guard<wpi::mutex> lock(can->mapMutex);
+  if (*status == 0) {
+    // fresh update
+    auto& msg = can->receives[id];
+    msg.length = dataSize;
+    *length = dataSize;
+    msg.lastTimeStamp = timestamp;
+    *receivedTimestamp = timestamp;
+    std::memcpy(msg.data, data, dataSize);
+  } else {
+    auto i = can->receives.find(id);
+    if (i != can->receives.end()) {
+      // Found, check if new enough
+      uint64_t now = HAL_GetFPGATime(status);
+      if (now - i->second.lastTimeStamp >
+          static_cast<uint64_t>(timeoutMs) * 1000) {
+        // Timeout, return bad status
+        *status = HAL_CAN_TIMEOUT;
+        return;
+      }
+      std::memcpy(i->second.data, data, i->second.length);
+      *length = i->second.length;
+      *receivedTimestamp = i->second.lastTimeStamp;
+      *status = 0;
+    }
+  }
+}

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

@@ -29,6 +29,7 @@ void InitializeHAL() {
   InitializeAnalogOutData();
   InitializeAnalogTriggerData();
   InitializeCanData();
+  InitializeCANAPI();
   InitializeDigitalPWMData();
   InitializeDIOData();
   InitializeDriverStationData();
@@ -196,6 +197,8 @@ const char* HAL_GetErrorMessage(int32_t code) {
       return VI_ERROR_INV_PARAMETER_MESSAGE;
     case HAL_PWM_SCALE_ERROR:
       return HAL_PWM_SCALE_ERROR_MESSAGE;
+    case HAL_CAN_TIMEOUT:
+      return HAL_CAN_TIMEOUT_MESSAGE;
     default:
       return "Unknown error status";
   }

hal/src/main/native/sim/HALInitializer.h

@@ -24,6 +24,7 @@ extern void InitializeAnalogInData();
 extern void InitializeAnalogOutData();
 extern void InitializeAnalogTriggerData();
 extern void InitializeCanData();
+extern void InitializeCANAPI();
 extern void InitializeDigitalPWMData();
 extern void InitializeDIOData();
 extern void InitializeDriverStationData();