diff --git a/include/sta/rtos/system/can_bus.hpp b/include/sta/rtos/system/can_bus.hpp new file mode 100644 index 0000000..0810ac3 --- /dev/null +++ b/include/sta/rtos/system/can_bus.hpp @@ -0,0 +1,184 @@ +/** + * @file + * @brief Public interface for CAN driver thread. + */ +#ifndef STA_RTOS_SYSTEM_CAN_BUS_HPP +#define STA_RTOS_SYSTEM_CAN_BUS_HPP + +#include + + +/** + * @defgroup STA_RTOS_CanBus CAN driver + * @ingroup STA_RTOS_API + * @brief CAN bus system task. + * + * Check @ref STA_RTOS_BuildConfig for configuration options. + */ + +#ifdef DOXYGEN +/** + * @brief Enable module. + * + * @ingroup STA_RTOS_BuildConfig + */ +# define STA_RTOS_CAN_BUS_ENABLE +#endif // DOXYGEN + +/** + * @def STA_RTOS_CAN_BUS_TASK_NAME + * @brief Set name of CAN driver task. + * + * @ingroup STA_RTOS_BuildConfig + */ +#ifndef STA_RTOS_CAN_BUS_TASK_NAME +# define STA_RTOS_CAN_BUS_TASK_NAME canBus +#endif // !STA_RTOS_CAN_BUS_TASK_NAME + +/** + * @def STA_RTOS_CAN_BUS_ENTRY_FUNCTION + * @brief Set name of CAN driver task entry function. + * + * @ingroup STA_RTOS_BuildConfig + */ +#ifndef STA_RTOS_CAN_BUS_ENTRY_FUNCTION +# define STA_RTOS_CAN_BUS_ENTRY_FUNCTION STA_RTOS_MAKE_ENTRY_NAME(STA_RTOS_CAN_BUS_TASK_NAME) +#endif // !STA_RTOS_CAN_BUS_ENTRY_FUNCTION + + +#include +#ifdef STA_RTOS_CAN_BUS_ENABLE + +#include + +#include + + +/** + * @def STA_RTOS_CAN_BUS_MAX_FILTER + * @brief Set maximum number of usable filters. + * + * @ingroup STA_RTOS_BuildConfig + */ +#ifndef STA_RTOS_CAN_BUS_MAX_FILTER +# error "Must set STA_RTOS_CAN_BUS_MAX_FILTER in " +#endif // STA_RTOS_CAN_BUS_MAX_FILTER + +/** + * @def STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE + * @brief Set maximum payload size. + * + * @ingroup STA_RTOS_BuildConfig + */ +#ifndef STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE +# error "Must set STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE in " +#endif // STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE + + +/** + * @ingroup STA_RTOS_CanBus + * @{ + */ + + +/** + * @brief CAN frame available. + */ +#define STA_RTOS_CAN_FLAG_MSG_AVAIL 0x000010U +/** + * @brief Send CAN message. + */ +#define STA_RTOS_CAN_FLAG_MSG_SEND 0x000020U +/** + * @brief CAN data message in queue. + */ +#define STA_RTOS_CAN_FLAG_DATA_QUEUED 0x000040U +/** + * @brief CAN system message in queue. + */ +#define STA_RTOS_CAN_FLAG_SYS_QUEUED 0x000080U +/** + * @brief Show ISOTP statistics. + */ +#define STA_RTOS_CAN_FLAG_SHOW_STATS 0x000100U + + +/** + * @brief CAN SID bits used for system messages. + */ +#define STA_CAN_SID_SYS_BITS UINT32_C(0x3) + + +/** @} */ + + +namespace sta +{ + namespace rtos + { + /** + * @ingroup STA_RTOS_CanBus + * @{ + */ + + + /** + * @brief Extra initialization run at start of CAN bus task. + * + * May be overridden by application if required. + */ + void setupCanBus(); + + + + /** + * @brief Send notification to CAN driver. + * + * @param flags Event flags + */ + void notifyCanBus(uint32_t flags); + + + /** + * @brief Place data message in CAN driver TX queue. + * + * @param msg Message to transmit + * @param timeout Timeout for placing message (0 = no wait, osWaitForever = blocking) + * @return True if message was queued successfully + */ + bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout); + /** + * @brief Place system message in CAN driver TX queue. + * + * @param msg Message to transmit + * @param timeout Timeout for placing message (0 = no wait, osWaitForever = blocking) + * @return True if message was queued successfully + */ + bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout); + + /** + * @brief Retrieve data message from CAN driver TX queue. + * + * @param[out] msg Output address for retrieved message + * @param timeout Timeout for retrieving message (0 = no wait, osWaitForever = blocking) + * @return True if message was retrieved successfully + */ + bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout); + /** + * @brief Retrieve system message from CAN driver TX queue. + * + * @param[out] msg Destination for retrieved message + * @param timeout Timeout for retrieving message (0 = no wait, osWaitForever = blocking) + * @return True if message was retrieved successfully + */ + bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout); + + + /** @} */ + } // namespace rtos +} // namespace sta + + +#endif // STA_RTOS_CAN_BUS_ENABLE + +#endif // STA_RTOS_SYSTEM_CAN_BUS_HPP diff --git a/include/sta/rtos/system/can_msg.h b/include/sta/rtos/system/can_msg.h new file mode 100644 index 0000000..617da19 --- /dev/null +++ b/include/sta/rtos/system/can_msg.h @@ -0,0 +1,49 @@ +/** + * @file + * @brief CAN driver message request types for use in C code. + */ +#ifndef STA_RTOS_SYSTEM_CAN_MSG_H +#define STA_RTOS_SYSTEM_CAN_MSG_H + +#include + + +/** + * @brief CAN message header. + * + * @ingroup STA_RTOS_CanBus + */ +struct CanMsgHeader +{ + uint32_t sid; /**< Message SID */ + uint32_t eid; /**< Message EID */ + uint8_t format; /**< Message ID format */ + uint8_t payloadLength; /**< Payload length */ +}; + + +/** + * @brief Element type for CAN data message queue. + * + * @ingroup STA_RTOS_CanBus + */ +struct CanDataMsg +{ + struct CanMsgHeader header; /**< Message header data */ + uint8_t payload[64]; /**< Message payload */ +}; + + +/** + * @brief Element type for CAN system message queue. + * + * @ingroup STA_RTOS_CanBus + */ +struct CanSysMsg +{ + struct CanMsgHeader header; /**< Message header data */ + uint8_t payload[8]; /**< Message payload */ +}; + + +#endif // STA_RTOS_SYSTEM_CAN_MSG_H diff --git a/src/system/can_bus.cpp b/src/system/can_bus.cpp new file mode 100644 index 0000000..08c155e --- /dev/null +++ b/src/system/can_bus.cpp @@ -0,0 +1,408 @@ +/** + * @file + * @brief CAN driver thread. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include + + +#define STA_RTOS_MAKE_DATA_QUEUE_NAME(name) _STA_RTOS_CONCAT(name, DataQueue) +#define STA_RTOS_MAKE_SYS_QUEUE_NAME(name) _STA_RTOS_CONCAT(name, SysQueue) + + +#define STA_RTOS_CAN_BUS_THREAD STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_CAN_BUS_TASK_NAME) +#define STA_RTOS_CAN_BUS_DATA_QUEUE STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_MAKE_DATA_QUEUE_NAME(STA_RTOS_CAN_BUS_TASK_NAME)) +#define STA_RTOS_CAN_BUS_SYS_QUEUE STA_RTOS_MAKE_HANDLE_NAME(STA_RTOS_MAKE_SYS_QUEUE_NAME(STA_RTOS_CAN_BUS_TASK_NAME)) + + +// Access handles from freertos.c +extern osThreadId_t STA_RTOS_CAN_BUS_THREAD; +extern osMessageQueueId_t STA_RTOS_CAN_BUS_DATA_QUEUE; +extern osMessageQueueId_t STA_RTOS_CAN_BUS_SYS_QUEUE; + + +namespace sta +{ + namespace rtos + { + extern CanController * CanBusController; + + + STA_WEAK + void setupCanBus() + {} + + + void notifyCanBus(uint32_t flags) + { + // Send flags to thread + osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, flags); + } + + + bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout) + { + STA_ASSERT((msg.header.sid & STA_CAN_SID_SYS_BITS) == 0); + STA_ASSERT(msg.header.payloadLength <= sizeof(msg.payload)); + + if (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_DATA_QUEUE, &msg, 0, timeout)) + { + // Signal thread + osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, STA_RTOS_CAN_FLAG_DATA_QUEUED); + return true; + } + else + { + return false; + } + } + + bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout) + { + STA_ASSERT((msg.header.sid & ~STA_CAN_SID_SYS_BITS) == 0); + + if (osOK == osMessageQueuePut(STA_RTOS_CAN_BUS_SYS_QUEUE, &msg, 0, timeout)) + { + // Signal thread + osThreadFlagsSet(STA_RTOS_CAN_BUS_THREAD, STA_RTOS_CAN_FLAG_SYS_QUEUED); + return true; + } + else + { + return false; + } + } + + + bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout) + { + return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_DATA_QUEUE, msg, 0, timeout)); + } + + bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout) + { + return (osOK == osMessageQueueGet(STA_RTOS_CAN_BUS_SYS_QUEUE, msg, 0, timeout)); + } + } // namespace rtos +} // namespace sta + + +using namespace sta; + + +/**< ISOTP CAN transmitter */ +IsotpTransmitter gCanTx(rtos::CanBusController, HAL_GetTick); +/**< ISOTP CAN receiver */ +IsotpReceiver gCanRx(rtos::CanBusController, HAL_GetTick); + + +CanRxCallback filterCallbacks[STA_RTOS_CAN_BUS_MAX_FILTER]; + + +namespace debug +{ + /** + * @brief Display ISOTP TX/RX statistics. + */ + void showStatistics() + { + STA_DEBUG_PRINTLN(); + STA_DEBUG_PRINTLN("# ######################"); + STA_DEBUG_PRINTLN("# ## ISOTP statistics ##"); + STA_DEBUG_PRINTLN("# ######################"); + STA_DEBUG_PRINTLN("#"); + + STA_DEBUG_PRINTLN("# Transmitter"); + STA_DEBUG_PRINT("# messages: "); + STA_DEBUG_PRINTLN(gCanTx.stats().messages); + STA_DEBUG_PRINT("# blocks: "); + STA_DEBUG_PRINTLN(gCanTx.stats().blocks); + STA_DEBUG_PRINT("# frames: "); + STA_DEBUG_PRINTLN(gCanTx.stats().frames); + STA_DEBUG_PRINT("# timeouts: "); + STA_DEBUG_PRINTLN(gCanTx.stats().timeouts); + STA_DEBUG_PRINTLN("#"); + + STA_DEBUG_PRINTLN("# Receiver"); + STA_DEBUG_PRINT("# messages: "); + STA_DEBUG_PRINTLN(gCanRx.stats().messages); + STA_DEBUG_PRINT("# blocks: "); + STA_DEBUG_PRINTLN(gCanRx.stats().blocks); + STA_DEBUG_PRINT("# frames: "); + STA_DEBUG_PRINTLN(gCanRx.stats().frames); + STA_DEBUG_PRINT("# timeouts: "); + STA_DEBUG_PRINTLN(gCanRx.stats().timeouts); + STA_DEBUG_PRINT("# flow control errors: "); + STA_DEBUG_PRINTLN(gCanRx.stats().flowErrors); + STA_DEBUG_PRINT("# overflows: "); + STA_DEBUG_PRINTLN(gCanRx.stats().overflows); + STA_DEBUG_PRINTLN(); + } + + + /** + * @brief Output CAN frame ID to UART. + * + * @param id Frame ID + */ + void printFrameID(const sta::CanFrameId & id) + { + STA_DEBUG_PRINT("SID: "); + STA_DEBUG_PRINTLN(id.sid, sta::IntegerBase::HEX); + if (id.format == sta::CanIdFormat::EXT) + { + STA_DEBUG_PRINT("EID: "); + STA_DEBUG_PRINTLN(id.eid, sta::IntegerBase::HEX); + } + } + + /** + * @brief Output CAN frame payload to UART. + * + * @param payload Payload buffer + * @param size Payload size + */ + void printPayloadHex(const uint8_t * payload, uint8_t size) + { + // Write frame payload to DebugSerial + STA_DEBUG_PRINT("payload: "); + for (uint8_t i = 0; i < size; ++i) + { + STA_DEBUG_PRINT(payload[i], sta::IntegerBase::HEX); + STA_DEBUG_PRINT(' '); + } + STA_DEBUG_PRINTLN(); + } +} // namespace debug + + +namespace demo +{ + extern void handleRxMessage(const uint8_t * buffer, uint16_t size); +} // namespace demo + + +/** + * @brief Process received ISOTP messages. + * + * @param msg ISOTP message + */ +void handleRxMessage(const sta::IsotpMessage & msg) +{ + STA_DEBUG_PRINTLN("[event] RX message"); + + debug::printFrameID(msg.frameID); + + // TODO Forward message to other threads + demo::handleRxMessage(msg.buffer, msg.size); +} + + +/** + * @brief Handle received data message CAN frames. + * + * @param header CAN frame header + * @param payload Payload buffer + */ +void receiveDataCallback(const sta::CanRxHeader & header, const uint8_t * payload) +{ + // Write frame payload to DebugSerial + STA_DEBUG_PRINTLN("[event] RX data frame"); + debug::printPayloadHex(payload, header.payloadLength); + + // Process RX frame + auto handle = gCanRx.processFrame(header, payload); + + if (handle != sta::IsotpReceiver::INVALID_HANDLE) + { + // Get message if completed + sta::IsotpMessage msg; + if (gCanRx.getMessage(handle, &msg)) + { + handleRxMessage(msg); + } + + // Handle FC responses + gCanRx.processFC(handle); + } + + // Process TX frame + gCanTx.processFrame(header, payload); +} + +/** + * @brief Handle received system message CAN frames. + * + * @param header CAN frame header + * @param payload Payload buffer + */ +void receiveSysCallback(const sta::CanRxHeader & header, const uint8_t * payload) +{ + // Write frame payload to DebugSerial + STA_DEBUG_PRINTLN("[event] RX sys frame"); + + debug::printFrameID(header.id); + debug::printPayloadHex(payload, header.payloadLength); + + // TODO Forward message to other threads +} + + +/** + * @brief Configure CAN filters. + */ +void setupCanSubscriptions() +{ + // Make sure to receive all messages + CanFilter filter; + + // Clear previous subscriptions + rtos::CanBusController->clearFilters(); + + + // All bits except [0:1] of the SID must be zero for system messages + filter.obj = {0, 0}; + filter.mask = {~STA_CAN_SID_SYS_BITS, 0}; + filter.type = sta::CanFilterIdFormat::ANY; + filter.fifo = 0; + + filterCallbacks[0] = receiveSysCallback; + rtos::CanBusController->configureFilter(0, filter, true); + + + // TODO Limit which data messages are received + // Bits [0:1] of the SID must be zero for data messages + filter.obj = {0, 0}; + filter.mask = {STA_CAN_SID_SYS_BITS, 0}; + filter.type = sta::CanFilterIdFormat::ANY; + filter.fifo = 1; + filterCallbacks[1] = receiveDataCallback; + rtos::CanBusController->configureFilter(1, filter, true); +} + + +/** + * @brief Process received CAN messages. + */ +void processMessages() +{ + for (auto fifo : rtos::CanBusController->getPendingRxFifos()) + { + CanRxHeader header; + uint8_t payload[STA_RTOS_CAN_BUS_MAX_PAYLOAD_SIZE]; + + if (rtos::CanBusController->receiveFrame(fifo, &header, payload)) + { +// debug::displayFrameUART(frame); + + // Forward frame to filter callback + if (fifo <= STA_RTOS_CAN_BUS_MAX_FILTER && filterCallbacks[header.filter]) + { + filterCallbacks[header.filter](header, payload); + } + } + } +} + + +extern "C" +{ + /** + * @brief CAN driver thread entry function. + */ + void canBusTask(void *) + { + using namespace sta; + + // Initialize CAN controller + rtos::setupCanBus(); + + // Configure filters for + setupCanSubscriptions(); + + + rtos::waitForStartupEvent(); + + while (true) + { + uint32_t flags = osThreadFlagsWait(STA_RTOS_THREAD_FLAGS_VALID_BITS, osFlagsWaitAny, 50); + + if (flags != static_cast(osErrorTimeout)) + { + STA_ASSERT_MSG((flags & osStatusReserved) == flags, "Unexpected error occurred in wait"); + + if (flags & STA_RTOS_CAN_FLAG_SYS_QUEUED) + { + CanSysMsg msg; + CanTxHeader header; + + // Take messages from queue until empty + while (rtos::getCanBusMsg(&msg, 0)) + { + header.id.format = static_cast(msg.header.format); + header.id.sid = msg.header.sid & STA_CAN_SID_SYS_BITS; + header.id.eid = msg.header.eid; + header.payloadLength = msg.header.payloadLength; + + debug::printPayloadHex(msg.payload, header.payloadLength); + + rtos::CanBusController->sendFrame(header, msg.payload); + } + } + + if (flags & STA_RTOS_CAN_FLAG_DATA_QUEUED) + { + CanDataMsg msg; + CanFrameId frameID; + + // Take messages from queue until empty + while (rtos::getCanBusMsg(&msg, 0)) + { + frameID.format = static_cast(msg.header.format); + frameID.sid = msg.header.sid & ~STA_CAN_SID_SYS_BITS; + frameID.eid = msg.header.eid; + + // Transmit via ISO-TP + gCanTx.send(frameID, msg.payload, msg.header.payloadLength); + } + } + + if (flags & STA_RTOS_CAN_FLAG_MSG_AVAIL) + { + STA_DEBUG_PRINTLN("[event] CAN INT"); + + processMessages(); + } + + if (flags & STA_RTOS_CAN_FLAG_SHOW_STATS) + { + debug::showStatistics(); + } + } + + // Process ISOTP transmissions + gCanTx.process(); + } + } +}