├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include ├── canfetti │ ├── CanDevice.h │ ├── LocalNode.h │ ├── Node.h │ ├── ObjDict.h │ ├── OdData.h │ ├── Types.h │ └── services │ │ ├── Emcy.h │ │ ├── Nmt.h │ │ ├── Pdo.h │ │ ├── Sdo.h │ │ ├── Service.h │ │ └── sdo │ │ ├── Client.h │ │ ├── Protocol.h │ │ ├── Server.h │ │ └── ServerBlockMode.h └── platform │ ├── linux │ └── canfetti │ │ ├── LinuxCo.h │ │ └── System.h │ ├── odrive │ └── canfetti │ │ ├── ODriveCo.h │ │ └── System.h │ ├── teensy │ └── canfetti │ │ ├── System.h │ │ └── TyCo.h │ └── unittest │ └── canfetti │ └── System.h ├── package.xml └── src ├── CanDevice.cpp ├── LocalNode.cpp ├── ObjDict.cpp ├── OdData.cpp ├── platform ├── linux │ ├── LinuxCo.cpp │ └── test │ │ ├── blockmode.cpp │ │ ├── generation.cpp │ │ ├── threads.cpp │ │ └── vector.cpp ├── odrive │ └── ODriveCo.cpp ├── teensy │ └── TyCo.cpp └── unittest │ ├── test-callbacks.cpp │ ├── test-client.cpp │ ├── test-od.cpp │ ├── test.cpp │ └── test.h └── services ├── Emcy.cpp ├── Nmt.cpp ├── Pdo.cpp ├── Sdo.cpp └── sdo ├── Client.cpp ├── Protocol.cpp ├── Server.cpp └── ServerBlockMode.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | BreakBeforeBraces: Custom 4 | BraceWrapping: 5 | AfterFunction: true 6 | BeforeElse: true 7 | ColumnLimit: "0" 8 | AlignConsecutiveAssignments: true 9 | AllowShortLoopsOnASingleLine: true 10 | AllowShortBlocksOnASingleLine: true 11 | AllowShortCaseLabelsOnASingleLine: true 12 | IndentPPDirectives: BeforeHash 13 | IncludeBlocks: Merge 14 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | build/ 3 | .vscode/ 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(canfetti) 3 | 4 | # -std=gnu++17 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED YES) 7 | set(CMAKE_CXX_EXTENSIONS YES) 8 | 9 | find_package(catkin) 10 | if(catkin_FOUND) 11 | catkin_package( 12 | INCLUDE_DIRS include 13 | LIBRARIES canfetti) 14 | endif() 15 | 16 | set(CORE_SRC 17 | src/CanDevice.cpp 18 | src/LocalNode.cpp 19 | src/ObjDict.cpp 20 | src/OdData.cpp 21 | src/services/Emcy.cpp 22 | src/services/Nmt.cpp 23 | src/services/Pdo.cpp 24 | src/services/sdo/Client.cpp 25 | src/services/sdo/Protocol.cpp 26 | src/services/sdo/Server.cpp 27 | src/services/sdo/ServerBlockMode.cpp 28 | src/services/Sdo.cpp) 29 | 30 | add_library(canfetti SHARED 31 | ${CORE_SRC} 32 | src/platform/linux/LinuxCo.cpp) 33 | target_include_directories(canfetti PUBLIC 34 | include 35 | include/platform/linux) 36 | target_compile_options(canfetti PRIVATE -g -Og -Wall -Werror) 37 | target_link_libraries(canfetti PUBLIC pthread) 38 | 39 | if(NOT catkin_FOUND) 40 | add_executable(canfetti_unittest 41 | ${CORE_SRC} 42 | src/platform/unittest/test.cpp 43 | src/platform/unittest/test-od.cpp 44 | src/platform/unittest/test-client.cpp 45 | src/platform/unittest/test-callbacks.cpp 46 | ) 47 | target_include_directories(canfetti_unittest PUBLIC 48 | include 49 | include/platform/unittest) 50 | target_link_libraries(canfetti_unittest PRIVATE pthread gtest gmock gtest_main) 51 | target_compile_options(canfetti_unittest PRIVATE -g -O0) 52 | 53 | add_executable(canfetti_threadtest 54 | src/platform/linux/test/threads.cpp 55 | ) 56 | target_compile_options(canfetti_threadtest PRIVATE -g -Og) 57 | target_link_libraries(canfetti_threadtest PRIVATE canfetti) 58 | 59 | add_executable(canfetti_blocktest 60 | src/platform/linux/test/blockmode.cpp 61 | ) 62 | target_link_libraries(canfetti_blocktest PRIVATE canfetti) 63 | 64 | add_executable(canfetti_vectortest 65 | src/platform/linux/test/vector.cpp 66 | ) 67 | target_link_libraries(canfetti_vectortest PRIVATE canfetti) 68 | 69 | add_executable(canfetti_generationtest 70 | src/platform/linux/test/generation.cpp 71 | ) 72 | target_link_libraries(canfetti_generationtest PRIVATE canfetti) 73 | endif() 74 | 75 | if(catkin_FOUND) 76 | install( 77 | DIRECTORY 78 | include/canfetti 79 | # Dump the Linux platform headers into the same directory because Catkin 80 | # is unreasonable about exporting multiple include dirs. 81 | include/platform/linux/canfetti 82 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 83 | install( 84 | TARGETS canfetti 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 88 | endif() 89 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Scythe Robotics 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CANfetti 2 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) 3 | 4 | ### A CANopen stack by Scythe 🎊 5 | 6 | Currently supported platforms: 7 | - Linux 8 | - [Teensy](https://www.pjrc.com/teensy/) 9 | - [ODrive](https://odriverobotics.com/) 10 | 11 | ## Examples 12 | 13 | ### Teensy Example 14 | ``` 15 | static constexpr uint8_t MyNodeId = 1; 16 | static float temperatureC = 0.f; 17 | static canfetti::TyCoDev canDevice; 18 | static canfetti::TyCo co(canDevice, MyNodeId, "MyNodeName"); 19 | 20 | void setup() 21 | { 22 | tempmon_init(); 23 | 24 | co.init(); 25 | co.setHeartbeatPeriod(1000); // Send a NMT heartbeat @ 1Hz 26 | 27 | // TPDO1 @ 10Hz 28 | co.autoAddTPDO(1, 0x181, 100, canfetti::_p(temperatureC)); 29 | } 30 | 31 | void loop() { 32 | co.service(); // Runs the CANopen stack 33 | ... 34 | temperatureC = tempmonGetTemp(); 35 | ... 36 | } 37 | ``` -------------------------------------------------------------------------------- /include/canfetti/CanDevice.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "Types.h" 5 | 6 | namespace canfetti { 7 | 8 | class CanDevice { 9 | public: 10 | struct Stats { 11 | size_t droppedPrioTx = 0; 12 | size_t droppedTx = 0; 13 | size_t droppedRx = 0; 14 | size_t overruns = 0; 15 | }; 16 | 17 | CanDevice(); 18 | 19 | template 20 | Error write(uint16_t cobid, uint8_t (&payload)[N], bool prio = false) 21 | { 22 | canfetti::Msg msg = { 23 | .id = cobid, 24 | .rtr = false, 25 | .len = N, 26 | .data = payload, 27 | }; 28 | return prio ? writePriority(msg) : write(msg); 29 | } 30 | 31 | // If async is true and platform supports it: return success and actual write 32 | // happens later; caller cannot tell whether actual write succeeds 33 | virtual Error write(const Msg &msg, bool async = false) = 0; 34 | virtual Error writePriority(const Msg &msg) { return write(msg); } 35 | 36 | Stats stats; 37 | }; 38 | 39 | } // namespace canfetti -------------------------------------------------------------------------------- /include/canfetti/LocalNode.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "CanDevice.h" 4 | #include "Node.h" 5 | #include "ObjDict.h" 6 | #include "Types.h" 7 | #include "canfetti/System.h" 8 | #include "services/Emcy.h" 9 | #include "services/Nmt.h" 10 | #include "services/Pdo.h" 11 | #include "services/Sdo.h" 12 | 13 | namespace canfetti { 14 | 15 | class LocalNode : public Node { 16 | public: 17 | using Node::od; 18 | 19 | Error init(); 20 | inline void setSDOServerTimeout(uint32_t timeoutMs) { sdo.setServerSegmentTimeout(timeoutMs); } 21 | inline size_t getActiveTransactionCount() { return sdo.getActiveTransactionCount(); } 22 | inline Error addSDOServer(uint16_t rxCobid, uint16_t txCobid, uint8_t clientId) { return sdo.addSDOServer(rxCobid, txCobid, clientId); } 23 | inline Error addSDOClient(uint32_t txCobid, uint16_t rxCobid, uint8_t serverId) { return sdo.addSDOClient(txCobid, rxCobid, serverId); } 24 | inline Error addSDOServer(uint8_t sdoId, uint8_t remoteNode) { return sdo.addSDOServer(0x600 + sdoId, 0x580 + sdoId, remoteNode); } 25 | inline Error addSDOClient(uint8_t sdoId, uint8_t remoteNode) { return sdo.addSDOClient(0x600 + sdoId, 0x580 + sdoId, remoteNode); } 26 | inline Error triggerTPDO(uint16_t pdoNum, bool async = false) { return pdo.sendTxPdo(0x1800 + pdoNum, async); } 27 | inline Error triggerAllTPDOs() { return pdo.sendAllTpdos(); } 28 | inline Error updateTpdoEventTime(uint16_t pdoNum, uint16_t periodMs) { return pdo.updateTpdoEventTime(0x1800 + pdoNum, periodMs); } 29 | inline Error disableTPDO(uint16_t pdoNum) { return pdo.disablePdo(0x1800 + pdoNum); } 30 | inline Error enableTPDO(uint16_t pdoNum) { return pdo.enablePdo(0x1800 + pdoNum); } 31 | inline Error setHeartbeatPeriod(uint16_t periodMs) { return nmt.setHeartbeatPeriod(periodMs); } 32 | inline Error setRemoteTimeout(uint8_t node, uint16_t timeoutMs) { return nmt.setRemoteTimeout(node, timeoutMs); } 33 | inline Error addRPDO(uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t timeoutMs = 0, PdoService::TimeoutCb cb = nullptr) { return pdo.addRPDO(cobid, mapping, numMapping, timeoutMs, cb); } 34 | inline Error addRPDO(uint16_t cobid, std::initializer_list> mapping, uint16_t timeoutMs = 0, PdoService::TimeoutCb cb = nullptr) { return pdo.addRPDO(cobid, mapping.begin(), mapping.size(), timeoutMs, cb); } 35 | inline Error addTPDO(uint16_t pdoNum, uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t periodMs = 0, bool enabled = true) { return pdo.addTPDO(pdoNum, cobid, mapping, numMapping, periodMs, enabled); } 36 | inline Error addTPDO(uint16_t pdoNum, uint16_t cobid, std::initializer_list> mapping, uint16_t periodMs = 0, bool enabled = true) { return pdo.addTPDO(pdoNum, cobid, mapping.begin(), mapping.size(), periodMs, enabled); } 37 | inline Error sendEmcy(uint16_t error, uint32_t specific = 0, EmcyService::ErrorType type = EmcyService::ErrorType::Generic) { return emcy.sendEmcy(error, specific, type); } 38 | inline Error sendEmcy(uint16_t error, std::array &specific, EmcyService::ErrorType type = EmcyService::ErrorType::Generic) { return emcy.sendEmcy(error, specific, type); } 39 | inline Error clearEmcy(uint16_t error, EmcyService::ErrorType type = EmcyService::ErrorType::Generic) { return emcy.clearEmcy(error, type); } 40 | inline const char *getDeviceName() { return deviceName; } 41 | Error setState(State s); 42 | canfetti::Error registerEmcyCallback(EmcyService::EmcyCallback cb) { return emcy.registerCallback(cb); } 43 | 44 | template 45 | canfetti::Error autoAddTPDO(uint16_t pdoNum, uint16_t cobid, uint16_t periodMs, Args &&...args) 46 | { 47 | std::vector> map; 48 | _autoInsertAll(map, canfetti::Access::RO, std::forward(args)...); 49 | addTPDO(pdoNum, cobid, map.data(), map.size(), periodMs); 50 | return canfetti::Error::Success; 51 | } 52 | 53 | template 54 | canfetti::Error autoAddRPDO(uint16_t cobid, Args &&...args) 55 | { 56 | std::vector> map; 57 | _autoInsertAll(map, canfetti::Access::WO, std::forward(args)...); 58 | addRPDO(cobid, map.data(), map.size()); 59 | return canfetti::Error::Success; 60 | } 61 | 62 | template 63 | canfetti::Error autoAddTimeoutRPDO(uint16_t cobid, uint16_t timeoutMs, PdoService::TimeoutCb cb, Args &&...args) 64 | { 65 | std::vector> map; 66 | _autoInsertAll(map, canfetti::Access::WO, std::forward(args)...); 67 | addRPDO(cobid, map.data(), map.size(), timeoutMs, cb); 68 | return canfetti::Error::Success; 69 | } 70 | 71 | // Todo refactor into a RemoteNode class 72 | inline std::tuple getRemoteState(uint8_t node) { return nmt.getRemoteState(node); } 73 | inline Error setRemoteState(uint8_t node, SlaveState state) { return nmt.setRemoteState(node, state); } 74 | Error registerRemoteStateCb(NmtService::RemoteStateCb cb) { return nmt.addRemoteStateCb(NmtService::AllNodes, cb); } 75 | Error registerRemoteStateCb(uint8_t node, NmtService::RemoteStateCb cb) { return nmt.addRemoteStateCb(node, cb); } 76 | 77 | template 78 | inline Error read(uint8_t node, uint16_t idx, uint8_t subIdx, std::function cb, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 79 | { 80 | OdVariant *v = new OdVariant(T()); 81 | 82 | if (!v) { 83 | return Error::OutOfMemory; 84 | } 85 | 86 | Error err = sdo.clientTransaction(true, node, idx, subIdx, *v, segmentTimeout, [=](Error e) { 87 | if (cb) cb(e, *std::get_if(v)); 88 | delete v; 89 | }); 90 | 91 | if (err != Error::Success) { 92 | delete v; 93 | } 94 | return err; 95 | } 96 | 97 | template 98 | inline Error readData(uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, std::function cb, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 99 | { 100 | OdVariant *v = new OdVariant(data); 101 | 102 | if (!v) { 103 | return Error::OutOfMemory; 104 | } 105 | 106 | Error err = sdo.clientTransaction(true, node, idx, subIdx, *v, segmentTimeout, [=](Error e) { 107 | if (cb) cb(e); 108 | delete v; 109 | }); 110 | 111 | if (err != Error::Success) { 112 | delete v; 113 | } 114 | return err; 115 | } 116 | 117 | template 118 | Error write(uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, std::function cb, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 119 | { 120 | OdVariant *v = new OdVariant(data); 121 | 122 | if (!v) { 123 | return Error::OutOfMemory; 124 | } 125 | 126 | Error err = sdo.clientTransaction(false, node, idx, subIdx, *v, segmentTimeout, [=](Error e) { 127 | if (cb) cb(e); 128 | delete v; 129 | }); 130 | 131 | if (err != Error::Success) { 132 | delete v; 133 | } 134 | return err; 135 | } 136 | 137 | protected: 138 | LocalNode(CanDevice &d, System &sys, uint8_t nodeId, const char *deviceName, uint32_t deviceType); 139 | void processFrame(const Msg &m); 140 | 141 | template 142 | canfetti::Error _autoInsertAll(std::vector> &map, canfetti::Access access, Arg &&arg) 143 | { 144 | auto [err, idx] = od.autoInsert(access, std::forward(arg)); 145 | map.push_back(std::move(idx)); 146 | return err; 147 | } 148 | 149 | template 150 | canfetti::Error _autoInsertAll(std::vector> &map, canfetti::Access access, Arg &&arg, Args &&...args) 151 | { 152 | auto [err, idx] = od.autoInsert(access, std::forward(arg)); 153 | map.push_back(std::move(idx)); 154 | if (err == canfetti::Error::Success) return _autoInsertAll(map, access, std::forward(args)...); 155 | return err; 156 | } 157 | 158 | NmtService nmt; 159 | PdoService pdo; 160 | SdoService sdo; 161 | EmcyService emcy; 162 | const char *deviceName; 163 | uint32_t deviceType; 164 | }; 165 | 166 | } // namespace canfetti 167 | -------------------------------------------------------------------------------- /include/canfetti/Node.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "CanDevice.h" 4 | #include "ObjDict.h" 5 | #include "Types.h" 6 | 7 | namespace canfetti { 8 | 9 | class Node { 10 | public: 11 | ObjDict od; 12 | uint8_t nodeId; 13 | CanDevice &bus; 14 | System &sys; 15 | 16 | virtual Error setState(State s) = 0; 17 | State getState() const { return state; } 18 | 19 | protected: 20 | State state = State::Bootup; 21 | 22 | Node(CanDevice &d, System &sys, uint8_t nodeId) 23 | : nodeId(nodeId), bus(d), sys(sys) {} 24 | }; 25 | 26 | } // namespace canfetti 27 | -------------------------------------------------------------------------------- /include/canfetti/ObjDict.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "canfetti/OdData.h" 9 | #include "canfetti/System.h" 10 | 11 | namespace canfetti { 12 | 13 | class ObjDict { 14 | public: 15 | using OdTable = std::map>; 16 | 17 | void dumpTable(); 18 | Error registerCallback(uint16_t idx, uint8_t subIdx, ChangedCallback cb); 19 | Error fireCallbacks(uint16_t idx, uint8_t subIdx); 20 | Error generation(uint16_t idx, uint8_t subIdx, uint32_t &generationOut); 21 | std::tuple makeProxy(uint16_t idx, uint8_t subIdx); 22 | 23 | template 24 | Error get(uint16_t idx, uint8_t subIdx, T &v) 25 | { 26 | if (OdEntry *entry = lookup(idx, subIdx)) { 27 | if constexpr (std::is_same_v) { 28 | if (entry->lock()) { 29 | OdProxy src(idx, subIdx, entry->data); 30 | OdProxy dst(idx, subIdx, v); 31 | dst.copyFrom(src); 32 | entry->unlock(); 33 | return Error::Success; 34 | } 35 | return Error::Timeout; 36 | } 37 | else { 38 | if (T *p = std::get_if(&entry->data)) { 39 | if (entry->lock()) { 40 | v = *p; 41 | entry->unlock(); 42 | return Error::Success; 43 | } 44 | return Error::Timeout; 45 | } 46 | else { 47 | return Error::ParamIncompatibility; 48 | } 49 | } 50 | } 51 | return Error::IndexNotFound; 52 | } 53 | 54 | template 55 | Error set(uint16_t idx, uint8_t subIdx, const T &v) 56 | { 57 | if (OdEntry *entry = lookup(idx, subIdx)) { 58 | if constexpr (std::is_same_v) { 59 | if (entry->lock()) { 60 | OdProxy dst(idx, subIdx, entry->data); 61 | OdProxy src(idx, subIdx, v); 62 | dst.copyFrom(src); 63 | 64 | entry->unlock(); 65 | entry->fireCallbacks(); 66 | return Error::Success; 67 | } 68 | return Error::Timeout; 69 | } 70 | else { 71 | if (auto p = std::get_if(&entry->data)) { 72 | if (entry->lock()) { 73 | *p = v; 74 | entry->unlock(); 75 | entry->bumpGeneration(); 76 | entry->fireCallbacks(); 77 | return Error::Success; 78 | } 79 | return Error::Timeout; 80 | } 81 | else { 82 | return Error::ParamIncompatibility; 83 | } 84 | } 85 | } 86 | return Error::IndexNotFound; 87 | } 88 | 89 | template 90 | CANFETTI_NO_INLINE Error insert(uint16_t idx, uint8_t subIdx, canfetti::Access access, T &&v, ChangedCallback cb = nullptr, bool cbOnInsert = false) 91 | { 92 | if (lookup(idx, subIdx)) { 93 | LogInfo("Warning: index %x[%d] already exists in the OD", idx, subIdx); 94 | return Error::Error; 95 | } 96 | 97 | buildSubEntry(idx, subIdx, access, std::forward(v), cb); 98 | 99 | if (cbOnInsert) cb(idx, subIdx); 100 | return Error::Success; 101 | } 102 | 103 | // When it doesn't matter where the variable is mapped (i.e. RPDO values) 104 | template 105 | std::tuple> autoInsert(canfetti::Access access, T &&v, ChangedCallback cb = nullptr, uint16_t startIdx = 0x3500, uint16_t endIdx = 0x4000) 106 | { 107 | uint16_t subIdx = 0; 108 | 109 | for (; startIdx < endIdx; startIdx++) { 110 | if (lookup(startIdx, subIdx)) continue; 111 | buildSubEntry(startIdx, subIdx, access, std::forward(v), cb); 112 | return std::make_tuple(Error::Success, std::make_tuple(startIdx, subIdx)); 113 | } 114 | 115 | return std::make_tuple(Error::IndexNotFound, std::make_tuple(0, 0)); 116 | } 117 | 118 | // Helper to insert an array reference 119 | template 120 | CANFETTI_NO_INLINE Error insert(uint16_t idx, canfetti::Access access, std::array &v, ChangedCallback cb = nullptr) 121 | { 122 | if (lookup(idx, 0)) { 123 | LogInfo("Warning: index %x[%d] already exists in the OD", idx, 0); 124 | return Error::Error; 125 | } 126 | 127 | buildSubEntry(idx, 0, canfetti::Access::RO, _u8(N), cb); 128 | 129 | for (size_t i = 0; i < N; i++) { 130 | buildSubEntry(idx, i + 1, access, _p(v[i]), cb); 131 | } 132 | 133 | return Error::Success; 134 | } 135 | 136 | // Helper to insert an array reference 137 | template 138 | CANFETTI_NO_INLINE Error insert(uint16_t idx, canfetti::Access access, T (&v)[N], ChangedCallback cb = nullptr) 139 | { 140 | if (lookup(idx, 0)) { 141 | LogInfo("Warning: index %x[%d] already exists in the OD", idx, 0); 142 | return Error::Error; 143 | } 144 | 145 | buildSubEntry(idx, 0, canfetti::Access::RO, _u8(N), cb); 146 | 147 | for (size_t i = 0; i < N; i++) { 148 | buildSubEntry(idx, i + 1, access, _p(v[i]), cb); 149 | } 150 | 151 | return Error::Success; 152 | } 153 | 154 | inline bool entryExists(uint16_t idx, uint8_t subIdx) 155 | { 156 | return lookup(idx, subIdx) != nullptr; 157 | } 158 | 159 | inline size_t entrySize(uint16_t idx, uint8_t subIdx) 160 | { 161 | auto entry = lookup(idx, subIdx); 162 | return entry ? canfetti::size(entry->data) : 0; 163 | } 164 | 165 | protected: 166 | OdTable table; 167 | OdEntry *lookup(uint16_t idx, uint8_t subIdx); 168 | 169 | template 170 | constexpr void buildSubEntry(uint16_t idx, uint8_t subIdx, Args &&...args) 171 | { 172 | table[idx].emplace(std::piecewise_construct, std::forward_as_tuple(subIdx), std::forward_as_tuple(idx, subIdx, std::forward(args)...)); 173 | } 174 | }; 175 | 176 | } // namespace canfetti 177 | -------------------------------------------------------------------------------- /include/canfetti/OdData.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "Types.h" 8 | #include "canfetti/System.h" 9 | 10 | namespace canfetti { 11 | 12 | struct OdBuffer { 13 | uint8_t *buf; 14 | size_t len; 15 | OdBuffer() = default; 16 | OdBuffer(void *buf, size_t len) : buf(static_cast(buf)), len(len) {} 17 | OdBuffer(const OdBuffer &o) 18 | { 19 | buf = o.buf; 20 | len = o.len; 21 | } 22 | }; 23 | 24 | using OdDynamicVarCopyFrom = std::function; 25 | using OdDynamicVarCopyInto = std::function; 26 | using OdDynamicVarGetSize = std::function; 27 | using OdDynamicVarResize = std::function; 28 | using OdDynamicVarBeginAccess = std::function; 29 | using OdDynamicVarEndAccess = std::function; 30 | 31 | struct OdDynamicVar { 32 | OdDynamicVarCopyFrom copyFrom = nullptr; 33 | OdDynamicVarCopyInto copyInto = nullptr; 34 | OdDynamicVarGetSize size = nullptr; 35 | OdDynamicVarResize resize = nullptr; 36 | OdDynamicVarBeginAccess beginAccess = nullptr; 37 | OdDynamicVarEndAccess endAccess = nullptr; 38 | }; 39 | 40 | using OdVariant = std::variant, std::string, OdBuffer, OdDynamicVar>; 45 | 46 | enum class Access { 47 | RO, 48 | WO, 49 | RW 50 | }; 51 | 52 | using ChangedCallback = std::function; 53 | 54 | struct OdEntry { 55 | OdEntry(uint16_t i, uint8_t s, Access a, OdVariant d, ChangedCallback cb = nullptr); 56 | OdEntry(OdEntry &&o) = delete; 57 | OdEntry(const OdEntry &o) = delete; 58 | Access access; 59 | OdVariant data; 60 | 61 | bool lock(); 62 | void unlock(); 63 | bool isLocked(); 64 | void fireCallbacks(); 65 | void addCallback(ChangedCallback cb); 66 | inline unsigned generation() { return generation_; } 67 | inline void bumpGeneration() { generation_ = newGeneration(); } 68 | 69 | private: 70 | const uint16_t idx; 71 | const uint8_t subIdx; 72 | bool locked = false; 73 | unsigned generation_ = newGeneration(); 74 | std::vector callbacks; 75 | }; 76 | 77 | class OdProxy { 78 | public: 79 | OdProxy(); 80 | OdProxy(uint16_t, uint8_t, const OdVariant &); 81 | OdProxy(uint16_t, uint8_t, OdVariant &); 82 | OdProxy(uint16_t, uint8_t, OdEntry &); 83 | OdProxy(OdProxy &&); 84 | OdProxy(const OdProxy &) = delete; 85 | OdProxy &operator=(OdProxy &&) = delete; 86 | OdProxy &operator=(const OdProxy &) = delete; 87 | ~OdProxy(); 88 | 89 | bool resize(size_t newSize); 90 | Error copyInto(uint8_t *b, size_t s); // Copy from variant 91 | Error copyFrom(uint8_t *b, size_t s); // Write to variant 92 | Error copyFrom(const OdProxy &other); 93 | Error reset(); 94 | size_t remaining(); 95 | void suppressCallbacks(); 96 | 97 | const uint16_t idx; 98 | const uint8_t subIdx; 99 | 100 | private: 101 | bool readOnly = false; 102 | bool changed = false; 103 | bool callbacksSuppressed = false; 104 | OdVariant *v = nullptr; 105 | const OdVariant *roV = nullptr; 106 | OdEntry *e = nullptr; 107 | uint8_t *ptr = nullptr; 108 | size_t off = 0; 109 | size_t len = 0; 110 | OdDynamicVar *dVar = nullptr; 111 | }; 112 | 113 | template 114 | static inline constexpr auto _e(E x) 115 | { 116 | return static_cast>(x); 117 | } 118 | 119 | template 120 | static inline constexpr uint8_t _s8(T x) 121 | { 122 | return static_cast(x); 123 | } 124 | 125 | template 126 | static inline constexpr uint16_t _s16(T x) 127 | { 128 | return static_cast(x); 129 | } 130 | 131 | template 132 | static inline constexpr uint32_t _s32(T x) 133 | { 134 | return static_cast(x); 135 | } 136 | 137 | template 138 | static inline constexpr uint64_t _s64(T x) 139 | { 140 | return static_cast(x); 141 | } 142 | 143 | template 144 | static inline constexpr uint8_t _u8(T x) 145 | { 146 | return static_cast(x); 147 | } 148 | 149 | template 150 | static inline constexpr uint16_t _u16(T x) 151 | { 152 | return static_cast(x); 153 | } 154 | 155 | template 156 | static inline constexpr uint32_t _u32(T x) 157 | { 158 | return static_cast(x); 159 | } 160 | 161 | template 162 | static inline constexpr uint64_t _u64(T x) 163 | { 164 | return static_cast(x); 165 | } 166 | 167 | template 168 | static inline constexpr uint32_t _f32(T x) 169 | { 170 | return static_cast(x); 171 | } 172 | 173 | inline OdBuffer _p(std::string &val) 174 | { 175 | return OdBuffer{(uint8_t *)val.c_str(), val.size()}; 176 | } 177 | 178 | template 179 | static inline OdBuffer _p(T &val) 180 | { 181 | return OdBuffer{(uint8_t *)&val, sizeof(val)}; 182 | } 183 | 184 | size_t size(const OdVariant &v); 185 | 186 | } // namespace canfetti 187 | -------------------------------------------------------------------------------- /include/canfetti/Types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "canfetti/System.h" 8 | 9 | namespace canfetti { 10 | 11 | namespace StaticDataTypeIndex { 12 | constexpr std::tuple Integer8 = std::make_tuple(0x0002, 0); 13 | constexpr std::tuple Integer16 = std::make_tuple(0x0003, 0); 14 | constexpr std::tuple Integer32 = std::make_tuple(0x0004, 0); 15 | constexpr std::tuple Unsigned8 = std::make_tuple(0x0005, 0); 16 | constexpr std::tuple Unsigned16 = std::make_tuple(0x0006, 0); 17 | constexpr std::tuple Unsigned32 = std::make_tuple(0x0007, 0); 18 | constexpr std::tuple Real32 = std::make_tuple(0x0008, 0); 19 | } // namespace StaticDataTypeIndex 20 | 21 | enum State { 22 | Bootup = 0x00, 23 | Stopped = 0x04, 24 | Operational = 0x05, 25 | PreOperational = 0x7f, 26 | Offline = 0xff, // Not part of spec. Used only for uninitialized internal state 27 | }; 28 | 29 | enum SlaveState { 30 | GoOperational = 0x01, 31 | Stop = 0x02, 32 | GoPreOperational = 0x80, 33 | ResetNode = 0x81, 34 | ResetComms = 0x82, 35 | }; 36 | 37 | enum class Error { 38 | // Custom Errors 39 | Success = 0, 40 | InternalError = 1, 41 | 42 | // CANopen Errors from spec 43 | NotToggled = 0x05030000, // Toggle bit not alternated. 44 | Timeout = 0x05040000, // SDO protocol timed out. 45 | InvalidCmd = 0x05040001, // Client/server command specifier not valid or unknown. 46 | InvalidBlkSize = 0x05040002, // Invalid block size (block mode only). 47 | InvalidSeqNum = 0x05040003, // Invalid sequence number (block mode only). 48 | CrcError = 0x05040004, // CRC error (block mode only). 49 | OutOfMemory = 0x05040005, // Out of memory 50 | UnsupportedAccess = 0x06010000, // Unsupported access to an object. 51 | ReadViolation = 0x06010001, // Attempt to read a write only object. 52 | WriteViolation = 0x06010002, // Attempt to write a read only object 53 | IndexNotFound = 0x06020000, // Object does not exist in the object dictionary. 54 | ObjMappingError = 0x06040041, // Object cannot be mapped to the PDO. 55 | PdoSizeViolation = 0x06040042, // The number and length of the objects to e mapped would exceed PDO length. 56 | ParamIncompatibility = 0x06040043, // General parameter incompatibility reason. 57 | DeviceIncompatibility = 0x06040047, // General internal incompatibility in the device. 58 | HwError = 0x06060000, // Access failed due to a hardware error. 59 | ParamLength = 0x06070010, // Data type does not match; length of service parameter does not match. 60 | ParamLengthHigh = 0x06070012, // Data type does not match; length of service parameter too high. 61 | ParamLengthLow = 0x06070013, // Data type does not match; length of service parameter too low. 62 | InvalidSubIndex = 0x06090011, // Sub-index does not exist. 63 | ValueRange = 0x06090030, // Value range of parameter exceeded (only for write access). 64 | ValueRangeHigh = 0x06090031, // Value of parameter written too high. 65 | ValueRangeLow = 0x06090032, // Value of parameter written too low. 66 | MinMaxKerfuffle = 0x06090036, // Maximum value is less than minimum value. 67 | Error = 0x08000000, // General error. 68 | DataXfer = 0x08000020, // Data cannot be transferred or stored to the application. 69 | DataXferLocal = 0x08000021, // Data cannot be transferred or stored to the application because of local control. 70 | DataXferState = 0x08000022, // Data cannot be transferred or stored to the application because of the present device state. 71 | OdGenFail = 0x08000023, // Object dictionary dynamic generation fails or no object dictionary is present (e.g. object dictionary is generated from file and generation fails because of a file error). 72 | }; 73 | 74 | static constexpr const char* trimSlash(const char* const str, const char* const last_slash) 75 | { 76 | return *str == '\0' ? last_slash : *str == '/' ? trimSlash(str + 1, str + 1) 77 | : trimSlash(str + 1, last_slash); 78 | } 79 | 80 | static constexpr const char* trimSlash(const char* const str) 81 | { 82 | return trimSlash(str, str); 83 | } 84 | 85 | template 86 | struct LogInfo { 87 | LogInfo(const char* fmt, Args&&... args, const std::experimental::source_location& loc = std::experimental::source_location::current()) 88 | { 89 | const char* filename = trimSlash(loc.file_name()); 90 | std::string s(fmt); 91 | if (Logger::needNewline) { 92 | s += "\n"; 93 | } 94 | if constexpr (sizeof...(args) == 0) { 95 | Logger::logger.emitLogMessage(filename, loc.function_name(), loc.line(), "%s", s.c_str()); 96 | } 97 | else { 98 | Logger::logger.emitLogMessage(filename, loc.function_name(), loc.line(), s.c_str(), args...); 99 | } 100 | } 101 | }; 102 | 103 | template 104 | LogInfo(const char* f, Ts&&...) -> LogInfo; 105 | 106 | template 107 | struct LogDebug { 108 | LogDebug(const char* fmt, Args&&... args, const std::experimental::source_location& loc = std::experimental::source_location::current()) 109 | { 110 | if (Logger::logger.debug) { 111 | const char* filename = trimSlash(loc.file_name()); 112 | std::string s(fmt); 113 | if (Logger::needNewline) { 114 | s += "\n"; 115 | } 116 | if constexpr (sizeof...(args) == 0) { 117 | Logger::logger.emitLogMessage(filename, loc.function_name(), loc.line(), "%s", s.c_str()); 118 | } 119 | else { 120 | Logger::logger.emitLogMessage(filename, loc.function_name(), loc.line(), s.c_str(), args...); 121 | } 122 | } 123 | } 124 | }; 125 | 126 | template 127 | LogDebug(const char* f, Ts&&...) -> LogDebug; 128 | 129 | struct Msg { 130 | uint32_t id; 131 | bool rtr; 132 | uint8_t len; 133 | uint8_t* data; 134 | 135 | inline uint16_t getFunction() const { return id & (0xF << 7); } 136 | inline uint8_t getNode() const { return id & ((1 << 7) - 1); } 137 | }; 138 | 139 | } // namespace canfetti 140 | -------------------------------------------------------------------------------- /include/canfetti/services/Emcy.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "Service.h" 5 | 6 | namespace canfetti { 7 | 8 | class EmcyService : public canfetti::Service { 9 | public: 10 | enum ErrorType : uint8_t { 11 | Generic = 1 << 0, 12 | Current = 1 << 1, 13 | Voltage = 1 << 2, 14 | Temperature = 1 << 3, 15 | Communication = 1 << 4, 16 | ProfileSpecific = 1 << 5, 17 | Reserved = 1 << 6, 18 | ManufacturerSpecific = 1 << 7, 19 | }; 20 | 21 | using EmcyCallback = std::function &specific)>; 22 | EmcyService(Node &co); 23 | 24 | canfetti::Error processMsg(const canfetti::Msg &msg); 25 | canfetti::Error sendEmcy(uint16_t error, uint32_t specific, ErrorType type); 26 | canfetti::Error sendEmcy(uint16_t error, std::array &specific, ErrorType type); 27 | canfetti::Error clearEmcy(uint16_t error, ErrorType type); 28 | canfetti::Error registerCallback(EmcyCallback cb); 29 | 30 | private: 31 | EmcyCallback cb = nullptr; 32 | std::unordered_map errorHistory; 33 | uint8_t setErrorReg(ErrorType type); 34 | uint8_t clearErrorReg(ErrorType type); 35 | }; 36 | 37 | } // namespace canfetti 38 | -------------------------------------------------------------------------------- /include/canfetti/services/Nmt.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include "Service.h" 6 | 7 | namespace canfetti { 8 | 9 | class NmtService : public canfetti::Service { 10 | public: 11 | using RemoteStateCb = std::function; 12 | static constexpr uint8_t AllNodes = 0xFF; 13 | 14 | NmtService(Node &co); 15 | 16 | canfetti::Error setHeartbeatPeriod(uint16_t periodMs); 17 | canfetti::Error addRemoteStateCb(uint8_t node, RemoteStateCb cb); 18 | canfetti::Error setRemoteTimeout(uint8_t node, uint16_t timeoutMs); 19 | canfetti::Error sendHeartbeat(); 20 | canfetti::Error processMsg(const canfetti::Msg &msg); 21 | canfetti::Error processHeartbeat(const canfetti::Msg &msg); 22 | canfetti::Error setRemoteState(uint8_t node, canfetti::SlaveState state); 23 | std::tuple getRemoteState(uint8_t node); 24 | 25 | private: 26 | struct NodeState { 27 | canfetti::State state; 28 | System::TimerHdl timer; 29 | uint16_t timeoutMs; 30 | unsigned generation; 31 | }; 32 | 33 | struct NodeStateCb { 34 | uint8_t node; 35 | RemoteStateCb cb; 36 | }; 37 | 38 | System::TimerHdl hbTimer = System::InvalidTimer; 39 | std::array slaveStateCbs; 40 | std::unordered_map peerStates; 41 | 42 | void peerHeartbeatExpired(unsigned generation, uint8_t node); 43 | void resetNode(); 44 | void resetComms(); 45 | void notifyRemoteStateCbs(uint8_t node, canfetti::State state); 46 | }; 47 | 48 | } // namespace canfetti 49 | -------------------------------------------------------------------------------- /include/canfetti/services/Pdo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "Service.h" 5 | 6 | namespace canfetti { 7 | 8 | class PdoService : public Service { 9 | public: 10 | using TimeoutCb = std::function; 11 | PdoService(Node &co); 12 | 13 | Error processMsg(const Msg &msg); 14 | Error enablePdoEvents(); 15 | Error disablePdoEvents(); 16 | Error sendTxPdo(uint16_t paramIdx, bool async = false, bool rtr = false); 17 | Error disablePdo(uint16_t paramIdx); 18 | Error enablePdo(uint16_t paramIdx); 19 | Error requestTxPdo(uint16_t cobid); 20 | Error sendAllTpdos(); 21 | Error addRPDO(uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t timeoutMs, PdoService::TimeoutCb cb); 22 | Error addTPDO(uint16_t pdoNum, uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t periodMs, bool enabled); 23 | Error updateTpdoEventTime(uint16_t paramIdx, uint16_t periodMs); 24 | 25 | private: 26 | bool pdoEnabled = false; 27 | std::vector configuredTPDONums; 28 | std::unordered_map tpdoTimers; 29 | std::unordered_map> rpdoTimers; 30 | void enableTpdoEvent(uint16_t idx); 31 | void enableRpdoEvent(uint16_t idx); 32 | void rpdoTimeout(unsigned generation, uint16_t idx); 33 | Error addPdoEntry(uint16_t paramIdx, uint32_t cobid, uint16_t eventTime, 34 | const std::tuple *mapping, size_t numMapping, bool enabled, bool rtrAllowed, canfetti::ChangedCallback changedCb); 35 | }; 36 | 37 | } // namespace canfetti 38 | -------------------------------------------------------------------------------- /include/canfetti/services/Sdo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "Service.h" 5 | #include "sdo/Client.h" 6 | #include "sdo/Server.h" 7 | 8 | namespace canfetti { 9 | 10 | class SdoService : public Service { 11 | public: 12 | static constexpr uint32_t DefaultSegmentXferTimeoutMs = 50; 13 | using FinishCallback = std::function; 14 | 15 | SdoService(Node &co); 16 | Error init(); 17 | Error processMsg(const Msg &msg); 18 | Error clientTransaction(bool read, uint8_t node, uint16_t idx, uint8_t subIdx, 19 | OdVariant &data, uint32_t segmentTimeout, FinishCallback cb); 20 | Error addSDOServer(uint16_t rxCobid, uint16_t txCobid, uint8_t clientId); 21 | Error addSDOClient(uint32_t txCobid, uint16_t rxCobid, uint8_t serverId); 22 | size_t getActiveTransactionCount(); 23 | inline void setServerSegmentTimeout(uint32_t timeoutMs) { serverSegmentTimeoutMs = timeoutMs; } 24 | 25 | private: 26 | struct TransactionState { 27 | std::shared_ptr protocol; 28 | System::TimerHdl timer; 29 | unsigned generation; 30 | FinishCallback cb; 31 | }; 32 | 33 | void transactionTimeout(unsigned generation, uint16_t key); 34 | void removeTransaction(uint16_t key); 35 | Error addSdoEntry(uint16_t paramIdx, uint16_t clientToServer, uint16_t serverToClient, uint8_t node); 36 | Error syncServices(); 37 | std::unordered_map activeTransactions; 38 | std::unordered_map> servers; 39 | uint32_t serverSegmentTimeoutMs; 40 | }; 41 | 42 | } // namespace canfetti 43 | -------------------------------------------------------------------------------- /include/canfetti/services/Service.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "canfetti/Node.h" 3 | 4 | namespace canfetti { 5 | class Service { 6 | public: 7 | Service(Node &co) : co(co) {} 8 | virtual Error init() { return Error::Success; } 9 | virtual canfetti::Error processMsg(const canfetti::Msg &msg) = 0; 10 | 11 | protected: 12 | Node &co; 13 | }; 14 | } // namespace canfetti 15 | -------------------------------------------------------------------------------- /include/canfetti/services/sdo/Client.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "Protocol.h" 5 | 6 | namespace canfetti::Sdo { 7 | 8 | class Client : public Protocol { 9 | public: 10 | using Protocol::Protocol; 11 | 12 | static std::tuple> initiateRead(uint16_t idx, uint8_t subIdx, 13 | OdVariant &data, uint16_t txCobid, Node &co); 14 | static std::tuple> initiateWrite(uint16_t idx, uint8_t subIdx, 15 | OdVariant &data, uint16_t txCobid, Node &co); 16 | 17 | bool processMsg(const canfetti::Msg &msg); 18 | 19 | private: 20 | uint8_t lastBlockBytes; 21 | canfetti::Error checkSize(uint32_t msgLen, bool tooBigCheck); 22 | void segmentWrite(); 23 | void segmentRead(); 24 | void blockSegmentWrite(uint8_t seqno); 25 | }; 26 | } // namespace canfetti::Sdo 27 | -------------------------------------------------------------------------------- /include/canfetti/services/sdo/Protocol.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "canfetti/CanDevice.h" 4 | #include "canfetti/Node.h" 5 | #include "canfetti/ObjDict.h" 6 | 7 | namespace canfetti::Sdo { 8 | 9 | class Protocol { 10 | public: 11 | static const size_t BlockModeThreshold = 100; 12 | 13 | Protocol(uint16_t txCobid, canfetti::OdProxy proxy, Node &co); 14 | virtual ~Protocol(); 15 | 16 | virtual bool processMsg(const canfetti::Msg &msg) = 0; 17 | virtual void finish(canfetti::Error status, bool sendAbort = true); 18 | std::tuple isFinished(); 19 | 20 | protected: 21 | static inline bool isAbortMsg(const Msg &m) { return (m.data[0] & (0b111 << 5)) == (4 << 5); } 22 | static uint32_t getInitiateDataLen(const canfetti::Msg &m); 23 | static void abort(canfetti::Error status, uint16_t txCobid, uint16_t idx, uint8_t subIdx, CanDevice &bus); 24 | 25 | bool abortCheck(const Msg &msg); 26 | 27 | bool finished = false; 28 | canfetti::Error finishedStatus = canfetti::Error::Success; 29 | uint16_t txCobid; 30 | bool toggle = false; 31 | canfetti::OdProxy proxy; 32 | Node &co; 33 | }; 34 | 35 | } // namespace canfetti::Sdo 36 | -------------------------------------------------------------------------------- /include/canfetti/services/sdo/Server.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "Protocol.h" 4 | 5 | namespace canfetti::Sdo { 6 | 7 | class Server : public Protocol { 8 | public: 9 | using Protocol::Protocol; 10 | 11 | static std::shared_ptr processInitiate(const Msg &msg, uint16_t txCobid, Node &co); 12 | virtual bool processMsg(const canfetti::Msg &msg); 13 | canfetti::Error initiateRead(); 14 | canfetti::Error initiateWrite(); 15 | 16 | private: 17 | bool blockMode = false; 18 | uint8_t receivedFistSegment = false; 19 | uint8_t lastSegmentData[7]; 20 | void segmentWrite(); 21 | bool segmentRead(); 22 | static bool sendUploadInitRsp(uint16_t txCobid, uint16_t idx, uint8_t subIdx, OdProxy &proxy, CanDevice &bus); 23 | }; 24 | } // namespace canfetti::Sdo -------------------------------------------------------------------------------- /include/canfetti/services/sdo/ServerBlockMode.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "Server.h" 3 | 4 | namespace canfetti::Sdo { 5 | 6 | class ServerBlockMode : public Server { 7 | public: 8 | static inline bool isDownloadBlockMsg(const canfetti::Msg &m) { return (m.data[0] >> 5) == 6; } 9 | 10 | ServerBlockMode(uint16_t txCobid, 11 | canfetti::OdProxy proxy, 12 | Node &co, 13 | uint32_t totalsize); 14 | 15 | bool processMsg(const canfetti::Msg &msg); 16 | void sendInitiateResponse(); 17 | 18 | private: 19 | const uint8_t NumSubBlockSegments = 127; 20 | 21 | enum State { 22 | Start, 23 | SubBlock, 24 | End, 25 | }; 26 | 27 | uint32_t totalsize; 28 | State state = State::Start; 29 | uint8_t lastSegmentData[7]; 30 | uint8_t expectedSeqNo; 31 | }; 32 | } // namespace canfetti::Sdo -------------------------------------------------------------------------------- /include/platform/linux/canfetti/LinuxCo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "canfetti/LocalNode.h" 12 | #include "canfetti/System.h" 13 | #include "linux/can.h" 14 | 15 | namespace canfetti { 16 | 17 | class LinuxCoDev : public canfetti::CanDevice { 18 | public: 19 | LinuxCoDev(uint32_t baudrate); 20 | canfetti::Error open(const char* device); 21 | canfetti::Error write(const canfetti::Msg& msg, bool async = false) override; 22 | canfetti::Error read(struct can_frame& frame, bool nonblock); 23 | void flushAsyncFrames(); 24 | 25 | private: 26 | int s; 27 | // Accessed by write() and flushAsyncFrames() 28 | std::vector asyncFrames; 29 | }; 30 | 31 | class LinuxCo : public canfetti::LocalNode { 32 | public: 33 | LinuxCo(LinuxCoDev& d, uint8_t nodeId, const char* deviceName, uint32_t deviceType = 0); 34 | ~LinuxCo(); 35 | Error start(const char* dev); 36 | size_t getTimerCount() { return sys.getTimerCount(); } 37 | 38 | // All external callers must go through this to access node state, OD data, etc. f() must not block. 39 | template 40 | void doWithLock(F f) 41 | { 42 | std::lock_guard g(mtx); 43 | unsigned gen = sys.getTimerGeneration(); 44 | bool noPendingTpdos = pendingTpdos.empty(); 45 | f(); 46 | if (gen != sys.getTimerGeneration() || (noPendingTpdos && !pendingTpdos.empty())) { 47 | mainThreadWakeup.notify_one(); 48 | } 49 | } 50 | 51 | template 52 | Error blockingRead(uint8_t node, uint16_t idx, uint8_t subIdx, T &data, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 53 | { 54 | return blockingTransaction(true, node, idx, subIdx, data, segmentTimeout); 55 | } 56 | 57 | template 58 | Error blockingWrite(uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 59 | { 60 | return blockingTransaction(false, node, idx, subIdx, std::forward(data), segmentTimeout); 61 | } 62 | 63 | template 64 | Error blockingTransaction(bool read, uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, uint32_t segmentTimeout) 65 | { 66 | std::mutex mtx; 67 | std::condition_variable cv; 68 | bool done = false; 69 | Error result = Error::Error; 70 | OdVariant val(data); 71 | 72 | Error initErr; 73 | doWithLock([&]() { 74 | initErr = sdo.clientTransaction(read, node, idx, subIdx, val, segmentTimeout, [&](Error e) { 75 | std::lock_guard g(mtx); 76 | done = true; 77 | result = e; 78 | cv.notify_one(); 79 | }); 80 | }); 81 | if (initErr != Error::Success) return initErr; 82 | 83 | { 84 | std::unique_lock u(mtx); 85 | cv.wait(u, [&]() { return done; }); 86 | } 87 | if constexpr (!std::is_same_v, OdBuffer>) { 88 | data = std::move(*std::get_if>(&val)); 89 | } 90 | return result; 91 | } 92 | 93 | // Request async TPDO send. Requests are coalesced so that only one send per 94 | // TPDO happens per main loop iteration. This prevents an external caller 95 | // running faster than the main loop from enqueueing unbounded sends. 96 | Error triggerTPDOOnce(uint16_t pdoNum); 97 | 98 | private: 99 | // These are separate threads due to the difficulty of combining socket IO with timers 100 | void runMainThread(); 101 | void runRecvThread(); 102 | 103 | std::recursive_mutex mtx; 104 | // when pendingFrames becomes non-empty / timers have changed / async TPDOs are requested 105 | std::condition_variable_any mainThreadWakeup; 106 | // when pendingFrames becomes empty 107 | std::condition_variable_any recvThreadWakeup; 108 | System sys; 109 | std::vector pendingFrames; 110 | std::unique_ptr mainThread; 111 | std::unique_ptr recvThread; 112 | std::atomic shutdown{false}; 113 | std::unordered_set pendingTpdos; 114 | }; 115 | 116 | } // namespace canfetti 117 | -------------------------------------------------------------------------------- /include/platform/linux/canfetti/System.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // Always enable assertions. 12 | // FIXME: remove once all error handling paths are fleshed out 13 | #ifdef NDEBUG 14 | #undef NDEBUG 15 | #endif 16 | 17 | #define CANFETTI_NO_INLINE 18 | 19 | namespace canfetti { 20 | 21 | unsigned newGeneration(); 22 | 23 | class Logger { 24 | public: 25 | static constexpr bool needNewline = false; 26 | static Logger logger; 27 | 28 | using LogCallback = void (*)(const char*); 29 | void setLogCallback(LogCallback); 30 | 31 | template 32 | void emitLogMessage(const char* file, const char* func, int line, const char* fmt, Args... args) 33 | { 34 | (void)file; 35 | (void)func; 36 | (void)line; 37 | if (LogCallback cb = logCallback.load(std::memory_order_relaxed)) { 38 | static thread_local char msgbuf[4096]; 39 | snprintf(msgbuf, sizeof msgbuf, fmt, args...); 40 | msgbuf[sizeof msgbuf - 1] = 0; // sure why not 41 | cb(msgbuf); 42 | } 43 | } 44 | 45 | std::atomic logCallback; 46 | std::atomic_bool debug = false; 47 | }; 48 | 49 | class System { 50 | private: 51 | struct Timer { 52 | bool available = true; 53 | bool enable = false; 54 | bool repeat = false; 55 | std::chrono::steady_clock::duration interval; 56 | std::chrono::steady_clock::time_point deadline; 57 | std::function callback; 58 | }; 59 | 60 | public: 61 | using TimerHdl = std::shared_ptr; 62 | static const TimerHdl InvalidTimer; 63 | 64 | TimerHdl resetTimer(TimerHdl& hdl); 65 | void deleteTimer(TimerHdl& hdl); 66 | void disableTimer(TimerHdl& hdl); 67 | TimerHdl scheduleDelayed(uint32_t delayMs, std::function cb); 68 | TimerHdl schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart = true); 69 | 70 | // Return a time point no later than the earliest timer deadline. If there 71 | // are no timers, return an arbitrarily distant future time point. 72 | std::chrono::steady_clock::time_point nextTimerDeadline(); 73 | // Invoke callbacks of any timers that have hit their deadline 74 | void serviceTimers(); 75 | 76 | size_t getTimerCount(); 77 | // For LinuxCo::doWithLock() to detect that timers have changed and main 78 | // thread should be woken 79 | unsigned getTimerGeneration() { return generation; } 80 | 81 | private: 82 | // vector instead of [unordered_]set to allow mutation during iteration in 83 | // service(). Size only increases. 84 | std::vector timers; 85 | TimerHdl getAvailableTimer(); 86 | std::mt19937 prng; // use default seed 87 | unsigned generation = newGeneration(); 88 | }; 89 | 90 | } // namespace canfetti 91 | -------------------------------------------------------------------------------- /include/platform/odrive/canfetti/ODriveCo.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | #include 4 | #include 5 | #include "canfetti/LocalNode.h" 6 | #include "canfetti/System.h" 7 | #include "fibre/callback.hpp" 8 | #include "interfaces/canbus.hpp" 9 | 10 | namespace canfetti { 11 | 12 | class ODriveCo : public canfetti::CanDevice, public canfetti::LocalNode { 13 | public: 14 | static constexpr uint32_t DefaultNodeId = 0x7f; 15 | 16 | struct Config_t { 17 | uint32_t id = DefaultNodeId; 18 | }; 19 | 20 | ODriveCo(CanBusBase* canbus); 21 | canfetti::Error write(const canfetti::Msg& msg, bool async = false) override; 22 | canfetti::Error writePriority(const canfetti::Msg& msg) override; 23 | void handle_can_message(const can_Message_t& msg); 24 | void init(fibre::Callback, float, fibre::Callback> timer, fibre::Callback&> timerCancel, uint32_t numPrioTxSlots); 25 | void initObjDict(); 26 | void configHwFilters(); 27 | 28 | Config_t config_; 29 | 30 | private: 31 | System sys; 32 | CanBusBase::CanSubscription* canbusSubscription[5]; 33 | CanBusBase* canbus; 34 | uint32_t txPrioSlot; 35 | uint32_t numTxPrioSlots; 36 | }; 37 | 38 | } // namespace canfetti 39 | -------------------------------------------------------------------------------- /include/platform/odrive/canfetti/System.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "fibre/callback.hpp" 11 | 12 | #ifndef assert 13 | #include 14 | #endif 15 | 16 | #define CANFETTI_NO_INLINE __attribute__((noinline)) 17 | 18 | namespace canfetti { 19 | 20 | unsigned newGeneration(); 21 | 22 | class Logger { 23 | public: 24 | static constexpr bool needNewline = true; 25 | static Logger logger; 26 | bool debug = false; 27 | 28 | template 29 | inline void emitLogMessage(const char* file, const char* func, int line, const char* fmt, Args... args) 30 | { 31 | printf(fmt, args...); 32 | } 33 | }; 34 | 35 | class ODriveCo; 36 | 37 | class System { 38 | struct TimerData { 39 | std::optional handle; 40 | uint32_t periodMs; 41 | std::function cb; 42 | bool available = true; 43 | System* parent; 44 | void trigger() 45 | { 46 | parent->timerHelper(this); 47 | } 48 | }; 49 | std::array timers; 50 | void timerHelper(TimerData* td); 51 | 52 | public: 53 | using TimerHdl = TimerData*; 54 | static constexpr TimerHdl InvalidTimer = nullptr; 55 | 56 | bool init(fibre::Callback, float, fibre::Callback> timer, fibre::Callback&> timerCancel); 57 | 58 | void deleteTimer(TimerHdl& hdl); 59 | TimerHdl scheduleDelayed(uint32_t delayMs, std::function cb); 60 | TimerHdl schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart = true); 61 | 62 | private: 63 | fibre::Callback, float, fibre::Callback> timer; 64 | fibre::Callback&> timerCancel; 65 | }; 66 | 67 | // Global logger 68 | extern Logger logger; 69 | 70 | } // namespace canfetti 71 | -------------------------------------------------------------------------------- /include/platform/teensy/canfetti/System.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "Arduino.h" 10 | 11 | //****************************************************************************** 12 | // Teensy specific implementation 13 | //****************************************************************************** 14 | #define CANFETTI_NO_INLINE __attribute__((noinline)) 15 | 16 | namespace canfetti { 17 | 18 | unsigned newGeneration(); 19 | 20 | class Logger { 21 | public: 22 | static constexpr bool needNewline = true; 23 | static Logger logger; 24 | bool debug = false; 25 | 26 | template 27 | inline void emitLogMessage(const char* file, const char* func, int line, const char* fmt, Args... args) 28 | { 29 | Serial.printf(fmt, args...); 30 | } 31 | }; 32 | 33 | class System { 34 | struct TimerData { 35 | uint32_t lastFireTime; 36 | uint32_t delay; 37 | uint32_t period; 38 | std::function cb; 39 | bool enable = false; 40 | bool available = true; 41 | }; 42 | std::vector timers; 43 | 44 | public: 45 | using TimerHdl = size_t; 46 | static constexpr TimerHdl InvalidTimer = SIZE_MAX; 47 | 48 | void deleteTimer(TimerHdl& hdl); 49 | TimerHdl scheduleDelayed(uint32_t delayMs, std::function cb); 50 | TimerHdl schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart = true); 51 | 52 | void service(); 53 | }; 54 | 55 | } // namespace canfetti 56 | -------------------------------------------------------------------------------- /include/platform/teensy/canfetti/TyCo.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "FlexCAN_T4/FlexCAN_T4.h" 5 | #include "canfetti/LocalNode.h" 6 | #include "canfetti/System.h" 7 | 8 | namespace canfetti { 9 | 10 | class TyCoDev : public canfetti::CanDevice { 11 | public: 12 | canfetti::Error init(uint32_t baudrate, FlexCAN_T4 *busInstance); 13 | canfetti::Error write(const Msg &msg, bool async = false) override; 14 | canfetti::Error writePriority(const Msg &msg) override; 15 | 16 | inline bool read(CAN_message_t &m) { return bus->readMB(m); } 17 | inline size_t newDroppedRx() { return bus->readAndClearDroppedRx(); } 18 | 19 | private: 20 | FlexCAN_T4 *bus; 21 | }; 22 | 23 | class TyCo : public canfetti::LocalNode { 24 | public: 25 | TyCo(TyCoDev &d, uint8_t nodeId, const char *deviceName, uint32_t deviceType = 0); 26 | void service(size_t maxRxBatch = 1); 27 | 28 | template 29 | Error blockingRead(uint8_t node, uint16_t idx, uint8_t subIdx, T &data, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 30 | { 31 | return blockingTransaction(true, node, idx, subIdx, data, segmentTimeout); 32 | } 33 | 34 | template 35 | Error blockingWrite(uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, uint32_t segmentTimeout = SdoService::DefaultSegmentXferTimeoutMs) 36 | { 37 | return blockingTransaction(false, node, idx, subIdx, std::forward(data), segmentTimeout); 38 | } 39 | 40 | template 41 | Error blockingTransaction(bool read, uint8_t node, uint16_t idx, uint8_t subIdx, T &&data, uint32_t segmentTimeout) 42 | { 43 | bool done = false; 44 | Error result = Error::Error; 45 | OdVariant val(data); 46 | 47 | Error initErr = sdo.clientTransaction(read, node, idx, subIdx, val, segmentTimeout, [&](Error e) { 48 | done = true; 49 | result = e; 50 | }); 51 | if (initErr != Error::Success) return initErr; 52 | 53 | while (!done) { 54 | service(); 55 | } 56 | 57 | if constexpr (!std::is_same_v, OdBuffer>) { 58 | data = std::move(*std::get_if>(&val)); 59 | } 60 | return result; 61 | } 62 | 63 | private: 64 | TyCoDev &dev; 65 | System sys; 66 | }; 67 | 68 | } // namespace canfetti 69 | -------------------------------------------------------------------------------- /include/platform/unittest/canfetti/System.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define CANFETTI_NO_INLINE 12 | 13 | namespace canfetti { 14 | 15 | unsigned newGeneration(); 16 | 17 | class Logger { 18 | public: 19 | static constexpr bool needNewline = false; 20 | static Logger logger; 21 | bool debug = false; 22 | template 23 | void emitLogMessage(const char* file, const char* func, int line, const char* fmt, Args... args) 24 | { 25 | if (debug) printf(fmt, args...); 26 | } 27 | }; 28 | 29 | class System { 30 | public: 31 | using TimerHdl = int; 32 | static constexpr TimerHdl InvalidTimer = -1; 33 | 34 | virtual TimerHdl resetTimer(TimerHdl& hdl) = 0; 35 | virtual void deleteTimer(TimerHdl& hdl) = 0; 36 | virtual void disableTimer(TimerHdl& hdl) = 0; 37 | virtual TimerHdl scheduleDelayed(uint32_t delayMs, std::function cb) = 0; 38 | virtual TimerHdl schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart = true) = 0; 39 | }; 40 | 41 | } // namespace canfetti 42 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | canfetti 4 | 1.0.0 5 | CANopen stack by Scythe 6 | Scythe Robotics 7 | MIT 8 | catkin 9 | 10 | -------------------------------------------------------------------------------- /src/CanDevice.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/CanDevice.h" 2 | 3 | using namespace canfetti; 4 | 5 | CanDevice::CanDevice() 6 | { 7 | } 8 | -------------------------------------------------------------------------------- /src/LocalNode.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/LocalNode.h" 2 | 3 | using namespace canfetti; 4 | 5 | LocalNode::LocalNode(CanDevice &d, System &sys, uint8_t nodeId, const char *deviceName, uint32_t deviceType) 6 | : Node(d, sys, nodeId), nmt(*this), pdo(*this), sdo(*this), emcy(*this), deviceName(deviceName), deviceType(deviceType) 7 | { 8 | } 9 | 10 | Error LocalNode::init() 11 | { 12 | using namespace canfetti::StaticDataTypeIndex; 13 | 14 | if (Error e = nmt.init(); e != Error::Success) { 15 | return e; 16 | } 17 | if (Error e = pdo.init(); e != Error::Success) { 18 | return e; 19 | } 20 | if (Error e = sdo.init(); e != Error::Success) { 21 | return e; 22 | } 23 | if (Error e = emcy.init(); e != Error::Success) { 24 | return e; 25 | } 26 | 27 | // 28 | // Create default entries 29 | // 30 | 31 | // Supported Static Data Types 32 | if (Error e = od.insert(std::get<0>(Integer8), std::get<1>(Integer8), canfetti::Access::RW, _s8(0)); e != Error::Success) { 33 | return e; 34 | } 35 | 36 | if (Error e = od.insert(std::get<0>(Integer16), std::get<1>(Integer16), canfetti::Access::RW, _s16(0)); e != Error::Success) { 37 | return e; 38 | } 39 | 40 | if (Error e = od.insert(std::get<0>(Integer32), std::get<1>(Integer32), canfetti::Access::RW, _s32(0)); e != Error::Success) { 41 | return e; 42 | } 43 | 44 | if (Error e = od.insert(std::get<0>(Unsigned8), std::get<1>(Unsigned8), canfetti::Access::RW, _u8(0)); e != Error::Success) { 45 | return e; 46 | } 47 | 48 | if (Error e = od.insert(std::get<0>(Unsigned16), std::get<1>(Unsigned16), canfetti::Access::RW, _u16(0)); e != Error::Success) { 49 | return e; 50 | } 51 | 52 | if (Error e = od.insert(std::get<0>(Unsigned32), std::get<1>(Unsigned32), canfetti::Access::RW, _u32(0)); e != Error::Success) { 53 | return e; 54 | } 55 | 56 | if (Error e = od.insert(std::get<0>(Real32), std::get<1>(Real32), canfetti::Access::RW, _f32(0)); e != Error::Success) { 57 | return e; 58 | } 59 | 60 | // Device type 61 | if (Error e = od.insert(0x1000, 0, canfetti::Access::RO, deviceType); e != Error::Success) { 62 | return e; 63 | } 64 | 65 | // canfetti::Error register 66 | if (Error e = od.insert(0x1001, 0, canfetti::Access::RW, _u32(0)); e != Error::Success) { 67 | return e; 68 | } 69 | 70 | // Device name 71 | if (Error e = od.insert(0x1008, 0, canfetti::Access::RO, deviceName); e != Error::Success) { 72 | return e; 73 | } 74 | 75 | return Error::Success; 76 | } 77 | 78 | void LocalNode::processFrame(const Msg &msg) 79 | { 80 | switch (msg.getFunction()) { 81 | case 0x000: 82 | nmt.processMsg(msg); 83 | break; 84 | 85 | case 0x080: 86 | if (msg.id == 0x080) { 87 | // Sync isn't used. 88 | } 89 | else { 90 | emcy.processMsg(msg); 91 | } 92 | break; 93 | 94 | case 0x100: 95 | // Timestamp 96 | break; 97 | 98 | case 0x180: 99 | case 0x200: 100 | case 0x280: 101 | case 0x300: 102 | case 0x380: 103 | case 0x400: 104 | case 0x480: 105 | case 0x500: 106 | pdo.processMsg(msg); 107 | break; 108 | 109 | case 0x580: 110 | case 0x600: 111 | sdo.processMsg(msg); 112 | break; 113 | 114 | case 0x700: 115 | nmt.processHeartbeat(msg); 116 | break; 117 | 118 | case 0x7E4: 119 | case 0x7E5: 120 | // LSS 121 | break; 122 | 123 | default: 124 | LogInfo("Unknown cobid received: %x", msg.id); 125 | break; 126 | } 127 | } 128 | 129 | Error LocalNode::setState(State s) 130 | { 131 | if (s == state) return Error::Success; 132 | 133 | switch (s) { 134 | case State::Operational: 135 | LogInfo("State: Operational"); 136 | pdo.enablePdoEvents(); 137 | break; 138 | 139 | case State::Bootup: 140 | LogInfo("State: Bootup"); 141 | pdo.disablePdoEvents(); 142 | break; 143 | 144 | case State::Stopped: 145 | LogInfo("State: Stopped"); 146 | pdo.disablePdoEvents(); 147 | break; 148 | 149 | case State::PreOperational: 150 | LogInfo("State: PreOp"); 151 | pdo.disablePdoEvents(); 152 | break; 153 | 154 | default: 155 | break; 156 | } 157 | 158 | state = s; 159 | return Error::Success; 160 | } 161 | -------------------------------------------------------------------------------- /src/ObjDict.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/ObjDict.h" 2 | 3 | using namespace std; 4 | using namespace canfetti; 5 | 6 | //****************************************************************************** 7 | // ObjDict 8 | //****************************************************************************** 9 | OdEntry *ObjDict::lookup(uint16_t idx, uint8_t subIdx) 10 | { 11 | if (auto subIdxs = table.find(idx); subIdxs != table.end()) { 12 | if (auto entry = subIdxs->second.find(subIdx); entry != subIdxs->second.end()) { 13 | return &entry->second; 14 | } 15 | } 16 | 17 | return nullptr; 18 | } 19 | 20 | Error ObjDict::registerCallback(uint16_t idx, uint8_t subIdx, ChangedCallback cb) 21 | { 22 | if (auto entry = lookup(idx, subIdx)) { 23 | entry->addCallback(std::move(cb)); 24 | return Error::Success; 25 | } 26 | 27 | return Error::IndexNotFound; 28 | } 29 | 30 | Error ObjDict::fireCallbacks(uint16_t idx, uint8_t subIdx) 31 | { 32 | if (auto entry = lookup(idx, subIdx)) { 33 | entry->fireCallbacks(); 34 | return Error::Success; 35 | } 36 | 37 | return Error::IndexNotFound; 38 | } 39 | 40 | Error ObjDict::generation(uint16_t idx, uint8_t subIdx, uint32_t& generationOut) 41 | { 42 | if (auto entry = lookup(idx, subIdx)) { 43 | generationOut = entry->generation(); 44 | return Error::Success; 45 | } 46 | 47 | return Error::IndexNotFound; 48 | } 49 | 50 | std::tuple ObjDict::makeProxy(uint16_t idx, uint8_t subIdx) 51 | { 52 | OdEntry *entry = lookup(idx, subIdx); 53 | 54 | if (entry != nullptr) { 55 | if (entry->lock()) { 56 | return make_tuple(Error::Success, OdProxy(idx, subIdx, *entry)); 57 | } 58 | return make_tuple(Error::DataXferLocal, OdProxy()); 59 | } 60 | 61 | return make_tuple(Error::IndexNotFound, OdProxy()); 62 | } 63 | 64 | void ObjDict::dumpTable() 65 | { 66 | for (auto &idx : table) { 67 | LogInfo("%x:", idx.first); 68 | 69 | for (auto &subIdx : idx.second) { 70 | auto f = [&](auto &&arg) { 71 | using T = std::decay_t; 72 | const char *a = subIdx.second.access == canfetti::Access::RO ? "RO" : subIdx.second.access == canfetti::Access::RW ? "RW" : "WO"; 73 | 74 | if constexpr (std::is_same_v>) { 75 | LogInfo(" %d: (%s) u8[%zu]", subIdx.first, a, arg.size()); 76 | } 77 | else if constexpr (std::is_same_v) { 78 | LogInfo(" %d: (%s) \"%s\"", subIdx.first, a, arg.c_str()); 79 | } 80 | else if constexpr (std::is_same_v) { 81 | LogInfo(" %d: (%s) %s", subIdx.first, a, "Proxy"); 82 | } 83 | else if constexpr (std::is_same_v) { 84 | LogInfo(" %d: (%s) %s", subIdx.first, a, "Dynamic"); 85 | } 86 | else if constexpr (std::is_same_v) { 87 | LogInfo(" %d: (%s) %f", subIdx.first, a, (double)arg); 88 | } 89 | else { 90 | LogInfo(" %d: (%s) 0x%x", subIdx.first, a, (unsigned)arg); 91 | } 92 | }; 93 | 94 | std::visit(f, subIdx.second.data); 95 | } 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /src/OdData.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/OdData.h" 2 | #include 3 | 4 | using namespace canfetti; 5 | 6 | //****************************************************************************** 7 | // Helpers 8 | //****************************************************************************** 9 | size_t canfetti::size(const OdVariant &v) 10 | { 11 | auto f = [=](auto &&arg) { 12 | using T = std::decay_t; 13 | 14 | if constexpr (std::is_same_v>) { 15 | return arg.size(); 16 | } 17 | else if constexpr (std::is_same_v) { 18 | return arg.size(); 19 | } 20 | else if constexpr (std::is_same_v) { 21 | return arg.len; 22 | } 23 | else { 24 | return sizeof(arg); 25 | } 26 | }; 27 | 28 | return std::visit(f, v); 29 | } 30 | 31 | //****************************************************************************** 32 | // OdEntry 33 | //****************************************************************************** 34 | OdEntry::OdEntry(uint16_t i, uint8_t s, Access a, OdVariant d, ChangedCallback cb) : access(a), data(d), idx(i), subIdx(s) 35 | { 36 | addCallback(std::move(cb)); 37 | } 38 | 39 | bool OdEntry::lock() 40 | { 41 | if (locked) return false; 42 | locked = true; 43 | if (OdDynamicVar *dvar = std::get_if(&data); dvar && dvar->beginAccess) { 44 | dvar->beginAccess(idx, subIdx); 45 | } 46 | return true; 47 | } 48 | 49 | void OdEntry::unlock() 50 | { 51 | if (OdDynamicVar *dvar = std::get_if(&data); locked && dvar && dvar->endAccess) { 52 | dvar->endAccess(idx, subIdx); 53 | } 54 | locked = false; 55 | } 56 | 57 | bool OdEntry::isLocked() 58 | { 59 | return locked; 60 | } 61 | 62 | void OdEntry::fireCallbacks() 63 | { 64 | // May mutate in callback but never reduces in size 65 | for (size_t i = 0; i < callbacks.size(); ++i) { 66 | callbacks[i](idx, subIdx); 67 | } 68 | } 69 | 70 | void OdEntry::addCallback(ChangedCallback cb) 71 | { 72 | if (cb) { 73 | callbacks.emplace_back(std::move(cb)); 74 | } 75 | } 76 | 77 | //****************************************************************************** 78 | // OdProxy 79 | //****************************************************************************** 80 | OdProxy::OdProxy() : idx(0), subIdx(0) 81 | { 82 | } 83 | 84 | OdProxy::OdProxy(uint16_t idx, uint8_t subIdx, const OdVariant &v) : idx(idx), subIdx(subIdx), roV(&v) 85 | { 86 | readOnly = true; 87 | reset(); 88 | } 89 | 90 | OdProxy::OdProxy(uint16_t idx, uint8_t subIdx, OdVariant &v) : idx(idx), subIdx(subIdx), v(&v) 91 | { 92 | reset(); 93 | } 94 | 95 | OdProxy::OdProxy(uint16_t idx, uint8_t subIdx, OdEntry &e) : idx(idx), subIdx(subIdx), v(&e.data), e(&e) 96 | { 97 | assert(e.isLocked()); 98 | reset(); 99 | } 100 | 101 | OdProxy::OdProxy(OdProxy &&o) 102 | : idx(o.idx), subIdx(o.subIdx), changed(o.changed), v(o.v), e(o.e), ptr(o.ptr), off(o.off), len(o.len), dVar(o.dVar) 103 | { 104 | o.e = nullptr; 105 | } 106 | 107 | OdProxy::~OdProxy() 108 | { 109 | if (e) { 110 | e->unlock(); 111 | if (changed) { 112 | e->bumpGeneration(); 113 | if (!callbacksSuppressed) { 114 | e->fireCallbacks(); 115 | } 116 | } 117 | } 118 | } 119 | 120 | Error OdProxy::reset() 121 | { 122 | auto f = [&](auto &&arg) { 123 | using T = std::decay_t; 124 | 125 | if constexpr (std::is_same_v>) { 126 | ptr = (uint8_t *)arg.data(); 127 | off = 0; 128 | len = arg.size(); 129 | dVar = nullptr; 130 | } 131 | else if constexpr (std::is_same_v) { 132 | ptr = (uint8_t *)arg.c_str(); 133 | off = 0; 134 | len = arg.size(); 135 | dVar = nullptr; 136 | } 137 | else if constexpr (std::is_same_v) { 138 | ptr = arg.buf; 139 | off = 0; 140 | len = arg.len; 141 | dVar = nullptr; 142 | } 143 | else if constexpr (std::is_same_v) { 144 | assert(arg.size); 145 | ptr = nullptr; 146 | off = 0; 147 | len = arg.size(idx, subIdx); 148 | dVar = &arg; 149 | } 150 | else { 151 | ptr = (uint8_t *)&arg; 152 | off = 0; 153 | len = sizeof(arg); 154 | dVar = nullptr; 155 | } 156 | }; 157 | 158 | changed = false; 159 | std::visit(f, readOnly ? *const_cast(roV) : *v); 160 | 161 | return Error::Success; 162 | } 163 | 164 | bool OdProxy::resize(size_t newSize) 165 | { 166 | if (readOnly) return false; 167 | 168 | auto f = [=](auto &&arg) { 169 | using T = std::decay_t; 170 | 171 | if constexpr (std::is_same_v>) { 172 | arg.resize(newSize); 173 | this->reset(); 174 | return true; 175 | } 176 | else if constexpr (std::is_same_v) { 177 | arg.resize(newSize); 178 | this->reset(); 179 | return true; 180 | } 181 | else if constexpr (std::is_same_v) { 182 | if (arg.resize && arg.resize(idx, subIdx, newSize)) { 183 | this->reset(); 184 | return true; 185 | } 186 | else { 187 | return false; 188 | } 189 | } 190 | else { 191 | return false; 192 | } 193 | }; 194 | 195 | return std::visit(f, *v); 196 | } 197 | 198 | size_t OdProxy::remaining() 199 | { 200 | return len - off; 201 | } 202 | 203 | Error OdProxy::copyInto(uint8_t *b, size_t s) 204 | { 205 | if (len - off < s) { 206 | LogInfo("Data length violation on %x[%d]", idx, subIdx); 207 | return Error::ParamLength; 208 | } 209 | 210 | if (e && e->access == Access::WO) { 211 | LogDebug("Read access violation on %x[%d]", idx, subIdx); 212 | return Error::ReadViolation; 213 | } 214 | 215 | if (dVar) { 216 | if (!dVar->copyInto) return Error::UnsupportedAccess; 217 | 218 | auto err = dVar->copyInto(idx, subIdx, off, b, s); 219 | if (err == Error::Success) { 220 | off += s; 221 | } 222 | return err; 223 | } 224 | 225 | memcpy(b, ptr + off, s); 226 | off += s; 227 | 228 | return Error::Success; 229 | } 230 | 231 | Error OdProxy::copyFrom(uint8_t *b, size_t s) 232 | { 233 | if (readOnly) return Error::UnsupportedAccess; 234 | 235 | if (len - off < s) { 236 | LogInfo("Data length violation on %x[%d]", idx, subIdx); 237 | return Error::ParamLength; 238 | } 239 | 240 | if (e && e->access == Access::RO) { 241 | LogDebug("Write access violation on %x[%d]", idx, subIdx); 242 | return Error::WriteViolation; 243 | }; 244 | 245 | changed = true; 246 | 247 | if (dVar) { 248 | if (!dVar->copyFrom) return Error::UnsupportedAccess; 249 | 250 | auto err = dVar->copyFrom(idx, subIdx, off, b, s); 251 | if (err == Error::Success) { 252 | off += s; 253 | } 254 | return err; 255 | } 256 | 257 | memcpy(ptr + off, b, s); 258 | off += s; 259 | return Error::Success; 260 | } 261 | 262 | Error OdProxy::copyFrom(const OdProxy &other) 263 | { 264 | return copyFrom(other.ptr, other.len); 265 | } 266 | 267 | void OdProxy::suppressCallbacks() 268 | { 269 | callbacksSuppressed = true; 270 | } 271 | -------------------------------------------------------------------------------- /src/platform/linux/LinuxCo.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/LinuxCo.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "linux/can/raw.h" 10 | #include "net/if.h" 11 | 12 | using namespace canfetti; 13 | 14 | unsigned canfetti::newGeneration() 15 | { 16 | static std::atomic g{0}; 17 | return g.fetch_add(1, std::memory_order_relaxed); 18 | } 19 | 20 | Logger Logger::logger; 21 | 22 | //****************************************************************************** 23 | // Log Impl 24 | //****************************************************************************** 25 | 26 | void Logger::setLogCallback(void (*cb)(const char *)) 27 | { 28 | logCallback.store(cb, std::memory_order_relaxed); 29 | } 30 | 31 | //****************************************************************************** 32 | // System Impl 33 | //****************************************************************************** 34 | 35 | const System::TimerHdl System::InvalidTimer{nullptr}; 36 | 37 | System::TimerHdl System::resetTimer(System::TimerHdl &hdl) 38 | { 39 | if (!hdl) return InvalidTimer; 40 | hdl->enable = true; 41 | hdl->deadline = std::chrono::steady_clock::now() + hdl->interval; 42 | generation = newGeneration(); 43 | return hdl; 44 | } 45 | 46 | void System::deleteTimer(System::TimerHdl &hdl) 47 | { 48 | if (!hdl) return; 49 | hdl->available = true; 50 | hdl->enable = false; 51 | hdl = InvalidTimer; 52 | } 53 | 54 | void System::disableTimer(System::TimerHdl &hdl) 55 | { 56 | if (!hdl) return; 57 | hdl->enable = false; 58 | } 59 | 60 | System::TimerHdl System::scheduleDelayed(uint32_t delayMs, std::function cb) 61 | { 62 | auto hdl = getAvailableTimer(); 63 | hdl->available = false; 64 | hdl->enable = true; 65 | hdl->repeat = false; 66 | hdl->interval = std::chrono::milliseconds(delayMs); 67 | hdl->deadline = std::chrono::steady_clock::now() + hdl->interval; 68 | hdl->callback = cb; 69 | generation = newGeneration(); 70 | return hdl; 71 | } 72 | 73 | System::TimerHdl System::schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart) 74 | { 75 | auto hdl = getAvailableTimer(); 76 | auto staggeredDelay = std::chrono::milliseconds(staggeredStart ? prng() % (periodMs << 1) : 0); 77 | hdl->available = false; 78 | hdl->enable = true; 79 | hdl->repeat = true; 80 | hdl->interval = std::chrono::milliseconds(periodMs); 81 | hdl->deadline = std::chrono::steady_clock::now() + hdl->interval + staggeredDelay; 82 | hdl->callback = cb; 83 | generation = newGeneration(); 84 | return hdl; 85 | } 86 | 87 | void System::serviceTimers() 88 | { 89 | auto now = std::chrono::steady_clock::now(); 90 | // May mutate in callback, but never decreases in size 91 | for (size_t i = 0; i < timers.size(); ++i) { 92 | auto &t = timers[i]; 93 | if (t->enable && t->deadline <= now) { 94 | if (t->repeat) { 95 | t->deadline = now + t->interval; 96 | } 97 | else { 98 | t->enable = false; 99 | } 100 | t->callback(); 101 | } 102 | } 103 | } 104 | 105 | size_t System::getTimerCount() 106 | { 107 | size_t n = 0; 108 | for (size_t i = 0; i < timers.size(); ++i) { 109 | if (!timers[i]->available) ++n; 110 | } 111 | return n; 112 | } 113 | 114 | std::chrono::steady_clock::time_point System::nextTimerDeadline() 115 | { 116 | auto deadline = std::chrono::steady_clock::now() + std::chrono::hours(1); 117 | for (const auto &t : timers) { 118 | if (t->enable && deadline > t->deadline) { 119 | deadline = t->deadline; 120 | } 121 | } 122 | return deadline; 123 | } 124 | 125 | System::TimerHdl System::getAvailableTimer() 126 | { 127 | for (const auto &t : timers) { 128 | if (t->available) return t; 129 | } 130 | auto t = std::make_shared(); 131 | timers.push_back(t); 132 | return t; 133 | } 134 | 135 | //****************************************************************************** 136 | // Device 137 | //****************************************************************************** 138 | LinuxCoDev::LinuxCoDev(uint32_t baudrate) 139 | { 140 | } 141 | 142 | canfetti::Error LinuxCoDev::open(const char *device) 143 | { 144 | struct sockaddr_can addr = {0}; 145 | struct ifreq ifr = {0}; 146 | 147 | s = socket(PF_CAN, SOCK_RAW, CAN_RAW); 148 | 149 | if (s == -1) { 150 | perror("socket failed"); 151 | return Error::HwError; 152 | } 153 | 154 | strcpy(ifr.ifr_name, device); 155 | if (ioctl(s, SIOCGIFINDEX, &ifr) == -1) { 156 | perror("ioctl(SIOCGIFINDEX) failed"); 157 | return Error::HwError; 158 | } 159 | 160 | addr.can_family = AF_CAN; 161 | addr.can_ifindex = ifr.ifr_ifindex; 162 | 163 | if (bind(s, (struct sockaddr *)&addr, sizeof(addr)) == -1) { 164 | perror("bind failed"); 165 | return Error::HwError; 166 | } 167 | 168 | size_t ms = 500; 169 | struct timeval tv; 170 | tv.tv_sec = 0; 171 | 172 | while (ms >= 1000) { 173 | tv.tv_sec++; 174 | ms -= 1000; 175 | } 176 | 177 | tv.tv_usec = ms * 1000; 178 | if (setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, sizeof tv) == -1) { 179 | perror("setsockopt failed"); 180 | return Error::HwError; 181 | } 182 | 183 | return Error::Success; 184 | } 185 | 186 | Error LinuxCoDev::read(struct can_frame &frame, bool nonblock) 187 | { 188 | ssize_t r = ::recv(s, &frame, sizeof(frame), nonblock ? MSG_DONTWAIT : 0); 189 | 190 | if (r < 0) { 191 | if (errno != EAGAIN) { 192 | perror("can raw socket read"); 193 | return Error::HwError; 194 | } 195 | } 196 | 197 | return r > 0 ? Error::Success : Error::Timeout; 198 | } 199 | 200 | Error LinuxCoDev::write(const Msg &msg, bool async) 201 | { 202 | struct can_frame frame = {0}; 203 | assert(msg.len <= sizeof(frame.data)); 204 | 205 | frame.can_id = msg.id & ((1 << 29) - 1); 206 | if (frame.can_id >= 0x800) 207 | frame.can_id |= CAN_EFF_FLAG; 208 | frame.can_dlc = msg.len; 209 | if (msg.rtr) 210 | frame.can_id |= CAN_RTR_FLAG; 211 | else 212 | memcpy(frame.data, msg.data, msg.len); 213 | 214 | if (async) { 215 | asyncFrames.push_back(frame); 216 | } 217 | else if (ssize_t written = ::write(s, &frame, sizeof(frame)); written < 0) { 218 | LogDebug("can socket write: errno %d", errno); 219 | stats.droppedTx++; 220 | return Error::HwError; 221 | } 222 | 223 | return Error::Success; 224 | } 225 | 226 | void LinuxCoDev::flushAsyncFrames() 227 | { 228 | for (const struct can_frame& frame : asyncFrames) { 229 | if (ssize_t written = ::write(s, &frame, sizeof(frame)); written < 0) { 230 | LogDebug("can socket write (async): errno %d", errno); 231 | stats.droppedTx++; 232 | } 233 | } 234 | asyncFrames.clear(); 235 | } 236 | 237 | //****************************************************************************** 238 | // CanOpen Impl 239 | //****************************************************************************** 240 | LinuxCo::LinuxCo(LinuxCoDev &d, uint8_t nodeId, const char *deviceName, uint32_t deviceType) 241 | : LocalNode(d, sys, nodeId, deviceName, deviceType) 242 | { 243 | } 244 | 245 | LinuxCo::~LinuxCo() 246 | { 247 | shutdown.store(true); 248 | if (mainThread) mainThread->join(); 249 | if (recvThread) recvThread->join(); 250 | } 251 | 252 | Error LinuxCo::start(const char *dev) 253 | { 254 | if (Error e = LocalNode::init(); e != Error::Success) return e; 255 | if (Error e = static_cast(bus).open(dev); e != Error::Success) return e; 256 | mainThread = std::make_unique([=]() { this->runMainThread(); }); 257 | recvThread = std::make_unique([=]() { this->runRecvThread(); }); 258 | // Just to aid debugging 259 | std::string name; 260 | if (od.get(0x1008, 0, name) == Error::Success) { 261 | std::string mainName = name + ".main"; 262 | pthread_setname_np(mainThread->native_handle(), mainName.c_str()); 263 | std::string recvName = name + ".recv"; 264 | pthread_setname_np(recvThread->native_handle(), recvName.c_str()); 265 | } 266 | return Error::Success; 267 | } 268 | 269 | void LinuxCo::runMainThread() 270 | { 271 | while (!shutdown.load()) { 272 | std::unique_lock u(mtx); 273 | unsigned gen = sys.getTimerGeneration(); 274 | auto deadline = std::min(sys.nextTimerDeadline(), std::chrono::steady_clock::now() + std::chrono::milliseconds(500)); 275 | mainThreadWakeup.wait_until(u, deadline, [&]() { 276 | return !pendingFrames.empty() || gen != sys.getTimerGeneration() || !pendingTpdos.empty(); 277 | }); 278 | sys.serviceTimers(); 279 | for (auto& frame : pendingFrames) { 280 | Msg msg; 281 | msg.id = frame.can_id & CAN_EFF_MASK; 282 | msg.len = frame.can_dlc; 283 | msg.data = frame.data; 284 | msg.rtr = !!(frame.can_id & CAN_RTR_FLAG); 285 | processFrame(msg); 286 | } 287 | if (!pendingFrames.empty()) { 288 | recvThreadWakeup.notify_one(); 289 | } 290 | pendingFrames.clear(); 291 | pendingTpdos.clear(); 292 | u.unlock(); 293 | static_cast(bus).flushAsyncFrames(); 294 | } 295 | } 296 | 297 | void LinuxCo::runRecvThread() 298 | { 299 | constexpr size_t MAX_FRAMES_PER_BATCH = 64; 300 | std::vector frames; 301 | while (!shutdown.load()) { 302 | { 303 | std::unique_lock u(mtx); 304 | // Wait until all frames have been processed before reading another batch 305 | if (!recvThreadWakeup.wait_for(u, std::chrono::milliseconds(500), [&]() {return pendingFrames.empty();})) continue; 306 | } 307 | struct can_frame frame = {0}; 308 | bool nonblock = false; 309 | Error e; 310 | while ((e = static_cast(bus).read(frame, nonblock)) == Error::Success) { 311 | // Allow the first read to block for SO_RCVTMEO so we don't spin, then grab anything else in the queue ASAP to minimize batch latency 312 | nonblock = true; 313 | frames.push_back(frame); 314 | // Don't starve the node or allocate unbounded messages under heavy traffic 315 | if (frames.size() >= MAX_FRAMES_PER_BATCH) break; 316 | } 317 | if (!frames.empty()) { 318 | std::lock_guard g(mtx); 319 | assert(pendingFrames.empty()); 320 | pendingFrames.swap(frames); 321 | mainThreadWakeup.notify_one(); 322 | } 323 | if (e != Error::Success && e != Error::Timeout) { 324 | // Don't spin on unexpected errors 325 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 326 | } 327 | } 328 | } 329 | 330 | Error LinuxCo::triggerTPDOOnce(uint16_t pdoNum) 331 | { 332 | if (pendingTpdos.find(pdoNum) != pendingTpdos.end()) return Error::Success; 333 | if (Error e = triggerTPDO(pdoNum, /* async */ true); e != Error::Success) return e; 334 | pendingTpdos.insert(pdoNum); 335 | return Error::Success; 336 | } 337 | -------------------------------------------------------------------------------- /src/platform/linux/test/blockmode.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "canfetti/LinuxCo.h" 11 | 12 | using namespace std::chrono_literals; 13 | using namespace std; 14 | using namespace canfetti; 15 | 16 | int main() 17 | { 18 | pthread_setname_np(pthread_self(), "main"); 19 | 20 | constexpr uint8_t SDO_CHANNEL = 2; 21 | constexpr uint8_t SERVER_NODE_ID = 5; 22 | constexpr uint8_t CLIENT_NODE_ID = 8; 23 | 24 | constexpr uint16_t TEST_IDX = 0x2022; 25 | constexpr uint8_t TEST_SUBIDX = 0; 26 | constexpr uint16_t TEST_PDO_COBID = 0x445; 27 | 28 | uint8_t srcData[1000]; 29 | uint8_t dstData[1000]; 30 | memset(srcData, 'x', sizeof srcData); 31 | srcData[999] = 0; 32 | 33 | Logger::logger.debug = true; 34 | Logger::logger.setLogCallback([](const char* m) { 35 | char name[64] = {0}; 36 | pthread_getname_np(pthread_self(), name, sizeof(name)); 37 | cout << "[" << name << "] " << dec << m << endl; 38 | }); 39 | 40 | LinuxCoDev dev0(125000); 41 | LinuxCo server(dev0, SERVER_NODE_ID, "server"); 42 | server.start("vcan0"); 43 | server.doWithLock([&]() { 44 | server.addSDOServer(SDO_CHANNEL, CLIENT_NODE_ID); 45 | server.od.insert(TEST_IDX, TEST_SUBIDX, Access::RW, OdBuffer{dstData, sizeof dstData}); 46 | }); 47 | 48 | LinuxCoDev dev1(125000); 49 | LinuxCo client(dev1, CLIENT_NODE_ID, "client"); 50 | client.start("vcan0"); 51 | client.doWithLock([&]() { 52 | client.addSDOClient(SDO_CHANNEL, SERVER_NODE_ID); 53 | }); 54 | 55 | thread t([&]() { 56 | Error e = client.blockingWrite(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, OdBuffer{srcData, sizeof srcData}); 57 | assert(e == Error::Success); 58 | assert(memcmp(srcData, dstData, sizeof dstData) == 0); 59 | }); 60 | 61 | t.join(); 62 | this_thread::sleep_for(chrono::milliseconds(100)); 63 | 64 | server.doWithLock([&]() { 65 | assert(server.getActiveTransactionCount() == 0); 66 | assert(server.getTimerCount() == 0); 67 | }); 68 | 69 | client.doWithLock([&]() { 70 | assert(client.getActiveTransactionCount() == 0); 71 | assert(client.getTimerCount() == 0); 72 | }); 73 | 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /src/platform/linux/test/generation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include "canfetti/LinuxCo.h" 11 | 12 | using namespace std::chrono_literals; 13 | using namespace std; 14 | using namespace canfetti; 15 | 16 | int main() 17 | { 18 | pthread_setname_np(pthread_self(), "main"); 19 | 20 | constexpr uint8_t SDO_CHANNEL = 2; 21 | constexpr uint8_t SERVER_NODE_ID = 5; 22 | constexpr uint8_t CLIENT_NODE_ID = 8; 23 | 24 | constexpr uint16_t TEST_IDX = 0x2022; 25 | constexpr uint8_t TEST_SUBIDX = 0; 26 | 27 | Logger::logger.debug = true; 28 | Logger::logger.setLogCallback([](const char* m) { 29 | char name[64] = {0}; 30 | pthread_getname_np(pthread_self(), name, sizeof(name)); 31 | cout << "[" << name << "] " << dec << m << endl; 32 | }); 33 | 34 | LinuxCoDev dev0(125000); 35 | LinuxCo server(dev0, SERVER_NODE_ID, "server"); 36 | server.start("vcan0"); 37 | unsigned gen; 38 | server.doWithLock([&]() { 39 | server.addSDOServer(SDO_CHANNEL, CLIENT_NODE_ID); 40 | server.od.insert(TEST_IDX, TEST_SUBIDX, Access::RW, _u8(42)); 41 | assert(server.od.generation(TEST_IDX, TEST_SUBIDX, gen) == Error::Success); 42 | }); 43 | 44 | LinuxCoDev dev1(125000); 45 | LinuxCo client(dev1, CLIENT_NODE_ID, "client"); 46 | client.start("vcan0"); 47 | client.doWithLock([&]() { 48 | client.addSDOClient(SDO_CHANNEL, SERVER_NODE_ID); 49 | }); 50 | 51 | thread([&]() { 52 | Error e = client.blockingWrite(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, _u8(42)); 53 | assert(e == Error::Success); 54 | }).join(); 55 | 56 | server.doWithLock([&]() { 57 | unsigned g; 58 | assert(server.od.generation(TEST_IDX, TEST_SUBIDX, g) == Error::Success); 59 | assert(gen != g); 60 | }); 61 | 62 | return 0; 63 | } 64 | -------------------------------------------------------------------------------- /src/platform/linux/test/threads.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "canfetti/LinuxCo.h" 10 | 11 | using namespace std::chrono_literals; 12 | using namespace std; 13 | using namespace canfetti; 14 | 15 | int main() 16 | { 17 | pthread_setname_np(pthread_self(), "main"); 18 | 19 | constexpr uint8_t SDO_1_CHANNEL = 1; 20 | constexpr uint8_t SDO_2_CHANNEL = 2; 21 | constexpr uint8_t SERVER_NODE_ID = 5; 22 | constexpr uint8_t CLIENT_1_NODE_ID = 7; 23 | constexpr uint8_t CLIENT_2_NODE_ID = 8; 24 | 25 | constexpr uint16_t TEST_IDX1 = 0x2021; 26 | constexpr uint8_t TEST_SUBIDX1 = 0; 27 | constexpr uint16_t TEST_PDO_COBID1 = 0x444; 28 | constexpr uint32_t expectedVal1 = 1234567; 29 | 30 | constexpr uint16_t TEST_IDX2 = 0x2022; 31 | constexpr uint8_t TEST_SUBIDX2 = 0; 32 | constexpr uint16_t TEST_PDO_COBID2 = 0x445; 33 | string expectedVal2 = "a long string yo"; 34 | 35 | // logger.logDebug = true; 36 | Logger::logger.setLogCallback([](const char* m) { 37 | char name[64] = {0}; 38 | pthread_getname_np(pthread_self(), name, sizeof(name)); 39 | cout << "[" << name << "] " << dec << m << endl; 40 | }); 41 | 42 | LinuxCoDev dev0(125000); 43 | LinuxCo server(dev0, SERVER_NODE_ID, "server"); 44 | server.start("vcan0"); 45 | 46 | LinuxCoDev dev1(125000); 47 | LinuxCo client1(dev1, CLIENT_1_NODE_ID, "client 1"); 48 | client1.start("vcan0"); 49 | client1.doWithLock([&]() { 50 | client1.addSDOClient(SDO_1_CHANNEL, SERVER_NODE_ID); 51 | }); 52 | server.doWithLock([&]() { 53 | server.addSDOServer(SDO_1_CHANNEL, CLIENT_1_NODE_ID); 54 | server.od.insert(TEST_IDX1, TEST_SUBIDX1, Access::RW, expectedVal1); 55 | }); 56 | 57 | LinuxCoDev dev2(125000); 58 | LinuxCo client2(dev2, CLIENT_2_NODE_ID, "client 2"); 59 | client2.start("vcan0"); 60 | client2.doWithLock([&]() { 61 | client2.addSDOClient(SDO_2_CHANNEL, SERVER_NODE_ID); 62 | }); 63 | server.doWithLock([&]() { 64 | server.addSDOServer(SDO_2_CHANNEL, CLIENT_2_NODE_ID); 65 | server.od.insert(TEST_IDX2, TEST_SUBIDX2, Access::RW, expectedVal2); 66 | }); 67 | 68 | vector clientThreads; 69 | 70 | for (int i = 0; i < 4; i++) { 71 | clientThreads.emplace_back([&]() { 72 | for (int i = 0; i < 500; i++) { 73 | uint32_t value = 0; 74 | Error e = client1.blockingRead(SERVER_NODE_ID, TEST_IDX1, TEST_SUBIDX1, value); 75 | if (e == Error::Success) { 76 | assert(value == expectedVal1); 77 | } 78 | 79 | this_thread::sleep_for(chrono::milliseconds(std::rand() % 10)); 80 | } 81 | }); 82 | 83 | clientThreads.emplace_back([&]() { 84 | for (int i = 0; i < 500; i++) { 85 | string value = "aaaaaaaaaaa"; 86 | Error e = client2.blockingWrite(SERVER_NODE_ID, TEST_IDX2, TEST_SUBIDX2, value); 87 | if (e == Error::Success) { 88 | assert(value == "aaaaaaaaaaa"); 89 | } 90 | 91 | this_thread::sleep_for(chrono::milliseconds(std::rand() % 10)); 92 | } 93 | }); 94 | } 95 | 96 | for (auto& t : clientThreads) { 97 | t.join(); 98 | } 99 | 100 | this_thread::sleep_for(chrono::milliseconds(100)); 101 | 102 | server.doWithLock([&]() { 103 | assert(server.getActiveTransactionCount() == 0); 104 | assert(server.getTimerCount() == 0); 105 | }); 106 | 107 | client1.doWithLock([&]() { 108 | assert(client1.getActiveTransactionCount() == 0); 109 | assert(client1.getTimerCount() == 0); 110 | }); 111 | 112 | client2.doWithLock([&]() { 113 | assert(client2.getActiveTransactionCount() == 0); 114 | assert(client2.getTimerCount() == 0); 115 | }); 116 | 117 | return 0; 118 | } 119 | -------------------------------------------------------------------------------- /src/platform/linux/test/vector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "canfetti/LinuxCo.h" 9 | 10 | using namespace std::chrono_literals; 11 | using namespace std; 12 | using namespace canfetti; 13 | 14 | int main() 15 | { 16 | pthread_setname_np(pthread_self(), "main"); 17 | 18 | constexpr uint8_t SDO_CHANNEL = 2; 19 | constexpr uint8_t SERVER_NODE_ID = 5; 20 | constexpr uint8_t CLIENT_NODE_ID = 8; 21 | 22 | constexpr uint16_t TEST_IDX = 0x2022; 23 | constexpr uint8_t TEST_SUBIDX = 0; 24 | 25 | const std::vector empty; 26 | const std::string full_ = "The quick brown fox jumps over the lazy dog"; 27 | const std::vector full(full_.begin(), full_.end()); 28 | 29 | Logger::logger.debug = true; 30 | Logger::logger.setLogCallback([](const char* m) { 31 | char name[64] = {0}; 32 | pthread_getname_np(pthread_self(), name, sizeof(name)); 33 | cout << "[" << name << "] " << dec << m << endl; 34 | }); 35 | 36 | LinuxCoDev dev0(125000); 37 | LinuxCo server(dev0, SERVER_NODE_ID, "server"); 38 | server.start("vcan0"); 39 | server.doWithLock([&]() { 40 | server.addSDOServer(SDO_CHANNEL, CLIENT_NODE_ID); 41 | server.od.insert(TEST_IDX, TEST_SUBIDX, Access::RW, empty); 42 | }); 43 | 44 | LinuxCoDev dev1(125000); 45 | LinuxCo client(dev1, CLIENT_NODE_ID, "client"); 46 | client.start("vcan0"); 47 | client.doWithLock([&]() { 48 | client.addSDOClient(SDO_CHANNEL, SERVER_NODE_ID); 49 | }); 50 | 51 | { 52 | server.doWithLock([&]() { 53 | server.od.set(TEST_IDX, TEST_SUBIDX, empty); 54 | }); 55 | auto buf = full; 56 | Error e = client.blockingRead(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, buf); 57 | assert(e == Error::Success); 58 | assert(buf == empty); 59 | } 60 | 61 | { 62 | server.doWithLock([&]() { 63 | server.od.set(TEST_IDX, TEST_SUBIDX, full); 64 | }); 65 | auto buf = empty; 66 | Error e = client.blockingRead(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, buf); 67 | assert(e == Error::Success); 68 | assert(buf == full); 69 | } 70 | 71 | { 72 | server.od.set(TEST_IDX, TEST_SUBIDX, full); 73 | Error e = client.blockingWrite(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, vector(empty)); 74 | assert(e == Error::Success); 75 | auto buf = full; 76 | server.doWithLock([&]() { 77 | e = server.od.get(TEST_IDX, TEST_SUBIDX, buf); 78 | }); 79 | assert(e == Error::Success); 80 | assert(buf == empty); 81 | } 82 | 83 | { 84 | server.od.set(TEST_IDX, TEST_SUBIDX, empty); 85 | Error e = client.blockingWrite(SERVER_NODE_ID, TEST_IDX, TEST_SUBIDX, vector(full)); 86 | assert(e == Error::Success); 87 | auto buf = empty; 88 | server.doWithLock([&]() { 89 | e = server.od.get(TEST_IDX, TEST_SUBIDX, buf); 90 | }); 91 | assert(e == Error::Success); 92 | assert(buf == full); 93 | } 94 | 95 | return 0; 96 | } 97 | -------------------------------------------------------------------------------- /src/platform/odrive/ODriveCo.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "canfetti/ODriveCo.h" 3 | #include 4 | #include 5 | #include 6 | #include "odrive_main.h" 7 | 8 | using namespace canfetti; 9 | 10 | unsigned canfetti::newGeneration() 11 | { 12 | static std::atomic g{0}; 13 | return g.fetch_add(1, std::memory_order_relaxed); 14 | } 15 | 16 | Logger canfetti::logger; 17 | 18 | //****************************************************************************** 19 | // System Impl 20 | //****************************************************************************** 21 | 22 | bool System::init(fibre::Callback, float, fibre::Callback> timer, fibre::Callback &> timerCancel) 23 | { 24 | this->timer = timer; 25 | this->timerCancel = timerCancel; 26 | return true; 27 | } 28 | 29 | void System::timerHelper(TimerData *td) 30 | { 31 | td->handle.reset(); // The underlying timer was already freed 32 | 33 | if (td->periodMs) { 34 | td->handle = timer.invoke((float)td->periodMs / 1000.0f, MEMBER_CB(td, trigger)); 35 | if (!td->handle) { 36 | LogInfo("Timer recreation failed"); 37 | } 38 | } 39 | 40 | td->cb(); 41 | } 42 | 43 | void System::deleteTimer(System::TimerHdl &hdl) 44 | { 45 | if (hdl) { 46 | timerCancel.invoke(hdl->handle); 47 | hdl->available = true; 48 | } 49 | } 50 | 51 | System::TimerHdl System::scheduleDelayed(uint32_t delayMs, std::function cb) 52 | { 53 | for (size_t i = 0; i < timers.size(); i++) { 54 | TimerData &td = timers[i]; 55 | if (td.available) { 56 | auto hdl = timer.invoke((float)delayMs / 1000.0f, MEMBER_CB(&td, trigger)); 57 | if (hdl) { 58 | td.available = false; 59 | td.cb = cb; 60 | td.periodMs = 0; 61 | td.handle = hdl; 62 | td.parent = this; 63 | 64 | return &td; 65 | } 66 | } 67 | } 68 | 69 | LogInfo("Ran out of timers. Fixme"); 70 | return System::InvalidTimer; 71 | } 72 | 73 | System::TimerHdl System::schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart) 74 | { 75 | for (size_t i = 0; i < timers.size(); i++) { 76 | TimerData &td = timers[i]; 77 | if (td.available) { 78 | auto hdl = timer.invoke((float)periodMs / 1000.0f, MEMBER_CB(&td, trigger)); 79 | if (hdl) { 80 | td.available = false; 81 | td.cb = cb; 82 | td.periodMs = periodMs; 83 | td.handle = hdl; 84 | td.parent = this; 85 | 86 | return &td; 87 | } 88 | } 89 | } 90 | 91 | LogInfo("Ran out of timers. Fixme"); 92 | return System::InvalidTimer; 93 | } 94 | 95 | //****************************************************************************** 96 | // CanOpen Impl 97 | //****************************************************************************** 98 | Logger Logger::logger; 99 | 100 | ODriveCo::ODriveCo(CanBusBase *canbus) 101 | : LocalNode(*this, sys, config_.id, "odrive", 0), canbus(canbus) 102 | { 103 | } 104 | 105 | void ODriveCo::init(fibre::Callback, float, fibre::Callback> timer, fibre::Callback &> timerCancel, uint32_t numPrioTxSlots) 106 | { 107 | this->numTxPrioSlots = numPrioTxSlots; 108 | this->txPrioSlot = 0; 109 | 110 | LocalNode::init(); 111 | sys.init(timer, timerCancel); 112 | initObjDict(); 113 | configHwFilters(); 114 | } 115 | 116 | Error ODriveCo::write(const Msg &msg, bool /* async */) 117 | { 118 | can_Message_t txmsg = {0}; 119 | 120 | if (msg.len <= sizeof(txmsg.buf)) { 121 | txmsg.id = msg.id; 122 | txmsg.len = msg.len; 123 | memcpy(txmsg.buf, msg.data, msg.len); 124 | 125 | bool e = canbus->send_message(~0, txmsg, [](bool b) {}); 126 | return e ? Error::Success : Error::Error; 127 | } 128 | 129 | return Error::Error; 130 | } 131 | 132 | Error ODriveCo::writePriority(const Msg &msg) 133 | { 134 | can_Message_t txmsg = {0}; 135 | 136 | if (msg.len <= sizeof(txmsg.buf)) { 137 | txmsg.id = msg.id; 138 | txmsg.len = msg.len; 139 | memcpy(txmsg.buf, msg.data, msg.len); 140 | 141 | uint32_t slot = txPrioSlot++; 142 | if (txPrioSlot == numTxPrioSlots) { 143 | txPrioSlot = 0; 144 | } 145 | 146 | bool e = canbus->send_message(slot, txmsg, [](bool b) {}); 147 | return e ? Error::Success : Error::Error; 148 | } 149 | 150 | return Error::Error; 151 | } 152 | 153 | void ODriveCo::handle_can_message(const can_Message_t &m) 154 | { 155 | Msg msg = { 156 | .id = m.id, 157 | .rtr = m.rtr, 158 | .len = m.len, 159 | .data = const_cast(m.buf), 160 | }; 161 | 162 | processFrame(msg); 163 | } 164 | -------------------------------------------------------------------------------- /src/platform/teensy/TyCo.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "canfetti/TyCo.h" 3 | #include "version.h" 4 | 5 | using namespace canfetti; 6 | 7 | unsigned canfetti::newGeneration() 8 | { 9 | static unsigned g = 0; 10 | return g++; 11 | } 12 | 13 | Logger Logger::logger; 14 | 15 | //****************************************************************************** 16 | // System Impl 17 | //****************************************************************************** 18 | 19 | void System::service() 20 | { 21 | uint32_t now = millis(); 22 | 23 | for (auto td = timers.begin(); td != timers.end(); ++td) { 24 | if (td->enable && (now - td->lastFireTime) >= td->delay) { 25 | if (td->period) { 26 | td->lastFireTime = now; 27 | td->delay = td->period; 28 | } 29 | else { 30 | td->enable = false; 31 | } 32 | if (td->cb) td->cb(); 33 | } 34 | } 35 | } 36 | 37 | void System::deleteTimer(System::TimerHdl &hdl) 38 | { 39 | if (hdl != System::InvalidTimer) { 40 | assert(hdl < timers.size()); 41 | timers[hdl].enable = false; 42 | timers[hdl].available = true; 43 | hdl = System::InvalidTimer; 44 | } 45 | } 46 | 47 | System::TimerHdl System::scheduleDelayed(uint32_t delayMs, std::function cb) 48 | { 49 | for (size_t i = 0; i < timers.size(); i++) { 50 | TimerData &td = timers[i]; 51 | if (td.available) { 52 | td.lastFireTime = millis(); 53 | td.delay = delayMs; 54 | td.enable = true; 55 | td.available = false; 56 | td.period = 0; 57 | td.cb = cb; 58 | return i; 59 | } 60 | } 61 | 62 | timers.push_back({ 63 | .lastFireTime = millis(), 64 | .delay = delayMs, 65 | .period = 0, 66 | .cb = cb, 67 | .enable = true, 68 | .available = false, 69 | }); 70 | 71 | return timers.size() - 1; 72 | } 73 | 74 | System::TimerHdl System::schedulePeriodic(uint32_t periodMs, std::function cb, bool staggeredStart) 75 | { 76 | uint32_t staggeredDelay = staggeredStart ? random(periodMs << 1) : 0; 77 | 78 | for (size_t i = 0; i < timers.size(); i++) { 79 | TimerData &td = timers[i]; 80 | if (td.available) { 81 | td.lastFireTime = millis(); 82 | td.delay = periodMs + staggeredDelay; 83 | td.enable = true; 84 | td.available = false; 85 | td.period = periodMs; 86 | td.cb = cb; 87 | return i; 88 | } 89 | } 90 | 91 | timers.push_back({ 92 | .lastFireTime = millis(), 93 | .delay = periodMs + staggeredDelay, 94 | .period = periodMs, 95 | .cb = cb, 96 | .enable = true, 97 | .available = false, 98 | }); 99 | 100 | return timers.size() - 1; 101 | } 102 | 103 | //****************************************************************************** 104 | // TyCoDev 105 | //****************************************************************************** 106 | 107 | canfetti::Error TyCoDev::init(uint32_t baudrate, FlexCAN_T4 *busInstance) 108 | { 109 | assert(busInstance); 110 | this->bus = busInstance; 111 | 112 | bus->begin(); 113 | bus->setBaudRate(baudrate); 114 | 115 | #if defined(__IMXRT1062__) 116 | bus->setPartition({RX, STD, 4, std::array{0x081, 0x0FF}}); 117 | bus->setPartition({RX, STD, 50, ACCEPT_ALL}); 118 | bus->setPartition({TX_PRIO, STD, 2}); 119 | bus->setPartition({TX, STD, 8}); 120 | #elif defined(__MK20DX256__) || defined(__MK64FX512__) || defined(__MK66FX1M0__) 121 | bus->setPartition({RX, STD, 2, std::array{0x081, 0x0FF}}); 122 | bus->setPartition({RX, STD, 9, ACCEPT_ALL}); 123 | bus->setPartition({TX_PRIO, STD, 1}); 124 | bus->setPartition({TX, STD, 4}); 125 | #endif 126 | 127 | return canfetti::Error::Success; 128 | } 129 | 130 | canfetti::Error TyCoDev::write(const Msg &msg, bool /* async */) 131 | { 132 | CAN_message_t m; 133 | assert(msg.len <= sizeof(m.buf)); 134 | 135 | memcpy(m.buf, msg.data, msg.len); 136 | m.id = msg.id & ((1 << 29) - 1); 137 | m.len = msg.len; 138 | m.flags.remote = msg.rtr; 139 | m.flags.extended = false; 140 | 141 | if (int e = bus->write(m); e != 1) { 142 | #if 0 143 | static unsigned next = 0; 144 | static size_t lastDropped = 0; 145 | unsigned now = millis(); 146 | if (next < now) { 147 | LogInfo("CAN write failed: %d (delta: %d)", e, stats.droppedTx - lastDropped); 148 | next = now + 5000; 149 | lastDropped = stats.droppedTx; 150 | } 151 | #endif 152 | stats.droppedTx++; 153 | return canfetti::Error::HwError; 154 | } 155 | 156 | return canfetti::Error::Success; 157 | } 158 | 159 | canfetti::Error TyCoDev::writePriority(const Msg &msg) 160 | { 161 | CAN_message_t m; 162 | assert(msg.len <= sizeof(m.buf)); 163 | 164 | memcpy(m.buf, msg.data, msg.len); 165 | m.id = msg.id & ((1 << 29) - 1); 166 | m.len = msg.len; 167 | m.flags.remote = msg.rtr; 168 | m.flags.extended = false; 169 | 170 | if (int e = bus->writePriority(m); e != 1) { 171 | #if 0 172 | static unsigned next = 0; 173 | static size_t lastDropped = 0; 174 | unsigned now = millis(); 175 | if (next < now) { 176 | LogInfo("CAN write failed: %d (delta: %d)", e, stats.droppedPrioTx - lastDropped); 177 | next = now + 5000; 178 | lastDropped = stats.droppedTx; 179 | } 180 | #endif 181 | stats.droppedPrioTx++; 182 | return canfetti::Error::Error; 183 | } 184 | 185 | return canfetti::Error::Success; 186 | } 187 | 188 | //****************************************************************************** 189 | // CanOpen Impl 190 | //****************************************************************************** 191 | TyCo::TyCo(TyCoDev &d, uint8_t nodeId, const char *deviceName, uint32_t deviceType) 192 | : LocalNode(d, sys, nodeId, deviceName, deviceType), dev(d) 193 | { 194 | } 195 | 196 | void TyCo::service(size_t maxRxBatch) 197 | { 198 | CAN_message_t m; 199 | 200 | while (dev.read(m)) { 201 | Msg msg = { 202 | .id = m.id, 203 | .rtr = !!m.flags.remote, 204 | .len = m.len, 205 | .data = m.buf, 206 | }; 207 | 208 | if (m.flags.overrun) { 209 | dev.stats.overruns++; 210 | } 211 | 212 | dev.stats.droppedRx += dev.newDroppedRx(); 213 | 214 | processFrame(msg); 215 | if (--maxRxBatch == 0) break; 216 | } 217 | 218 | sys.service(); 219 | } 220 | -------------------------------------------------------------------------------- /src/platform/unittest/test-callbacks.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test.h" 3 | 4 | using namespace canfetti; 5 | using namespace std; 6 | 7 | using ::testing::_; 8 | using ::testing::ElementsAre; 9 | using ::testing::Invoke; 10 | using ::testing::MockFunction; 11 | using ::testing::FieldsAre; 12 | 13 | namespace { 14 | class MockSystem : public canfetti::System { 15 | public: 16 | MOCK_METHOD(System::TimerHdl, resetTimer, (System::TimerHdl & hdl), (override)); 17 | MOCK_METHOD(void, deleteTimer, (System::TimerHdl & hdl), (override)); 18 | MOCK_METHOD(void, disableTimer, (System::TimerHdl & hdl), (override)); 19 | MOCK_METHOD(System::TimerHdl, scheduleDelayed, (uint32_t delayMs, std::function cb), (override)); 20 | MOCK_METHOD(System::TimerHdl, schedulePeriodic, (uint32_t periodMs, std::function cb, bool staggeredStart), (override)); 21 | }; 22 | 23 | class MockCanDevice : public CanDevice { 24 | public: 25 | MOCK_METHOD(Error, write, (const Msg &msg, bool /* async */), (override)); 26 | }; 27 | 28 | class MockLocalNode : public LocalNode { 29 | public: 30 | MockLocalNode() : LocalNode(dev, sys, 1, "Test Device", 0) {} 31 | Error sendResponse(const Msg &m, bool /* async */) 32 | { 33 | processFrame(m); 34 | return Error::Success; 35 | } 36 | MockSystem sys; 37 | MockCanDevice dev; 38 | }; 39 | } 40 | 41 | TEST(LinuxCoTest, emcyCallback) 42 | { 43 | MockLocalNode co; 44 | co.init(); 45 | 46 | ::testing::MockFunction mcb; 47 | EXPECT_CALL(mcb, Call(co.nodeId, 0x1234, ElementsAre(0, 0, 0, 0, 0))); 48 | 49 | co.registerEmcyCallback(mcb.AsStdFunction()); 50 | 51 | EXPECT_CALL(co.dev, write(FieldsAre(0x081, false, 8, _), _)).WillOnce(Invoke(&co, &MockLocalNode::sendResponse)); 52 | 53 | co.sendEmcy(0x1234); 54 | } 55 | -------------------------------------------------------------------------------- /src/platform/unittest/test-client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "test.h" 7 | 8 | using namespace canfetti; 9 | using namespace std; 10 | 11 | using ::testing::_; 12 | using ::testing::Invoke; 13 | using ::testing::MockFunction; 14 | using ::testing::Return; 15 | using ::testing::SetArgReferee; 16 | using ::testing::FieldsAre; 17 | 18 | namespace { 19 | class MockSystem : public canfetti::System { 20 | public: 21 | MOCK_METHOD(System::TimerHdl, resetTimer, (System::TimerHdl & hdl), (override)); 22 | MOCK_METHOD(void, deleteTimer, (System::TimerHdl & hdl), (override)); 23 | MOCK_METHOD(void, disableTimer, (System::TimerHdl & hdl), (override)); 24 | MOCK_METHOD(System::TimerHdl, scheduleDelayed, (uint32_t delayMs, std::function cb), (override)); 25 | MOCK_METHOD(System::TimerHdl, schedulePeriodic, (uint32_t periodMs, std::function cb, bool staggeredStart), (override)); 26 | }; 27 | 28 | class MockCanDevice : public CanDevice { 29 | public: 30 | MOCK_METHOD(Error, write, (const Msg &msg, bool /* async */), (override)); 31 | }; 32 | 33 | class MockLocalNode : public LocalNode { 34 | public: 35 | MockLocalNode() : LocalNode(dev, sys, 1, "Test Device", 0) {} 36 | void sendResponse(Msg m) 37 | { 38 | processFrame(m); 39 | } 40 | MockSystem sys; 41 | MockCanDevice dev; 42 | }; 43 | } 44 | 45 | TEST(LinuxCoTest, readAsyncExpedited) 46 | { 47 | uint32_t readback = 0x11223344; 48 | System::TimerHdl tmrHdl = 4321; 49 | System::TimerHdl invalidTmrHdl = -1; 50 | 51 | constexpr uint16_t node = 5; 52 | constexpr uint16_t idx = 0x2003; 53 | constexpr uint8_t subIdx = 3; 54 | constexpr uint8_t remaining = sizeof(readback); 55 | 56 | uint8_t payload[8] = { 57 | 2 << 5 | (4 - remaining) << 2 | 0b11, 58 | static_cast(idx & 0xFF), 59 | static_cast(idx >> 8), 60 | subIdx, 61 | 0x44, 0x33, 0x22, 0x11}; 62 | 63 | Msg m = { 64 | .id = 0x580 + node, 65 | .rtr = false, 66 | .len = sizeof(payload), 67 | .data = payload}; 68 | MockLocalNode co; 69 | co.init(); 70 | 71 | EXPECT_EQ(co.addSDOClient(node, node), Error::Success); 72 | 73 | bool sentInit = false; 74 | EXPECT_CALL(co.dev, write(FieldsAre(0x605, false, 8, _), _)) 75 | .WillOnce( 76 | Invoke([&](const Msg& m, bool /* async */) { 77 | sentInit = true; 78 | return Error::Success; 79 | })); 80 | 81 | // Make sure it sets a transaction timeout 82 | EXPECT_CALL(co.sys, scheduleDelayed) 83 | .WillOnce(Return(tmrHdl)); 84 | 85 | // And cleans up the transaction timeout 86 | EXPECT_CALL(co.sys, deleteTimer(tmrHdl)) 87 | .WillOnce(SetArgReferee<0>(invalidTmrHdl)); 88 | 89 | auto mockCb = MockFunction(); 90 | 91 | EXPECT_CALL(mockCb, Call(Error::Success, readback)); 92 | 93 | EXPECT_EQ(co.read(node, idx, subIdx, mockCb.AsStdFunction()), Error::Success); 94 | if (sentInit) { 95 | co.sendResponse(m); 96 | } 97 | } 98 | -------------------------------------------------------------------------------- /src/platform/unittest/test-od.cpp: -------------------------------------------------------------------------------- 1 | #include "test.h" 2 | 3 | using namespace canfetti; 4 | using namespace std; 5 | 6 | static Error odWrite(const uint16_t idx, const uint8_t subIdx, size_t off, uint8_t* buf, size_t s) 7 | { 8 | return Error::Success; 9 | } 10 | 11 | static Error odRead(const uint16_t idx, const uint8_t subIdx, size_t off, uint8_t* buf, size_t s) 12 | { 13 | uint32_t v = 2345; 14 | if (s == 4) { 15 | memcpy(buf, &v, 4); 16 | return Error::Success; 17 | } 18 | return Error::ParamIncompatibility; 19 | } 20 | 21 | static size_t odSize(const uint16_t idx, const uint8_t subIdx) 22 | { 23 | return 4; 24 | } 25 | 26 | TEST(ObjDict, InsertGetSet) 27 | { 28 | ObjDict od; 29 | uint16_t idx = 0x2000; 30 | 31 | { 32 | uint8_t v = 0; 33 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, _u8(123)), Error::Success); 34 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 35 | EXPECT_EQ(v, 123); 36 | 37 | EXPECT_EQ(od.set(idx, 0, _u8(44)), Error::Success); 38 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 39 | EXPECT_EQ(v, 44); 40 | } 41 | 42 | { 43 | idx++; 44 | uint32_t v = 0; 45 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, _u32(555)), Error::Success); 46 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 47 | EXPECT_EQ(v, 555); 48 | 49 | EXPECT_EQ(od.set(idx, 0, _u32(222)), Error::Success); 50 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 51 | EXPECT_EQ(v, 222); 52 | } 53 | 54 | { 55 | idx++; 56 | float v = 0; 57 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, 12.2f), Error::Success); 58 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 59 | EXPECT_EQ(v, 12.2f); 60 | 61 | EXPECT_EQ(od.set(idx, 0, 555.f), Error::Success); 62 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 63 | EXPECT_EQ(v, 555.f); 64 | } 65 | 66 | { 67 | idx++; 68 | string v; 69 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, "a string"), Error::Success); 70 | EXPECT_EQ(od.get(idx, 0, v), Error::Success); 71 | EXPECT_EQ(v, string("a string")); 72 | } 73 | 74 | { 75 | idx++; 76 | uint32_t somevalue = 4532; 77 | uint32_t v = 0; 78 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, _p(somevalue)), Error::Success); 79 | auto [e, p] = od.makeProxy(idx, 0); 80 | ASSERT_EQ(e, Error::Success); 81 | p.copyInto((uint8_t*)&v, 4); 82 | EXPECT_EQ(v, 4532); 83 | } 84 | 85 | { 86 | idx++; 87 | uint32_t v = 0; 88 | auto dvar = canfetti::OdDynamicVar{.copyFrom = odWrite, .copyInto = odRead, .size = odSize}; 89 | EXPECT_EQ(od.insert(idx, 0, canfetti::Access::RO, dvar), Error::Success); 90 | auto [e, p] = od.makeProxy(idx, 0); 91 | ASSERT_EQ(e, Error::Success); 92 | p.copyInto((uint8_t*)&v, 4); 93 | EXPECT_EQ(v, 2345); 94 | } 95 | } 96 | 97 | TEST(ObjDict, Size) 98 | { 99 | ObjDict od; 100 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u8(123)), Error::Success); 101 | 102 | EXPECT_EQ(od.entrySize(0x2000, 0), 1); 103 | EXPECT_EQ(od.entrySize(0x2000, 1), 0); 104 | } 105 | 106 | TEST(ObjDict, Exists) 107 | { 108 | ObjDict od; 109 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u8(123)), Error::Success); 110 | 111 | EXPECT_FALSE(od.entryExists(0x2000, 1)); 112 | EXPECT_TRUE(od.entryExists(0x2000, 0)); 113 | } 114 | 115 | TEST(ObjDict, GetInvalid) 116 | { 117 | ObjDict od; 118 | float v = 0; 119 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u32(123)), Error::Success); 120 | EXPECT_EQ(od.get(0x2000, 0, v), Error::ParamIncompatibility); 121 | } 122 | 123 | TEST(ObjDict, InsertDuplicate) 124 | { 125 | ObjDict od; 126 | EXPECT_EQ(od.insert(0x2000, 1, canfetti::Access::RO, _u8(1)), Error::Success); 127 | EXPECT_EQ(od.insert(0x2000, 1, canfetti::Access::RO, _u8(1)), Error::Error); 128 | EXPECT_EQ(od.insert(0x2000, 1, canfetti::Access::RO, _u32(1)), Error::Error); 129 | } 130 | 131 | TEST(ObjDict, SimpleLocking) 132 | { 133 | ObjDict od; 134 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u8(55)), Error::Success); 135 | 136 | { 137 | // p1 holds the lock 138 | auto [e1, p1] = od.makeProxy(0x2000, 0); 139 | ASSERT_EQ(e1, Error::Success); 140 | 141 | auto [e2, p2] = od.makeProxy(0x2000, 0); 142 | ASSERT_EQ(e2, Error::DataXferLocal); 143 | 144 | uint8_t v; 145 | EXPECT_EQ(od.get(0x2000, 0, v), Error::Timeout); 146 | EXPECT_EQ(od.set(0x2000, 0, v), Error::Timeout); 147 | } 148 | 149 | { 150 | auto [e, p] = od.makeProxy(0x2000, 0); 151 | ASSERT_EQ(e, Error::Success); 152 | } 153 | 154 | { 155 | uint8_t v; 156 | EXPECT_EQ(od.get(0x2000, 0, v), Error::Success); 157 | EXPECT_EQ(od.set(0x2000, 0, v), Error::Success); 158 | } 159 | } 160 | 161 | TEST(ObjDict, Access) 162 | { 163 | ObjDict od; 164 | 165 | { 166 | uint8_t v = 0; 167 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u8(55)), Error::Success); 168 | auto [e, p] = od.makeProxy(0x2000, 0); 169 | ASSERT_EQ(e, Error::Success); 170 | 171 | EXPECT_EQ(p.copyFrom(&v, 1), Error::WriteViolation); 172 | EXPECT_EQ(p.reset(), Error::Success); 173 | EXPECT_EQ(p.copyInto(&v, 1), Error::Success); 174 | } 175 | 176 | { 177 | uint8_t v = 0; 178 | EXPECT_EQ(od.insert(0x2000, 1, canfetti::Access::RW, _u8(1)), Error::Success); 179 | auto [e, p] = od.makeProxy(0x2000, 1); 180 | ASSERT_EQ(e, Error::Success); 181 | 182 | ASSERT_EQ(p.idx, 0x2000); 183 | ASSERT_EQ(p.subIdx, 1); 184 | 185 | EXPECT_EQ(p.copyFrom(&v, 1), Error::Success); 186 | EXPECT_EQ(p.reset(), Error::Success); 187 | EXPECT_EQ(p.copyInto(&v, 1), Error::Success); 188 | } 189 | 190 | { 191 | uint8_t v = 0; 192 | EXPECT_EQ(od.insert(0x2000, 2, canfetti::Access::WO, _u8(1)), Error::Success); 193 | auto [e, p] = od.makeProxy(0x2000, 2); 194 | ASSERT_EQ(e, Error::Success); 195 | 196 | EXPECT_EQ(p.copyFrom(&v, 1), Error::Success); 197 | EXPECT_EQ(p.reset(), Error::Success); 198 | EXPECT_EQ(p.copyInto(&v, 1), Error::ReadViolation); 199 | } 200 | } 201 | 202 | TEST(ObjDict, Callback) 203 | { 204 | ObjDict od; 205 | uint8_t subIdx = 0; 206 | 207 | // No callback 208 | { 209 | ::testing::MockFunction mcb; 210 | EXPECT_CALL(mcb, Call(0x2000, subIdx)).Times(0); 211 | EXPECT_EQ(od.insert(0x2000, subIdx, canfetti::Access::RO, _u8(1), mcb.AsStdFunction()), Error::Success); 212 | } 213 | 214 | // Callback on insert 215 | { 216 | subIdx++; 217 | ::testing::MockFunction mcb; 218 | EXPECT_CALL(mcb, Call(0x2000, subIdx)).Times(1); 219 | EXPECT_EQ(od.insert(0x2000, subIdx, canfetti::Access::RO, _u8(1), mcb.AsStdFunction(), true), Error::Success); 220 | } 221 | 222 | // Callback on set 223 | { 224 | subIdx++; 225 | ::testing::MockFunction mcb; 226 | EXPECT_CALL(mcb, Call(0x2000, subIdx)).Times(1); 227 | EXPECT_EQ(od.insert(0x2000, subIdx, canfetti::Access::RO, _u8(1), mcb.AsStdFunction()), Error::Success); 228 | od.set(0x2000, subIdx, _u8(4)); 229 | } 230 | 231 | // Callback on insert & set 232 | { 233 | subIdx++; 234 | ::testing::MockFunction mcb; 235 | EXPECT_CALL(mcb, Call(0x2000, subIdx)).Times(2); 236 | EXPECT_EQ(od.insert(0x2000, subIdx, canfetti::Access::RO, _u8(1), mcb.AsStdFunction(), true), Error::Success); 237 | od.set(0x2000, subIdx, _u8(4)); 238 | } 239 | } 240 | 241 | TEST(ObjDict, Generation) 242 | { 243 | ObjDict od; 244 | EXPECT_EQ(od.insert(0x2000, 0, canfetti::Access::RO, _u8(42)), Error::Success); 245 | unsigned gen; 246 | EXPECT_EQ(od.generation(0x2000, 0, gen), Error::Success); 247 | 248 | // Bump generation on set 249 | { 250 | unsigned g; 251 | EXPECT_EQ(od.set(0x2000, 0, _u8(43)), Error::Success); 252 | EXPECT_EQ(od.generation(0x2000, 0, g), Error::Success); 253 | EXPECT_NE(gen, g); 254 | gen = g; 255 | } 256 | 257 | // Bump generation on set even if value not changed 258 | { 259 | unsigned g; 260 | EXPECT_EQ(od.set(0x2000, 0, _u8(43)), Error::Success); 261 | EXPECT_EQ(od.generation(0x2000, 0, g), Error::Success); 262 | EXPECT_NE(gen, g); 263 | gen = g; 264 | } 265 | } 266 | -------------------------------------------------------------------------------- /src/platform/unittest/test.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/System.h" 2 | 3 | namespace canfetti { 4 | Logger Logger::logger; 5 | 6 | unsigned newGeneration() 7 | { 8 | static unsigned g = 0; 9 | return g++; 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /src/platform/unittest/test.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "canfetti/LocalNode.h" 8 | #include "canfetti/System.h" 9 | 10 | namespace canfetti { 11 | 12 | static constexpr auto toNumber(const Error& e) -> ::std::underlying_type::type 13 | { 14 | return static_cast<::std::underlying_type::type>(e); 15 | } 16 | 17 | static ::std::ostream& operator<<(::std::ostream& out, const Error& e) 18 | { 19 | return out << "0x" << std::hex << toNumber(e); 20 | } 21 | 22 | } // namespace canfetti -------------------------------------------------------------------------------- /src/services/Emcy.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/Emcy.h" 2 | #include 3 | 4 | using namespace canfetti; 5 | 6 | EmcyService::EmcyService(Node &co) : Service(co) 7 | { 8 | } 9 | 10 | uint8_t EmcyService::setErrorReg(ErrorType type) 11 | { 12 | uint8_t errorReg = 0; 13 | 14 | co.od.get(0x1001, 0, errorReg); 15 | errorReg |= type; 16 | co.od.set(0x1001, 0, errorReg); 17 | return errorReg; 18 | } 19 | 20 | uint8_t EmcyService::clearErrorReg(ErrorType type) 21 | { 22 | uint8_t errorReg = 0; 23 | 24 | co.od.get(0x1001, 0, errorReg); 25 | errorReg &= ~type; 26 | co.od.set(0x1001, 0, errorReg); 27 | return errorReg; 28 | } 29 | 30 | canfetti::Error EmcyService::sendEmcy(uint16_t error, uint32_t specific, ErrorType type) 31 | { 32 | std::array arr; 33 | memcpy(arr.begin(), &specific, 4); 34 | arr[4] = 0; 35 | 36 | return sendEmcy(error, arr, type); 37 | } 38 | 39 | canfetti::Error EmcyService::sendEmcy(uint16_t error, std::array &specific, ErrorType type) 40 | { 41 | uint8_t errorReg = setErrorReg(type); 42 | uint8_t payload[8] = { 43 | static_cast(error & 0xff), 44 | static_cast((error >> 8) & 0xff), 45 | errorReg, 46 | }; 47 | memcpy(&payload[3], specific.begin(), 5); 48 | 49 | errorHistory[error]++; 50 | return co.bus.write(0x080 | co.nodeId, payload, true); 51 | } 52 | 53 | canfetti::Error EmcyService::clearEmcy(uint16_t error, ErrorType type) 54 | { 55 | if (auto i = errorHistory.find(error); i != errorHistory.end()) { 56 | auto &[err, cnt] = *i; 57 | (void)err; // Silence unused variable warning 58 | 59 | if (--cnt == 0) { 60 | errorHistory.erase(i); 61 | uint16_t errorReg = clearErrorReg(type); 62 | 63 | if (errorReg == 0) { 64 | uint8_t payload[8] = {0}; 65 | return co.bus.write(0x080 | co.nodeId, payload, true); 66 | } 67 | } 68 | } 69 | 70 | return canfetti::Error::Success; 71 | } 72 | 73 | canfetti::Error EmcyService::registerCallback(EmcyCallback cb) 74 | { 75 | if (this->cb) return Error::Error; // Only allow 1 for now 76 | this->cb = cb; 77 | return Error::Success; 78 | } 79 | 80 | canfetti::Error EmcyService::processMsg(const canfetti::Msg &msg) 81 | { 82 | if (msg.len != 8 || msg.rtr) { 83 | LogInfo("Invalid Emcy received (len: %d, rtr: %d)", msg.len, msg.rtr); 84 | return canfetti::Error::Error; 85 | } 86 | 87 | uint16_t errCode = (msg.data[1] << 8) | msg.data[0]; 88 | //uint8_t errRegister = msg.data[2]; 89 | 90 | std::array specific; 91 | memcpy(specific.begin(), &msg.data[3], 5); 92 | 93 | if (cb) cb(msg.getNode(), errCode, specific); 94 | 95 | return canfetti::Error::Success; 96 | } 97 | -------------------------------------------------------------------------------- /src/services/Nmt.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/Nmt.h" 2 | 3 | using namespace canfetti; 4 | 5 | NmtService::NmtService(Node &co) : Service(co) 6 | { 7 | } 8 | 9 | canfetti::Error NmtService::addRemoteStateCb(uint8_t node, RemoteStateCb cb) 10 | { 11 | for (auto &&x : slaveStateCbs) { 12 | if (!x.cb) { 13 | x.cb = cb; 14 | x.node = node; 15 | return Error::Success; 16 | } 17 | } 18 | 19 | return Error::OutOfMemory; 20 | } 21 | 22 | void NmtService::notifyRemoteStateCbs(uint8_t node, canfetti::State state) 23 | { 24 | for (auto &&x : slaveStateCbs) { 25 | if (x.cb && (x.node == node || x.node == AllNodes)) { 26 | x.cb(node, state); 27 | } 28 | } 29 | } 30 | 31 | canfetti::Error NmtService::setHeartbeatPeriod(uint16_t periodMs) 32 | { 33 | Error e = co.od.set(0x1017, 0, periodMs); 34 | 35 | if (e == canfetti::Error::IndexNotFound) { 36 | e = co.od.insert( 37 | 0x1017, 0, Access::RW, periodMs, [&](uint16_t idx, uint8_t subIdx) { 38 | co.sys.deleteTimer(hbTimer); 39 | if (uint16_t newTime; co.od.get(idx, subIdx, newTime) == Error::Success) { 40 | LogInfo("Setting heartbeat period to %d ms", newTime); 41 | hbTimer = co.sys.schedulePeriodic(newTime, std::bind(&NmtService::sendHeartbeat, this)); 42 | } 43 | }, 44 | true); 45 | } 46 | 47 | return e; 48 | } 49 | 50 | canfetti::Error NmtService::setRemoteTimeout(uint8_t node, uint16_t timeoutMs) 51 | { 52 | uint32_t value = node << 16 | timeoutMs; 53 | 54 | Error e = co.od.set(0x1016, 0, value); 55 | 56 | if (e == canfetti::Error::IndexNotFound) { 57 | e = co.od.insert( 58 | 0x1016, 0, Access::RW, value, [&](uint16_t idx, uint8_t subIdx) { 59 | if (uint32_t v; co.od.get(idx, subIdx, v) == Error::Success) { 60 | uint8_t node = v >> 16; 61 | uint32_t time = v & 0xffff; 62 | LogInfo("Setting heartbeat timeout for node %x to %d ms", node, time); 63 | 64 | unsigned gen = newGeneration(); 65 | if (peerStates.find(node) == peerStates.end()) { 66 | peerStates[node].timer = System::InvalidTimer; 67 | peerStates[node].state = canfetti::State::Offline; 68 | peerStates[node].generation = gen; 69 | peerStates[node].timeoutMs = time; 70 | } 71 | else { 72 | co.sys.deleteTimer(peerStates[node].timer); 73 | peerStates[node].generation = gen; 74 | } 75 | 76 | if (time != 0) { 77 | peerStates[node].timer = co.sys.scheduleDelayed(time, std::bind(&NmtService::peerHeartbeatExpired, this, gen, node)); 78 | } 79 | } 80 | }, 81 | true); 82 | } 83 | 84 | return e; 85 | } 86 | 87 | void NmtService::resetNode() 88 | { 89 | } 90 | 91 | void NmtService::resetComms() 92 | { 93 | } 94 | 95 | canfetti::Error NmtService::setRemoteState(uint8_t node, canfetti::SlaveState state) 96 | { 97 | uint8_t d[] = {static_cast(state), node}; 98 | 99 | canfetti::Msg m = { 100 | .id = 0x000u, 101 | .rtr = false, 102 | .len = 2, 103 | .data = d, 104 | }; 105 | 106 | return co.bus.write(m); 107 | } 108 | 109 | std::tuple NmtService::getRemoteState(uint8_t node) 110 | { 111 | if (auto i = peerStates.find(node); i != peerStates.end()) { 112 | return std::make_tuple(Error::Success, i->second.state); 113 | } 114 | 115 | return std::make_tuple(Error::IndexNotFound, canfetti::State::Offline); 116 | } 117 | 118 | canfetti::Error NmtService::processMsg(const canfetti::Msg &msg) 119 | { 120 | if (msg.len != 2) { 121 | LogInfo("Invalid NMT received (len: %d)", msg.len); 122 | return canfetti::Error::Error; 123 | } 124 | 125 | uint8_t node = msg.data[1]; 126 | uint8_t raw = msg.data[0]; 127 | 128 | if (node && node != co.nodeId) { 129 | return canfetti::Error::Success; 130 | } 131 | 132 | if (raw == canfetti::SlaveState::GoOperational) { 133 | co.setState(canfetti::State::Operational); 134 | } 135 | else if (raw == canfetti::SlaveState::GoPreOperational) { 136 | co.setState(canfetti::State::PreOperational); 137 | } 138 | else if (raw == canfetti::SlaveState::Stop) { 139 | co.setState(canfetti::State::Stopped); 140 | } 141 | else if (raw == canfetti::SlaveState::ResetComms) { 142 | // Todo 143 | co.setState(canfetti::State::PreOperational); 144 | } 145 | else if (raw == canfetti::SlaveState::ResetNode) { 146 | // Todo 147 | co.setState(canfetti::State::PreOperational); 148 | } 149 | else { 150 | LogInfo("Invalid nmt state received: %x", raw); 151 | return canfetti::Error::Error; 152 | } 153 | 154 | return canfetti::Error::Success; 155 | } 156 | 157 | void NmtService::peerHeartbeatExpired(unsigned generation, uint8_t node) 158 | { 159 | if (auto i = peerStates.find(node); i != peerStates.end()) { 160 | // Was the timer invalidated before the callback fired? 161 | if (i->second.generation != generation) return; 162 | 163 | i->second.state = canfetti::State::Offline; 164 | co.sys.deleteTimer(i->second.timer); 165 | i->second.generation = newGeneration(); 166 | notifyRemoteStateCbs(node, canfetti::State::Offline); 167 | } 168 | } 169 | 170 | canfetti::Error NmtService::sendHeartbeat() 171 | { 172 | uint8_t s = co.getState(); 173 | 174 | canfetti::Msg m = { 175 | .id = 0x700u + co.nodeId, 176 | .rtr = false, 177 | .len = 1, 178 | .data = &s, 179 | }; 180 | 181 | return co.bus.write(m); 182 | } 183 | 184 | canfetti::Error NmtService::processHeartbeat(const canfetti::Msg &msg) 185 | { 186 | if (msg.len != 1) { 187 | LogInfo("Invalid heartbeat received (len: %d)", msg.len); 188 | return canfetti::Error::Error; 189 | } 190 | 191 | uint8_t raw = msg.data[0]; 192 | canfetti::State s; 193 | 194 | if (raw == canfetti::State::Bootup) { 195 | s = canfetti::State::Bootup; 196 | } 197 | else if (raw == canfetti::State::Operational) { 198 | s = canfetti::State::Operational; 199 | } 200 | else if (raw == canfetti::State::PreOperational) { 201 | s = canfetti::State::PreOperational; 202 | } 203 | else if (raw == canfetti::State::Stopped) { 204 | s = canfetti::State::Stopped; 205 | } 206 | else { 207 | LogInfo("Invalid nmt state received: %x", raw); 208 | return canfetti::Error::Error; 209 | } 210 | 211 | uint8_t node = msg.getNode(); 212 | 213 | if (auto i = peerStates.find(node); i != peerStates.end()) { 214 | i->second.generation = newGeneration(); 215 | co.sys.deleteTimer(i->second.timer); 216 | if (i->second.timeoutMs != 0) { 217 | i->second.timer = co.sys.scheduleDelayed(i->second.timeoutMs, std::bind(&NmtService::peerHeartbeatExpired, this, i->second.generation, node)); 218 | } 219 | 220 | if (i->second.state != s) { 221 | i->second.state = s; 222 | notifyRemoteStateCbs(node, s); 223 | } 224 | } 225 | else { 226 | // A new node on the network that we dont have a heartbeat timeout for. Track it 227 | peerStates[node].state = s; 228 | peerStates[node].timer = System::InvalidTimer; 229 | peerStates[node].timeoutMs = 0; 230 | peerStates[node].generation = newGeneration(); 231 | notifyRemoteStateCbs(node, s); 232 | } 233 | 234 | return canfetti::Error::Success; 235 | } 236 | -------------------------------------------------------------------------------- /src/services/Pdo.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/Pdo.h" 2 | #include 3 | 4 | using namespace canfetti; 5 | 6 | static constexpr size_t MAX_MAPPINGS = 8; // 1 per payload byte in classic CAN 7 | 8 | //****************************************************************************** 9 | // Private Helpers 10 | //****************************************************************************** 11 | 12 | static inline uint32_t canIdMask(uint32_t cobid) 13 | { 14 | return cobid & ((1 << 29) - 1); 15 | } 16 | 17 | static inline bool isDisabled(uint32_t cobid) 18 | { 19 | return !!(cobid & (1 << 31)); 20 | } 21 | 22 | static inline bool isRtrAllowed(uint32_t cobid) 23 | { 24 | return !(cobid & (1 << 30)); 25 | } 26 | 27 | static bool isEventDriven(ObjDict &od, uint16_t paramIdx) 28 | { 29 | uint8_t transmissionType = 0; 30 | 31 | if (od.get(paramIdx, 2, transmissionType) != canfetti::Error::Success) { 32 | assert(0 && "OD not configured properly"); 33 | } 34 | 35 | // Event-driven means that the PDO may be received at any time. 36 | // The CANopen device will actualize the data immediately. 37 | 38 | // 0xFE - event-driven (manufacturer-specific) 39 | // 0xFF - event-driven (device-profile and application profile specific) 40 | return transmissionType == 0xFE || transmissionType == 0xFF; 41 | } 42 | 43 | //****************************************************************************** 44 | // Public API 45 | //****************************************************************************** 46 | 47 | PdoService::PdoService(Node &co) : Service(co) 48 | { 49 | } 50 | 51 | Error PdoService::addPdoEntry(uint16_t paramIdx, uint32_t cobid, uint16_t eventTime, 52 | const std::tuple *mapping, size_t numMapping, bool enabled, bool rtrAllowed, canfetti::ChangedCallback changedCb) 53 | { 54 | if (co.od.entryExists(paramIdx, 0) || numMapping > 0xFFu) { 55 | return Error::Error; 56 | } 57 | 58 | LogDebug("Adding PDO %x (Cobid: %x, Timer: %d, En: %d, RTR: %d)", paramIdx, cobid, eventTime, enabled, rtrAllowed); 59 | 60 | cobid |= ((!enabled) << 31) | ((!rtrAllowed) << 30); 61 | 62 | uint16_t mappingIdx = paramIdx + 0x200; 63 | 64 | // Create Params entry 65 | co.od.insert(paramIdx, 0, canfetti::Access::RO, _u8(5)); 66 | co.od.insert(paramIdx, 1, canfetti::Access::RO, cobid, changedCb); 67 | co.od.insert(paramIdx, 2, canfetti::Access::RO, _u8(0xFE)); // Transmission type 68 | co.od.insert(paramIdx, 3, canfetti::Access::RO, _u16(0)); // Inhibit time 69 | co.od.insert(paramIdx, 4, canfetti::Access::RO, _u8(0)); // unused 70 | co.od.insert(paramIdx, 5, canfetti::Access::RW, eventTime, changedCb); // Event timer 71 | 72 | // Create Mapping entry 73 | size_t subIdx = 0; 74 | co.od.insert(mappingIdx, subIdx++, canfetti::Access::RO, _u8(numMapping)); 75 | 76 | for (size_t i = 0; i < numMapping; ++i) { 77 | uint16_t refIdx = std::get<0>(mapping[i]); 78 | uint8_t refSubIdx = std::get<1>(mapping[i]); 79 | 80 | assert(co.od.entryExists(refIdx, refSubIdx) && "Variable for PDO doesnt exist"); 81 | 82 | uint32_t val = (refIdx << 16) | (refSubIdx << 8) | ((co.od.entrySize(refIdx, refSubIdx) * 8) & 0xff); 83 | co.od.insert(mappingIdx, subIdx++, canfetti::Access::RO, val); 84 | } 85 | 86 | return Error::Success; 87 | } 88 | 89 | void PdoService::enableTpdoEvent(uint16_t tpdoIdx) 90 | { 91 | if (!pdoEnabled) return; 92 | 93 | if (uint32_t cobid; co.od.get(tpdoIdx, 1, cobid) == canfetti::Error::Success) { 94 | if (uint16_t period; co.od.get(tpdoIdx, 5, period) == canfetti::Error::Success) { 95 | // sync PDOs not currently supported 96 | if (isEventDriven(co.od, tpdoIdx) && !isDisabled(cobid) && period) { 97 | uint16_t busCobid = canIdMask(cobid); 98 | // Async because nothing can observe the return value 99 | auto hdl = co.sys.schedulePeriodic(period, std::bind(&PdoService::sendTxPdo, this, tpdoIdx, /* async */ true, /* rtr */ false)); 100 | auto t = tpdoTimers.find(busCobid); 101 | 102 | // Update existing timer 103 | if (t != tpdoTimers.end()) { 104 | co.sys.deleteTimer(t->second); 105 | t->second = std::move(hdl); 106 | LogInfo("Updated event-driven TPDO %x @ %d ms", tpdoIdx, period); 107 | } 108 | // Create new timer 109 | else { 110 | tpdoTimers.emplace(std::make_pair(busCobid, std::move(hdl))); 111 | LogDebug("Created event-driven TPDO %x @ %d ms", tpdoIdx, period); 112 | } 113 | } 114 | } 115 | } 116 | } 117 | 118 | void PdoService::enableRpdoEvent(uint16_t rpdoIdx) 119 | { 120 | if (!pdoEnabled) return; 121 | 122 | if (uint32_t cobid; co.od.get(rpdoIdx, 1, cobid) == canfetti::Error::Success) { 123 | if (uint16_t period; co.od.get(rpdoIdx, 5, period) == canfetti::Error::Success) { 124 | // sync PDOs not currently supported 125 | if (isEventDriven(co.od, rpdoIdx) && !isDisabled(cobid) && period) { 126 | uint16_t busCobid = canIdMask(cobid); 127 | auto t = rpdoTimers.find(busCobid); 128 | 129 | if (t != rpdoTimers.end()) { 130 | auto &[timer, gen, storedPeriod, cb] = t->second; 131 | gen = newGeneration(); 132 | storedPeriod = period; 133 | 134 | if (timer != System::InvalidTimer) { 135 | co.sys.deleteTimer(timer); 136 | timer = System::InvalidTimer; 137 | } 138 | 139 | if (cb) { 140 | timer = co.sys.scheduleDelayed(period, std::bind(&PdoService::rpdoTimeout, this, gen, busCobid)); 141 | } 142 | 143 | LogInfo("Updated event-driven RPDO %x @ %d ms", rpdoIdx, period); 144 | } 145 | } 146 | } 147 | } 148 | } 149 | 150 | void PdoService::rpdoTimeout(unsigned generation, uint16_t idx) 151 | { 152 | if (auto r = rpdoTimers.find(idx); r != rpdoTimers.end()) { 153 | auto &[timer, gen, periodMs, cb] = r->second; 154 | (void)timer; // Silence unused variable warning 155 | (void)periodMs; 156 | 157 | // Was the timer invalidated before the callback fired? 158 | if (generation != gen) return; 159 | 160 | if (cb) { 161 | cb(idx); 162 | } 163 | } 164 | } 165 | 166 | Error PdoService::addRPDO(uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t timeoutMs, PdoService::TimeoutCb cb) 167 | { 168 | const uint16_t MaxPdoEntries = 512; 169 | const uint16_t StartRpdoParamIdx = 0x1400; 170 | 171 | uint16_t i = 0; 172 | while (co.od.entryExists(StartRpdoParamIdx + i, 0) && i < MaxPdoEntries) { 173 | i++; 174 | } 175 | 176 | auto err = addPdoEntry(StartRpdoParamIdx + i, cobid, timeoutMs, mapping, numMapping, true, false, std::bind(&PdoService::enableRpdoEvent, this, std::placeholders::_1)); 177 | 178 | if (err == Error::Success && cb) { 179 | assert(rpdoTimers.find(cobid) == rpdoTimers.end() && "Multiple RPDO timers arent supported"); 180 | rpdoTimers.emplace(std::make_pair(cobid, std::make_tuple(System::InvalidTimer, newGeneration(), timeoutMs, cb))); 181 | enableRpdoEvent(StartRpdoParamIdx + i); 182 | } 183 | 184 | return err; 185 | } 186 | 187 | Error PdoService::addTPDO(uint16_t pdoNum, uint16_t cobid, const std::tuple *mapping, size_t numMapping, uint16_t periodMs, bool enabled) 188 | { 189 | uint16_t paramIdx = 0x1800 + pdoNum; 190 | 191 | auto err = addPdoEntry(paramIdx, cobid, periodMs, mapping, numMapping, enabled, true, std::bind(&PdoService::enableTpdoEvent, this, std::placeholders::_1)); 192 | if (err == Error::Success) { 193 | configuredTPDONums.push_back(pdoNum); 194 | enableTpdoEvent(paramIdx); 195 | } 196 | 197 | return err; 198 | } 199 | 200 | Error PdoService::updateTpdoEventTime(uint16_t paramIdx, uint16_t periodMs) 201 | { 202 | return co.od.set(paramIdx, 5, periodMs); 203 | } 204 | 205 | Error PdoService::disablePdo(uint16_t paramIdx) 206 | { 207 | uint32_t cobid; 208 | 209 | if (co.od.get(paramIdx, 1, cobid) != canfetti::Error::Success) { 210 | LogDebug("Invalid PDO idx %x", paramIdx); 211 | return canfetti::Error::IndexNotFound; 212 | } 213 | 214 | LogInfo("Disabling TPDO %x, cobid: %x", paramIdx, cobid); 215 | cobid |= 1 << 31; 216 | 217 | return co.od.set(paramIdx, 1, cobid); 218 | } 219 | 220 | Error PdoService::enablePdo(uint16_t paramIdx) 221 | { 222 | uint32_t cobid; 223 | 224 | if (co.od.get(paramIdx, 1, cobid) != canfetti::Error::Success) { 225 | LogDebug("Invalid PDO idx %x", paramIdx); 226 | return canfetti::Error::IndexNotFound; 227 | } 228 | 229 | LogInfo("Enabling TPDO %x, cobid: %x", paramIdx, cobid); 230 | cobid &= ~(1 << 31); 231 | 232 | return co.od.set(paramIdx, 1, cobid); 233 | } 234 | 235 | canfetti::Error PdoService::sendTxPdo(uint16_t paramIdx, bool async, bool rtr) 236 | { 237 | constexpr uint16_t mappingOffset = 0x200; 238 | uint16_t mappingIdx = paramIdx + mappingOffset; 239 | uint8_t numMappings; 240 | uint32_t cobid; 241 | uint8_t d[8]; 242 | 243 | if (co.od.get(paramIdx, 1, cobid) != canfetti::Error::Success) { 244 | return canfetti::Error::IndexNotFound; 245 | } 246 | 247 | if (isDisabled(cobid)) { 248 | return canfetti::Error::DataXfer; 249 | } 250 | 251 | canfetti::Msg msg = {.id = canIdMask(cobid), .rtr = rtr, .len = 0, .data = d}; 252 | 253 | if (co.od.get(mappingIdx, 0, numMappings) != canfetti::Error::Success) { 254 | return canfetti::Error::IndexNotFound; 255 | } 256 | 257 | if (numMappings > MAX_MAPPINGS) { 258 | return canfetti::Error::InternalError; 259 | } 260 | 261 | { 262 | std::optional proxies[MAX_MAPPINGS]; 263 | 264 | // Create all proxies up front so we don't partially fill the payload if some entries are locked 265 | for (size_t i = 0; i < numMappings; ++i) { 266 | uint32_t map; 267 | if (Error e = co.od.get(mappingIdx, i + 1, map); e != canfetti::Error::Success) { 268 | LogInfo("Failed to look up TPDO mapping at %x[%zx] for TPDO 0x%03x: error 0x%08x", mappingIdx, i + 1, msg.id, e); 269 | return e; 270 | } 271 | uint16_t valIdx = (map >> 16) & 0xffff; 272 | uint8_t valSubIdx = (map >> 8) & 0xff; 273 | if (auto [e, p] = co.od.makeProxy(valIdx, valSubIdx); e == Error::Success) { 274 | proxies[i].emplace(std::move(p)); 275 | } 276 | else { 277 | LogInfo("Failed to make proxy at %x[%x] for TPDO 0x%03x: error 0x%08x", valIdx, valSubIdx, msg.id, e); 278 | return e; 279 | } 280 | } 281 | 282 | uint8_t *pdoData = msg.data; 283 | for (size_t i = 0; i < numMappings; ++i) { 284 | uint8_t maxPdoRemaining = 8 - (pdoData - msg.data); 285 | auto len = proxies[i]->remaining(); 286 | assert(len <= maxPdoRemaining); // Catch programming errors. PDOs can only have 8 bytes 287 | (void)maxPdoRemaining; 288 | if (Error e = proxies[i]->copyInto(pdoData, len); e != Error::Success) { 289 | LogInfo("Failed to read OD at %x[%x] for TPDO 0x%03x: error 0x%08x", proxies[i]->idx, proxies[i]->subIdx, msg.id, e); 290 | return e; 291 | } 292 | pdoData += len; 293 | msg.len += len; 294 | } 295 | } 296 | 297 | return co.bus.write(msg, async); 298 | } 299 | 300 | canfetti::Error PdoService::enablePdoEvents() 301 | { 302 | uint32_t cfgCobid; 303 | disablePdoEvents(); 304 | 305 | pdoEnabled = true; 306 | 307 | // TPDO timers 308 | for (auto pdoNum : configuredTPDONums) { 309 | uint16_t paramIdx = 0x1800 + pdoNum; 310 | enableTpdoEvent(paramIdx); 311 | } 312 | 313 | // RPDO timers 314 | for (uint16_t paramIdx = 0x1400; co.od.get(paramIdx, 1, cfgCobid) == canfetti::Error::Success; paramIdx++) { 315 | enableRpdoEvent(paramIdx); 316 | } 317 | 318 | return canfetti::Error::Success; 319 | } 320 | 321 | canfetti::Error PdoService::disablePdoEvents() 322 | { 323 | pdoEnabled = false; 324 | 325 | LogInfo("Disabling all TPDOs"); 326 | 327 | for (auto t : tpdoTimers) { 328 | co.sys.deleteTimer(t.second); 329 | } 330 | tpdoTimers.clear(); 331 | 332 | for (auto &r : rpdoTimers) { 333 | auto &[timer, gen, periodMs, cb] = r.second; 334 | (void)periodMs; // Silence unused variable warning 335 | (void)cb; 336 | gen = newGeneration(); 337 | co.sys.deleteTimer(timer); 338 | } 339 | 340 | return canfetti::Error::Success; 341 | } 342 | 343 | Error PdoService::requestTxPdo(uint16_t cobid) 344 | { 345 | canfetti::Msg msg = {.id = cobid, .rtr = true, .len = 0, .data = nullptr}; 346 | return co.bus.write(msg); 347 | } 348 | 349 | Error PdoService::sendAllTpdos() 350 | { 351 | uint32_t cfgCobid; 352 | for (auto pdoNum : configuredTPDONums) { 353 | uint16_t tpdoParamIdx = 0x1800 + pdoNum; 354 | if (co.od.get(tpdoParamIdx, 1, cfgCobid) != canfetti::Error::Success) continue; 355 | 356 | if (isDisabled(cfgCobid)) { 357 | continue; 358 | } 359 | 360 | if (Error e = sendTxPdo(tpdoParamIdx); e != Error::Success) { 361 | return e; 362 | } 363 | } 364 | 365 | return Error::Success; 366 | } 367 | 368 | Error PdoService::processMsg(const canfetti::Msg &msg) 369 | { 370 | constexpr uint16_t mappingOffset = 0x200; 371 | uint32_t cfgCobid; 372 | 373 | if (co.getState() != canfetti::State::Operational) return canfetti::Error::Success; 374 | 375 | if (msg.rtr) { 376 | for (auto pdoNum : configuredTPDONums) { 377 | uint16_t tpdoParamIdx = 0x1800 + pdoNum; 378 | 379 | if (co.od.get(tpdoParamIdx, 1, cfgCobid) != canfetti::Error::Success) continue; 380 | if (canIdMask(cfgCobid) != msg.id) continue; 381 | 382 | if (isDisabled(cfgCobid)) { 383 | LogInfo("Ignoring RTR on TPDO %x because it is disabled", canIdMask(cfgCobid)); 384 | break; 385 | } 386 | else if (!isRtrAllowed(cfgCobid)) { 387 | LogInfo("Ignoring RTR on TPDO %x because RTR is not allowed", canIdMask(cfgCobid)); 388 | break; 389 | } 390 | 391 | return sendTxPdo(tpdoParamIdx, /* async */ false, /* rtr */ true); 392 | } 393 | } 394 | else { 395 | for (uint16_t rpdoParamIdx = 0x1400; co.od.get(rpdoParamIdx, 1, cfgCobid) == canfetti::Error::Success; rpdoParamIdx++) { 396 | uint16_t busCobid = canIdMask(cfgCobid); 397 | if (busCobid != msg.id) { 398 | continue; 399 | } 400 | 401 | if (isDisabled(cfgCobid)) { 402 | LogDebug("Ignoring RPDO because it is disabled"); 403 | continue; 404 | } 405 | 406 | uint8_t numMappings = 0; 407 | uint16_t rpdoMappingIdx = rpdoParamIdx + mappingOffset; 408 | 409 | if (co.od.get(rpdoMappingIdx, 0, numMappings) != canfetti::Error::Success) { 410 | assert(0 && "OD not configured properly"); 411 | continue; 412 | } 413 | 414 | // LogInfo("RPDO %x", msg.id); 415 | 416 | if (!isEventDriven(co.od, rpdoParamIdx)) { 417 | LogInfo("Only event driven PDO are supported"); 418 | continue; 419 | } 420 | 421 | if (numMappings > MAX_MAPPINGS) { 422 | LogInfo("Too many mappings"); 423 | continue; 424 | } 425 | 426 | std::tuple entries[MAX_MAPPINGS]; 427 | 428 | { 429 | std::optional proxies[MAX_MAPPINGS]; 430 | 431 | // Create all proxies up front so we don't partially apply the payload if some entries are locked 432 | for (size_t i = 0; i < numMappings; ++i) { 433 | uint32_t map; 434 | if (Error e = co.od.get(rpdoMappingIdx, i + 1, map); e != Error::Success) { 435 | LogInfo("Failed to look up RPDO mapping at %x[%zx] for RPDO 0x%03x: error 0x%08x", rpdoMappingIdx, i + 1, busCobid, e); 436 | return e; 437 | } 438 | uint16_t valIdx = (map >> 16) & 0xffff; 439 | uint8_t valSubIdx = (map >> 8) & 0xff; 440 | entries[i] = {valIdx, valSubIdx}; 441 | if (auto [e, p] = co.od.makeProxy(valIdx, valSubIdx); e == Error::Success) { 442 | proxies[i].emplace(std::move(p)); 443 | } 444 | else { 445 | LogInfo("Failed to make proxy at %x[%x] for RPDO 0x%03x: error 0x%08x", valIdx, valSubIdx, busCobid, e); 446 | return e; 447 | } 448 | } 449 | 450 | uint8_t *pdoData = msg.data; 451 | for (size_t i = 0; i < numMappings; ++i) { 452 | assert(pdoData - msg.data < 8); // Catch programming errors. PDOs can only have 8 bytes 453 | proxies[i]->suppressCallbacks(); // Defer until mapped entries are unlocked; allow callbacks to access them 454 | auto len = proxies[i]->remaining(); 455 | if (Error e = proxies[i]->copyFrom(pdoData, len); e != Error::Success) { 456 | LogInfo("Failed to write OD at %x[%x] for RPDO 0x%03x: error 0x%08x", proxies[i]->idx, proxies[i]->subIdx, busCobid, e); 457 | return e; 458 | } 459 | pdoData += len; 460 | } 461 | } 462 | 463 | // Reset timer if active 464 | if (auto r = rpdoTimers.find(busCobid); r != rpdoTimers.end()) { 465 | auto &[timer, gen, periodMs, cb] = r->second; 466 | (void)cb; // Silence unused variable warning 467 | gen = newGeneration(); 468 | if (timer != System::InvalidTimer) { 469 | co.sys.deleteTimer(timer); 470 | timer = co.sys.scheduleDelayed(periodMs, std::bind(&PdoService::rpdoTimeout, this, gen, busCobid)); 471 | } 472 | } 473 | 474 | // Fire callbacks after timer reset in case they mess with it 475 | for (size_t i = 0; i < numMappings; ++i) { 476 | auto [idx, subIdx] = entries[i]; 477 | if (Error e = co.od.fireCallbacks(idx, subIdx); e != Error::Success) { 478 | LogInfo("Failed to fire callbacks for %x[%x]: error 0x%08x", idx, subIdx, e); 479 | return e; 480 | } 481 | } 482 | } 483 | } 484 | 485 | return canfetti::Error::Success; 486 | } 487 | -------------------------------------------------------------------------------- /src/services/Sdo.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/Sdo.h" 2 | 3 | using namespace canfetti; 4 | using namespace Sdo; 5 | 6 | SdoService::SdoService(Node &co) : Service(co) 7 | { 8 | } 9 | 10 | Error SdoService::init() 11 | { 12 | if (Error e = Service::init(); e != Error::Success) { 13 | return e; 14 | } 15 | 16 | serverSegmentTimeoutMs = DefaultSegmentXferTimeoutMs; 17 | 18 | // Mandatory default SDO server 19 | return addSDOServer(0x600 + co.nodeId, 0x580 + co.nodeId, 0); 20 | } 21 | 22 | Error SdoService::syncServices() 23 | { 24 | uint8_t node; 25 | servers.clear(); 26 | 27 | for (uint16_t clientIdx = 0x1200; co.od.get(clientIdx, 3, node) == Error::Success; clientIdx++) { 28 | uint16_t clientToServer, serverToClient; 29 | if (co.od.get(clientIdx, 1, clientToServer) != Error::Success || co.od.get(clientIdx, 2, serverToClient) != Error::Success) { 30 | LogInfo("Malformed OD: %x", clientIdx); 31 | break; 32 | } 33 | 34 | servers.emplace(clientToServer, std::make_tuple(serverToClient, node)); 35 | } 36 | 37 | return Error::Success; 38 | } 39 | 40 | Error SdoService::addSdoEntry(uint16_t paramIdx, uint16_t clientToServer, uint16_t serverToClient, uint8_t node) 41 | { 42 | while (co.od.entryExists(paramIdx, 0)) { 43 | paramIdx++; 44 | } 45 | 46 | LogInfo("Adding SDO %x, [c: %x, s: %x, n: %x]", paramIdx, clientToServer, serverToClient, node); 47 | 48 | // Create Params entry 49 | co.od.insert(paramIdx, 0, Access::RO, _u8(3)); 50 | co.od.insert(paramIdx, 1, Access::RO, clientToServer); 51 | co.od.insert(paramIdx, 2, Access::RO, serverToClient); 52 | co.od.insert(paramIdx, 3, Access::RO, node); 53 | 54 | return Error::Success; 55 | } 56 | 57 | Error SdoService::addSDOServer(uint16_t rxCobid, uint16_t txCobid, uint8_t clientId) 58 | { 59 | Error err = addSdoEntry(0x1200, rxCobid, txCobid, clientId); 60 | 61 | if (err == Error::Success) { 62 | err = syncServices(); 63 | } 64 | 65 | return err; 66 | } 67 | 68 | Error SdoService::addSDOClient(uint32_t txCobid, uint16_t rxCobid, uint8_t serverId) 69 | { 70 | Error err = addSdoEntry(0x1280, txCobid, rxCobid, serverId); 71 | 72 | if (err == Error::Success) { 73 | err = syncServices(); 74 | } 75 | 76 | return err; 77 | } 78 | 79 | Error SdoService::clientTransaction(bool read, uint8_t remoteNode, uint16_t idx, uint8_t subIdx, 80 | OdVariant &data, uint32_t segmentTimeout, FinishCallback cb) 81 | { 82 | uint8_t node; 83 | 84 | for (uint16_t clientIdx = 0x1280; co.od.get(clientIdx, 3, node) == Error::Success; clientIdx++) { 85 | if (node != remoteNode) { 86 | continue; 87 | } 88 | 89 | uint16_t clientToServer, serverToClient; 90 | if (co.od.get(clientIdx, 1, clientToServer) != Error::Success || co.od.get(clientIdx, 2, serverToClient) != Error::Success) { 91 | LogInfo("Malformed OD: %x", clientIdx); 92 | break; 93 | } 94 | 95 | if (auto i = activeTransactions.find(serverToClient); i != activeTransactions.end()) { 96 | LogInfo("Transaction already in progress for node: %d", node); 97 | return Error::Error; 98 | } 99 | 100 | auto [err, client] = read ? Client::initiateRead(idx, subIdx, data, clientToServer, co) : Client::initiateWrite(idx, subIdx, data, clientToServer, co); 101 | 102 | if (client) { 103 | unsigned gen = newGeneration(); 104 | TransactionState state = { 105 | .protocol = client, 106 | .timer = co.sys.scheduleDelayed(segmentTimeout, std::bind(&SdoService::transactionTimeout, this, gen, serverToClient)), 107 | .generation = gen, 108 | .cb = cb, 109 | }; 110 | auto [i, success] = activeTransactions.emplace(serverToClient, state); 111 | (void)i; // Silence unused variable warning 112 | if (!success) err = Error::Error; 113 | } 114 | 115 | return err; 116 | } 117 | 118 | LogInfo("No SDO client found for node: %d", remoteNode); 119 | return Error::Error; 120 | } 121 | 122 | void SdoService::transactionTimeout(unsigned generation, uint16_t key) 123 | { 124 | if (auto i = activeTransactions.find(key); i != activeTransactions.end()) { 125 | auto &state = i->second; 126 | // Was the timer invalidated before the callback fired? 127 | if (state.generation != generation) return; 128 | 129 | state.protocol->finish(Error::Timeout, true); 130 | removeTransaction(key); 131 | } 132 | } 133 | 134 | void SdoService::removeTransaction(uint16_t key) 135 | { 136 | if (auto i = activeTransactions.find(key); i != activeTransactions.end()) { 137 | auto &state = i->second; 138 | auto cb = state.cb; 139 | auto [finished, err] = state.protocol->isFinished(); 140 | 141 | if (!finished) { 142 | LogInfo("*** Removing a transaction that wasn't finished?? ***"); 143 | } 144 | 145 | co.sys.deleteTimer(state.timer); 146 | activeTransactions.erase(i); 147 | 148 | if (cb) cb(finished ? err : Error::InternalError); 149 | } 150 | } 151 | 152 | Error SdoService::processMsg(const Msg &msg) 153 | { 154 | if (msg.len != 8) { 155 | LogInfo("SDO not 8 bytes"); 156 | return Error::ParamLength; 157 | } 158 | 159 | if (auto &&c = activeTransactions.find(msg.id); c != activeTransactions.end()) { 160 | if (c->second.protocol->processMsg(msg)) { 161 | removeTransaction(msg.id); 162 | } 163 | else { 164 | c->second.generation = newGeneration(); 165 | co.sys.deleteTimer(c->second.timer); 166 | c->second.timer = co.sys.scheduleDelayed(serverSegmentTimeoutMs, std::bind(&SdoService::transactionTimeout, this, c->second.generation, msg.id)); 167 | } 168 | } 169 | else if (auto &&s = servers.find(msg.id); s != servers.end()) { 170 | auto [tx, node] = s->second; 171 | (void)node; // Silence unused variable warning 172 | auto server = Server::processInitiate(msg, tx, co); 173 | 174 | if (server) { 175 | unsigned gen = newGeneration(); 176 | TransactionState state = { 177 | .protocol = server, 178 | .timer = co.sys.scheduleDelayed(serverSegmentTimeoutMs, std::bind(&SdoService::transactionTimeout, this, gen, msg.id)), 179 | .generation = gen, 180 | .cb = nullptr, 181 | }; 182 | auto [i, success] = activeTransactions.emplace(msg.id, state); 183 | (void)i; // Silence unused variable warning 184 | if (!success) { 185 | return Error::Error; 186 | } 187 | } 188 | } 189 | 190 | return Error::Success; 191 | } 192 | 193 | size_t SdoService::getActiveTransactionCount() 194 | { 195 | return activeTransactions.size(); 196 | } 197 | -------------------------------------------------------------------------------- /src/services/sdo/Client.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/sdo/Client.h" 2 | #include 3 | 4 | using namespace canfetti::Sdo; 5 | 6 | static inline bool isExpedited(const canfetti::Msg &m) { return (m.data[0] >> 1) & 0b1; } 7 | static inline bool isUploadResponse(const canfetti::Msg &m) { return (m.data[0] >> 5) == 2; } 8 | static inline bool isUploadSegResponse(const canfetti::Msg &m) { return (m.data[0] >> 5) == 0; } 9 | static inline bool isDownloadResponse(const canfetti::Msg &m) { return (m.data[0] >> 5) == 3; } 10 | static inline bool isDownloadSegResponse(const canfetti::Msg &m) { return (m.data[0] >> 5) == 1; } 11 | static inline bool isDownloadBlockResponse(const canfetti::Msg &m) { return (m.data[0] >> 5) == 5; } 12 | 13 | std::tuple> Client::initiateRead(uint16_t idx, uint8_t subIdx, 14 | OdVariant &data, uint16_t txCobid, Node &co) 15 | { 16 | LogDebug("Initiating read to cobid %x: %x[%d]", txCobid, idx, subIdx); 17 | 18 | OdProxy proxy(idx, subIdx, data); 19 | 20 | uint8_t payload[8] = { 21 | static_cast(2 << 5), 22 | static_cast(proxy.idx & 0xFF), 23 | static_cast(proxy.idx >> 8), 24 | proxy.subIdx, 25 | 0, 0, 0, 0}; 26 | 27 | auto err = co.bus.write(txCobid, payload); 28 | auto ptr = err == Error::Success ? std::make_shared(txCobid, std::move(proxy), co) : nullptr; 29 | return std::make_tuple(err, ptr); 30 | } 31 | 32 | std::tuple> Client::initiateWrite(uint16_t idx, uint8_t subIdx, 33 | OdVariant &data, uint16_t txCobid, Node &co) 34 | { 35 | OdProxy proxy(idx, subIdx, data); 36 | 37 | if (proxy.remaining() < BlockModeThreshold) { 38 | LogDebug("Initiating write to cobid %x: %x[%d]", txCobid, proxy.idx, proxy.subIdx); 39 | 40 | uint8_t payload[8] = { 41 | static_cast(1 << 5), 42 | static_cast(proxy.idx & 0xFF), 43 | static_cast(proxy.idx >> 8), 44 | proxy.subIdx, 45 | 0, 0, 0, 0}; 46 | 47 | if (auto len = proxy.remaining(); len > 0 && len <= 4) { // Expedited transfer 48 | payload[0] |= 0b11; // expedited, size indicated 49 | payload[0] |= (4 - len) << 2; // size = 4-n 50 | 51 | if (canfetti::Error e = proxy.copyInto(&payload[4], proxy.remaining()); e != canfetti::Error::Success) { 52 | return std::make_tuple(e, nullptr); 53 | } 54 | } 55 | else { // Segmented transfer 56 | payload[0] |= 0b01; // !expedited, size indicated 57 | uint32_t l = proxy.remaining(); 58 | memcpy(&payload[4], &l, sizeof(l)); 59 | } 60 | 61 | auto err = co.bus.write(txCobid, payload); 62 | auto ptr = err == Error::Success ? std::make_shared(txCobid, std::move(proxy), co) : nullptr; 63 | return std::make_tuple(err, ptr); 64 | } 65 | else { 66 | LogDebug("Initiating block write to cobid %x: %x[%d]", txCobid, proxy.idx, proxy.subIdx); 67 | 68 | uint8_t payload[8] = { 69 | 6 << 5 | 1 << 1, // Size indicated, no crc, initiate download request 70 | static_cast(proxy.idx & 0xFF), 71 | static_cast(proxy.idx >> 8), 72 | proxy.subIdx}; 73 | 74 | uint32_t l = proxy.remaining(); 75 | memcpy(&payload[4], &l, sizeof(l)); 76 | 77 | auto err = co.bus.write(txCobid, payload); 78 | auto ptr = err == Error::Success ? std::make_shared(txCobid, std::move(proxy), co) : nullptr; 79 | return std::make_tuple(err, ptr); 80 | } 81 | } 82 | 83 | void Client::blockSegmentWrite(uint8_t seqno) 84 | { 85 | uint8_t payload[8] = {0}; 86 | uint32_t len = proxy.remaining(); 87 | bool complete = len <= 7; 88 | lastBlockBytes = complete ? len : 7; 89 | 90 | payload[0] = (complete << 7) | seqno; 91 | 92 | if (canfetti::Error e = proxy.copyInto(&payload[1], lastBlockBytes); e != canfetti::Error::Success) { 93 | LogInfo("Error writing data: %x", (unsigned)e); 94 | finish(e); 95 | } 96 | 97 | co.bus.write(txCobid, payload); 98 | } 99 | 100 | void Client::segmentWrite() 101 | { 102 | uint8_t payload[8] = {0}; 103 | uint32_t len = proxy.remaining(); 104 | bool complete = len <= 7; 105 | uint32_t toSend = complete ? len : 7; 106 | 107 | payload[0] = (0 << 5) | (toggle << 4) | ((7 - toSend) << 1) | complete; 108 | toggle = !toggle; 109 | 110 | if (canfetti::Error e = proxy.copyInto(&payload[1], toSend); e != canfetti::Error::Success) { 111 | LogInfo("Error writing data: %x", (unsigned)e); 112 | finish(e); 113 | } 114 | 115 | // LogInfo("(%d) %d %d", complete, proxy.len, toSend); 116 | 117 | co.bus.write(txCobid, payload); 118 | } 119 | 120 | void Client::segmentRead() 121 | { 122 | uint8_t payload[8] = {0}; 123 | payload[0] = (3 << 5) | (toggle << 4); 124 | toggle = !toggle; 125 | 126 | co.bus.write(txCobid, payload); 127 | } 128 | 129 | canfetti::Error Client::checkSize(uint32_t msgLen, bool tooBigCheck) 130 | { 131 | if ((msgLen > proxy.remaining()) && !proxy.resize(msgLen)) { 132 | LogInfo("Supplied buf too small on %x[%d] [remote: %d > local: %ld]", proxy.idx, proxy.subIdx, msgLen, proxy.remaining()); 133 | return canfetti::Error::ParamLengthLow; 134 | } 135 | else if (tooBigCheck && (msgLen < proxy.remaining()) && !proxy.resize(msgLen)) { 136 | LogInfo("Supplied buf too big on %x[%d] [remote: %d < local: %ld]", proxy.idx, proxy.subIdx, msgLen, proxy.remaining()); 137 | return canfetti::Error::ParamLengthHigh; 138 | } 139 | 140 | return canfetti::Error::Success; 141 | } 142 | 143 | bool Client::processMsg(const canfetti::Msg &msg) 144 | { 145 | if (Protocol::abortCheck(msg)) return true; 146 | 147 | if (isUploadResponse(msg)) { // Read 148 | uint32_t msgLen = getInitiateDataLen(msg); 149 | 150 | if (isExpedited(msg)) { 151 | canfetti::Error e = checkSize(msgLen, true); 152 | if (e != canfetti::Error::Success) { 153 | finish(e, false); 154 | } 155 | else { 156 | e = proxy.copyFrom(&msg.data[4], msgLen); 157 | finish(e); 158 | } 159 | 160 | return true; 161 | } 162 | else { 163 | // Proactive resize 164 | canfetti::Error e = checkSize(msgLen, true); 165 | if (e != canfetti::Error::Success) { 166 | finish(e); 167 | return true; 168 | } 169 | 170 | segmentRead(); 171 | return false; 172 | } 173 | } 174 | else if (isUploadSegResponse(msg)) { 175 | uint32_t len = 7 - ((msg.data[0] >> 1) & 0b111); 176 | bool complete = msg.data[0] & 0b1; 177 | 178 | if (canfetti::Error e = checkSize(len, false); e != canfetti::Error::Success) { 179 | finish(e, !complete); 180 | return true; 181 | } 182 | else if (canfetti::Error e = proxy.copyFrom(&msg.data[1], len); e != canfetti::Error::Success) { 183 | LogInfo("Error reading data: %x", (unsigned)e); 184 | finish(e, !complete); 185 | return true; 186 | } 187 | else if (!complete) { 188 | segmentRead(); 189 | return false; 190 | } 191 | 192 | finish(canfetti::Error::Success); 193 | return true; 194 | } 195 | else if (isDownloadResponse(msg) || isDownloadSegResponse(msg)) { // Write 196 | if (!proxy.remaining()) { 197 | finish(canfetti::Error::Success); 198 | return true; 199 | } 200 | else { 201 | segmentWrite(); 202 | return false; 203 | } 204 | } 205 | else if (isDownloadBlockResponse(msg)) { 206 | uint8_t ss = msg.data[0] & 3; 207 | if (!proxy.remaining()) { 208 | if (ss == 2) { 209 | uint8_t payload[8] = { 210 | static_cast((6 << 5) | ((7 - lastBlockBytes) << 2)), 211 | 0, 0, 0, 0, 0, 0, 0}; 212 | co.bus.write(txCobid, payload); 213 | return false; 214 | } 215 | else if (ss == 1) { 216 | finish(canfetti::Error::Success, false); 217 | return true; 218 | } 219 | } 220 | else { 221 | for (uint32_t i = 1; i <= 127 && proxy.remaining(); ++i) { 222 | blockSegmentWrite(i); 223 | } 224 | return false; 225 | } 226 | } 227 | 228 | LogInfo("Unhandled SDO protocol: %x", msg.data[0]); 229 | finish(canfetti::Error::InvalidCmd); 230 | return true; 231 | } 232 | -------------------------------------------------------------------------------- /src/services/sdo/Protocol.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/sdo/Protocol.h" 2 | #include 3 | #include 4 | 5 | using namespace canfetti; 6 | using namespace Sdo; 7 | 8 | Protocol::Protocol(uint16_t txCobid, OdProxy proxy, Node &co) 9 | : txCobid(txCobid), proxy(std::move(proxy)), co(co) 10 | { 11 | } 12 | 13 | Protocol::~Protocol() 14 | { 15 | } 16 | 17 | void Protocol::abort(canfetti::Error status, uint16_t txCobid, uint16_t idx, uint8_t subIdx, CanDevice &bus) 18 | { 19 | uint8_t payload[8] = { 20 | 4 << 5, 21 | static_cast(idx & 0xFF), 22 | static_cast(idx >> 8), 23 | subIdx, 24 | }; 25 | 26 | uint32_t err = static_cast(status); 27 | memcpy(&payload[4], &err, 4); 28 | 29 | canfetti::Msg msg = { 30 | .id = txCobid, 31 | .rtr = false, 32 | .len = 8, 33 | .data = payload, 34 | }; 35 | 36 | bus.write(msg); 37 | } 38 | 39 | void Protocol::finish(Error status, bool sendAbort) 40 | { 41 | if (sendAbort && status != Error::Success) { 42 | LogInfo("Sending abort (%x) at %x: %x[%d]", (unsigned)status, txCobid, proxy.idx, proxy.subIdx); 43 | abort(status, txCobid, proxy.idx, proxy.subIdx, co.bus); 44 | } 45 | finished = true; 46 | finishedStatus = status; 47 | } 48 | 49 | std::tuple Protocol::isFinished() 50 | { 51 | return std::make_tuple(finished, finishedStatus); 52 | } 53 | 54 | bool Protocol::abortCheck(const Msg &msg) 55 | { 56 | if (isAbortMsg(msg)) { 57 | uint32_t abortCode; 58 | memcpy(&abortCode, &msg.data[4], 4); 59 | 60 | LogInfo("SDO %x[%d] aborted: %x", proxy.idx, proxy.subIdx, abortCode); 61 | 62 | // Todo bounds check https://stackoverflow.com/questions/4165439/generic-way-to-cast-int-to-enum-in-c 63 | finish(static_cast(abortCode), false); 64 | return true; 65 | } 66 | 67 | return false; 68 | } 69 | 70 | uint32_t Protocol::getInitiateDataLen(const Msg &m) 71 | { 72 | uint8_t es = m.data[0] & 0b11; 73 | switch (es) { 74 | case 0b01: { 75 | uint32_t d; 76 | memcpy(&d, &m.data[4], 4); 77 | return d; 78 | } 79 | 80 | case 0b11: { 81 | uint8_t n = (m.data[0] >> 2) & 0b11; 82 | return 4 - n; 83 | } 84 | 85 | case 0b10: 86 | return 0; // Unspecified number of bytes 87 | 88 | default: 89 | LogInfo("Invalid SDO cmd: %x", m.data[0]); 90 | break; 91 | } 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /src/services/sdo/Server.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/sdo/Server.h" 2 | #include 3 | #include "canfetti/services/sdo/ServerBlockMode.h" 4 | 5 | using namespace canfetti; 6 | using namespace canfetti::Sdo; 7 | 8 | static inline bool isExpedited(const canfetti::Msg &m) { return (m.data[0] >> 1) & 0b1; } 9 | 10 | static inline bool isUploadInitiate(const canfetti::Msg &m) { return (m.data[0] >> 5) == 2; } 11 | static inline bool isUploadSegRequest(const canfetti::Msg &m) { return (m.data[0] >> 5) == 3; } 12 | 13 | static inline bool isDownloadInitiate(const canfetti::Msg &m) { return (m.data[0] >> 5) == 1; } 14 | static inline bool isDownloadSegRequest(const canfetti::Msg &m) { return (m.data[0] >> 5) == 0; } 15 | 16 | static void sendDownloadInitRsp(uint16_t txCobid, uint16_t idx, uint8_t subIdx, OdProxy &proxy, CanDevice &bus) 17 | { 18 | uint8_t payload[8] = { 19 | 3 << 5, 20 | static_cast(proxy.idx & 0xFF), 21 | static_cast(proxy.idx >> 8), 22 | proxy.subIdx, 23 | }; 24 | 25 | bus.write(txCobid, payload); 26 | } 27 | 28 | bool Server::sendUploadInitRsp(uint16_t txCobid, uint16_t idx, uint8_t subIdx, OdProxy &proxy, CanDevice &bus) 29 | { 30 | uint8_t payload[8] = { 31 | 2 << 5, 32 | static_cast(proxy.idx & 0xFF), 33 | static_cast(proxy.idx >> 8), 34 | proxy.subIdx, 35 | }; 36 | 37 | size_t remaining = proxy.remaining(); 38 | 39 | bool expedited = remaining > 0 && remaining <= 4; 40 | 41 | if (expedited) { // Expedited transfer 42 | payload[0] |= 0b11; // expedited, size indicated 43 | payload[0] |= (4 - remaining) << 2; // size = 4-n 44 | if (Error err = proxy.copyInto(&payload[4], remaining); err != Error::Success) { 45 | abort(err, txCobid, idx, subIdx, bus); 46 | return true; 47 | } 48 | } 49 | else { // Segmented transfer 50 | payload[0] |= 0b01; // !expedited, size indicated 51 | uint32_t l = remaining; 52 | memcpy(&payload[4], &l, 4); 53 | } 54 | 55 | bus.write(txCobid, payload); 56 | return expedited; 57 | } 58 | 59 | std::shared_ptr Server::processInitiate(const Msg &msg, uint16_t txCobid, Node &co) 60 | { 61 | uint16_t idx = (msg.data[2] << 8) | msg.data[1]; 62 | uint8_t subIdx = msg.data[3]; 63 | 64 | if (isUploadInitiate(msg)) { // read from us 65 | auto [err, proxy] = co.od.makeProxy(idx, subIdx); 66 | 67 | if (err != Error::Success) { 68 | LogInfo("Bad SDO read for cobid %x: %x[%d], err %x", msg.id, idx, subIdx, (unsigned)err); 69 | abort(err, txCobid, idx, subIdx, co.bus); 70 | } 71 | else if (!sendUploadInitRsp(txCobid, idx, subIdx, proxy, co.bus)) { 72 | return std::make_shared(txCobid, std::move(proxy), co); 73 | } 74 | } 75 | else if (isDownloadInitiate(msg)) { // write to us 76 | auto [err, proxy] = co.od.makeProxy(idx, subIdx); 77 | 78 | if (err != Error::Success) { 79 | LogInfo("Bad SDO write for cobid %x: %x[%d], err %x", msg.id, idx, subIdx, (unsigned)err); 80 | abort(err, txCobid, idx, subIdx, co.bus); 81 | } 82 | else { 83 | uint32_t len = getInitiateDataLen(msg); 84 | 85 | if (proxy.remaining() != len && !proxy.resize(len)) { 86 | LogDebug("Buf too small for write"); 87 | abort(Error::ParamLength, txCobid, idx, subIdx, co.bus); 88 | } 89 | else if (isExpedited(msg)) { 90 | if (Error err = proxy.copyFrom(&msg.data[4], len); err != Error::Success) { 91 | abort(err, txCobid, idx, subIdx, co.bus); 92 | } 93 | else { 94 | sendDownloadInitRsp(txCobid, idx, subIdx, proxy, co.bus); 95 | } 96 | } 97 | else { // Non expedited 98 | sendDownloadInitRsp(txCobid, idx, subIdx, proxy, co.bus); 99 | if (proxy.remaining()) { 100 | return std::make_shared(txCobid, std::move(proxy), co); 101 | } 102 | } 103 | } 104 | } 105 | else if (ServerBlockMode::isDownloadBlockMsg(msg)) { 106 | uint32_t size = *(uint32_t *)&msg.data[4]; 107 | auto [err, proxy] = co.od.makeProxy(idx, subIdx); 108 | if (err == canfetti::Error::Success) { 109 | auto server = std::make_shared(txCobid, std::move(proxy), co, size); 110 | server->sendInitiateResponse(); 111 | return server; 112 | } 113 | } 114 | 115 | return nullptr; 116 | } 117 | 118 | canfetti::Error Server::initiateRead() 119 | { 120 | LogDebug("Initiating read to cobid %x: %x[%d]", txCobid, proxy.idx, proxy.subIdx); 121 | 122 | uint8_t payload[8] = { 123 | 2 << 5, 124 | static_cast(proxy.idx & 0xFF), 125 | static_cast(proxy.idx >> 8), 126 | proxy.subIdx, 127 | 0, 128 | 0, 129 | 0, 130 | 0, 131 | }; 132 | 133 | return co.bus.write(txCobid, payload); 134 | } 135 | 136 | canfetti::Error Server::initiateWrite() 137 | { 138 | LogDebug("Initiating write to cobid %x: %x[%d]", txCobid, proxy.idx, proxy.subIdx); 139 | 140 | uint8_t payload[8] = { 141 | 1 << 5, 142 | static_cast(proxy.idx & 0xFF), 143 | static_cast(proxy.idx >> 8), 144 | proxy.subIdx, 145 | }; 146 | 147 | if (proxy.remaining() <= 4) { // Expedited transfer 148 | payload[0] |= 0b11; // expedited, size indicated 149 | payload[0] |= (4 - proxy.remaining()) << 2; // size = 4-n 150 | 151 | if (Error err = proxy.copyInto(&payload[4], proxy.remaining()); err != Error::Success) { 152 | abort(err, txCobid, proxy.idx, proxy.subIdx, co.bus); 153 | return err; 154 | } 155 | } 156 | else { // Segmented transfer 157 | payload[0] |= 0b01; // !expedited, size indicated 158 | uint32_t l = proxy.remaining(); 159 | memcpy(&payload[4], &l, 4); 160 | } 161 | 162 | return co.bus.write(txCobid, payload); 163 | } 164 | 165 | bool Server::segmentRead() 166 | { 167 | uint8_t payload[8] = {0}; 168 | bool complete = proxy.remaining() <= 7; 169 | uint32_t toSend = complete ? proxy.remaining() : 7; 170 | 171 | payload[0] = (0 << 5) | (toggle << 4) | ((7 - toSend) << 1) | complete; 172 | toggle = !toggle; 173 | 174 | if (Error err = proxy.copyInto(&payload[1], toSend); err != Error::Success) { 175 | finish(err, true); 176 | return true; 177 | } 178 | 179 | // LogInfo("(%d) %d %d", complete, proxy.remaining(), toSend); 180 | 181 | co.bus.write(txCobid, payload); 182 | 183 | if (complete) finish(canfetti::Error::Success); 184 | return complete; 185 | } 186 | 187 | void Server::segmentWrite() 188 | { 189 | uint8_t payload[8] = {0}; 190 | payload[0] = (1 << 5) | (toggle << 4); 191 | toggle = !toggle; 192 | 193 | canfetti::Msg msg = { 194 | .id = txCobid, 195 | .rtr = false, 196 | .len = 8, 197 | .data = payload, 198 | }; 199 | 200 | co.bus.write(msg); 201 | } 202 | 203 | bool Server::processMsg(const canfetti::Msg &msg) 204 | { 205 | if (Protocol::abortCheck(msg)) return true; 206 | 207 | if (isUploadSegRequest(msg)) { // read us 208 | return segmentRead(); 209 | } 210 | else if (isDownloadSegRequest(msg)) { // write us 211 | uint32_t len = 7 - ((msg.data[0] >> 1) & 0b111); 212 | bool complete = msg.data[0] & 0b1; 213 | 214 | if (len > proxy.remaining()) { 215 | LogInfo("Supplied buf too small"); 216 | finish(canfetti::Error::ParamLength); 217 | return true; 218 | } 219 | 220 | if (Error err = proxy.copyFrom(&msg.data[1], len); err != Error::Success) { 221 | abort(err, txCobid, proxy.idx, proxy.subIdx, co.bus); 222 | return true; 223 | } 224 | 225 | // LogInfo("(%d) %d %d", complete, proxy.remaining(), len); 226 | 227 | segmentWrite(); 228 | 229 | if (complete) { 230 | finish(canfetti::Error::Success); 231 | } 232 | 233 | return complete; 234 | } 235 | 236 | LogInfo("Unhandled SDO protocol: %x", msg.data[0]); 237 | finish(canfetti::Error::InvalidCmd); 238 | return true; 239 | } 240 | -------------------------------------------------------------------------------- /src/services/sdo/ServerBlockMode.cpp: -------------------------------------------------------------------------------- 1 | #include "canfetti/services/sdo/ServerBlockMode.h" 2 | #include 3 | 4 | using namespace canfetti; 5 | using namespace canfetti::Sdo; 6 | 7 | //****************************************************************************** 8 | // Public API 9 | //****************************************************************************** 10 | 11 | ServerBlockMode::ServerBlockMode(uint16_t txCobid, 12 | canfetti::OdProxy proxy, 13 | Node &co, 14 | uint32_t totalsize) : Server(txCobid, std::move(proxy), co) 15 | { 16 | } 17 | 18 | void ServerBlockMode::sendInitiateResponse() 19 | { 20 | uint8_t payload[8] = { 21 | static_cast(5 << 5), 22 | static_cast(proxy.idx & 0xFF), 23 | static_cast(proxy.idx >> 8), 24 | proxy.subIdx, 25 | NumSubBlockSegments, 26 | }; 27 | 28 | expectedSeqNo = 1; 29 | co.bus.write(txCobid, payload); 30 | } 31 | 32 | bool ServerBlockMode::processMsg(const canfetti::Msg &msg) 33 | { 34 | switch (state) { 35 | case State::Start: { 36 | uint8_t seqNo = msg.data[0] & 0x7f; 37 | if (expectedSeqNo != seqNo) { 38 | LogInfo("Out of sequence frame (%x, %x)", expectedSeqNo, seqNo); 39 | finish(Error::DataXfer, true); 40 | state = State::End; 41 | return false; 42 | } 43 | 44 | memcpy(lastSegmentData, &msg.data[1], 7); 45 | state = State::SubBlock; 46 | expectedSeqNo = 2; 47 | return false; 48 | } 49 | 50 | case State::SubBlock: { 51 | uint8_t c = msg.data[0] >> 7; 52 | uint8_t seqNo = msg.data[0] & 0x7f; 53 | 54 | if (Error err = proxy.copyFrom(lastSegmentData, 7); err != Error::Success) { 55 | finish(err, true); 56 | state = State::End; 57 | return false; 58 | } 59 | 60 | if (expectedSeqNo != seqNo) { 61 | LogInfo("Out of sequence frame (%x, %x)", expectedSeqNo, seqNo); 62 | finish(Error::DataXfer, true); 63 | // TODO request a resend of the block by sending the last good seqno 64 | // uint8_t payload[8] = { 65 | // static_cast((5 << 5) | 2), 66 | // expectedSeqNo - 1, 67 | // NumSubBlockSegments, 68 | // }; 69 | // co.bus.write(txCobid, payload); 70 | state = State::End; 71 | return false; 72 | } 73 | 74 | memcpy(lastSegmentData, &msg.data[1], 7); 75 | 76 | expectedSeqNo = seqNo + 1; 77 | 78 | if (c) { 79 | state = State::End; 80 | } 81 | 82 | if (seqNo == NumSubBlockSegments || c) { 83 | uint8_t payload[8] = { 84 | static_cast((5 << 5) | 2), 85 | seqNo, 86 | NumSubBlockSegments, 87 | }; 88 | 89 | expectedSeqNo = 1; 90 | co.bus.write(txCobid, payload); 91 | } 92 | 93 | return false; 94 | } 95 | 96 | case State::End: { 97 | if (!isDownloadBlockMsg(msg)) { 98 | // LogInfo("Junk %x", msg.data[0]); 99 | return false; 100 | } 101 | 102 | uint8_t n = (msg.data[0] >> 2) & 0b111; 103 | uint8_t lastLen = 7 - n; 104 | 105 | if (!finished) { 106 | if (Error err = proxy.copyFrom(lastSegmentData, lastLen); err != Error::Success) { 107 | finish(err, true); 108 | } 109 | } 110 | 111 | if (!finished) { 112 | uint8_t payload[8] = {static_cast((5 << 5) | 1)}; 113 | co.bus.write(txCobid, payload); 114 | finish(Error::Success); 115 | } 116 | 117 | return true; 118 | } 119 | } 120 | 121 | return true; 122 | } 123 | --------------------------------------------------------------------------------