diff --git a/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java b/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java index ba9ac791ed7..80dd0e7b4f9 100644 --- a/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java +++ b/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() {} diff --git a/hal/src/main/native/include/hal/CAN.h b/hal/src/main/native/include/hal/CAN.h index 09b856e25fd..e61ddc2ae04 100644 --- a/hal/src/main/native/include/hal/CAN.h +++ b/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 diff --git a/hal/src/main/native/systemcore/CAN.cpp b/hal/src/main/native/systemcore/CAN.cpp index 3c822fc8c5c..7a318517a76 100644 --- a/hal/src/main/native/systemcore/CAN.cpp +++ b/hal/src/main/native/systemcore/CAN.cpp @@ -4,23 +4,273 @@ #include "hal/CAN.h" +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + #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>> + timers; + // ms to bus mask/packet + wpi::DenseMap>> + timedFrames; + // packet to time + wpi::DenseMap packetToTime; + + wpi::mutex readMutex[NUM_CAN_BUSES]; + // TODO(thadhouse) we need a MUCH better way of doing this masking + wpi::DenseMap 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(&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(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, diff --git a/hal/src/main/native/systemcore/CANAPI.cpp b/hal/src/main/native/systemcore/CANAPI.cpp index 751b0253f55..fdcefa0d20c 100644 --- a/hal/src/main/native/systemcore/CANAPI.cpp +++ b/hal/src/main/native/systemcore/CANAPI.cpp @@ -9,6 +9,7 @@ #include #include +#include #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, diff --git a/hal/src/main/native/systemcore/CANInternal.h b/hal/src/main/native/systemcore/CANInternal.h new file mode 100644 index 00000000000..d60d8451119 --- /dev/null +++ b/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 diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index 72b6da25211..2d043120921 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -25,6 +25,7 @@ #include #include +#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